diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 074f39ef54..afea3eb3b7 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.9 + 1.1.10

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_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 592d1257a2..022f32de59 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 @@ -25,6 +25,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 { @@ -87,6 +88,17 @@ 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_); + action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index cb9333efbb..d5117b45d9 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.9 + 1.1.10 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 38e85fd14a..16d6169e25 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp @@ -53,7 +53,7 @@ class Wait : public TimedBehavior Status onCycleUpdate() override; protected: - std::chrono::time_point wait_end_; + rclcpp::Time wait_end_; WaitAction::Feedback::SharedPtr feedback_; }; diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 6f6494b755..107c044bb7 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.9 + 1.1.10 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 5100ab01b0..11a643eee0 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -30,21 +30,19 @@ Wait::~Wait() = default; Status 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 Status::SUCCEEDED; } Status 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 Status::RUNNING; } else { return 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 cec4f52085..40cbd680c6 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -25,7 +25,7 @@ from launch_ros.actions import Node 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(): @@ -59,6 +59,15 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} + # 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, 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 6b018950c4..488f5486d3 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.9 + 1.1.10 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski 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..372dcdd3f6 --- /dev/null +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -0,0 +1,345 @@ +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 + 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 + 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/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 0406c8735e..09a82e85dd 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -43,7 +43,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 cdba8c07f1..15975252c7 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.9 + 1.1.10 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 4f0f690d01..c6a87f0013 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -28,8 +28,9 @@ 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)) { RCLCPP_INFO(get_logger(), "Creating"); @@ -83,11 +84,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/package.xml b/nav2_collision_monitor/package.xml index eb750b4e60..6ca825d1fa 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.9 + 1.1.10 Collision Monitor Alexey Merzlyakov Steve Macenski 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/package.xml b/nav2_common/package.xml index d215166a49..6c0d093102 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.9 + 1.1.10 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 df52455957..7c0f7fb415 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.9 + 1.1.10 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index d3ae1711be..9066014856 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.9 + 1.1.10 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c5a5fcae00..219e74f0a8 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.9 + 1.1.10 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 daf0c64fab..72e03fa90f 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.9 + 1.1.10 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 881883872c..d32c36b01e 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.9 + 1.1.10 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 93228c13f5..f135f17228 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.9 + 1.1.10 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 025990ae33..1fd6afba51 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.9 + 1.1.10 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 4ebe4e1404..0934647b56 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.9 + 1.1.10 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 f26bfef251..1a6611135d 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.9 + 1.1.10 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 b3e77825da..880ae32654 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.1.9 + 1.1.10 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 5561c30bdf..d41c64dd41 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.1.9 + 1.1.10 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 417e2e7d62..4cd7a6051e 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.1.9 + 1.1.10 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 c1ec76b748..e56ba055e8 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.9 + 1.1.10 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 94ea2640c8..08e007a505 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.9 + 1.1.10 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 6bf412eb25..e3ae2f67a2 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 @@ -44,7 +44,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 9f0803a23a..2c9f3aef8b 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 @@ -150,8 +150,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 93c1aae106..b21b1cf8b3 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)); } /** @@ -625,17 +625,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 017957da52..1cc0ae9e73 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.9 + 1.1.10 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 4568e1eb14..9c80134f49 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; @@ -76,7 +76,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 " @@ -84,7 +84,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) @@ -116,21 +116,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; @@ -138,7 +138,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; } @@ -198,7 +198,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 e3485f5449..a7bbd35002 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -285,8 +285,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); @@ -315,8 +315,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); @@ -357,16 +357,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); } @@ -377,8 +377,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 cfefb3b6d3..893f9286c8 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 a1a42aa776..d0f7a1bfe8 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -307,19 +307,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 b562da1668..45cf720370 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/package.xml b/nav2_msgs/package.xml index 49b585af2a..284eac8295 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.9 + 1.1.10 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 2b5ca37009..c56e2d4430 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.9 + 1.1.10 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 18873edabd..306fbe2098 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.9 + 1.1.10 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ba817d6617..2126503bae 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -592,6 +592,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 23e7cc44b5..7484baad8c 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.1.9 + 1.1.10 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 f97c366224..649ebd63fa 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.9 + 1.1.10 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 20200ebeef..b38dea33ce 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.9 + 1.1.10 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 3421c3f000..64f2117a08 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -46,8 +46,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 @@ -83,13 +83,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() diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 830d297706..33141e3623 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.9 + 1.1.10 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 0ab37485fc..91548a5bce 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.9 + 1.1.10 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index f361b621db..d26ccdea5e 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.9 + 1.1.10 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index bfdee60c71..7151035b7f 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.9 + 1.1.10 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 8a3c9511c5..bc99c50cd7 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -127,6 +127,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 796667c65d..996287dc4d 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 5b0c61ed9d..d4467e1de4 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_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7f0ca4cafd..7dcb6c7187 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.9 + 1.1.10 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/package.xml b/nav2_util/package.xml index 861f323f24..534459fa71 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.9 + 1.1.10 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 d0009942e7..878bde6076 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.9 + 1.1.10 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 d19fe4815b..de604b2c8f 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.9 + 1.1.10 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/package.xml b/nav2_waypoint_follower/package.xml index c1a88efec7..c6c4123638 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.9 + 1.1.10 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 43315f5da6..9c22eccb76 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.9 + 1.1.10 ROS2 Navigation Stack