Add nav2_gps_waypoint_follower

nav2_gps_waypoint_follower/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_gps_waypoint_follower)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_lifecycle_manager REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(robot_localization REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(executable_name gps_waypoint_follower)
+
+add_executable(${executable_name}
+  src/gps_waypoint_follower.cpp
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  nav2_util
+  nav2_lifecycle_manager
+  nav_msgs
+  nav2_msgs
+  nav2_core
+  tf2_ros
+  robot_localization
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name})
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()

nav2_gps_waypoint_follower/README.md

@@ -0,0 +1,26 @@
+# Nav2 GPS Waypoint Follower
+
+This package is analogous to `nav2_waypoint_follower`, `nav2_gps_waypoint_follower` provides an action server interface that accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower`.  The action msg has following properties; 
+
+```yaml
+#goal definition
+sensor_msgs/NavSatFix[] waypoints
+---
+#result definition
+int32[] missed_waypoints
+---
+#feedback
+uint32 current_waypoint
+```
+
+A use case 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_gps_waypoint_follower/include/nav2_gps_waypoint_follower/gps_waypoint_follower.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_
+#define NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_
+
+#include <vector>
+#include <string>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "geometry_msgs/msg/point32.hpp"
+#include "nav2_msgs/action/follow_gps_waypoints.hpp"
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+#include "nav2_msgs/action/follow_waypoints.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "robot_localization/srv/to_ll.hpp"
+#include "robot_localization/srv/from_ll.hpp"
+
+/**
+ * @brief namespace for way point following, points are from a yaml file
+ *
+ */
+namespace nav2_gps_waypoint_follower
+{
+enum class ActionStatus
+{
+  UNKNOWN = 0,
+  PROCESSING = 1,
+  FAILED = 2,
+  SUCCEEDED = 3
+};
+/**
+ * @brief A ros lifecyle node that drives robot through gievn way points from YAML file
+ *
+ */
+class GPSWaypointFollower : public nav2_util::LifecycleNode
+{
+public:
+  // Shorten the types
+  using ActionT = nav2_msgs::action::FollowGPSWaypoints;
+  using ClientT = nav2_msgs::action::FollowWaypoints;
+  using ActionServer = nav2_util::SimpleActionServer<ActionT>;
+  using ActionClient = rclcpp_action::Client<ClientT>;
+  using WaypointFollowerGoalHandle =
+    rclcpp_action::ClientGoalHandle<ClientT>;
+
+  /**
+   * @brief Construct a new Way Point Folllower Demo object
+   *
+   */
+  GPSWaypointFollower();
+
+  /**
+   * @brief Destroy the Way Point Folllower Demo object
+   *
+   */
+  ~GPSWaypointFollower();
+
+protected:
+  /**
+   * @brief Configures member variables
+   *
+   * Initializes action server for "FollowWaypoints"
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivates action server
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Resets member variables
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in shutdown state
+   * @param state Reference to LifeCycle node state
+   * @return SUCCESS or FAILURE
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Action client result callback
+   * @param result Result of action server updated asynchronously
+   */
+  void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result);
+
+  /**
+   * @brief Action client goal response callback
+   * @param goal Response of action server updated asynchronously
+   */
+  void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal);
+
+  /**
+   * @brief send robot through each of the pose in poses vector
+   *
+   * @param poses
+   */
+  void followGPSWaypoints();
+
+  /**
+   * @brief
+   *
+   */
+  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);
+
+  // dedicated node of client(FollowWaypoints)
+  rclcpp::Node::SharedPtr client_node_;
+  // FollowGPSWaypoints action server
+  std::unique_ptr<ActionServer> action_server_;
+  // client to call server from robot_localization to do UTM -> Map conversion
+  rclcpp::Client<robot_localization::srv::FromLL>::SharedPtr from_ll_to_map_client_;
+  // client to connect waypoint follower service(FollowWaypoints)
+  rclcpp_action::Client<ClientT>::SharedPtr waypoint_follower_action_client_;
+  // global var to get information about current goal state
+  ActionStatus current_goal_status_;
+  // stores the waypoints in a vector with additional info such as
+  // "int32[] missed_waypoints" and "uint32
+  // current_waypoint"
+  ClientT::Goal waypoint_follower_goal_;
+  // goal handler to query state of goal
+  WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
+
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+};
+}  // namespace nav2_gps_waypoint_follower
+
+#endif  // NAV2_GPS_WAYPOINT_FOLLOWER__GPS_WAYPOINT_FOLLOWER_HPP_

nav2_gps_waypoint_follower/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>nav2_gps_waypoint_follower</name>
+    <version>0.0.0</version>
+    <description>An action server interface for GPS waypoint following, converts GPS points to map frame with service 
+                 from `robot_localization` and navigates through the points
+                 with FollowWaypoints action server from `nav2_waypoint_follower`</description>
+    <author email="[email protected]">Fetullah Atas</author>
+    <license>Apache-2.0</license>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <depend>tf2_ros</depend>
+    <depend>nav2_common</depend>
+    <depend>rclcpp</depend>
+    <depend>rclcpp_action</depend>
+    <depend>rclcpp_lifecycle</depend>
+    <depend>nav_msgs</depend>
+    <depend>sensor_msgs</depend>
+    <depend>nav2_msgs</depend>
+    <depend>nav2_util</depend>
+    <depend>nav2_core</depend>
+    <depend>geometry_msgs</depend>
+    <depend>nav2_lifecycle_manager</depend>
+    <depend>std_msgs</depend>
+    <depend>tf2_geometry_msgs</depend>
+    <depend>visualization_msgs</depend>
+    <depend>robot_localization</depend>
+    <test_depend>ament_lint_common</test_depend>
+    <test_depend>ament_lint_auto</test_depend>
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>