diff --git a/nav2_gps_waypoint_follower_demo/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/CMakeLists.txt new file mode 100644 index 0000000..5e5dd24 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/CMakeLists.txt @@ -0,0 +1,80 @@ +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(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 +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_collector_executable_name gps_waypoint_collector) + +add_executable(${gps_waypoint_collector_executable_name} + src/gps_waypoint_collector.cpp) + +ament_target_dependencies(${gps_waypoint_collector_executable_name} + ${dependencies}) + +target_link_libraries(${gps_waypoint_collector_executable_name} ${YAML_CPP_LIBRARIES}) + +install(TARGETS ${gps_client_executable_name} ${gps_waypoint_collector_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() \ No newline at end of file diff --git a/nav2_gps_waypoint_follower_demo/README.md b/nav2_gps_waypoint_follower_demo/README.md new file mode 100644 index 0000000..d39e815 --- /dev/null +++ b/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 +``` diff --git a/nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_collector.hpp b/nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_collector.hpp new file mode 100644 index 0000000..ac0f020 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_collector.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_COLLECTOR_HPP_ +#define NAV2_GPS_WAYPOINT_FOLLOWER_DEMO____GPS_WAYPOINT_COLLECTOR_HPP_ + +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/subscription.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "nav2_msgs/msg/oriented_nav_sat_fix.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.h" +#include "yaml-cpp/emitter.h" + +namespace nav2_gps_waypoint_follower_demo +{ + +class GPSWaypointCollector : public rclcpp::Node +{ +public: + GPSWaypointCollector(/* args */); + ~GPSWaypointCollector(); + + /** + * @brief Typedefs for shortnening Approx time Syncer initialization. + * + */ + typedef message_filters::sync_policies::ApproximateTime + SensorDataApprxTimeSyncPolicy; + typedef message_filters::Synchronizer SensorDataApprxTimeSyncer; + +private: + void timerCallback(); + + void sensorDataCallback( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & gps, + const sensor_msgs::msg::Imu::ConstSharedPtr & imu); + + void dumpCollectedWaypoints(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr oriented_navsat_fix_publisher_; + message_filters::Subscriber navsat_fix_subscriber_; + message_filters::Subscriber imu_subscriber_; + std::shared_ptr sensor_data_approx_time_syncher_; + + sensor_msgs::msg::NavSatFix reusable_navsat_msg_; + sensor_msgs::msg::Imu reusable_imu_msg_; + + std::vector> collected_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_COLLECTOR_HPP_ diff --git a/nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp b/nav2_gps_waypoint_follower_demo/include/nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp new file mode 100644 index 0000000..e36b8b0 --- /dev/null +++ b/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 +#include + +#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.h" +/** + * @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; + + /** + * @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 nav2_msgs::msg::OrientedNavSatFix + * Note that this parameter needs to be an array of doubles + * + * @return nav2_msgs::msg::OrientedNavSatFix + */ + std::vector + loadGPSWaypointsFromYAML(); + + void goalResponseCallback(GPSWaypointFollowerGoalHandle::SharedPtr goal_handle); + + void feedbackCallback( + GPSWaypointFollowerGoalHandle::SharedPtr, + const std::shared_ptr 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::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 gps_poses_from_yaml_; +}; +} // namespace nav2_gps_waypoint_follower_demo + +#endif // NAV2_GPS_WAYPOINT_FOLLOWER_DEMO__GPS_WAYPOINT_FOLLOWER_DEMO_HPP_ diff --git a/nav2_gps_waypoint_follower_demo/launch/city_world.launch.py b/nav2_gps_waypoint_follower_demo/launch/city_world.launch.py new file mode 100644 index 0000000..68e33f7 --- /dev/null +++ b/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' + ), + ]) diff --git a/nav2_gps_waypoint_follower_demo/launch/demo_gps_waypoint_follower.launch.py b/nav2_gps_waypoint_follower_demo/launch/demo_gps_waypoint_follower.launch.py new file mode 100644 index 0000000..9a2baaf --- /dev/null +++ b/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, + ]) diff --git a/nav2_gps_waypoint_follower_demo/launch/dual_ekf_navsat_localization.launch.py b/nav2_gps_waypoint_follower_demo/launch/dual_ekf_navsat_localization.launch.py new file mode 100644 index 0000000..df14098 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/launch/dual_ekf_navsat_localization.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +import launch_ros.actions +import os +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + nav2_gps_waypoint_follower_demo_dir = get_package_share_directory('nav2_gps_waypoint_follower_demo') + parameters_file_dir = os.path.join(nav2_gps_waypoint_follower_demo_dir, 'params') + parameters_file_path = os.path.join(parameters_file_dir, 'dual_ekf_navsat_localization.yaml') + return LaunchDescription([ + launch_ros.actions.Node( + package='robot_localization', + executable='ekf_node', + name='ekf_local_filter_node', + output='screen', + parameters=[parameters_file_path], + remappings=[('odometry/filtered', 'odometry/local')] + ), + launch_ros.actions.Node( + package='robot_localization', + executable='ekf_node', + name='ekf_global_filter_node', + output='screen', + parameters=[parameters_file_path], + remappings=[('odometry/filtered', 'odometry/global')] + ), + launch_ros.actions.Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform_node', + output='screen', + parameters=[parameters_file_path], + remappings=[('imu/data', 'imu/data'), + ('gps/fix', 'gps/fix'), + ('gps/filtered', 'gps/filtered'), + ('odometry/gps', 'odometry/gps'), + ('odometry/filtered', 'odometry/global')] + ) +]) \ No newline at end of file diff --git a/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_collector.launch.py b/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_collector.launch.py new file mode 100644 index 0000000..832e8d2 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/launch/gps_waypoint_collector.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +import launch_ros.actions +import os +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + package='nav2_gps_waypoint_follower_demo', + executable='gps_waypoint_collector', + name='gps_waypoint_collector_node', + output='screen', + remappings=[('/gps', '/gps/fix'), + ('/imu', '/imu/data')], + parameters=[{'frequency': 1}, + {'yaml_file_out': "/home/atas/collectedpoints.yaml"}] + ) +]) diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/grass_plane/CMakeLists.txt new file mode 100644 index 0000000..9e1ccdc --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/CMakeLists.txt @@ -0,0 +1,9 @@ +set(MODEL_NAME grass_plane) +set (files + model.config + model.sdf +) + +add_subdirectory(materials) + +install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/) diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/CMakeLists.txt new file mode 100644 index 0000000..2e0c22a --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(scripts) +add_subdirectory(textures) diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/CMakeLists.txt new file mode 100644 index 0000000..129ba7a --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/CMakeLists.txt @@ -0,0 +1,5 @@ +set (files + grass.material +) + +install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/scripts/) diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/grass.material b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/grass.material new file mode 100644 index 0000000..af2d6e0 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/scripts/grass.material @@ -0,0 +1,20 @@ +material vrc/grass +{ + receive_shadows on + technique + { + pass + { + ambient 0.8 1.0 0.8 1.0 + diffuse 0.8 1.0 0.8 1.0 + specular 0.1 0.1 0.1 1.0 12.5 + + texture_unit + { + texture grass_dry.png + filtering anistropic + max_anisotropy 16 + } + } + } +} diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/CMakeLists.txt new file mode 100644 index 0000000..d754787 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/CMakeLists.txt @@ -0,0 +1,5 @@ +set (files + grass_dry.png +) + +install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/textures) diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/grass_dry.png b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/grass_dry.png new file mode 100644 index 0000000..fc6280d Binary files /dev/null and b/nav2_gps_waypoint_follower_demo/models/grass_plane/materials/textures/grass_dry.png differ diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/model.config b/nav2_gps_waypoint_follower_demo/models/grass_plane/model.config new file mode 100644 index 0000000..b6282f5 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/model.config @@ -0,0 +1,17 @@ + + + + grass plane + 1.0 + model.sdf + + + Nate Koenig + nate@osrfoundation.com + + + + A grass textured plane. + + + diff --git a/nav2_gps_waypoint_follower_demo/models/grass_plane/model.sdf b/nav2_gps_waypoint_follower_demo/models/grass_plane/model.sdf new file mode 100644 index 0000000..d70517a --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/grass_plane/model.sdf @@ -0,0 +1,1717 @@ + + + + true + + + + + 0 0 1 + 500 500 + + + + + + 0.5 + .5 + + + + + + + -40 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -20 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 0 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 20 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 -40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -40 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -20 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 0 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 20 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 -20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -40 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -20 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 0 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 20 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 0 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -40 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + -20 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 0 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 20 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 20 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 40 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 60 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 180 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 200 80 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 180 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 200 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 220 100 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 180 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 200 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 220 120 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + + 40 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 140 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 160 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 180 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 200 140 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 40 160 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 60 160 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 80 160 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 100 160 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + 120 160 0 0 0 0 + + false + + + 0 0 1 + 20 20 + + + + + + + + + + diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/CMakeLists.txt new file mode 100644 index 0000000..56ff222 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/CMakeLists.txt @@ -0,0 +1,9 @@ +set(MODEL_NAME vrc_heightmap_1) +set (files + model.config + model.sdf +) + +add_subdirectory(materials) + +install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/) diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/CMakeLists.txt new file mode 100644 index 0000000..240c301 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(textures) diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/CMakeLists.txt b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/CMakeLists.txt new file mode 100644 index 0000000..7649cf3 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/CMakeLists.txt @@ -0,0 +1,5 @@ +set (files + heightmap.png +) + +install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/textures/) diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/heightmap.png b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/heightmap.png new file mode 100644 index 0000000..900d29b Binary files /dev/null and b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/materials/textures/heightmap.png differ diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.config b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.config new file mode 100644 index 0000000..ee016bf --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.config @@ -0,0 +1,17 @@ + + + + VRC Heightmap 1 + 1.0 + model.sdf + + + Nate Koenig + nate@osrfoundation.com + + + + Terrain for task 1 of VRC. + + + diff --git a/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.sdf b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.sdf new file mode 100644 index 0000000..e0d90b8 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/models/vrc_heightmap_1/model.sdf @@ -0,0 +1,51 @@ + + + + true + + + + + model://vrc_heightmap_1/materials/textures/heightmap.png + 500 500 118 + 0 0 -15 + + + + + + + + + file://media/materials/textures/dirt_diffusespecular.png + file://media/materials/textures/flat_normal.png + 5 + + + file://media/materials/textures/grass_diffusespecular.png + file://media/materials/textures/flat_normal.png + 5 + + + file://media/materials/textures/fungus_diffusespecular.png + file://media/materials/textures/flat_normal.png + 20 + + + 15 + 5 + + + 30 + 10 + + model://vrc_heightmap_1/materials/textures/heightmap.png + 500 500 118 + 0 0 -15 + + + + + + + diff --git a/nav2_gps_waypoint_follower_demo/package.xml b/nav2_gps_waypoint_follower_demo/package.xml new file mode 100644 index 0000000..4b00f5a --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/package.xml @@ -0,0 +1,34 @@ + + + + nav2_gps_waypoint_follower_demo + 0.0.0 + 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` + Fetullah Atas + Fetullah Atas + Apache-2.0 + ament_cmake + tf2_ros + nav2_common + rclcpp + rclcpp_action + rclcpp_lifecycle + nav_msgs + sensor_msgs + nav2_msgs + nav2_util + nav2_core + nav2_waypoint_follower + geometry_msgs + nav2_lifecycle_manager + std_msgs + tf2_geometry_msgs + visualization_msgs + robot_localization + ament_lint_common + ament_lint_auto + + ament_cmake + + \ No newline at end of file diff --git a/nav2_gps_waypoint_follower_demo/params/demo_gps_waypoints.yaml b/nav2_gps_waypoint_follower_demo/params/demo_gps_waypoints.yaml new file mode 100644 index 0000000..b8f8d10 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/params/demo_gps_waypoints.yaml @@ -0,0 +1,9 @@ +gps_waypoint_follower_demo: + ros__parameters: + waypoints: [wp0,wp1,wp2,wp3,wp4] + #lat, long, alt, yaw(radians) + wp0: [49.89998982019267, 8.900039241979549, 0.6345732646788921 , 2.09] + wp1: [49.89998358037406, 8.900016823533321, 0.6345732646788921 , 1.85] + wp2: [49.89998680505144, 8.89999067488557, 0.6345732646788921 , 1.44] + wp3: [49.89998527600633, 8.899909345260799, 0.6345732646788921, 1.54] + wp4: [49.90005500137158, 8.899806649640007, 0.6345732646788921, -0.53] diff --git a/nav2_gps_waypoint_follower_demo/params/dual_ekf_navsat_localization.yaml b/nav2_gps_waypoint_follower_demo/params/dual_ekf_navsat_localization.yaml new file mode 100644 index 0000000..0be05ba --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/params/dual_ekf_navsat_localization.yaml @@ -0,0 +1,115 @@ +# This is configuration for local pose estmation EKF node +ekf_local_filter_node: + ros__parameters: + use_sim_time: true + clear_params: true + publish_tf: true + filter_type: "ekf" + frequency: 30.0 + sensor_timeout: 0.1 + odom0: /odometry/wheel # change this according to your odometry source + imu0: /imu/data # your imu topic + odom_frame: odom # odometry frame + base_link_frame: base_link + world_frame: odom + map_frame: map + odom0_config: [false, false, false, # X , Y , Z + false, false, false, # roll , pitch ,yaw + true, true, true, # dX , dY , dZ + false, false, false, # droll , dpitch ,dyaw + false, false, false] # ddX , ddY , ddZ + odom0_relative: false + odom0_differential: false + odom0_queue_size: 10 + imu0_config: [false, false, false, # X , Y , Z + false, false, true, # roll , pitch ,yaw + false, false, false, # dX , dY , dZ + false, false, true, # droll , dpitch ,dyaw + false, false, false] # ddX , ddY , ddZ + imu0_relative: false + imu0_differential: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] +# This is configuration for global pose estmation EKF node +ekf_global_filter_node: + ros__parameters: + use_sim_time: true + clear_params: true + publish_tf: true + filter_type: "ekf" + frequency: 30.0 + sensor_timeout: 0.1 + odom0: /odometry/wheel + odom1: /odometry/gps # attention to this, this is coming from below node: navsat_transform_node + imu0: /imu/data + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map # we set world frame o map here, menaing that globl frmae will be map + odom0_config: [false, false, false, # X , Y , Z + false, false, false, # roll , pitch ,yaw + true, true, true, # dX , dY , dZ + false, false, true, # droll , dpitch ,dyaw + false, false, false] # ddX , ddY , ddZ + odom0_relative: false + odom0_differential: false + odom0_queue_size: 10 + odom1_config: [true, true, false, # X , Y , Z + false, false, false, # roll , pitch ,yaw + false, false, false, # dX , dY , dZ + false, false, false, # droll , dpitch ,dyaw + false, false, false] # ddX , ddY , ddZ + odom1_relative: false + odom1_differential: false + odom1_queue_size: 10 + imu0_config: [false, false, false, # X , Y , Z + false, false, true, # roll , pitch ,yaw + false, false, false, # dX , dY , dZ + false, false, true, # droll , dpitch ,dyaw + false, false, false] # ddX , ddY , ddZ + imu0_relative: false + imu0_differential: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + +navsat_transform_node: + ros__parameters: + frequency: 10.0 + delay: 1.0 + magnetic_declination_radians: 0.0 + yaw_offset: 0.0 + zero_altitude: false + publish_filtered_gps: true + use_odometry_yaw: true + broadcast_utm_transform: false + broadcast_utm_transform_as_parent_frame: true # this is required when we convert GPS waypoint to map frame diff --git a/nav2_gps_waypoint_follower_demo/src/gps_waypoint_collector.cpp b/nav2_gps_waypoint_follower_demo/src/gps_waypoint_collector.cpp new file mode 100644 index 0000000..a330541 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/src/gps_waypoint_collector.cpp @@ -0,0 +1,131 @@ +// Copyright (c) 2020 Fetullah Atas, Norwegian University of Life Sciences +// +// 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. + +#include +#include +#include +#include +#include +#include +#include "nav2_gps_waypoint_follower_demo/gps_waypoint_collector.hpp" + +namespace nav2_gps_waypoint_follower_demo +{ + +GPSWaypointCollector::GPSWaypointCollector() +: Node("gps_waypoint_collector_rclcpp_node"), is_first_msg_recieved_(false) +{ + declare_parameter("frequency", 10); + declare_parameter("yaml_file_out", "/home/user_name/collected_waypoints.yaml"); + + get_parameter("frequency", frequency_); + get_parameter("yaml_file_out", yaml_file_out_); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(1000 / frequency_), + std::bind(&GPSWaypointCollector::timerCallback, this)); + + navsat_fix_subscriber_.subscribe(this, "/gps", rmw_qos_profile_sensor_data); + imu_subscriber_.subscribe(this, "/imu", rmw_qos_profile_sensor_data); + + oriented_navsat_fix_publisher_ = this->create_publisher( + "collected_gps_waypoints", rclcpp::SystemDefaultsQoS()); + + sensor_data_approx_time_syncher_.reset( + new SensorDataApprxTimeSyncer( + SensorDataApprxTimeSyncPolicy(10), navsat_fix_subscriber_, + imu_subscriber_)); + + sensor_data_approx_time_syncher_->registerCallback( + std::bind( + &GPSWaypointCollector::sensorDataCallback, this, std::placeholders::_1, + std::placeholders::_2)); +} + +GPSWaypointCollector::~GPSWaypointCollector() +{ + dumpCollectedWaypoints(); +} + +void GPSWaypointCollector::timerCallback() +{ + RCLCPP_INFO_ONCE(this->get_logger(), "Entering to timer callback, this is periodicly called"); + if (is_first_msg_recieved_) { + std::lock_guard guard(global_mutex_); + tf2::Quaternion q( + reusable_imu_msg_.orientation.x, + reusable_imu_msg_.orientation.y, + reusable_imu_msg_.orientation.z, + reusable_imu_msg_.orientation.w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + RCLCPP_INFO( + this->get_logger(), + "curr_gps_waypoint: [%.8f, %.8f, %.8f, %.8f]", reusable_navsat_msg_.latitude, + reusable_navsat_msg_.longitude, reusable_navsat_msg_.altitude, yaw); + + std::vector curr_waypoint_data = + {reusable_navsat_msg_.latitude, + reusable_navsat_msg_.longitude, + reusable_navsat_msg_.altitude, yaw}; + + collected_waypoints_vector_.push_back(curr_waypoint_data); + nav2_msgs::msg::OrientedNavSatFix curr_gps_waypoint; + curr_gps_waypoint.position = reusable_navsat_msg_; + curr_gps_waypoint.orientation = reusable_imu_msg_.orientation; + oriented_navsat_fix_publisher_->publish(curr_gps_waypoint); + } +} + +void GPSWaypointCollector::sensorDataCallback( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & gps, + const sensor_msgs::msg::Imu::ConstSharedPtr & imu) +{ + std::lock_guard guard(global_mutex_); + reusable_navsat_msg_ = *gps; + reusable_imu_msg_ = *imu; + is_first_msg_recieved_ = true; +} + +void GPSWaypointCollector::dumpCollectedWaypoints() +{ + YAML::Emitter waypoints; + waypoints << YAML::BeginMap; + waypoints << YAML::Key << "waypoints" << YAML::Value << "see all points colected here"; + for (size_t i = 0; i < collected_waypoints_vector_.size(); i++) { + std::string wp_name = "wp" + std::to_string(i); + waypoints << YAML::Key << wp_name << YAML::Value << wp_name; + waypoints << YAML::BeginSeq << + collected_waypoints_vector_[i][0] << + collected_waypoints_vector_[i][1] << + collected_waypoints_vector_[i][2] << + collected_waypoints_vector_[i][3] << YAML::EndSeq; + } + waypoints << YAML::EndMap; + std::ofstream fout(yaml_file_out_, std::ofstream::out); + fout << waypoints.c_str(); + fout.close(); +} + +} // namespace nav2_gps_waypoint_follower_demo + +int main(int argc, char const * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_gps_waypoint_follower_demo/src/gps_waypoint_follower_demo.cpp b/nav2_gps_waypoint_follower_demo/src/gps_waypoint_follower_demo.cpp new file mode 100644 index 0000000..9b363c0 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/src/gps_waypoint_follower_demo.cpp @@ -0,0 +1,211 @@ +// 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. + +#include +#include +#include "nav2_gps_waypoint_follower_demo/gps_waypoint_follower_demo.hpp" + +namespace nav2_gps_waypoint_follower_demo +{ +GPSWayPointFollowerClient::GPSWayPointFollowerClient() +: Node("GPSWaypointFollowerClient"), goal_done_(false) +{ + gps_waypoint_follower_action_client_ = + rclcpp_action::create_client( + this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "follow_gps_waypoints"); + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&GPSWayPointFollowerClient::startWaypointFollowing, this)); + // number of poses that robot will go throug, specified in yaml file + this->declare_parameter("waypoints", std::vector({"0"})); + gps_poses_from_yaml_ = loadGPSWaypointsFromYAML(); + RCLCPP_INFO( + this->get_logger(), + "Loaded %i GPS waypoints from YAML, gonna pass them to FollowGPSWaypoints...", + static_cast(gps_poses_from_yaml_.size())); + RCLCPP_INFO( + this->get_logger(), + "Created an Instance of GPSWayPointFollowerClient"); +} + +GPSWayPointFollowerClient::~GPSWayPointFollowerClient() +{ + RCLCPP_INFO( + this->get_logger(), + "Destroyed an Instance of GPSWayPointFollowerClient"); +} + +void GPSWayPointFollowerClient::startWaypointFollowing() +{ + using namespace std::placeholders; + this->timer_->cancel(); + this->goal_done_ = false; + + if (!this->gps_waypoint_follower_action_client_) { + RCLCPP_ERROR(this->get_logger(), "Action client not initialized"); + } + + auto is_action_server_ready = + gps_waypoint_follower_action_client_->wait_for_action_server( + std::chrono::seconds(1)); + if (!is_action_server_ready) { + RCLCPP_ERROR( + this->get_logger(), "FollowGPSWaypoints action server is not available." + " Make sure an instance of GPSWaypointFollower is up and running"); + this->goal_done_ = true; + return; + } + gps_waypoint_follower_goal_ = ClientT::Goal(); + // Send the goal poses + gps_waypoint_follower_goal_.gps_poses = gps_poses_from_yaml_; + + RCLCPP_INFO( + this->get_logger(), + "Sending a path of %zu gps_poses:", gps_waypoint_follower_goal_.gps_poses.size()); + for (auto pose : gps_waypoint_follower_goal_.gps_poses) { + RCLCPP_DEBUG( + this->get_logger(), + "\t(%lf, %lf)", pose.position.latitude, pose.position.longitude); + } + + auto goal_options = + rclcpp_action::Client::SendGoalOptions(); + + goal_options.goal_response_callback = std::bind( + &GPSWayPointFollowerClient::goalResponseCallback, this, _1); + + goal_options.feedback_callback = + std::bind(&GPSWayPointFollowerClient::feedbackCallback, this, _1, _2); + + goal_options.result_callback = std::bind( + &GPSWayPointFollowerClient::resultCallback, this, _1); + + auto future_goal_handle = gps_waypoint_follower_action_client_->async_send_goal( + gps_waypoint_follower_goal_, goal_options); + +} + +std::vector +GPSWayPointFollowerClient::loadGPSWaypointsFromYAML() +{ + std::vector waypoints_vector = + this->get_parameter("waypoints").as_string_array(); + std::vector gps_waypoint_msg_vector; + for (auto && curr_waypoint : waypoints_vector) { + try { + this->declare_parameter(curr_waypoint, std::vector({0})); + std::vector gps_waypoint_vector = + this->get_parameter(curr_waypoint).as_double_array(); + // throw exception if incorrect format was detected from yaml file reading + if (gps_waypoint_vector.size() != 4) { + RCLCPP_FATAL( + this->get_logger(), + "GPS waypoint that was loaded from YAML file seems to have incorrect" + " form, the right format is; wpN: [Lat, Long, Alt, yaw(in radians)] with double types"); + throw rclcpp::exceptions::InvalidParametersException( + "[ERROR] See above error" + " the right format is; wpN: [Lat, Long, Alt, yaw(in radians)] with double types" + "E.g gps_waypoint0; [0.0, 0.0, 0.0, 0.0], please chechk YAML file"); + } + // construct the gps waypoint and push them to their vector + // lat, long , alt + nav2_msgs::msg::OrientedNavSatFix gps_pose; + gps_pose.position.latitude = gps_waypoint_vector.at(0); + gps_pose.position.longitude = gps_waypoint_vector.at(1); + gps_pose.position.altitude = gps_waypoint_vector.at(2); + tf2::Quaternion gps_pose_quat; + gps_pose_quat.setRPY(0.0, 0.0, gps_waypoint_vector.at(3)); + gps_pose.orientation = tf2::toMsg(gps_pose_quat); + gps_waypoint_msg_vector.push_back(gps_pose); + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + } + } + // return the read pair of this gps waypoint to it's caller + return gps_waypoint_msg_vector; +} + +void GPSWayPointFollowerClient::goalResponseCallback( + GPSWaypointFollowerGoalHandle::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void GPSWayPointFollowerClient::feedbackCallback( + GPSWaypointFollowerGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_INFO( + this->get_logger(), + "Current waypoint: %i", feedback->current_waypoint); +} + +void GPSWayPointFollowerClient::resultCallback( + const GPSWaypointFollowerGoalHandle::WrappedResult & result) +{ + this->goal_done_ = true; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Result received"); + for (auto number : result.result->missed_waypoints) { + RCLCPP_INFO(this->get_logger(), "Missed Waypoint %i", number); + } +} + +bool GPSWayPointFollowerClient::is_goal_done() const +{ + return this->goal_done_; +} + +} // namespace nav2_gps_waypoint_follower_demo + +/** + * @brief Entry point for Way Point following demo Node + * + * @param argc + * @param argv + * @return int + */ +int main(int argc, char const * argv[]) +{ + rclcpp::init(argc, argv); + auto gps_waypoint_follower_client_node = std::make_shared + (); + + while (!gps_waypoint_follower_client_node->is_goal_done()) { + rclcpp::spin_some(gps_waypoint_follower_client_node); + } + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_gps_waypoint_follower_demo/worlds/city_world/city_world.model b/nav2_gps_waypoint_follower_demo/worlds/city_world/city_world.model new file mode 100644 index 0000000..12b7fd9 --- /dev/null +++ b/nav2_gps_waypoint_follower_demo/worlds/city_world/city_world.model @@ -0,0 +1,371 @@ + + + + + + + -11.660200 -5.942800 6.053900 0.000000 0.443643 0.352194 + + + + 0.2 0.2 0.2 1.0 + false + + + 4 + + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + false + 0 0 100 0 0 0 + 0.8 0.8 0.8 1 + 0.9 0.9 0.9 1 + + 1000 + 0.9 + 0.01 + 0.001 + + 0.2 0.2 -0.9 + + + false + -14 0 5 0 0 0 + 0.3 0.3 0.3 1 + 0.0 0.0 0.0 1 + + 80 + 0.9 + 0.01 + 0.001 + + + + model://vrc_heightmap_1 + + + model://grass_plane + + + model://speed_limit_sign + speed_sign_1 + true + 15 -5 0 0 0 1.5707 + + + model://speed_limit_sign + speed_sign_2 + true + 70 5 0 0 0 1.5707 + + + model://speed_limit_sign + speed_sign_3 + true + 121 13 0 0 0 2.356 + + + model://speed_limit_sign + speed_sign_4 + true + 155 68 0 0 0 3.1415 + + + model://speed_limit_sign + speed_sign_5 + true + 175 95 0 0 0 1.5707 + + + model://house_1 + house_1_a + 12 9.5 0 0 0 0 + + + model://house_1 + house_1_b + 129.5 46 0 0 0 1.5707 + + + model://mailbox + mailbox_0 + true + 18 4 0 0 0 0 + + + model://mailbox + mailbox_1 + true + 24.5 -4.45 0 0 0 0 + + + model://gas_station + gas_station + 77 15 0 0 0 -1.5707963267948966 + + + model://house_2 + house_2_a + 30 -8 0 0 0 0 + + + model://house_2 + house_2_b + 158 91 0 0 0 0 + + + model://house_3 + house_3_a + 44 7 0 0 0 0 + + + model://house_3 + house_3_b + 111 26 0 0 0 .78539 + + + model://house_3 + house_3_c + 170 106 0 0 0 0 + + + model://mailbox + mailbox_2 + true + 59 4.45 0 0 0 0 + + + model://house_3 + house_3_d + 95 7.0 0 0 0 0 + + + model://dumpster + true + 143 51.5 0 0 0 0 + + + model://house_2 + house_2_c + 135 84.8 0 0 0 0 + + + model://lamp_post + lamp_post_1 + true + 24 4 0 0 0 0 + + + model://lamp_post + lamp_post_2 + true + 54 4 0 0 0 0 + + + model://lamp_post + lamp_post_3 + true + 84 4 0 0 0 0 + + + model://lamp_post + lamp_post_4 + true + 14 -4 0 0 0 3.1415 + + + model://lamp_post + lamp_post_5 + true + 44 -4 0 0 0 3.1415 + + + model://lamp_post + lamp_post_6 + true + 74 -4 0 0 0 3.1415 + + + model://lamp_post + lamp_post_7 + true + 148 104 0 0 0 0 + + + model://lamp_post + lamp_post_8 + true + 178 104 0 0 0 0 + + + model://lamp_post + lamp_post_9 + true + 158 96 0 0 0 3.1415 + + + model://lamp_post + lamp_post_10 + true + 188 96 0 0 0 3.1415 + + + model://lamp_post + lamp_post_11 + true + 102 8 0 0 0 .785 + + + model://lamp_post + lamp_post_12 + true + 123 29 0 0 0 .785 + + + model://lamp_post + lamp_post_13 + true + 144 50 0 0 0 .785 + + + model://lamp_post + lamp_post_14 + true + 99 -4 0 0 0 3.1415 + + + model://lamp_post + lamp_post_15 + true + 120 14.5 0 0 0 -2.35 + + + model://lamp_post + lamp_post_16 + true + 141 35.5 0 0 0 -2.35 + + + 7.34 + 0 0 0.01 + 100 0 0.01 + 150 50 0.01 + 150 100 0.01 + 200 100 0.01 + + + 17 + 65 25 0.01 + 65 3.66 0.01 + + + true + + + 51 0 0 0 0 0 + + + 102 7.34 0.02 + + + + + + 0.8 + 0.8 + + + + + + 125 25 0 0 0 0.78539 + + + 72 7.34 0.02 + + + + + + 0.8 + 0.8 + + + + + + 150 75 0 0 0 0 + + + 7.34 53 0.02 + + + + + + 0.8 + 0.8 + + + + + + 173.5 100 0 0 0 0 + + + 54.5 7.34 0.02 + + + + + + 0.8 + 0.8 + + + + + + 198 106 0 0 0 0 + + + 6 5 0.02 + + + + + + 0.8 + 0.8 + + + + + + + + \ No newline at end of file