Add nav2_gps_waypoint_follower

doc/parameters/param_list.md

@@ -534,6 +534,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | ----------| --------| ------------|
 | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. |
 | loop_rate | 20 | Rate to check for results from current navigation task |
+| transform_tolerance | 0.1 | Amount of time in seconds to wait before a timeout exception is thrown while transforming a GPS waypoint form `utm` frame to `map ` frame. |
 | waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.|
 
 ## WaitAtWaypoint plugin

nav2_bringup/bringup/params/nav2_params.yaml

@@ -292,6 +292,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    transform_tolerance: 0.1
     waypoint_task_executor_plugin: "wait_at_waypoint"   
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"

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()
@@ -21,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/BehaviorTreeLog.msg"
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
+  "msg/OrientedNavSatFix.msg"
   "srv/GetCostmap.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"
@@ -36,7 +38,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
+nav2_msgs/OrientedNavSatFix[] gps_poses
+---
+#result definition
+int32[] missed_waypoints
+---
+#feedback
+uint32 current_waypoint

nav2_msgs/msg/OrientedNavSatFix.msg

@@ -0,0 +1,2 @@
+sensor_msgs/NavSatFix position
+geometry_msgs/Quaternion orientation

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

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_waypoint_follower)
 
+# Default to C++17
+set(CMAKE_CXX_STANDARD 17)
+
 # Try for OpenCV 4.X, but settle for whatever is installed
 find_package(OpenCV 4 QUIET)
 if(NOT OpenCV_FOUND)
@@ -21,6 +24,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 +57,7 @@ set(dependencies
   image_transport
   cv_bridge
   OpenCV
+  robot_localization
 )
 
 ament_target_dependencies(${executable_name}

nav2_waypoint_follower/README.md

@@ -24,3 +24,22 @@ 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 named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself.
+
+`robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude)
+to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. 
+The action msg definition for GPS waypoint following can be found [here](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/FollowGPSWaypoints.action).
+
+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

@@ -19,17 +19,24 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_msgs/msg/oriented_nav_sat_fix.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_msgs/action/follow_waypoints.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
 #include "nav2_util/node_utils.hpp"
+#include "nav2_msgs/action/follow_gps_waypoints.hpp"
+#include "nav2_util/service_client.hpp"
 #include "nav2_core/waypoint_task_executor.hpp"
-#include "pluginlib/class_loader.hpp"
-#include "pluginlib/class_list_macros.hpp"
+
+#include "robot_localization/srv/from_ll.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2_ros/transform_listener.h"
 
 namespace nav2_waypoint_follower
 {
@@ -55,6 +62,10 @@ 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 ActionServerGPS = nav2_util::SimpleActionServer<ActionTGPS>;
+
   /**
    * @brief A constructor for nav2_waypoint_follower::WaypointFollower class
    */
@@ -98,32 +109,87 @@ class WaypointFollower : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  /**
+   * @brief Templated function to perform internal logic behind waypoint following,
+   *        Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so.
+   *        Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details.
+   *
+   * @tparam T action_server
+   * @tparam V feedback
+   * @tparam Z result
+   * @param action_server
+   * @param poses
+   * @param feedback
+   * @param result
+   */
+  template<typename T, typename V, typename Z>
+  void followWaypointsLogic(
+    const T & action_server,
+    const V & feedback,
+    const Z & result);
+
   /**
    * @brief Action server callbacks
    */
-  void followWaypoints();
+  void followWaypointsCallback();
 
   /**
-   * @brief Action client result callback
-   * @param result Result of action server updated asynchronously
+   * @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 resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result);
+  void followGPSWaypointsCallback();
+
+  /**
+ * @brief Action client result callback
+ * @param result Result of action server updated asynchronously
+ */
+  template<typename T>
+  void resultCallback(const T & result);
 
   /**
    * @brief Action client goal response callback
    * @param goal Response of action server updated asynchronously
    */
-  void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal);
+  template<typename T>
+  void goalResponseCallback(const T & goal);
+
+/**
+ * @brief given some gps_poses, 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_poses
+ * @param parent_node
+ * @param fromll_client
+ * @return std::vector<geometry_msgs::msg::PoseStamped>
+ */
+  std::vector<geometry_msgs::msg::PoseStamped> convertGPSPoses2MapPoses(
+    const std::vector<nav2_msgs::msg::OrientedNavSatFix> & gps_poses,
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent_node,
+    const
+    std::unique_ptr<nav2_util::ServiceClient<robot_localization::srv::FromLL>> & fromll_client);
+
+  template<typename T>
+  std::vector<geometry_msgs::msg::PoseStamped> getLatestGoalPoses(const T & action_server);
+
+  // Common vars used for both GPS and cartesian point following
+  rclcpp::Node::SharedPtr client_node_;
+  std::vector<int> failed_ids_;
+  int loop_rate_;
+  bool stop_on_failure_;
 
-  // Our action server
+  // Our action server for waypoint following
   std::unique_ptr<ActionServer> action_server_;
   ActionClient::SharedPtr nav_to_pose_client_;
-  rclcpp::Node::SharedPtr 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_;
+  std::unique_ptr<nav2_util::ServiceClient<robot_localization::srv::FromLL>> from_ll_to_map_client_;
+  double transform_tolerance_;
 
   // Task Execution At Waypoint Plugin
   pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor>
@@ -132,6 +198,10 @@ class WaypointFollower : public nav2_util::LifecycleNode
   waypoint_task_executor_;
   std::string waypoint_task_executor_id_;
   std::string waypoint_task_executor_type_;
+
+  // tf buffer to get transfroms
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
 
 }  // namespace nav2_waypoint_follower

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>