add nav2_gps_waypoint_follower_demo

nav2_gps_waypoint_follower_demo/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_gps_waypoint_follower_demo)
+
+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_waypoint_follower 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(gps_client_executable_name gps_waypoint_follower_demo)
+
+add_executable(${gps_client_executable_name}
+  src/gps_waypoint_follower_demo.cpp)
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  nav2_util
+  nav2_lifecycle_manager
+  nav_msgs
+  nav2_msgs
+  nav2_core
+  nav2_waypoint_follower
+  tf2_ros
+  robot_localization
+)
+
+ament_target_dependencies(${gps_client_executable_name}
+  ${dependencies})
+
+target_link_libraries(${gps_client_executable_name})
+install(TARGETS ${gps_client_executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/
+  DESTINATION include/)
+
+install(
+  DIRECTORY launch params
+  DESTINATION share/${PROJECT_NAME})
+
+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_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp

@@ -0,0 +1,114 @@
+// 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_DEMO__GPS_WAYPOINT_FOLLOWER_DEMO_HPP_
+#define NAV2_GPS_WAYPOINT_FOLLOWER_DEMO__GPS_WAYPOINT_FOLLOWER_DEMO_HPP_
+
+#include <vector>
+#include <string>
+
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+#include "nav2_msgs/action/follow_waypoints.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "geometry_msgs/msg/point32.hpp"
+#include "nav2_waypoint_follower/waypoint_follower.hpp"
+#include "nav2_msgs/action/follow_gps_waypoints.hpp"
+/**
+ * @brief namespace for way point following, points are from a yaml file
+ *
+ */
+namespace nav2_gps_waypoint_follower_demo
+{
+enum class ActionStatus
+{
+  UNKNOWN = 0,
+  PROCESSING = 1,
+  FAILED = 2,
+  SUCCEEDED = 3
+};
+
+/**
+ * @brief A ros node that drives robot through gievn way points from YAML file
+ *
+ */
+class GPSWayPointFollowerClient : public rclcpp::Node
+{
+public:
+  using ClientT = nav2_msgs::action::FollowGPSWaypoints;
+  // shorten the Goal handler Client type
+  using GPSWaypointFollowerGoalHandle =
+    rclcpp_action::ClientGoalHandle<ClientT>;
+
+  /**
+   * @brief Construct a new WayPoint Folllower Demo object
+   *
+   */
+  GPSWayPointFollowerClient();
+
+  /**
+   * @brief Destroy the Way Point Folllower Demo object
+   *
+   */
+  ~GPSWayPointFollowerClient();
+
+  /**
+   * @brief send robot through each of the pose in poses vector
+   *
+   * @param poses
+   */
+  void startWaypointFollowing();
+
+  /**
+   * @brief
+   *
+   * @return true
+   * @return false
+   */
+  bool is_goal_done() const;
+
+  /**
+ * @brief given a parameter name on the yaml file, loads this parameter as sensor_msgs::msg::NavSatFix
+ *  Note that this parameter needs to be an array of doubles
+ *
+ * @param param_name
+ * @return sensor_msgs::msg::NavSatFix
+ */
+  std::vector<sensor_msgs::msg::NavSatFix>
+  loadGPSWaypointsFromYAML(std::string waypoint_name_prefix, int num_waypoints);
+
+  void goalResponseCallback(GPSWaypointFollowerGoalHandle::SharedPtr goal_handle);
+
+  void feedbackCallback(
+    GPSWaypointFollowerGoalHandle::SharedPtr,
+    const std::shared_ptr<const ClientT::Feedback> feedback);
+
+  void resultCallback(const GPSWaypointFollowerGoalHandle::WrappedResult & result);
+
+protected:
+  bool goal_done_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  // client to connect waypoint follower service(FollowWaypoints)
+  rclcpp_action::Client<ClientT>::SharedPtr
+    gps_waypoint_follower_action_client_;
+
+  // goal handler to query state of goal
+  ClientT::Goal gps_waypoint_follower_goal_;
+
+  GPSWaypointFollowerGoalHandle::SharedPtr gps_waypoint_follower_goalhandle_;
+
+  std::vector<sensor_msgs::msg::NavSatFix> gps_waypoints_from_yaml_;
+};
+}  // namespace nav2_gps_waypoint_follower_demo
+
+#endif  // NAV2_GPS_WAYPOINT_FOLLOWER_DEMO__GPS_WAYPOINT_FOLLOWER_DEMO_HPP_

nav2_gps_waypoint_follower_demo/launch/demo_gps_wpf.launch.py

@@ -0,0 +1,54 @@
+# 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.
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import LifecycleNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.actions import EmitEvent
+from launch.actions import RegisterEventHandler
+from launch_ros.events.lifecycle import ChangeState
+from launch_ros.events.lifecycle import matches_node_name
+from launch_ros.event_handlers import OnStateTransition
+from launch.actions import LogInfo
+from launch.events import matches_action
+from launch.event_handlers.on_shutdown import OnShutdown
+
+import lifecycle_msgs.msg
+import os
+
+
+def generate_launch_description():
+    share_dir = get_package_share_directory(
+        'nav2_gps_waypoint_follower_demo')
+    parameter_file = LaunchConfiguration('params_file')
+    node_name = 'gps_waypoint_follower_demo'
+
+    params_declare = DeclareLaunchArgument('params_file',
+                                           default_value=os.path.join(
+                                               share_dir, 'params', 'city_world_gps_waypoints.yaml'),
+                                           description='FPath to the ROS2 parameters file to use.')
+
+    driver_node = LifecycleNode(package='nav2_gps_waypoint_follower_demo',
+                                executable='gps_waypoint_follower_demo',
+                                name=node_name,
+                                output='screen',
+                                parameters=[parameter_file],
+                                )
+
+    return LaunchDescription([
+        params_declare,
+        driver_node,
+    ])

nav2_gps_waypoint_follower_demo/package.xml

@@ -0,0 +1,34 @@
+<?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_demo</name>
+    <version>0.0.0</version>
+    <description>Reads GPS waypoint from a file, uses `nav2_gps_waypoint_follower` to convert points to map frame and navigates through the points
+                 with FollowWaypoints server from `nav2_waypoint_follower`</description>
+    <author email="[email protected]">Fetullah Atas</author>
+    <maintainer email="[email protected]">Fetullah Atas</maintainer>
+    <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>nav2_waypoint_follower</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>

nav2_gps_waypoint_follower_demo/params/city_world_gps_waypoints.yaml

@@ -0,0 +1,26 @@
+gps_waypoint_follower_demo:
+  ros__parameters:
+    number_of_gps_waypoints: 21
+    #long, lat, alt
+    gps_waypoint0: [9.677703999088216e-07, -5.306676831178058e-05, 0.6442248001694679]
+    gps_waypoint1: [9.677703999088216e-07, -5.306676831178058e-05, 0.6442248001694679]
+    gps_waypoint2: [3.1139105755464975e-06, -0.00017456442820206204, 0.6444645449519157]
+    gps_waypoint3: [4.22315917828845e-06, -0.00023834113403818424, 0.6447720658034086]
+    gps_waypoint4: [5.733249480810798e-06, -0.00032208389604156264, 0.6448167748749256]
+    gps_waypoint5: [7.383338580758601e-06, -0.00041311675834673534, 0.6448821565136313]
+    gps_waypoint6: [9.214683482042934e-06, -0.0005185112536830417, 0.644977854564786]
+    gps_waypoint7: [1.0501804114763628e-05, -0.0005927400620565918, 0.6450572097674012]
+    gps_waypoint8: [4.169383611283205e-05, -0.0006143364570898212, 0.6346865268424153]
+    gps_waypoint9: [9.319715737387455e-05, -0.000620772355007051, 0.6348643703386188]
+    gps_waypoint10: [0.00016136807000342732, -0.000630472090460399, 0.6346333334222436]
+    gps_waypoint11: [0.00019289536480012745, -0.0006025216750314517, 0.6347960121929646]
+    gps_waypoint12: [0.00016725512736177336, -0.0005682180030561106, 0.6347330678254366]
+    gps_waypoint13: [9.548175925016343e-05, -0.0005566846481087024, 0.6348178731277585]
+    gps_waypoint14: [2.1370495894155417e-05, -0.0005427454926971877, 0.6447623465210199]
+    gps_waypoint15: [1.4067128652996008e-06, -0.0004089532192350932, 0.6448766821995378]
+    gps_waypoint16: [1.251103351683063e-05, -0.000262679057585972, 0.6447812123224139]
+    gps_waypoint17: [1.5709760573368857e-05, -0.00018905817925624448, 0.644750733859837]
+    gps_waypoint18: [1.4568988705645404e-05, -0.0001429542567161331, 0.6447363374754786]
+    gps_waypoint19: [1.2338132128329487e-05, -9.000069320371602e-05, 0.6447243737056851]
+    gps_waypoint20: [8.37498018946476e-06, -2.402470336058297e-05, 0.6447164406999946]
+