Add nav2_gps_waypoint_follower

nav2_msgs/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(sensor_msgs)
 find_package(action_msgs REQUIRED)
 
 nav2_package()
@@ -36,7 +37,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/Spin.action"
   "action/DummyRecovery.action"
   "action/FollowWaypoints.action"
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs
+  "action/FollowGPSWaypoints.action"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs sensor_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

nav2_msgs/action/FollowGPSWaypoints.action

@@ -0,0 +1,8 @@
+#goal definition
+sensor_msgs/NavSatFix[] waypoints
+---
+#result definition
+int32[] missed_waypoints
+---
+#feedback
+uint32 current_waypoint

nav2_msgs/package.xml

@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>action_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 

nav2_waypoint_follower/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(nav2_util REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(robot_localization REQUIRED) 
 
 nav2_package()
 
@@ -53,6 +54,7 @@ set(dependencies
   image_transport
   cv_bridge
   OpenCV
+  robot_localization
 )
 
 ament_target_dependencies(${executable_name}

nav2_waypoint_follower/README.md

@@ -24,3 +24,29 @@ In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a pro
 In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy.
 
 Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case.
+
+## Nav2 GPS Waypoint Follower
+
+`nav2_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself.  The action for GPS waypoint following has properties as; 
+
+```yaml
+#goal definition
+sensor_msgs/NavSatFix[] waypoints
+---
+#result definition
+int32[] missed_waypoints
+---
+#feedback
+uint32 current_waypoint
+```
+
+In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`.  
+For instance;
+
+```cpp
+using ClientT = nav2_msgs::action::FollowGPSWaypoints;
+rclcpp_action::Client<ClientT>::SharedPtr gps_waypoint_follower_action_client_;
+gps_waypoint_follower_action_client_ = rclcpp_action::create_client<ClientT>(this, "FollowGPSWaypoints");
+```
+
+All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor.  

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

@@ -30,6 +30,10 @@
 #include "nav2_core/waypoint_task_executor.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "nav2_msgs/action/follow_gps_waypoints.hpp"
+#include "robot_localization/srv/from_ll.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 namespace nav2_waypoint_follower
 {
@@ -55,6 +59,13 @@ class WaypointFollower : public nav2_util::LifecycleNode
   using ActionServer = nav2_util::SimpleActionServer<ActionT>;
   using ActionClient = rclcpp_action::Client<ClientT>;
 
+  // Shorten the types for GPS waypoint following
+  using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints;
+  using ClientTGPS = nav2_msgs::action::FollowWaypoints;
+  using ActionServerGPS = nav2_util::SimpleActionServer<ActionTGPS>;
+  using ActionClientGPS = rclcpp_action::Client<ClientTGPS>;
+  using WaypointFollowerGoalHandle =
+    rclcpp_action::ClientGoalHandle<ClientTGPS>;
   /**
    * @brief A constructor for nav2_waypoint_follower::WaypointFollower class
    */
@@ -115,16 +126,62 @@ class WaypointFollower : public nav2_util::LifecycleNode
    */
   void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal);
 
-  // Our action server
+  /**
+   * @brief send robot through each of GPS
+   *        point , which are converted to map frame first then using a client to
+   *        `FollowWaypoints` action.
+   *
+   * @param waypoints, acquired from action client
+   */
+  void followGPSWaypoints();
+
+/**
+ * @brief given some gps_waypoints, converts them to map frame using robot_localization's service `fromLL`.
+ *        Constructs a vector of stamped poses in map frame and returns them.
+ *
+ * @param gps_waypoints
+ * @param parent_node
+ * @param fromll_client
+ * @return std::vector<geometry_msgs::msg::PoseStamped>
+ */
+  static std::vector<geometry_msgs::msg::PoseStamped> convertGPSWaypointstoPosesinMap(
+    const
+    std::vector<sensor_msgs::msg::NavSatFix> & gps_waypoints,
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node,
+    const rclcpp::Client<robot_localization::srv::FromLL>::SharedPtr & fromll_client);
+
+  /**
+   * @brief Action client result callback
+   * @param result Result of action server updated asynchronously
+   */
+  void GPSResultCallback(const rclcpp_action::ClientGoalHandle<ClientTGPS>::WrappedResult & result);
+
+  /**
+   * @brief Action client goal response callback
+   * @param goal Response of action server updated asynchronously
+   */
+  void GPSGoalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientTGPS>::SharedPtr & goal);
+
+  // Our action server for waypoint following
   std::unique_ptr<ActionServer> action_server_;
   ActionClient::SharedPtr nav_to_pose_client_;
-  rclcpp::Node::SharedPtr client_node_;
+  rclcpp::Node::SharedPtr nav_to_pose_client_node_;
   std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
   bool stop_on_failure_;
   ActionStatus current_goal_status_;
   int loop_rate_;
   std::vector<int> failed_ids_;
 
+  // Our action server for GPS waypoint following
+  std::unique_ptr<ActionServerGPS> gps_action_server_;
+  rclcpp::Client<robot_localization::srv::FromLL>::SharedPtr from_ll_to_map_client_;
+  ActionClientGPS::SharedPtr waypoint_follower_action_client_;
+  rclcpp::Node::SharedPtr waypoint_follower_client_node_;
+  ActionStatus gps_current_goal_status_;
+  ClientTGPS::Goal waypoint_follower_goal_;
+  WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   // Task Execution At Waypoint Plugin
   pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor>
   waypoint_task_executor_loader_;

nav2_waypoint_follower/package.xml

@@ -21,7 +21,8 @@
   <depend>nav2_util</depend>
   <depend>nav2_core</depend>
   <depend>tf2_ros</depend>
-
+  <depend>robot_localization</depend> 
+
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>