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 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/include/nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp

@@ -0,0 +1,115 @@
+// 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
+ *
+ * @return sensor_msgs::msg::NavSatFix
+ */
+  std::vector<sensor_msgs::msg::NavSatFix>
+  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<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/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'
+        ),
+    ])

nav2_gps_waypoint_follower_demo/launch/demo_gps_waypoint_follower.launch.py

@@ -0,0 +1,55 @@
+# 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', 'demo_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,
+                                namespace='',
+                                output='screen',
+                                parameters=[parameter_file],
+                                )
+
+    return LaunchDescription([
+        params_declare,
+        driver_node,
+    ])

nav2_gps_waypoint_follower_demo/models/dumpster/materials/scripts/dumpster.material

@@ -0,0 +1,29 @@
+material Dumpster/Diffuse
+{
+  receive_shadows off
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture Dumpster_Diffuse.png
+      }
+    }
+  }
+}
+
+material Dumpster/Specular
+{
+  receive_shadows off
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture Dumpster_Spec.png
+      }
+    }
+  }
+}