Update nav2_gps_waypoint_follower_demo

nav2_gps_waypoint_follower_demo/CMakeLists.txt

@@ -0,0 +1,82 @@
+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(geographic_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(tf2_geometry_msgs REQUIRED)
+find_package(robot_localization REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(yaml-cpp REQUIRED )
+
+nav2_package()
+
+include_directories(
+  include
+  ${YAML_CPP_INCLUDE_DIR})
+
+set(dependencies
+rclcpp
+rclcpp_action
+rclcpp_lifecycle
+nav2_util
+nav2_lifecycle_manager
+nav_msgs
+geographic_msgs
+nav2_msgs
+nav2_core
+nav2_waypoint_follower
+tf2_ros
+tf2_geometry_msgs
+robot_localization
+)
+
+set(gps_client_executable_name gps_waypoint_follower_demo)
+
+add_executable(${gps_client_executable_name}
+  src/gps_waypoint_follower_demo.cpp)
+
+
+ament_target_dependencies(${gps_client_executable_name}
+  ${dependencies})
+
+target_link_libraries(${gps_client_executable_name})
+
+set(gps_waypoint_logger_executable_name gps_waypoint_logger)
+
+add_executable(${gps_waypoint_logger_executable_name}
+  src/gps_waypoint_logger.cpp)
+
+ament_target_dependencies(${gps_waypoint_logger_executable_name}
+  ${dependencies})
+
+target_link_libraries(${gps_waypoint_logger_executable_name} ${YAML_CPP_LIBRARIES})
+
+install(TARGETS ${gps_client_executable_name} ${gps_waypoint_logger_executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/
+  DESTINATION include/)
+
+install(
+  DIRECTORY launch params models worlds
+  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/README.md

@@ -0,0 +1,44 @@
+## nav2_gps_waypoint_follower_demo
+
+A tutorial package provided in `navigation2_tutorials` to showcase the usage of `FollowGPSWaypoints` action. 
+`FollowGPSWaypoints` is an action interface in `nav2_waypoint_follower`, which directly accepts GPS(lat,long,alt) waypoints and navigates robot through them. You can see the Readme page of `nav2_waypoint_follower` package for more details of this action but 
+in a glance, we can summarize this as: 
+`robot_localization`'s `navsat_transform_node` provides a service named `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude)
+to cartesian coordinates in map frame(x,y), then the existent Follow Waypoints logic from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. Finally this process is exposed as `FollowGPSWaypoins` action. 
+
+This package includes a sample Gazebo world that was used to collect GPS waypoints. You can collect GPS waypoints given that your robot is attached with an GPS sensor that provides (lat,long,alt). Then you can subscribe to the GPS topic and save the waypoints to YAML file given in `params` folder of this package. 
+e.g `ros2 topic echo /gps/fix`. 
+
+The format of yaml file is expected to have; 
+
+```yaml
+waypoints: [wp0,wp1,wp2,wp3,wp4]
+#lat, long, alt
+wp0: [9.677703999088216e-07, -5.306676831178058e-05, 0.6442248001694679]
+wp1: [9.677703999088216e-07, -5.306676831178058e-05, 0.6442248001694679]
+wp2: [4.169383611283205e-05, -0.0006143364570898212, 0.6346865268424153]
+wp3: [9.319715737387455e-05, -0.000620772355007051, 0.6348643703386188]
+wp4: [8.37498018946476e-06, -2.402470336058297e-05, 0.6447164406999946]
+```
+where `waypoints` is basically a vector that includes parameters name for each waypoint. Note that correct format when entering a waypoints is:  `wpN:[lat, long, alt]`, all three variables should be `double` types. 
+
+## Start the Gazebo world
+Fist you need to make sure `GAZEBO_MODEL_PATH` IS set such that the specifc models this package uses are in on the path. Do this by; 
+
+```bash
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/home/YOUR_USER_NAME/colcon_ws_rolling/src/navigation2_tutorials/nav2_gps_waypoint_follower_demo/models
+```
+```bash
+ros2 launch nav2_gps_waypoint_follower_demo city_world.launch.py
+```
+The above command should start gazebo world, at this point you should figure out a way to spawn your robot into that world.
+
+## Request waypoint following
+
+The package has an executable node named `gps_waypoint_follower_demo` which reads the yaml file in `params` and creates a client to `FollowGPSWaypoins` action, then requests the waypoint following. You need to make sure, `waypoint_follower` lifecycle node is up and runnning as the  `FollowGPSWaypoins` action is exposed by that. 
+
+You can launch this node by ;
+
+```bash
+ros2 launch nav2_gps_waypoint_follower_demo demo_gps_waypoint_follower.launch.py
+```

nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp

@@ -0,0 +1,117 @@
+// 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"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.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 geographic_msgs::msg::GeoPose
+ *  Note that this parameter needs to be an array of doubles
+ *
+ * @return geographic_msgs::msg::GeoPose
+ */
+  std::vector<geographic_msgs::msg::GeoPose>
+  loadGPSWaypointsFromYAML();
+
+  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<geographic_msgs::msg::GeoPose> gps_poses_from_yaml_;
+};
+}  // namespace nav2_gps_waypoint_follower_demo
+
+#endif  // NAV2_GPS_WAYPOINT_FOLLOWER_DEMO__GPS_WAYPOINT_FOLLOWER_DEMO_HPP_

nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_logger.hpp

@@ -0,0 +1,79 @@
+// 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_LOGGER_HPP_
+#define NAV2_GPS_WAYPOINT_FOLLOWER_DEMO____GPS_WAYPOINT_LOGGER_HPP_
+
+#include <mutex>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/subscription.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "geographic_msgs/msg/geo_pose.hpp"
+
+#include "message_filters/synchronizer.h"
+#include "message_filters/subscriber.h"
+#include "message_filters/sync_policies/approximate_time.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "yaml-cpp/emitter.h"
+
+namespace nav2_gps_waypoint_follower_demo
+{
+
+class GPSWaypointLogger : public rclcpp::Node
+{
+public:
+  GPSWaypointLogger(/* args */);
+  ~GPSWaypointLogger();
+
+  /**
+ * @brief Typedefs for shortnening Approx time Syncer initialization.
+ *
+ */
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::NavSatFix,
+      sensor_msgs::msg::Imu>
+    SensorDataApprxTimeSyncPolicy;
+  typedef message_filters::Synchronizer<SensorDataApprxTimeSyncPolicy> SensorDataApprxTimeSyncer;
+
+private:
+  void timerCallback();
+
+  void sensorDataCallback(
+    const sensor_msgs::msg::NavSatFix::ConstSharedPtr & gps,
+    const sensor_msgs::msg::Imu::ConstSharedPtr & imu);
+
+  void dumpLoggedWaypoints();
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<geographic_msgs::msg::GeoPose>::SharedPtr geopose_publisher_;
+  message_filters::Subscriber<sensor_msgs::msg::NavSatFix> navsat_fix_subscriber_;
+  message_filters::Subscriber<sensor_msgs::msg::Imu> imu_subscriber_;
+  std::shared_ptr<SensorDataApprxTimeSyncer> sensor_data_approx_time_syncher_;
+
+  sensor_msgs::msg::NavSatFix reusable_navsat_msg_;
+  sensor_msgs::msg::Imu reusable_imu_msg_;
+
+  std::vector<std::vector<double>> logged_waypoints_vector_;
+
+  // to ensure safety when accessing global
+  std::mutex global_mutex_;
+  bool is_first_msg_recieved_;
+
+  int frequency_;
+  std::string yaml_file_out_;
+};
+
+}  // namespace nav2_gps_waypoint_follower_demo
+
+#endif  // NAV2_GPS_WAYPOINT_FOLLOWER_DEMO____GPS_WAYPOINT_LOGGER_HPP_

nav2_gps_waypoint_follower_demo/launch/city_world.launch.py

@@ -0,0 +1,48 @@
+# 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.
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import ThisLaunchFileDir
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+
+#GAZEBO_WORLD = os.environ['GAZEBO_WORLD']
+GAZEBO_WORLD = 'city_world'
+
+def generate_launch_description():
+
+    DeclareLaunchArgument('gui', default_value='true',
+                          description='Set to "false" to run headless.'),
+
+    DeclareLaunchArgument('debug', default_value='true',
+                          description='Set to "false" not to run gzserver.'),
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    world_file_name = GAZEBO_WORLD + '/' + GAZEBO_WORLD +'.model'
+    world = os.path.join(get_package_share_directory(
+        'nav2_gps_waypoint_follower_demo'), 'worlds', world_file_name)
+    launch_file_dir = os.path.join(
+        get_package_share_directory('nav2_gps_waypoint_follower_demo'), 'launch')
+
+    return LaunchDescription([
+        ExecuteProcess(
+            cmd=['gazebo', '--verbose', world,
+                 '-s', 'libgazebo_ros_factory.so'],
+            output='screen'
+        ),
+    ])