diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 5a395091f0..90508778bb 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.2 + 1.2.3

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index db761cb31d..b2c30c432f 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 79b1df5bb4..2dcb21e42b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -196,6 +196,11 @@ class BtActionServer */ void populateErrorCode(typename std::shared_ptr result); + /** + * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs + */ + void cleanErrorCodes(); + // Action name std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 24126ed4ab..3e2ee0aee8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -26,6 +26,7 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { @@ -60,6 +61,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 900.0); + } std::vector error_code_names = { "follow_path_error_code", @@ -122,19 +126,38 @@ bool BtActionServer::on_configure() // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); + // Declare parameters for common client node applications to share with BT nodes + // Declare if not declared in case being used an external application, then copying + // all of the main node's parameters to the client for BT nodes to obtain + nav2_util::declare_parameter_if_not_declared( + node, "global_frame", rclcpp::ParameterValue(std::string("map"))); + nav2_util::declare_parameter_if_not_declared( + node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + nav2_util::copy_all_parameters(node, client_node_); + + // set the timeout in seconds for the action server to discard goal handles if not finished + double action_server_result_timeout; + node->get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name_, std::bind(&BtActionServer::executeCallback, this)); + action_name_, std::bind(&BtActionServer::executeCallback, this), + nullptr, std::chrono::milliseconds(500), false, server_options); // Get parameters for BT timeouts - int timeout; - node->get_parameter("bt_loop_duration", timeout); - bt_loop_duration_ = std::chrono::milliseconds(timeout); - node->get_parameter("default_server_timeout", timeout); - default_server_timeout_ = std::chrono::milliseconds(timeout); + int bt_loop_duration; + node->get_parameter("bt_loop_duration", bt_loop_duration); + bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration); + int default_server_timeout; + node->get_parameter("default_server_timeout", default_server_timeout); + default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -224,6 +247,7 @@ void BtActionServer::executeCallback() { if (!on_goal_received_callback_(action_server_->get_current_goal())) { action_server_->terminate_current(); + cleanErrorCodes(); return; } @@ -278,6 +302,8 @@ void BtActionServer::executeCallback() action_server_->terminate_all(result); break; } + + cleanErrorCodes(); } template @@ -304,6 +330,14 @@ void BtActionServer::populateErrorCode( } } +template +void BtActionServer::cleanErrorCodes() +{ + for (const auto & error_code : error_code_names_) { + blackboard_->set(error_code, 0); //NOLINT + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 1d449dca2d..6531272d72 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.2 + 1.2.3 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp index 332ac62578..4fd35c4f03 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp @@ -63,7 +63,7 @@ class Wait : public TimedBehavior CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;} protected: - std::chrono::time_point wait_end_; + rclcpp::Time wait_end_; WaitAction::Feedback::SharedPtr feedback_; }; diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 1c6addb764..9e86fb0ab7 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -133,9 +133,19 @@ class TimedBehavior : public nav2_core::Behavior node->get_parameter("robot_base_frame", robot_base_frame_); node->get_parameter("transform_tolerance", transform_tolerance_); + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 10.0); + } + + double action_server_result_timeout; + node->get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + action_server_ = std::make_shared( node, behavior_name_, - std::bind(&TimedBehavior::execute, this)); + std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds( + 500), false, server_options); local_collision_checker_ = local_collision_checker; global_collision_checker_ = global_collision_checker; diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 10311c5970..d469054d48 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.2 + 1.2.3 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index e47de4a8f8..91e95d6c97 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -30,21 +30,19 @@ Wait::~Wait() = default; ResultStatus Wait::onRun(const std::shared_ptr command) { - wait_end_ = std::chrono::steady_clock::now() + - rclcpp::Duration(command->time).to_chrono(); + wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time); return ResultStatus{Status::SUCCEEDED}; } ResultStatus Wait::onCycleUpdate() { - auto current_point = std::chrono::steady_clock::now(); - auto time_left = - std::chrono::duration_cast(wait_end_ - current_point).count(); + auto current_point = node_.lock()->now(); + auto time_left = wait_end_ - current_point; - feedback_->time_left = rclcpp::Duration(rclcpp::Duration::from_nanoseconds(time_left)); + feedback_->time_left = time_left; action_server_->publish_feedback(feedback_); - if (time_left > 0) { + if (time_left.nanoseconds() > 0) { return ResultStatus{Status::RUNNING}; } else { return ResultStatus{Status::SUCCEEDED}; diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 574a817ecb..16088452cb 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -1,8 +1,8 @@ # nav2_bringup -The `nav2_bringup` package is an example bringup system for Nav2 applications. +The `nav2_bringup` package is an example bringup system for Nav2 applications. -This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. +This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. @@ -10,8 +10,35 @@ Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/ * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. -To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. +To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. Note: * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. * spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. + +## Launch + +### Multi-robot Simulation + +This is how to launch multi-robot simulation with simple command line. Please see the Nav2 documentation for further augments. + +#### Cloned + +This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default. +The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. + +Please refer to below examples. + +```shell +ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" +``` + +#### Unique + +There are two robots including name and intitial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up. + +If you want to bringup more than two robots, you should modify the `unique_multi_tb3_simulation_launch.py` script. + +```shell +ros2 launch nav2_bringup unique_multi_tb3_simulation_launch.py +``` diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 24668627c8..89d3e136ce 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -23,9 +23,9 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ParameterFile -from nav2_common.launch import RewrittenYaml +from nav2_common.launch import RewrittenYaml, ReplaceString def generate_launch_description(): @@ -54,6 +54,15 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + # Only it applys when `use_namespace` is True. + # '' keyword shall be replaced by 'namespace' launch argument + # in config file 'nav2_multirobot_params.yaml' as a default & example. + # User defined config file should contain '' keyword for the replacements. + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace)) + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, @@ -113,7 +122,7 @@ def generate_launch_description(): # Specify the actions bringup_cmd_group = GroupAction([ - PushRosNamespace( + PushROSNamespace( condition=IfCondition(use_namespace), namespace=namespace), diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py new file mode 100644 index 0000000000..fc1499a0f2 --- /dev/null +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -0,0 +1,174 @@ +# Copyright (c) 2023 LG Electronics. +# +# 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 (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution +from nav2_common.launch import ParseMultiRobotPose + + +def generate_launch_description(): + """ + Bring up the multi-robots with given launch arguments. + + Launch arguments consist of robot name(which is namespace) and pose for initialization. + Keep general yaml format for pose information. + ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" + ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" + """ + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Simulation settings + world = LaunchConfiguration('world') + simulator = LaunchConfiguration('simulator') + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + rviz_config_file = LaunchConfiguration('rviz_config') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + log_settings = LaunchConfiguration('log_settings', default='true') + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), + description='Full path to world file to load') + + declare_simulator_cmd = DeclareLaunchArgument( + 'simulator', + default_value='gazebo', + description='The simulator to use (gazebo or gzserver)') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='false', + description='Automatically startup the stacks') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + description='Full path to the RVIZ config file to use.') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + # Start Gazebo with plugin providing the robot spawning service + start_gazebo_cmd = ExecuteProcess( + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') + + robots_list = ParseMultiRobotPose('robots').value() + + # Define commands for launching the navigation instances + bringup_cmd_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction([ + LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name':TextSubstitution(text=robot_name), }.items()) + ]) + + bringup_cmd_group.append(group) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) + + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['map yaml: ', map_yaml_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['params yaml: ', params_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['autostart: ', autostart])) + + for cmd in bringup_cmd_group: + ld.add_action(cmd) + + return ld diff --git a/nav2_bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py similarity index 100% rename from nav2_bringup/launch/multi_tb3_simulation_launch.py rename to nav2_bringup/launch/unique_multi_tb3_simulation_launch.py diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index b3fb936a3f..426ab34211 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.2 + 1.2.3 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index c2b2dda69a..692ca6ec80 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -277,6 +278,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 54704a2e7a..2d57aef383 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -276,6 +277,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml new file mode 100644 index 0000000000..6f7e66cd61 --- /dev/null +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -0,0 +1,347 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index cf26d389db..112a1de98b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -320,6 +321,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index d2a4b5e136..d8bade77d8 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -560,7 +560,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.6150002479553223 + Angle: -1.5708 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -570,11 +570,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 127.88431549072266 + Scale: 160 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: -0.044467076659202576 - Y: -0.38726311922073364 + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz index 57b2d7bf74..e95196a7fb 100644 --- a/nav2_bringup/rviz/nav2_namespaced_view.rviz +++ b/nav2_bringup/rviz/nav2_namespaced_view.rviz @@ -342,7 +342,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.5700000524520874 + Angle: -1.5708 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -352,11 +352,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 134.638427734375 + Scale: 160 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: -0.032615214586257935 - Y: -0.0801941454410553 + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: diff --git a/nav2_bringup/worlds/world_only.model b/nav2_bringup/worlds/world_only.model index 4c45d4e2f9..aed412a8d4 100644 --- a/nav2_bringup/worlds/world_only.model +++ b/nav2_bringup/worlds/world_only.model @@ -16,7 +16,7 @@ - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + 0 0 10 0 1.570796 0 orbit perspective diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index bc9c8049ce..2b552e1b02 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -44,7 +44,7 @@ class BtNavigator : public nav2_util::LifecycleNode * @brief A constructor for nav2_bt_navigator::BtNavigator class * @param options Additional options to control creation of the node. */ - explicit BtNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_bt_navigator::BtNavigator class */ diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 46c006a7fe..4d18bdaeea 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.2 + 1.2.3 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f25bd32265..a9b7af4cb6 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -29,8 +29,9 @@ using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator { -BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("bt_navigator", "", options), +BtNavigator::BtNavigator(rclcpp::NodeOptions options) +: nav2_util::LifecycleNode("bt_navigator", "", + options.automatically_declare_parameters_from_overrides(true)), class_loader_("nav2_core", "nav2_core::NavigatorBase") { RCLCPP_INFO(get_logger(), "Creating"); @@ -89,11 +90,16 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_is_battery_charging_condition_bt_node" }; - declare_parameter("plugin_lib_names", plugin_libs); - declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter("global_frame", std::string("map")); - declare_parameter("robot_base_frame", std::string("base_link")); - declare_parameter("odom_topic", std::string("odom")); + declare_parameter_if_not_declared( + this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + declare_parameter_if_not_declared( + this, "transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + this, "global_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + this, "odom_topic", rclcpp::ParameterValue(std::string("odom"))); } BtNavigator::~BtNavigator() diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 864b28bbe4..1c5b32dc7d 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -14,6 +14,10 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) +Demonstration of Collision Monitor abilities presented at 6th ROS Developers Day 2023, could be found below: + +[![cm-ros-devday.png](doc/cm_ros_devday.png)](https://www.youtube.com/watch?v=bWliK0PC5Ms) + ### Features The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". @@ -83,4 +87,4 @@ The zones around the robot and the data sources are the same as for the Collisio ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-detector.html). \ No newline at end of file +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). diff --git a/nav2_collision_monitor/doc/cm_ros_devday.png b/nav2_collision_monitor/doc/cm_ros_devday.png new file mode 100644 index 0000000000..63cc8fdea4 Binary files /dev/null and b/nav2_collision_monitor/doc/cm_ros_devday.png differ diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index ccd58c2e15..5440cfedc3 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -25,7 +25,7 @@ from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -84,7 +84,7 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), Node( @@ -110,7 +110,7 @@ def generate_launch_description(): condition=IfCondition(use_composition), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), LoadComposableNodes( diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 3d0006ac85..f4d0a953e3 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -25,7 +25,7 @@ from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace +from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -86,7 +86,7 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), Node( @@ -112,7 +112,7 @@ def generate_launch_description(): condition=IfCondition(use_composition), actions=[ SetParameter('use_sim_time', use_sim_time), - PushRosNamespace( + PushROSNamespace( condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), LoadComposableNodes( diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index b7f2439b5b..acd11e7acd 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.2 + 1.2.3 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e12d9795db..98433ead04 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -314,7 +314,7 @@ void CollisionDetector::process() state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( - collision_points) > polygon->getMinPoints()); + collision_points) >= polygon->getMinPoints()); } state_pub_->publish(std::move(state_msg)); diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index c25fadb220..7eba78e192 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -15,3 +15,4 @@ from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString +from .parse_multirobot_pose import ParseMultiRobotPose diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py new file mode 100644 index 0000000000..9025f967fc --- /dev/null +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -0,0 +1,82 @@ +# Copyright (c) 2023 LG Electronics. +# +# 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 yaml +import sys +from typing import Text, Dict + + +class ParseMultiRobotPose(): + """ + Parsing argument using sys module + """ + + def __init__(self, target_argument: Text): + """ + Parse arguments for multi-robot's pose + + for example, + `ros2 launch nav2_bringup bringup_multirobot_launch.py + robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0}; + robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"` + + `target_argument` shall be 'robots'. + Then, this will parse a string value for `robots` argument. + + Each robot name which is corresponding to namespace and pose of it will be separted by `;`. + The pose consists of x, y and yaw with YAML format. + + :param: target argument name to parse + """ + self.__args: Text = self.__parse_argument(target_argument) + + def __parse_argument(self, target_argument: Text) -> Text: + """ + get value of target argument + """ + if len(sys.argv) > 4: + argv = sys.argv[4:] + for arg in argv: + if arg.startswith(target_argument + ":="): + return arg.replace(target_argument + ":=", "") + return "" + + def value(self) -> Dict: + """ + get value of target argument + """ + args = self.__args + parsed_args = list() if len(args) == 0 else args.split(';') + multirobots = dict() + for arg in parsed_args: + key_val = arg.strip().split('=') + if len(key_val) != 2: + continue + key = key_val[0].strip() + val = key_val[1].strip() + robot_pose = yaml.safe_load(val) + if 'x' not in robot_pose: + robot_pose['x'] = 0.0 + if 'y' not in robot_pose: + robot_pose['y'] = 0.0 + if 'z' not in robot_pose: + robot_pose['z'] = 0.0 + if 'roll' not in robot_pose: + robot_pose['roll'] = 0.0 + if 'pitch' not in robot_pose: + robot_pose['pitch'] = 0.0 + if 'yaw' not in robot_pose: + robot_pose['yaw'] = 0.0 + multirobots[key] = robot_pose + return multirobots diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index 4b5c82e757..9d22f1732d 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -15,6 +15,7 @@ from typing import Dict from typing import List from typing import Text +from typing import Optional import tempfile import launch @@ -27,7 +28,8 @@ class ReplaceString(launch.Substitution): def __init__(self, source_file: launch.SomeSubstitutionsType, - replacements: Dict) -> None: + replacements: Dict, + condition: Optional[launch.Condition] = None) -> None: super().__init__() from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop @@ -35,28 +37,38 @@ def __init__(self, self.__replacements = {} for key in replacements: self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key]) + self.__condition = condition @property def name(self) -> List[launch.Substitution]: """Getter for name.""" return self.__source_file + @property + def condition(self) -> Optional[launch.Condition]: + """Getter for condition.""" + return self.__condition + def describe(self) -> Text: """Return a description of this substitution as a string.""" return '' def perform(self, context: launch.LaunchContext) -> Text: - output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) - replacements = self.resolve_replacements(context) - try: - input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r') - self.replace(input_file, output_file, replacements) - except Exception as err: # noqa: B902 - print('ReplaceString substitution error: ', err) - finally: - input_file.close() - output_file.close() - return output_file.name + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + if self.__condition is None or self.__condition.evaluate(context): + output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + replacements = self.resolve_replacements(context) + try: + input_file = open(yaml_filename, 'r') + self.replace(input_file, output_file, replacements) + except Exception as err: # noqa: B902 + print('ReplaceString substitution error: ', err) + finally: + input_file.close() + output_file.close() + return output_file.name + else: + return yaml_filename def resolve_replacements(self, context): resolved_replacements = {} diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 2887fa5503..071c884864 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -87,6 +87,7 @@ def perform(self, context: launch.LaunchContext) -> Text: param_rewrites, keys_rewrites = self.resolve_rewrites(context) data = yaml.safe_load(open(yaml_filename, 'r')) self.substitute_params(data, param_rewrites) + self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) if self.__root_key is not None: root_key = launch.utilities.perform_substitutions(context, self.__root_key) @@ -121,6 +122,15 @@ def substitute_params(self, yaml, param_rewrites): yaml_keys = path.split('.') yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + def add_params(self, yaml, param_rewrites): + # add new total path parameters + yaml_paths = self.pathify(yaml) + for path in param_rewrites: + if not path in yaml_paths: + new_val = self.convert(param_rewrites[path]) + yaml_keys = path.split('.') + if 'ros__parameters' in yaml_keys: + yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): for key in yaml_key_list: diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 57fd9fd6d8..4420a52659 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.2 + 1.2.3 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 2b77f4f313..b2af83d24c 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.2 + 1.2.3 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 911e423484..996d9513d2 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.2 + 1.2.3 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d27cb6692d..879225e8f2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -50,6 +50,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("controller_frequency", 20.0); + declare_parameter("action_server_result_timeout", 10.0); + declare_parameter("progress_checker_plugins", default_progress_checker_ids_); declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); @@ -227,6 +229,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_sub_ = std::make_unique(node); vel_publisher_ = create_publisher("cmd_vel", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action server that we implement with our followPath method action_server_ = std::make_unique( shared_from_this(), @@ -234,7 +241,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&ControllerServer::computeControl, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); // Set subscribtion to the speed limiting topic speed_limit_sub_ = create_subscription( diff --git a/nav2_core/package.xml b/nav2_core/package.xml index e6205ad906..af5af445c7 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.2 + 1.2.3 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index d720978e51..9095f83b9e 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.2 + 1.2.3 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index c219b78f55..e16e291033 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.2 + 1.2.3 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index d3cac0e84d..e9f6b30b9f 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.2 + 1.2.3 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 6fed02f0ce..26bb2f7dd5 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.2 + 1.2.3 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 4382f87c61..d04e49d659 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.2 + 1.2.3 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 6cc98d1d7f..592efb5989 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.2 + 1.2.3 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index edf48aec1d..c386e1f628 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.2.2 + 1.2.3 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index c84bb28975..ebe24156cb 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.2.2 + 1.2.3 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index a41b6c9e45..3bd612c2eb 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.2.2 + 1.2.3 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 9aa6059df7..1472336aa0 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.2 + 1.2.3 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index fa99a5c97d..3467e2b92f 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.2 + 1.2.3 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 4abc9c7454..5cc8a8c0e9 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -10,6 +10,7 @@ set(XTENSOR_USE_OPENMP 0) find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) +find_package(xsimd REQUIRED) set(dependencies_pkgs rclcpp @@ -46,7 +47,8 @@ if(COMPILER_SUPPORTS_FMA) add_compile_options(-mfma) endif() -add_compile_options(-O3 -finline-limit=1000000 -ffp-contract=fast -ffast-math) +# If building one the same hardware to be deployed on, try `-march=native`! +add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic) add_library(mppi_controller SHARED src/controller.cpp diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index a026b0fb2a..fe17906bae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -54,7 +54,7 @@ class ObstaclesCritic : public CriticFunction * @param cost Costmap cost * @return bool if in collision */ - bool inCollision(float cost) const; + inline bool inCollision(float cost) const; /** * @brief cost at a robot pose @@ -63,14 +63,14 @@ class ObstaclesCritic : public CriticFunction * @param theta theta of pose * @return Collision information at pose */ - CollisionCost costAtPose(float x, float y, float theta); + inline CollisionCost costAtPose(float x, float y, float theta); /** * @brief Distance to obstacle from cost * @param cost Costmap cost * @return float Distance to the obstacle represented by cost */ - float distanceToObstacle(const CollisionCost & cost); + inline float distanceToObstacle(const CollisionCost & cost); /** * @brief Find the min cost of the inflation decay function for which the robot MAY be @@ -79,14 +79,14 @@ class ObstaclesCritic : public CriticFunction * @return double circumscribed cost, any higher than this and need to do full footprint collision checking * since some element of the robot could be in collision */ - double findCircumscribedCost(std::shared_ptr costmap); + float findCircumscribedCost(std::shared_ptr costmap); protected: nav2_costmap_2d::FootprintCollisionChecker collision_checker_{nullptr}; bool consider_footprint_{true}; - double collision_cost_{0}; + float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; float possibly_inscribed_cost_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index f7ba486a0c..b137270aba 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -72,7 +72,7 @@ class PathAngleCritic : public CriticFunction void score(CriticData & data) override; protected: - double max_angle_to_furthest_{0}; + float max_angle_to_furthest_{0}; float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index b7f9b6f3cc..5e1885c271 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp @@ -24,10 +24,10 @@ namespace mppi::models */ struct ControlConstraints { - double vx_max; - double vx_min; - double vy; - double wz; + float vx_max; + float vx_min; + float vy; + float wz; }; /** @@ -36,9 +36,9 @@ struct ControlConstraints */ struct SamplingStd { - double vx; - double vy; - double wz; + float vx; + float vy; + float wz; }; } // namespace mppi::models diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 468e4964a4..eeb28fa01b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -151,8 +151,8 @@ class PathHandler double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; - double inversion_xy_tolerance_{0.2}; - double inversion_yaw_tolerance{0.4}; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance{0.4}; bool enforce_path_inversion_{false}; unsigned int inversion_locale_{0u}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ce2d0570c0..6baa27873e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -309,7 +309,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; size_t max_id_by_trajectories = 0; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); for (size_t i = 0; i < dists.shape(0); i++) { size_t min_id_by_path = 0; @@ -337,7 +337,7 @@ inline size_t findPathTrajectoryInitialPoint(const CriticData & data) const auto dy = data.path.y - data.trajectories.y(0, 0); const auto dists = dx * dx + dy * dy; - double min_distance_by_path = std::numeric_limits::max(); + float min_distance_by_path = std::numeric_limits::max(); size_t min_id = 0; for (size_t j = 0; j < dists.shape(0); j++) { if (dists(j) < min_distance_by_path) { @@ -420,23 +420,23 @@ inline void setPathCostsIfNotSet( * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { - double pose_x = pose.position.x; - double pose_y = pose.position.y; - double pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); - double yaw = atan2(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); // If no preference for forward, return smallest angle either in heading or 180 of heading if (!forward_preference) { return std::min( - abs(angles::shortest_angular_distance(yaw, pose_yaw)), - abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); } - return abs(angles::shortest_angular_distance(yaw, pose_yaw)); + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); } /** @@ -447,21 +447,21 @@ inline double posePointAngle( * @param point_yaw Yaw of the point to consider along Z axis * @return Angle between two points */ -inline double posePointAngle( +inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, double point_yaw) { - double pose_x = pose.position.x; - double pose_y = pose.position.y; - double pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); - double yaw = atan2(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); - if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { + if (fabs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { yaw = angles::normalize_angle(yaw + M_PI); } - return abs(angles::shortest_angular_distance(yaw, pose_yaw)); + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); } /** @@ -650,17 +650,17 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) // Iterating through the path to determine the position of the path inversion for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - + float oa_x = path.poses[idx].pose.position.x - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - + float oa_y = path.poses[idx].pose.position.y - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - + float ab_x = path.poses[idx + 1].pose.position.x - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - + float ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0) { return idx + 1; } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index cd68d15b5f..c85582ab49 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.2 + 1.2.3 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 4dc2bcf1de..e9774c7775 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -34,8 +34,8 @@ void ConstraintCritic::initialize() getParentParam(vx_min, "vx_min", -0.35); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; - max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); + max_vel_ = sqrtf(vx_max * vx_max + vy_max * vy_max); + min_vel_ = min_sgn * sqrtf(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index e38a98ed6b..852db11529 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -57,7 +57,7 @@ void GoalCritic::score(CriticData & data) xt::pow(traj_x - goal_x, 2) + xt::pow(traj_y - goal_y, 2)); - data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_); + data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 60383d00a0..80ac77e10f 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,7 +32,7 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1) { + if (possibly_inscribed_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -50,7 +50,7 @@ void ObstaclesCritic::initialize() "footprint" : "circular"); } -double ObstaclesCritic::findCircumscribedCost( +float ObstaclesCritic::findCircumscribedCost( std::shared_ptr costmap) { double result = -1.0; @@ -75,7 +75,7 @@ double ObstaclesCritic::findCircumscribedCost( if (!inflation_layer_found) { RCLCPP_WARN( - rclcpp::get_logger("computeCircumscribedCost"), + logger_, "No inflation layer found in costmap configuration. " "If this is an SE2-collision checking plugin, it cannot use costmap potential " "field to speed up collision checking by only checking the full footprint " @@ -83,7 +83,7 @@ double ObstaclesCritic::findCircumscribedCost( "significantly slow down planning times and not avoid anything but absolute collisions!"); } - return result; + return static_cast(result); } float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) @@ -115,21 +115,21 @@ void ObstaclesCritic::score(CriticData & data) } auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - raw_cost.fill(0.0); + raw_cost.fill(0.0f); auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - repulsive_cost.fill(0.0); + repulsive_cost.fill(0.0f); const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - float traj_cost = 0.0; + float traj_cost = 0.0f; const auto & traj = data.trajectories; CollisionCost pose_cost; for (size_t j = 0; j < traj_len; j++) { pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (pose_cost.cost < 1) {continue;} // In free space + if (pose_cost.cost < 1.0f) {continue;} // In free space if (inCollision(pose_cost.cost)) { trajectory_collide = true; @@ -137,7 +137,7 @@ void ObstaclesCritic::score(CriticData & data) } // Cannot process repulsion if inflation layer does not exist - if (inflation_radius_ == 0 || inflation_scale_factor_ == 0) { + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) { continue; } @@ -197,7 +197,9 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) } cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2585193ade..4ff12d4e55 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -88,12 +88,12 @@ void PathAlignCritic::score(CriticData & data) return; } - float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; - double min_dist_sq = std::numeric_limits::max(); + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); size_t min_s = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0; + summed_dist = 0.0f; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { min_dist_sq = std::numeric_limits::max(); min_s = 0; @@ -118,7 +118,7 @@ void PathAlignCritic::score(CriticData & data) // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += std::sqrt(min_dist_sq); + summed_dist += sqrtf(min_dist_sq); } } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 60173789f0..09ee4ab92d 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -94,14 +94,14 @@ void NoiseGenerator::generateNoisedControls() auto & s = settings_; xt::noalias(noises_vx_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.vx); xt::noalias(noises_wz_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.wz); if (is_holonomic_) { xt::noalias(noises_vy_) = xt::random::randn( - {s.batch_size, s.time_steps}, 0.0, + {s.batch_size, s.time_steps}, 0.0f, s.sampling_std.vy); } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 55362fd7a8..6d0a3ceb6c 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -286,8 +286,8 @@ void Optimizer::integrateStateVelocities( const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _)); - xt::noalias(xt::view(yaw_cos, 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::range(1, _))) = xt::cos(yaw_offseted); xt::noalias(xt::view(yaw_sin, xt::range(1, _))) = xt::sin(yaw_offseted); @@ -316,8 +316,8 @@ void Optimizer::integrateStateVelocities( auto && yaw_cos = xt::xtensor::from_shape(trajectories.yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(trajectories.yaws.shape()); - xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::all(), xt::range(1, _))) = xt::cos(yaws_cutted); xt::noalias(xt::view(yaw_sin, xt::all(), xt::range(1, _))) = xt::sin(yaws_cutted); @@ -358,16 +358,16 @@ void Optimizer::updateControlSequence() auto bounded_noises_vx = state_.cvx - control_sequence_.vx; auto bounded_noises_wz = state_.cwz - control_sequence_.wz; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vx, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vx, 2) * xt::sum( xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.wz, 2) * xt::sum( + s.gamma / powf(s.sampling_std.wz, 2) * xt::sum( xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); if (isHolonomic()) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vy, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vy, 2) * xt::sum( xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, 1, immediate); } @@ -378,8 +378,10 @@ void Optimizer::updateControlSequence() auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis())); xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate); - xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate); + if (isHolonomic()) { + xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); + } applyControlSequenceConstraints(); } diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index f1022513f2..ddf2b403b6 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -166,8 +166,8 @@ bool PathHandler::transformPose( double PathHandler::getMaxCostmapDist() { const auto & costmap = costmap_->getCostmap(); - return std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; + return static_cast(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) * + costmap->getResolution() * 0.50; } void PathHandler::setPath(const nav_msgs::msg::Path & plan) @@ -190,11 +190,11 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam { // Keep full path if we are within tolerance of the inversion pose const auto last_pose = global_plan_up_to_inversion_.poses.back(); - double distance = std::hypot( + float distance = hypotf( robot_pose.pose.position.x - last_pose.pose.position.x, robot_pose.pose.position.y - last_pose.pose.position.y); - double angle_distance = angles::shortest_angular_distance( + float angle_distance = angles::shortest_angular_distance( tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(last_pose.pose.orientation)); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index ce2896122c..8b5efae8da 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -380,19 +380,19 @@ TEST(CriticTests, PreferForwardCritic) path.reset(10); path.x(9) = 10.0; critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; state.vx = xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all reverse motion state.vx = -1.0 * xt::ones({1000, 30}); critic.score(data); - EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 15.0, 1e-6); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length + EXPECT_GT(xt::sum(costs, immediate)(), 0.0f); + EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length } TEST(CriticTests, TwirlingCritic) diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index f27471d28a..7c7f8a7691 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -432,20 +432,20 @@ TEST(OptimizerTests, SpeedLimitTests) // Test Speed limits API auto [v_min, v_max] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max, 0.5); - EXPECT_EQ(v_min, -0.35); + EXPECT_EQ(v_max, 0.5f); + EXPECT_EQ(v_min, -0.35f); optimizer_tester.setSpeedLimit(0, false); auto [v_min2, v_max2] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max2, 0.5); - EXPECT_EQ(v_min2, -0.35); + EXPECT_EQ(v_max2, 0.5f); + EXPECT_EQ(v_min2, -0.35f); optimizer_tester.setSpeedLimit(50.0, true); auto [v_min3, v_max3] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3); EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3); optimizer_tester.setSpeedLimit(0, true); auto [v_min4, v_max4] = optimizer_tester.getVelLimits(); - EXPECT_EQ(v_max4, 0.5); - EXPECT_EQ(v_min4, -0.35); + EXPECT_EQ(v_max4, 0.5f); + EXPECT_EQ(v_min4, -0.35f); optimizer_tester.setSpeedLimit(0.75, false); auto [v_min5, v_max5] = optimizer_tester.getVelLimits(); EXPECT_NEAR(v_max5, 0.75, 1e-3); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 836f7219df..e588ff0fa5 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) +find_package(geographic_msgs) find_package(action_msgs REQUIRED) nav2_package() @@ -49,7 +50,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Spin.action" "action/DummyBehavior.action" "action/FollowWaypoints.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs + "action/FollowGPSWaypoints.action" + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 38c7f70a5d..8c289027c8 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -3,12 +3,12 @@ uint16 NONE=0 uint16 UNKNOWN=300 uint16 INVALID_PLANNER=301 -uint16 TF_ERROR=3002 +uint16 TF_ERROR=302 uint16 START_OUTSIDE_MAP=303 uint16 GOAL_OUTSIDE_MAP=304 uint16 START_OCCUPIED=305 uint16 GOAL_OCCUPIED=306 -uint16 TIMEOUT=3007 +uint16 TIMEOUT=307 uint16 NO_VALID_PATH=308 uint16 NO_VIAPOINTS_GIVEN=309 diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action new file mode 100644 index 0000000000..707e9a2b37 --- /dev/null +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -0,0 +1,17 @@ +#goal definition +uint32 number_of_loops +uint32 goal_index 0 +geographic_msgs/GeoPose[] gps_poses +--- +#result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + +MissedWaypoint[] missed_waypoints +--- +#feedback +uint32 current_waypoint diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 4a70fa9229..19611e79fb 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.2 + 1.2.3 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski @@ -19,6 +19,7 @@ geometry_msgs action_msgs nav_msgs + geographic_msgs rosidl_interface_packages diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 598250de9f..ea38dfd32f 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.2 + 1.2.3 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 57d0b51d5c..96299aef0f 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.2 + 1.2.3 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e8c63fcd1e..4b914e92d0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -53,6 +53,8 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); + declare_parameter("action_server_result_timeout", 10.0); + get_parameter("planner_plugins", planner_ids_); if (planner_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { @@ -139,6 +141,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action servers for path planning to a pose and through poses action_server_pose_ = std::make_unique( shared_from_this(), @@ -146,7 +153,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&PlannerServer::computePlan, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); action_server_poses_ = std::make_unique( shared_from_this(), @@ -154,7 +161,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&PlannerServer::computePlanThroughPoses, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); return nav2_util::CallbackReturn::SUCCESS; } @@ -643,6 +650,7 @@ void PlannerServer::isPathValid( * The lethal check starts at the closest point to avoid points that have already been passed * and may have become occupied */ + std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; unsigned int my = 0; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index ac40ab131d..44fdd3d25f 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.2.2 + 1.2.3 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 44e1b7a748..45b5fc53b1 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.2 + 1.2.3 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 55e0a88d82..f41eb20a8a 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.2 + 1.2.3 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index a43fab366e..339d986ed4 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -14,6 +14,8 @@ See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) fo The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. +New as of September 2023: the simple navigator constructor will accept a `namespace` field to support multi-robot applications or namespaced Nav2 launches. + | Robot Navigator Method | Description | | --------------------------------- | -------------------------------------------------------------------------- | | setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d5daa3cb09..76a6c3d79b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,7 +25,8 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \ + NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -46,8 +47,8 @@ class TaskResult(Enum): class BasicNavigator(Node): - def __init__(self, node_name='basic_navigator'): - super().__init__(node_name=node_name) + def __init__(self, node_name='basic_navigator', namespace=''): + super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() self.initial_pose.header.frame_id = 'map' self.goal_handle = None @@ -67,6 +68,8 @@ def __init__(self, node_name='basic_navigator'): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, + 'follow_gps_waypoints') self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') @@ -83,13 +86,13 @@ def __init__(self, node_name='basic_navigator'): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) - self.change_maps_srv = self.create_client(LoadMap, '/map_server/load_map') + self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map') self.clear_costmap_global_srv = self.create_client( - ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap') + ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap') self.clear_costmap_local_srv = self.create_client( - ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap') - self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') - self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap') + self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap') + self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap') def destroyNode(self): self.destroy_node() @@ -182,6 +185,28 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + def followGpsWaypoints(self, gps_poses): + """Send a `FollowGPSWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_gps_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowGPSWaypoints.Goal() + goal_msg.gps_poses = gps_poses + + self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') + send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def spin(self, spin_dist=1.57, time_allowance=10): self.debug("Waiting for 'Spin' action server") while not self.spin_client.wait_for_server(timeout_sec=1.0): @@ -310,7 +335,8 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - self._waitForNodeToActivate(localizer) + if localizer != "robot_localization": # non-lifecycle node + self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() self._waitForNodeToActivate(navigator) diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index abadd764fc..92475f9cc4 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.2 + 1.2.3 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 69ef0f7950..7658fdac9e 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.2 + 1.2.3 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 2ae0fcb4f4..80577fcb55 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -263,7 +263,7 @@ void SmacPlannerHybrid::deactivate() _raw_plan_publisher->on_deactivate(); if (_debug_visualizations) { _expansions_publisher->on_deactivate(); - _planned_footprints_publisher->on_activate(); + _planned_footprints_publisher->on_deactivate(); } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index a07a324f6b..c59ae7ffd2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.2 + 1.2.3 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 503521f3e6..f0456ab6f0 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -53,6 +53,8 @@ SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) rclcpp::ParameterValue(std::string("base_link"))); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("smoother_plugins", default_ids_); + + declare_parameter("action_server_result_timeout", 10.0); } SmootherServer::~SmootherServer() @@ -104,6 +106,11 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) // Initialize pubs & subs plan_publisher_ = create_publisher("plan_smoothed", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action server that we implement with our smoothPath method action_server_ = std::make_unique( shared_from_this(), @@ -111,7 +118,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) std::bind(&SmootherServer::smoothPlan, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 352f9f2277..2674b65e74 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -114,6 +114,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/spin) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 3aacdace39..70882d1634 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.2 + 1.2.3 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 0e20792c72..7667d3081f 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -119,6 +119,7 @@ controller_server: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True prune_distance: 1.0 + forward_prune_distance: 1.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 5e21a7241a..61c5392137 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'yaml_filename': filter_mask_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 433dbc184e..971fcdd5e0 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'yaml_filename': filter_mask_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt new file mode 100644 index 0000000000..3342b6cd58 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -0,0 +1,11 @@ +ament_add_test(test_gps_waypoint_follower + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py new file mode 100644 index 0000000000..852436615b --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -0,0 +1,63 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# 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 launch import LaunchDescription +import launch_ros.actions +import os +import launch.actions + + +def generate_launch_description(): + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml") + os.environ["FILE_PATH"] = str(launch_dir) + return LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + "output_final_position", default_value="false" + ), + launch.actions.DeclareLaunchArgument( + "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_odom", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/local")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_map", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/global")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="navsat_transform_node", + name="navsat_transform", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[ + ("imu/data", "imu/data"), + ("gps/fix", "gps/fix"), + ("gps/filtered", "gps/filtered"), + ("odometry/gps", "odometry/gps"), + ("odometry/filtered", "odometry/global"), + ], + ), + ] + ) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml new file mode 100644 index 0000000000..6783a7b1db --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml @@ -0,0 +1,127 @@ +# For parameter descriptions, please refer to the template parameter files for each node. + +ekf_filter_node_odom: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1e-3, 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, 1e-3, 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, 1e-3, 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.3, 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.3, 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.5, 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.5, 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.1, 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.3, 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.3, 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.3, 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.3, 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.3, 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.3] + +ekf_filter_node_map: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + odom1: odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 10 + odom1_differential: false + odom1_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1.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, 1.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, 1e-3, 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.3, 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.3, 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.5, 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.5, 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.1, 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.3, 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.3, 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.3, 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.3, 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.3, 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.3] + +navsat_transform: + ros__parameters: + frequency: 30.0 + delay: 3.0 + magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998 + yaw_offset: 0.0 + zero_altitude: true + broadcast_utm_transform: true + publish_filtered_gps: true + use_odometry_yaw: true + wait_for_datum: false diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml new file mode 100644 index 0000000000..2539cef7b5 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -0,0 +1,307 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.1 + # When using GPS navigation you will potentially traverse huge environments which are not practical to + # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to + # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 + rolling_window: True + width: 50 + height: 50 + track_unknown_space: true + # no static map + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + # outdoors there will probably be more inf points + inf_is_valid: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py new file mode 100755 index 0000000000..cd90fdb620 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -0,0 +1,132 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# 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 +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + world = os.getenv("TEST_WORLD") + + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml") + bringup_dir = get_package_share_directory("nav2_bringup") + + configured_params = RewrittenYaml( + source_file=params_file, root_key="", param_rewrites="", convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "1"), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + "gzserver", + "-s", + "libgazebo_ros_init.so", + "--minimal_comms", + world, + ], + output="screen", + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_footprint", "base_link"], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_scan"], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=[ + "-0.32", + "0", + "0.068", + "0", + "0", + "0", + "base_link", + "imu_link", + ], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "gps_link"], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": "True", + "params_file": configured_params, + "autostart": "True", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "dual_ekf_navsat.launch.py") + ), + ), + ] + ) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")], + name="tester_node", + output="screen", + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py new file mode 100755 index 0000000000..aab8833668 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -0,0 +1,222 @@ +#! /usr/bin/env python3 +# Copyright 2019 Samsung Research America +# +# 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 sys +import time + +from action_msgs.msg import GoalStatus +from geographic_msgs.msg import GeoPose +from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints +from nav2_msgs.srv import ManageLifecycleNodes +from rcl_interfaces.srv import SetParameters +from rclpy.parameter import Parameter + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + + +class GpsWaypointFollowerTest(Node): + def __init__(self): + super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") + self.waypoints = None + self.action_client = ActionClient( + self, FollowGPSWaypoints, "follow_gps_waypoints" + ) + self.goal_handle = None + self.action_result = None + + self.param_cli = self.create_client(SetParameters, + '/waypoint_follower/set_parameters') + + def setWaypoints(self, waypoints): + self.waypoints = [] + for wp in waypoints: + msg = GeoPose() + msg.position.latitude = wp[0] + msg.position.longitude = wp[1] + msg.orientation.w = 1.0 + self.waypoints.append(msg) + + def run(self, block, cancel): + # if not self.waypoints: + # rclpy.error_msg("Did not set valid waypoints before running test!") + # return False + + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg( + "'follow_gps_waypoints' action server not available, waiting...") + + action_request = FollowGPSWaypoints.Goal() + action_request.gps_poses = self.waypoints + + self.info_msg("Sending goal request...") + send_goal_future = self.action_client.send_goal_async(action_request) + try: + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f"Service call failed {e!r}") + + if not self.goal_handle.accepted: + self.error_msg("Goal rejected") + return False + + self.info_msg("Goal accepted") + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() + if cancel: + time.sleep(2) + self.cancel_goal() + + self.info_msg("Waiting for 'follow_gps_waypoints' action to complete") + try: + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + result = get_result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f"Service call failed {e!r}") + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f"Goal failed with status code: {status}") + return False + if len(result.missed_waypoints) > 0: + self.info_msg( + "Goal failed to process all waypoints," + " missed {0} wps.".format(len(result.missed_waypoints)) + ) + return False + + self.info_msg("Goal succeeded!") + return True + + def setStopFailureParam(self, value): + req = SetParameters.Request() + req.parameters = [Parameter('stop_on_failure', + Parameter.Type.BOOL, value).to_parameter_msg()] + future = self.param_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + + def shutdown(self): + self.info_msg("Shutting down") + + self.action_client.destroy() + self.info_msg("Destroyed follow_gps_waypoints action client") + + transition_service = "lifecycle_manager_navigation/manage_nodes" + mgr_client = self.create_client( + ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg( + f"{transition_service} service not available, waiting...") + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f"{transition_service} service call failed {e!r}") + + self.info_msg(f"{transition_service} finished") + + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + + def info_msg(self, msg: str): + self.get_logger().info(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + wps = [[55.944831, -3.186998], [55.944818, -3.187075], [55.944782, -3.187060]] + + test = GpsWaypointFollowerTest() + test.setWaypoints(wps) + + # wait for poseCallback + + result = test.run(True, False) + assert result + + # preempt with new point + test.setWaypoints([wps[0]]) + result = test.run(False, False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False, False) + + # cancel + time.sleep(2) + test.cancel_goal() + + # set waypoint outside of map + time.sleep(2) + test.setWaypoints([[35.0, -118.0]]) + result = test.run(True, False) + assert not result + result = not result + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Result().GOAL_OUTSIDE_MAP + + # stop on failure test with bogous waypoint + test.setStopFailureParam(True) + bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]] + test.setWaypoints(bwps) + result = test.run(True, False) + assert not result + result = not result + mwps = test.action_result.missed_waypoints + result = (len(mwps) == 1) & (mwps[0] == 1) + test.setStopFailureParam(False) + + # Zero goal test + test.setWaypoints([]) + result = test.run(True, False) + + # Cancel test + test.setWaypoints(wps) + result = test.run(True, True) + assert not result + result = not result + + test.shutdown() + test.info_msg("Done Shutting Down.") + + if not result: + test.info_msg("Exiting failed") + exit(1) + else: + test.info_msg("Exiting passed") + exit(0) + + +if __name__ == "__main__": + main() diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index d323f97567..ea7e00db93 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -24,7 +24,7 @@ IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution -from launch_ros.actions import Node, PushRosNamespace +from launch_ros.actions import Node, PushROSNamespace from launch_testing.legacy import LaunchTestService @@ -95,7 +95,7 @@ def generate_launch_description(): group = GroupAction([ # Instances use the robot's name for namespace - PushRosNamespace(robot['name']), + PushROSNamespace(robot['name']), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index adeb839c48..51ccbb451e 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -41,10 +41,13 @@ def generate_launch_description(): params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file + param_substitutions = { + 'controller_server.ros__parameters.FollowPath.prune_distance': '1.0', + 'controller_server.ros__parameters.FollowPath.forward_prune_distance': '1.0'} configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites='', + param_rewrites=param_substitutions, convert_types=True) context = LaunchContext() diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 796047677e..df67bd61e0 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -236,9 +236,7 @@ def main(argv=sys.argv[1:]): # stop on failure test with bogous waypoint test.setStopFailureParam(True) bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]] - starting_pose = [-2.0, -0.5] test.setWaypoints(bwps) - test.setInitialPose(starting_pose) result = test.run(True, False) assert not result result = not result diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world new file mode 100644 index 0000000000..94b72499a2 --- /dev/null +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world @@ -0,0 +1,555 @@ + + + + + + + EARTH_WGS84 + ENU + 55.944831 + -3.186998 + 0.0 + 180.0 + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + -2.0 -0.5 0.01 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=/imu/data + + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + true + 1 + 0 0 0 0 0 0 + + + + + 0.0 + 0.1 + + + + + 0.0 + 0.0 + + + + + + + ~/out:=/gps/fix + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.064 0 0.121 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.032 0 0.171 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + false + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + imu_link + -0.032 0 0.068 0 0 0 + + 0 0 1 + + + + + base_link + gps_link + -0.05 0 0.05 0 0 0 + + 0 0 1 + + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + /tf:=tf + + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + + true + false + false + + odom + base_link + + + + + + + ~/out:=joint_states + + 250 + wheel_left_joint + wheel_right_joint + + + + + + + diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index a0e20937c8..dcde97f783 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.2 + 1.2.3 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 86fa5fdaee..4635bab0b0 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -32,6 +32,7 @@ set(dependencies bondcpp bond action_msgs + rcl_interfaces ) nav2_package() diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d1a39d4a5b..62201624a6 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -15,8 +15,10 @@ #ifndef NAV2_UTIL__NODE_UTILS_HPP_ #define NAV2_UTIL__NODE_UTILS_HPP_ +#include #include #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" namespace nav2_util { @@ -150,6 +152,25 @@ std::string get_plugin_type_param( return plugin_type; } +/** + * @brief A method to copy all parameters from one node (parent) to another (child). + * May throw parameter exceptions in error conditions + * @param parent Node to copy parameters from + * @param child Node to copy parameters to + */ +template +void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) +{ + using Parameters = std::vector; + std::vector param_names = parent->list_parameters({}, 0).names; + Parameters params = parent->get_parameters(param_names); + for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { + if (!child->has_parameter(iter->get_name())) { + child->declare_parameter(iter->get_name(), iter->get_parameter_value()); + } + } +} + } // namespace nav2_util #endif // NAV2_UTIL__NODE_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index be7340e8fb..6f25965d1e 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -26,7 +26,7 @@ namespace nav2_util * @class nav2_util::ServiceClient * @brief A simple wrapper on ROS2 services for invoke() and block-style calling */ -template +template class ServiceClient { public: @@ -37,14 +37,14 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - const rclcpp::Node::SharedPtr & provided_node) + const NodeT & provided_node) : service_name_(service_name), node_(provided_node) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - client_ = node_->create_client( + client_ = node_->template create_client( service_name, rclcpp::SystemDefaultsQoS(), callback_group_); @@ -147,7 +147,7 @@ class ServiceClient protected: std::string service_name_; - rclcpp::Node::SharedPtr node_; + NodeT node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; typename rclcpp::Client::SharedPtr client_; diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 4b1557a95c..50dc458515 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.2 + 1.2.3 TODO Michael Jeronimo Mohammad Haghighipanah @@ -28,6 +28,7 @@ launch launch_testing_ament_cmake action_msgs + rcl_interfaces libboost-program-options diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index 7c43927161..9d6d6ba133 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,3 +94,35 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*"); } + +TEST(TestParamCopying, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + nav2_util::copy_all_parameters(node1, node2); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); +} diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index b1b1ab2807..7bd9a7fd27 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.2 + 1.2.3 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index f3952abefb..d63be646a3 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.2 + 1.2.3 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index a3b46942bc..1d5405878b 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) +find_package(robot_localization REQUIRED) +find_package(geographic_msgs REQUIRED) nav2_package() @@ -57,6 +59,7 @@ set(dependencies image_transport cv_bridge OpenCV + robot_localization ) ament_target_dependencies(${executable_name} diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index cd0a2366b9..5d6a049857 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -27,3 +27,22 @@ In the first, the ``nav2_waypoint_follower`` is weakly sufficient to create a pr In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy. Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case. + +## Nav2 GPS Waypoint Follower + +`nav2_waypoint_follower` provides an action server named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. + +`robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) +to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. +The action msg definition for GPS waypoint following can be found [here](../nav2_msgs/action/FollowGPSWaypoints.action). + +In a common use case, an client node 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::SharedPtr gps_waypoint_follower_action_client_; +gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "follow_gps_waypoints"); +``` + +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index e5c59aa20b..8dd0d70cd7 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -20,18 +20,26 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "geographic_msgs/msg/geo_pose.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav2_msgs/msg/missed_waypoint.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - #include "nav2_util/node_utils.hpp" +#include "nav2_util/string_utils.hpp" +#include "nav2_msgs/action/follow_gps_waypoints.hpp" +#include "nav2_util/service_client.hpp" #include "nav2_core/waypoint_task_executor.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" + +#include "robot_localization/srv/from_ll.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_listener.h" namespace nav2_waypoint_follower { @@ -63,6 +71,10 @@ class WaypointFollower : public nav2_util::LifecycleNode using ActionServer = nav2_util::SimpleActionServer; using ActionClient = rclcpp_action::Client; + // Shorten the types for GPS waypoint following + using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints; + using ActionServerGPS = nav2_util::SimpleActionServer; + /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class * @param options Additional options to control creation of the node. @@ -107,15 +119,40 @@ class WaypointFollower : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Templated function to perform internal logic behind waypoint following, + * Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so. + * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details. + * + * @tparam T action_server + * @tparam V feedback + * @tparam Z result + * @param action_server + * @param poses + * @param feedback + * @param result + */ + template + void followWaypointsHandler(const T & action_server, const V & feedback, const Z & result); + /** * @brief Action server callbacks */ - void followWaypoints(); + void followWaypointsCallback(); /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously + * @brief send robot through each of GPS + * point , which are converted to map frame first then using a client to + * `FollowWaypoints` action. + * + * @param waypoints, acquired from action client */ + void followGPSWaypointsCallback(); + + /** + * @brief Action client result callback + * @param result Result of action server updated asynchronously + */ void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); /** @@ -124,6 +161,32 @@ class WaypointFollower : public nav2_util::LifecycleNode */ void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + /** + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_poses, from the action server + * @return std::vector + */ + std::vector convertGPSPosesToMapPoses( + const std::vector & gps_poses); + + + /** + * @brief get the latest poses on the action server goal. If they are GPS poses, + * convert them to the global cartesian frame using /fromLL robot localization + * server + * + * @param action_server, to which the goal was sent + * @return std::vector + */ + template + std::vector getLatestGoalPoses(const T & action_server); + + // Common vars used for both GPS and cartesian point following + std::vector failed_ids_; + std::string global_frame_id_{"map"}; + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -135,12 +198,17 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Our action server - std::unique_ptr action_server_; + std::unique_ptr xyz_action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + // Our action server for GPS waypoint following + std::unique_ptr gps_action_server_; + std::unique_ptr>> from_ll_to_map_client_; + bool stop_on_failure_; int loop_rate_; GoalStatus current_goal_status_; diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 25568c488d..40b77b565d 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.2 + 1.2.3 A waypoint follower navigation server Steve Macenski Apache-2.0 @@ -21,7 +21,9 @@ nav2_util nav2_core tf2_ros - + robot_localization + geographic_msgs + ament_lint_common ament_lint_auto ament_cmake_gtest diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2298aae03a..208268a249 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -36,6 +36,11 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); + + declare_parameter("action_server_result_timeout", 900.0); + + declare_parameter("global_frame_id", "map"); + nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), rclcpp::ParameterValue(std::string("wait_at_waypoint"))); @@ -58,6 +63,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); + global_frame_id_ = get_parameter("global_frame_id").as_string(); + global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, @@ -71,12 +78,37 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "navigate_to_pose", callback_group_); - action_server_ = std::make_unique( + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + + xyz_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this)); + "follow_waypoints", std::bind( + &WaypointFollower::followWaypointsCallback, + this), nullptr, std::chrono::milliseconds( + 500), false, server_options); + + from_ll_to_map_client_ = std::make_unique< + nav2_util::ServiceClient>>( + "/fromLL", + node); + + gps_action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "follow_gps_waypoints", + std::bind( + &WaypointFollower::followGPSWaypointsCallback, + this), nullptr, std::chrono::milliseconds( + 500), false, server_options); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( @@ -102,7 +134,8 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - action_server_->activate(); + xyz_action_server_->activate(); + gps_action_server_->activate(); auto node = shared_from_this(); // Add callback for dynamic parameters @@ -120,9 +153,9 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + xyz_action_server_->deactivate(); + gps_action_server_->deactivate(); dyn_params_handler_.reset(); - // destroy bond connection destroyBond(); @@ -134,8 +167,10 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - action_server_.reset(); + xyz_action_server_.reset(); nav_to_pose_client_.reset(); + gps_action_server_.reset(); + from_ll_to_map_client_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -147,29 +182,55 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -WaypointFollower::followWaypoints() +template +std::vector WaypointFollower::getLatestGoalPoses( + const T & action_server) { - auto goal = action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); + std::vector poses; + + // compile time static check to decide which block of code to be built + if constexpr (std::is_same>::value) { + // If normal waypoint following callback was called, we build here + poses = action_server->get_current_goal()->poses; + } else { + // If GPS waypoint following callback was called, we build here + poses = convertGPSPosesToMapPoses( + action_server->get_current_goal()->gps_poses); + } + return poses; +} + +template +void WaypointFollower::followWaypointsHandler( + const T & action_server, + const V & feedback, + const Z & result) +{ + auto goal = action_server->get_current_goal(); // handling loops unsigned int current_loop_no = 0; auto no_of_loops = goal->number_of_loops; - // Check if request is valid - if (!action_server_ || !action_server_->is_server_active()) { + std::vector poses; + poses = getLatestGoalPoses(action_server); + + if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); return; } RCLCPP_INFO( get_logger(), "Received follow waypoint request with %i waypoints.", - static_cast(goal->poses.size())); + static_cast(poses.size())); - if (goal->poses.size() == 0) { - action_server_->succeeded_current(result); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of waypoints passed to waypoint following " + "action potentially due to conversation failure or empty request." + ); + action_server->terminate_current(result); return; } @@ -181,20 +242,29 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action - if (action_server_->is_cancel_requested()) { + if (action_server->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); callback_group_executor_.spin_until_future_complete(cancel_future); // for result callback processing callback_group_executor_.spin_some(); - action_server_->terminate_all(); + action_server->terminate_all(); return; } // Check if asked to process another action - if (action_server_->is_preempt_requested()) { + if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); - goal = action_server_->accept_pending_goal(); - goal_index = goal->goal_index; + goal = action_server->accept_pending_goal(); + poses = getLatestGoalPoses(action_server); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of Waypoints passed to waypoint following logic. " + "Nothing to execute, returning with failure!"); + action_server->terminate_current(result); + return; + } + goal_index = 0; new_goal = true; } @@ -202,25 +272,32 @@ WaypointFollower::followWaypoints() if (new_goal) { new_goal = false; ClientT::Goal client_goal; - client_goal.pose = goal->poses[goal_index]; + client_goal.pose = poses[goal_index]; + client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); - send_goal_options.goal_response_callback = - std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); + send_goal_options.result_callback = std::bind( + &WaypointFollower::resultCallback, this, + std::placeholders::_1); + send_goal_options.goal_response_callback = std::bind( + &WaypointFollower::goalResponseCallback, + this, std::placeholders::_1); + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_.status = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; - action_server_->publish_feedback(feedback); + action_server->publish_feedback(feedback); - if (current_goal_status_.status == ActionStatus::FAILED) { + if ( + current_goal_status_.status == ActionStatus::FAILED || + current_goal_status_.status == ActionStatus::UNKNOWN) + { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = current_goal_status_.error_code; result->missed_waypoints.push_back(missedWaypoint); @@ -229,7 +306,7 @@ WaypointFollower::followWaypoints() get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - action_server_->terminate_current(result); + action_server->terminate_current(result); current_goal_status_.error_code = 0; return; } else { @@ -242,7 +319,7 @@ WaypointFollower::followWaypoints() get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); bool is_task_executed = waypoint_task_executor_->processAtWaypoint( - goal->poses[goal_index], goal_index); + poses[goal_index], goal_index); RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); @@ -250,8 +327,9 @@ WaypointFollower::followWaypoints() if (!is_task_executed) { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; - missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; + missedWaypoint.goal = poses[goal_index]; + missedWaypoint.error_code = + nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; result->missed_waypoints.push_back(missedWaypoint); } // if task execution was failed and stop_on_failure_ is on , terminate action @@ -261,7 +339,7 @@ WaypointFollower::followWaypoints() " stop on failure is enabled." " Terminating action.", goal_index); - action_server_->terminate_current(result); + action_server->terminate_current(result); current_goal_status_.error_code = 0; return; } else { @@ -271,18 +349,16 @@ WaypointFollower::followWaypoints() } } - if (current_goal_status_.status != ActionStatus::PROCESSING && - current_goal_status_.status != ActionStatus::UNKNOWN) - { + if (current_goal_status_.status != ActionStatus::PROCESSING) { // Update server state goal_index++; new_goal = true; - if (goal_index >= goal->poses.size()) { + if (goal_index >= poses.size()) { if (current_loop_no == no_of_loops) { RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", - goal->poses.size()); - action_server_->succeeded_current(result); + poses.size()); + action_server->succeeded_current(result); current_goal_status_.error_code = 0; return; } @@ -299,6 +375,30 @@ WaypointFollower::followWaypoints() } } +void WaypointFollower::followWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsHandler, + ActionT::Feedback::SharedPtr, + ActionT::Result::SharedPtr>( + xyz_action_server_, + feedback, result); +} + +void WaypointFollower::followGPSWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsHandler, + ActionTGPS::Feedback::SharedPtr, + ActionTGPS::Result::SharedPtr>( + gps_action_server_, + feedback, result); +} + void WaypointFollower::resultCallback( const rclcpp_action::ClientGoalHandle::WrappedResult & result) @@ -323,6 +423,7 @@ WaypointFollower::resultCallback( current_goal_status_.status = ActionStatus::FAILED; return; default: + RCLCPP_ERROR(get_logger(), "Received an UNKNOWN result code from navigation action!"); current_goal_status_.status = ActionStatus::UNKNOWN; return; } @@ -365,6 +466,58 @@ WaypointFollower::dynamicParametersCallback(std::vector param return result; } +std::vector +WaypointFollower::convertGPSPosesToMapPoses( + const std::vector & gps_poses) +{ + RCLCPP_INFO( + this->get_logger(), "Converting GPS waypoints to %s Frame..", + global_frame_id_.c_str()); + + std::vector poses_in_map_frame_vector; + int waypoint_index = 0; + for (auto && curr_geopose : gps_poses) { + auto request = std::make_shared(); + auto response = std::make_shared(); + request->ll_point.latitude = curr_geopose.position.latitude; + request->ll_point.longitude = curr_geopose.position.longitude; + request->ll_point.altitude = curr_geopose.position.altitude; + + from_ll_to_map_client_->wait_for_service((std::chrono::seconds(1))); + if (!from_ll_to_map_client_->invoke(request, response)) { + RCLCPP_ERROR( + this->get_logger(), + "fromLL service of robot_localization could not convert %i th GPS waypoint to" + "%s frame, going to skip this point!" + "Make sure you have run navsat_transform_node of robot_localization", + waypoint_index, global_frame_id_.c_str()); + if (stop_on_failure_) { + RCLCPP_ERROR( + this->get_logger(), + "Conversion of %i th GPS waypoint to" + "%s frame failed and stop_on_failure is set to true" + "Not going to execute any of waypoints, exiting with failure!", + waypoint_index, global_frame_id_.c_str()); + return std::vector(); + } + continue; + } else { + geometry_msgs::msg::PoseStamped curr_pose_map_frame; + curr_pose_map_frame.header.frame_id = global_frame_id_; + curr_pose_map_frame.header.stamp = this->now(); + curr_pose_map_frame.pose.position = response->map_point; + curr_pose_map_frame.pose.orientation = curr_geopose.orientation; + poses_in_map_frame_vector.push_back(curr_pose_map_frame); + } + waypoint_index++; + } + RCLCPP_INFO( + this->get_logger(), + "Converted all %i GPS waypoint to %s frame", + static_cast(poses_in_map_frame_vector.size()), global_frame_id_.c_str()); + return poses_in_map_frame_vector; +} + } // namespace nav2_waypoint_follower #include "rclcpp_components/register_node_macro.hpp" diff --git a/navigation2/package.xml b/navigation2/package.xml index 2908a8554f..ab6e564ec9 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.2 + 1.2.3 ROS2 Navigation Stack diff --git a/tools/underlay.repos b/tools/underlay.repos index 947aa5c66a..2e092f2517 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -19,10 +19,22 @@ repositories: # type: git # url: https://github.com/ros/bond_core.git # version: ros2 + # ros/diagnostics: + # type: git + # url: https://github.com/ros/diagnostics.git + # version: ros2-devel + # ros/geographic_info: + # type: git + # url: https://github.com/ros-geographic-info/geographic_info.git + # version: ros2 # ompl/ompl: # type: git # url: https://github.com/ompl/ompl.git # version: main + # robot_localization/robot_localization: + # type: git + # url: https://github.com/cra-ros-pkg/robot_localization.git + # version: ros2 # ros-simulation/gazebo_ros_pkgs: # type: git # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git