🕹️ Waypoint Navigation DEMO¶

Launch a DEMO¶

To launch the default DEMO of the Waypoint Navigation, you can use the following command:

ros2 launch pal_advanced_navigation_demo pal_wp_navigation_demo_1.launch.py robot_name:=<your_robot> base_type:=<your_base>

In this demo you will see the robot navigating through the waypoints and performing some actions at each waypoint.

At the first waypoint the robot will wait for 2 seconds;
At the second waypoint the robot will perform a spin for 6.28 radiants;
At the third waypoint the robot will wait for 5 seconds and subsequently spin in place for 3.14 radiants.

You can keep track of the tasks’ status by reading in the log.

Create your own Waypoint Navigation DEMO¶

If you want to create and launch your customized Wapoint Navigation DEMO you should create your node and launch the DEMO like this:

open a terminal and launch the following command:
ros2 launch pal_advanced_navigation_demo pal_wp_navigation_demo_base.launch.py robot_name:=<your_robot> base_type:=<your_base>
open another terminal and run your node:
ros2 run <your_demo_package> <your_wp_navigation_demo_node>

After including all the necessary headers, you need to initialize the node and create the action client:

rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("your_demo_node");

auto ac_wp_nav =
rclcpp_action::create_client<pal_nav2_msgs::action::NavigateThroughWaypoints>(node, "navigate_through_waypoints");

class NavigateClient(Node):
    def __init__(self):
    super().__init__('your_demo_node')
    self._action_client = ActionClient(self, NavigateThroughWaypoints, 'navigate_through_waypoints')

Then you need to create the goal message and fill it with the waypoints and the actions you want to perform at each waypoint. You can also set a navigation logic by choosing a behavior tree.

pal_nav2_msgs::action::NavigateThroughWaypoints::Goal goal;
goal.number_of_loops = 0;
goal.behavior_tree = "follow_waypoints_w_process";

/* define some waypoints and
the actions to be performed at each waypoint */

goal.waypoints.push_back(your_waypoint);

goal = NavigateThroughWaypoints.Goal()
goal.number_of_loops = 0
goal.behavior_tree = "follow_waypoints_w_process"

# define some waypoints and
#  the actions to be performed at each waypoint

goal.waypoints.append(your_waypoint)

Finally, you need to send the goal to the action server and wait for the result.

auto send_goal_options = rclcpp_action::Client<pal_nav2_msgs::action::NavigateThroughWaypoints>::SendGoalOptions();
ac_wp_nav->async_send_goal(goal, send_goal_options);

def send_goal(self, goal):
    self._action_client.wait_for_server()

    self._send_goal_future = self._action_client.send_goal_async(
        goal,
        feedback_callback=self.feedback_callback
    )
    self._send_goal_future.add_done_callback(self.goal_response_callback)


def main(args=None):
    rclpy.init(args=args)
    navigate_client = NavigateClient()

    # Here some code to define waypoints and actions

    navigate_client.send_goal(goal)