diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 630c2a0226..c1cfa2787f 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -9,6 +9,7 @@ | Ticket(s) this addresses | (add tickets here #1) | | Primary OS tested on | (Ubuntu, MacOS, Windows) | | Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) | +| Does this PR contain AI generated software? | (No; Yes and it is marked inline in the code) | --- diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index e6eef2992f..39c5c5f7d9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Initialize the input and output messages goal_ = typename ActionT::Goal(); @@ -93,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -462,6 +464,9 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the action server acknowledgement when a new goal is sent std::shared_ptr::SharedPtr>> future_goal_handle_; 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 467e4c80d0..331ca237c8 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 @@ -248,6 +248,9 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; 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 8d06ee7075..49574999c4 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 @@ -135,7 +135,7 @@ bool BtActionServer::on_configure() 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_); + rclcpp::copy_all_parameter_values(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished double action_server_result_timeout; @@ -158,6 +158,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int wait_for_service_timeout; + node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); + wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -172,6 +175,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -235,6 +241,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index 179c93e4d2..ea3e41d859 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { @@ -89,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is canceled std::chrono::milliseconds server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index a027ac7760..1afdb5adfd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); @@ -77,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - if (!service_client_->wait_for_service(1s)) { + if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", service_node_name.c_str()); @@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the server response when a new request is sent std::shared_future future_result_; bool request_sent_{false}; diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index 38033598d0..4bb9905759 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a7af3efef6..40f6f65c88 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6f0d54e7ca..85e3d3764d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index 35ab9d2f88..99e6eb1b45 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "back_up"); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index 12c25d30d6..a283dee33e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("on_cancelled_triggered", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index 4351152acf..88173e8420 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 40bbeb1a37..ec619b0120 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index ca5bcea666..d726186cd3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 5128391d03..290c9133a5 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 043f1aedb6..06478df588 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index cc2e7f0693..5378123a46 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "drive_on_heading_cancel"); diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 4fa291c4b7..c4fcc57519 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index bd028827af..5fa2c9aa03 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 13e3a2a2ca..264b775a68 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 122e6d0e0b..2f7a36e7fa 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index d1c930b5e4..c2e4389021 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 56f5cdae74..a6d9f36d57 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index d773e88bca..ea3b01b983 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "spin"); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 2df39599a9..f54250a2b6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index e8d4d053c7..0773c72c6d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 2f438c66e0..36cf6384cd 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -45,9 +45,10 @@ def generate_launch_description(): 'smoother_server', 'planner_server', 'behavior_server', + 'velocity_smoother', + 'collision_monitor', 'bt_navigator', 'waypoint_follower', - 'velocity_smoother', ] # Map fully qualified names to relative ones so the node's namespace can be prepended. @@ -198,7 +199,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + name='collision_monitor', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, ), Node( package='nav2_lifecycle_manager', @@ -266,7 +278,14 @@ def generate_launch_description(): name='velocity_smoother', parameters=[configured_params], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings, ), ComposableNode( package='nav2_lifecycle_manager', diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 692ca6ec80..baceb0e531 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -284,3 +285,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot1/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 2d57aef383..583bb64b56 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -283,3 +284,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot2/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 6f7e66cd61..5fb4b9fe63 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -345,3 +346,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 112a1de98b..39d72a5297 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -121,6 +122,7 @@ controller_server: progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -341,3 +343,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 0f65dd27d1..2078341ae0 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -77,6 +77,20 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); + auto node = shared_from_this(); + nav2_util::declare_parameter_if_not_declared( + node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3..406ca377c3 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat // Smooth plan auto costmap = costmap_sub_->getCostmap(); + std::lock_guard lock(*(costmap->getMutex())); if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) { RCLCPP_WARN( logger_, diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ccae77f77..6687b3452d 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -82,7 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 544155ebb3..992c8fab04 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -265,6 +265,7 @@ class ControllerServer : public nav2_util::LifecycleNode double min_theta_velocity_threshold_; double failure_tolerance_; + bool use_realtime_priority_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0386c06fc7..76754c5815 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -62,6 +62,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); + declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -126,6 +127,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string speed_limit_topic; get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); + get_parameter("use_realtime_priority", use_realtime_priority_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -221,13 +223,19 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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(), - "follow_path", - std::bind(&ControllerServer::computeControl, this), - nullptr, - std::chrono::milliseconds(500), - true, server_options); + // This may throw due to real-time prioritzation if user doesn't have real-time permissions + try { + action_server_ = std::make_unique( + shared_from_this(), + "follow_path", + std::bind(&ControllerServer::computeControl, this), + nullptr, + std::chrono::milliseconds(500), + true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } // Set subscribtion to the speed limiting topic speed_limit_sub_ = create_subscription( @@ -282,11 +290,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -312,11 +316,9 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + + costmap_ros_->cleanup(); + // Release any allocated resources action_server_.reset(); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 2074ad5ea6..08b09504e5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -48,6 +48,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "map_msgs/msg/occupancy_grid_update.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "tf2/transform_datatypes.h" #include "nav2_util/lifecycle_node.hpp" @@ -91,6 +92,7 @@ class Costmap2DPublisher costmap_pub_->on_activate(); costmap_update_pub_->on_activate(); costmap_raw_pub_->on_activate(); + costmap_raw_update_pub_->on_activate(); } /** @@ -101,6 +103,7 @@ class Costmap2DPublisher costmap_pub_->on_deactivate(); costmap_update_pub_->on_deactivate(); costmap_raw_pub_->on_deactivate(); + costmap_raw_update_pub_->on_deactivate(); } /** @@ -136,9 +139,16 @@ class Costmap2DPublisher void prepareGrid(); void prepareCostmap(); + /** @brief Prepare OccupancyGridUpdate msg for publication. */ + std::unique_ptr createGridUpdateMsg(); + /** @brief Prepare CostmapUpdate msg for publication. */ + std::unique_ptr createCostmapUpdateMsg(); + /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); + void updateGridParams(); + /** @brief GetCostmap callback service */ void costmap_service_callback( const std::shared_ptr request_header, @@ -164,12 +174,14 @@ class Costmap2DPublisher // Publisher for raw costmap values as msg::Costmap from layered costmap rclcpp_lifecycle::LifecyclePublisher::SharedPtr costmap_raw_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + costmap_raw_update_pub_; // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - float grid_resolution; - unsigned int grid_width, grid_height; + float grid_resolution_; + unsigned int grid_width_, grid_height_; std::unique_ptr grid_; std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index f319e52f33..b1ddedceeb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode public: /** * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. */ - Costmap2DROS(); + Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief as a child-LifecycleNode : + * Costmap2DROS may be launched by another Lifecycle Node as a composed module + * If composed, its parents will handle the shutdown, which includes this module + */ + void on_rcl_preshutdown() override + { + if (is_lifecycle_follower_) { + // Transitioning handled by parent node + return; + } + + // Else, if this is an independent node, this node needs to handle itself. + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); + } + /** * @brief Subscribes to sensor topics if necessary and starts costmap * updates, can be called to restart the costmap after calls to either @@ -381,6 +404,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode double transform_tolerance_{0}; ///< The timeout before transform errors double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e4ea745015..4f50614876 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_util/lifecycle_node.hpp" namespace nav2_costmap_2d @@ -52,25 +53,36 @@ class CostmapSubscriber ~CostmapSubscriber() {} /** - * @brief A Get the costmap from topic + * @brief Get current costmap */ std::shared_ptr getCostmap(); - - /** - * @brief Convert an occ grid message into a costmap object - */ - void toCostmap2D(); /** * @brief Callback for the costmap topic */ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); + /** + * @brief Callback for the costmap's update topic + */ + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); protected: + bool isCostmapReceived() {return costmap_ != nullptr;} + void processCurrentCostmapMsg(); + + bool haveCostmapParametersChanged(); + bool hasCostmapSizeChanged(); + bool hasCostmapResolutionChanged(); + bool hasCostmapOriginPositionChanged(); + + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr costmap_update_sub_; + std::shared_ptr costmap_; nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; + std::string topic_name_; - bool costmap_received_{false}; - rclcpp::Subscription::SharedPtr costmap_sub_; + std::mutex costmap_msg_mutex_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7ca..58c7d43d56 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher( custom_qos); costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); + costmap_raw_update_pub_ = node->create_publisher( + topic_name + "_raw_updates", custom_qos); // Create a service that will use the callback function to handle requests. costmap_service_ = node->create_service( @@ -115,32 +117,37 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub.publish(grid_); } */ + +void Costmap2DPublisher::updateGridParams() +{ + saved_origin_x_ = costmap_->getOriginX(); + saved_origin_y_ = costmap_->getOriginY(); + grid_resolution_ = costmap_->getResolution(); + grid_width_ = costmap_->getSizeInCellsX(); + grid_height_ = costmap_->getSizeInCellsY(); +} + // prepare grid_ message for publication. void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - grid_resolution = costmap_->getResolution(); - grid_width = costmap_->getSizeInCellsX(); - grid_height = costmap_->getSizeInCellsY(); grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; grid_->header.stamp = clock_->now(); - grid_->info.resolution = grid_resolution; + grid_->info.resolution = grid_resolution_; - grid_->info.width = grid_width; - grid_->info.height = grid_height; + grid_->info.width = grid_width_; + grid_->info.height = grid_height_; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_->info.origin.position.x = wx - grid_resolution / 2; - grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.x = wx - grid_resolution_ / 2; + grid_->info.origin.position.y = wy - grid_resolution_ / 2; grid_->info.origin.position.z = 0.0; grid_->info.origin.orientation.w = 1.0; - saved_origin_x_ = costmap_->getOriginX(); - saved_origin_y_ = costmap_->getOriginY(); grid_->data.resize(grid_->info.width * grid_->info.height); @@ -181,44 +188,74 @@ void Costmap2DPublisher::prepareCostmap() } } -void Costmap2DPublisher::publishCostmap() +std::unique_ptr Costmap2DPublisher::createGridUpdateMsg() +{ + auto update = std::make_unique(); + + update->header.stamp = clock_->now(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + update->data[i++] = cost_translation_table_[costmap_->getCost(x, y)]; + } + } + return update; +} + +std::unique_ptr Costmap2DPublisher::createCostmapUpdateMsg() { - if (costmap_raw_pub_->get_subscription_count() > 0) { - prepareCostmap(); - costmap_raw_pub_->publish(std::move(costmap_raw_)); + auto msg = std::make_unique(); + + msg->header.stamp = clock_->now(); + msg->header.frame_id = global_frame_; + msg->x = x0_; + msg->y = y0_; + msg->size_x = xn_ - x0_; + msg->size_y = yn_ - y0_; + msg->data.resize(msg->size_x * msg->size_y); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + msg->data[i++] = costmap_->getCost(x, y); + } } + return msg; +} +void Costmap2DPublisher::publishCostmap() +{ float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || - grid_width != costmap_->getSizeInCellsX() || - grid_height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution_ != resolution || + grid_width_ != costmap_->getSizeInCellsX() || + grid_height_ != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { + updateGridParams(); if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } + if (costmap_raw_pub_->get_subscription_count() > 0) { + prepareCostmap(); + costmap_raw_pub_->publish(std::move(costmap_raw_)); + } } else if (x0_ < xn_) { + // Publish just update msgs + std::unique_lock lock(*(costmap_->getMutex())); if (costmap_update_pub_->get_subscription_count() > 0) { - std::unique_lock lock(*(costmap_->getMutex())); - // Publish Just an Update - auto update = std::make_unique(); - update->header.stamp = rclcpp::Time(); - update->header.frame_id = global_frame_; - update->x = x0_; - update->y = y0_; - update->width = xn_ - x0_; - update->height = yn_ - y0_; - update->data.resize(update->width * update->height); - unsigned int i = 0; - for (unsigned int y = y0_; y < yn_; y++) { - for (unsigned int x = x0_; x < xn_; x++) { - unsigned char cost = costmap_->getCost(x, y); - update->data[i++] = cost_translation_table_[cost]; - } - } - costmap_update_pub_->publish(std::move(update)); + costmap_update_pub_->publish(createGridUpdateMsg()); + } + if (costmap_raw_update_pub_->get_subscription_count() > 0) { + costmap_raw_update_pub_->publish(createCostmapUpdateMsg()); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c908..bc887eeeb3 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,8 +61,8 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) : Costmap2DROS(name, "/", name, use_sim_time) {} -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), name_("costmap"), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ @@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS() "nav2_costmap_2d::InflationLayer"} { declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + is_lifecycle_follower_ = false; init(); } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e9..0426b87be3 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -14,22 +14,30 @@ #include #include +#include #include "nav2_costmap_2d/costmap_subscriber.hpp" namespace nav2_costmap_2d { +constexpr int costmapUpdateQueueDepth = 10; + CostmapSubscriber::CostmapSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } CostmapSubscriber::CostmapSubscriber( @@ -38,60 +46,119 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } std::shared_ptr CostmapSubscriber::getCostmap() { - if (!costmap_received_) { + if (!isCostmapReceived()) { throw std::runtime_error("Costmap is not available"); } - toCostmap2D(); + if (costmap_msg_) { + processCurrentCostmapMsg(); + } return costmap_; } -void CostmapSubscriber::toCostmap2D() +void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); - - if (costmap_ == nullptr) { - costmap_ = std::make_shared( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || - costmap_->getResolution() != current_costmap_msg->metadata.resolution || - costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || - costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { - // Update the size of the costmap - costmap_->resizeMap( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, - current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); + std::lock_guard lock(costmap_msg_mutex_); + costmap_msg_ = msg; } + if (!isCostmapReceived()) { + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); - unsigned char * master_array = costmap_->getCharMap(); - unsigned int index = 0; - for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { - for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { - master_array[index] = current_costmap_msg->data[index]; - ++index; + processCurrentCostmapMsg(); + } +} + +void CostmapSubscriber::costmapUpdateCallback( + const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg) +{ + if (isCostmapReceived()) { + if (costmap_msg_) { + processCurrentCostmapMsg(); + } + + std::lock_guard lock(*(costmap_->getMutex())); + + auto map_cell_size_x = costmap_->getSizeInCellsX(); + auto map_call_size_y = costmap_->getSizeInCellsY(); + + if (map_cell_size_x < update_msg->x + update_msg->size_x || + map_call_size_y < update_msg->y + update_msg->size_y) + { + RCLCPP_WARN( + logger_, "Update area outside of original map area. Costmap bounds: %d X %d, " + "Update origin: %d, %d bounds: %d X %d", map_cell_size_x, map_call_size_y, + update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y); + return; + } + unsigned char * master_array = costmap_->getCharMap(); + // copy update msg row-wise + for (size_t y = 0; y < update_msg->size_y; ++y) { + auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x + + update_msg->x; + + std::copy_n( + update_msg->data.begin() + (y * update_msg->size_x), + update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]); } + } else { + RCLCPP_WARN(logger_, "No costmap received."); } } -void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) +void CostmapSubscriber::processCurrentCostmapMsg() { - std::atomic_store(&costmap_msg_, msg); - if (!costmap_received_) { - costmap_received_ = true; + std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_); + if (haveCostmapParametersChanged()) { + costmap_->resizeMap( + costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, + costmap_msg_->metadata.resolution, + costmap_msg_->metadata.origin.position.x, + costmap_msg_->metadata.origin.position.y); } + + unsigned char * master_array = costmap_->getCharMap(); + std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array); + costmap_msg_.reset(); +} + +bool CostmapSubscriber::haveCostmapParametersChanged() +{ + return hasCostmapSizeChanged() || + hasCostmapResolutionChanged() || + hasCostmapOriginPositionChanged(); +} + +bool CostmapSubscriber::hasCostmapSizeChanged() +{ + return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || + costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y; +} + +bool CostmapSubscriber::hasCostmapResolutionChanged() +{ + return costmap_->getResolution() != costmap_msg_->metadata.resolution; +} + +bool CostmapSubscriber::hasCostmapOriginPositionChanged() +{ + return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || + costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..868b5081a1 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -76,6 +76,17 @@ target_link_libraries(test_costmap_publisher_exec layers ) +ament_add_gtest_executable(test_costmap_subscriber_exec +test_costmap_subscriber.cpp +) +ament_target_dependencies(test_costmap_subscriber_exec + ${dependencies} +) +target_link_libraries(test_costmap_subscriber_exec + nav2_costmap_2d_core + nav2_costmap_2d_client +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -136,6 +147,16 @@ ament_add_test(test_costmap_publisher_exec TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_subscriber_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp new file mode 100644 index 0000000000..65fa120cd6 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -0,0 +1,184 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; +class TestCostmapSubscriberShould : public ::testing::Test +{ +public: + TestCostmapSubscriberShould() + : topicName("/costmap"), node(nav2_util::LifecycleNode::make_shared("test_subscriber")) + { + dummyCostmapMsgSubscriber = node->create_subscription( + topicName + "_raw", 10, + std::bind(&TestCostmapSubscriberShould::costmapCallback, this, std::placeholders::_1)); + + dummyCostmapUpdateMsgSubscriber = + node->create_subscription( + topicName + "_raw_updates", 10, std::bind( + &TestCostmapSubscriberShould::costmapUpdateCallback, this, + std::placeholders::_1)); + } + + void SetUp() override + { + fullCostmapMsgCount = 0; + updateCostmapMsgCount = 0; + + costmapSubscriber = + std::make_unique(node, topicName + "_raw"); + + costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); + } + + void TearDown() + { + costmapSubscriber.reset(); + costmapToSend.reset(); + } + + void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr) + { + this->fullCostmapMsgCount++; + } + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr) + { + this->updateCostmapMsgCount++; + } + + struct CostmapObservation + { + std::uint32_t x; + std::uint32_t y; + std::uint8_t cost; + }; + + struct MapChange + { + std::vector observations; + std::uint32_t x0; + std::uint32_t xn; + std::uint32_t y0; + std::uint32_t yn; + }; + +/* *INDENT-OFF* */ + const std::vector mapChanges {{{{2, 2, 255}, {2, 3, 255}, {3, 2, 255}}, 2, 4, 2, 4}, + {{{7, 7, 255}, {7, 8, 255}}, 7, 8, 7, 9}, + {{{2, 2, 0}, {2, 3, 0}, {3, 2, 0}}, 2, 4, 2, 4}}; +/* *INDENT-ON* */ + +protected: + std::vector getCurrentCharMapFromSubscriber() + { + auto currentSubscriberCostmap = costmapSubscriber->getCostmap(); + return + std::vector( + currentSubscriberCostmap->getCharMap(), + currentSubscriberCostmap->getCharMap() + currentSubscriberCostmap->getSizeInCellsX() * + currentSubscriberCostmap->getSizeInCellsX()); + } + + std::vector getCurrentCharMapToSend() + { + return std::vector( + costmapToSend->getCharMap(), + costmapToSend->getCharMap() + costmapToSend->getSizeInCellsX() * + costmapToSend->getSizeInCellsX()); + } + + int fullCostmapMsgCount; + int updateCostmapMsgCount; + std::string topicName; + + nav2_util::LifecycleNode::SharedPtr node; + rclcpp::Logger logger {rclcpp::get_logger("test_costmap_subscriber_should")}; + + std::unique_ptr costmapSubscriber; + std::shared_ptr costmapToSend; + rclcpp::Subscription::SharedPtr dummyCostmapMsgSubscriber; + rclcpp::Subscription::SharedPtr dummyCostmapUpdateMsgSubscriber; +}; + +TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) +{ + bool always_send_full_costmap = true; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, mapChanges.size()); + ASSERT_EQ(updateCostmapMsgCount, 0); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) +{ + bool always_send_full_costmap = false; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, 1); + ASSERT_EQ(updateCostmapMsgCount, mapChanges.size() - 1); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F( + TestCostmapSubscriberShould, + throwExceptionIfGetCostmapMethodIsCalledBeforeAnyCostmapMsgReceived) +{ + ASSERT_ANY_THROW(costmapSubscriber->getCostmap()); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 466fd015c8..ec6748f980 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -63,7 +63,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index 8a657d81ad..a8ab817df3 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -22,7 +22,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); - auto costmap = std::make_shared("test_costmap"); + auto costmap = std::make_shared(rclcpp::NodeOptions()); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index ed3cabc3d0..bfed0c2ec5 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -35,12 +35,13 @@ #ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ #define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ +#include #include #include #include -#include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" namespace dwb_plugins { @@ -75,6 +76,36 @@ struct KinematicParameters inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;} inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;} + /** + * @brief Check to see whether the combined x/y/theta velocities are valid + * @return True if the magnitude hypot(x,y) and theta are within the robot's + * absolute limits + * + * This is based on three parameters: min_speed_xy, max_speed_xy and + * min_speed_theta. The speed is valid if 1) The combined magnitude hypot(x,y) + * is less than max_speed_xy (or max_speed_xy is negative) AND 2) min_speed_xy + * is negative or min_speed_theta is negative or hypot(x,y) is greater than + * min_speed_xy or fabs(theta) is greater than min_speed_theta. + */ + inline bool isValidSpeed(double x, double y, double theta) + { + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && + vmag_sq > max_speed_xy_sq_ + std::numeric_limits::epsilon()) + { + return false; + } + if ( + min_speed_xy_ >= 0.0 && vmag_sq + std::numeric_limits::epsilon() < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && + fabs(theta) + std::numeric_limits::epsilon() < min_speed_theta_) + { + return false; + } + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; + } + protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -127,8 +158,8 @@ class KinematicsHandler * @brief Callback executed when a paramter change is detected * @param parameters list of changed parameters */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 88c18046ed..68ac9405c2 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -58,18 +58,6 @@ class XYThetaIterator : public VelocityIterator nav_2d_msgs::msg::Twist2D nextTwist() override; protected: - /** - * @brief Check to see whether the combined x/y/theta velocities are valid - * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits - * - * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. - * The speed is valid if - * 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) - * AND - * 2) min_speed_xy is negative or min_speed_theta is negative or - * hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta. - */ - bool isValidSpeed(double x, double y, double theta); virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 50b19aa337..5ca1992c6b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -41,8 +41,6 @@ #include "nav_2d_utils/parameters.hpp" #include "nav2_util/node_utils.hpp" -#define EPSILON 1E-5 - namespace dwb_plugins { void XYThetaIterator::initialize( @@ -92,29 +90,10 @@ void XYThetaIterator::startNewIteration( } } -bool XYThetaIterator::isValidSpeed(double x, double y, double theta) -{ - KinematicParameters kinematics = kinematics_handler_->getKinematics(); - double vmag_sq = x * x + y * y; - if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) { - return false; - } - if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() && - kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta()) - { - return false; - } - if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) { - return false; - } - return true; -} - bool XYThetaIterator::isValidVelocity() { - return isValidSpeed( - x_it_->getVelocity(), y_it_->getVelocity(), - th_it_->getVelocity()); + return kinematics_handler_->getKinematics().isValidSpeed( + x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } bool XYThetaIterator::hasMoreTwists() diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e588ff0fa5..206ae86322 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapUpdate.msg" "msg/CostmapFilterInfo.msg" "msg/SpeedLimit.msg" "msg/VoxelGrid.msg" diff --git a/nav2_msgs/msg/CostmapUpdate.msg b/nav2_msgs/msg/CostmapUpdate.msg new file mode 100644 index 0000000000..3c865d326b --- /dev/null +++ b/nav2_msgs/msg/CostmapUpdate.msg @@ -0,0 +1,11 @@ +# Update msg for Costmap containing the modified part of Costmap +std_msgs/Header header + +uint32 x +uint32 y + +uint32 size_x +uint32 size_y + +# The cost data, in row-major order, starting with (x,y) from 0-255 in Costmap format rather than OccupancyGrid 0-100. +uint8[] data diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 84edc6e4df..fea40ea405 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -218,11 +218,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -247,15 +243,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plan_publisher_.reset(); tf_.reset(); - /* - * Double check whether something else transitioned it to INACTIVE - * already, e.g. the rcl preshutdown callback. - */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 9434b105f4..76837754e8 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -426,14 +426,16 @@ double RegulatedPurePursuitController::findVelocitySignChange( return hypot( transformed_plan.poses[pose_id].pose.position.x, transformed_plan.poses[pose_id].pose.position.y); - } else if ( + } + + if ( (hypot(oa_x, oa_y) == 0.0 && - transformed_plan.poses[pose_id - 1].pose.orientation != - transformed_plan.poses[pose_id].pose.orientation) + transformed_plan.poses[pose_id - 1].pose.orientation != + transformed_plan.poses[pose_id].pose.orientation) || (hypot(ab_x, ab_y) == 0.0 && - transformed_plan.poses[pose_id].pose.orientation != - transformed_plan.poses[pose_id + 1].pose.orientation)) + transformed_plan.poses[pose_id].pose.orientation != + transformed_plan.poses[pose_id + 1].pose.orientation)) { // returning the distance since the points overlap // but are not simply duplicate points (e.g. in place rotation) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 31dae78968..650d8d4adf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -72,8 +72,7 @@ class AStarAlgorithm typedef std::priority_queue, NodeComparator> NodeQueue; /** - * @brief A constructor for nav2_smac_planner::PlannerServer - * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + * @brief A constructor for nav2_smac_planner::AStarAlgorithm */ explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); @@ -196,7 +195,7 @@ class AStarAlgorithm inline NodePtr getNextNode(); /** - * @brief Get pointer to next goal in open set + * @brief Add a node to the open set * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ @@ -204,8 +203,7 @@ class AStarAlgorithm /** * @brief Adds node to graph - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set + * @param index Node index to add */ inline NodePtr addToGraph(const unsigned int & index); @@ -218,9 +216,8 @@ class AStarAlgorithm /** * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node */ inline float getHeuristicCost(const NodePtr & node); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index eb3680ec1c..0fbace4360 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -47,7 +47,6 @@ class NodeBasic public: /** * @brief A constructor for nav2_smac_planner::NodeBasic - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ explicit NodeBasic(const unsigned int index) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 7f6a82d31c..4fe6a284cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -58,8 +58,6 @@ struct LatticeMotionTable /** * @brief Initializing state lattice planner's motion model * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes * @param search_info Parameters for searching */ void initMotionModel( diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a82..0b494d96c6 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -72,6 +72,8 @@ bool SimpleSmoother::smooth( std::vector path_segments = findDirectionalPathSegments(path); + std::lock_guard lock(*(costmap->getMutex())); + for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { // Populate path segment diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 67472eb46b..8fb1b6ad0d 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -146,7 +146,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48ba35038c..48b97848f8 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -138,6 +138,8 @@ class BehaviorTreeHandler "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 7667d3081f..f5d79153ec 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -307,3 +307,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "keepout_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 58b92e608f..bf65fdf9ba 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index a3be1396bc..3799062cf8 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True 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 index 2539cef7b5..b8bc078103 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -5,6 +5,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -305,3 +306,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 78d84fb8db..e0c1692792 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -32,13 +32,15 @@ class NodeThread * @brief A background thread to process node callbacks constructor * @param node_base Interface to Node to spin in thread */ - explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + explicit NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); /** * @brief A background thread to process executor's callbacks constructor * @param executor Interface to executor to spin in thread */ - explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + explicit NodeThread( + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); /** * @brief A background thread to process node callbacks constructor diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d0bb65cf41..99570224ad 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -153,23 +154,11 @@ std::string get_plugin_type_param( } /** - * @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 + * @brief Sets the caller thread to have a soft-realtime prioritization by + * increasing the priority level of the host thread. + * May throw exception if unable to set prioritization successfully */ -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()); - } - } -} +void setSoftRealTimePriority(); } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index f05a4602ac..c74dcf55f2 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_util/node_thread.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -57,6 +58,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ template explicit SimpleActionServer( @@ -66,13 +69,15 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, completion_callback, server_timeout, spin_thread, options) + action_name, execute_callback, completion_callback, + server_timeout, spin_thread, options, realtime) {} /** @@ -83,6 +88,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -94,7 +101,8 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), node_logging_interface_(node_logging_interface), @@ -106,6 +114,7 @@ class SimpleActionServer spin_thread_(spin_thread) { using namespace std::placeholders; // NOLINT + use_realtime_prioritization_ = realtime; if (spin_thread_) { callback_group_ = node_base_interface->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -170,6 +179,17 @@ class SimpleActionServer return rclcpp_action::CancelResponse::ACCEPT; } + /** + * @brief Sets thread priority level + */ + void setSoftRealTimePriority() + { + if (use_realtime_prioritization_) { + nav2_util::setSoftRealTimePriority(); + debug_msg("Soft realtime prioritization successfully set!"); + } + } + /** * @brief Handles accepted goals and adds to preempted queue to switch to * @param Goal A server goal handle to cancel @@ -202,7 +222,11 @@ class SimpleActionServer // Return quickly to avoid blocking the executor, so spin up a new thread debug_msg("Executing goal asynchronously."); - execution_future_ = std::async(std::launch::async, [this]() {work();}); + execution_future_ = std::async( + std::launch::async, [this]() { + setSoftRealTimePriority(); + work(); + }); } } @@ -509,6 +533,7 @@ class SimpleActionServer CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; + bool use_realtime_prioritization_{false}; mutable std::recursive_mutex update_mutex_; bool server_active_{false}; diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index c19fbd9847..adcbe50b78 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include - #include "nav2_util/node_thread.hpp" namespace nav2_util { -NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +NodeThread::NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { executor_ = std::make_shared(); @@ -35,7 +35,10 @@ NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nod NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) : executor_(executor) { - thread_ = std::make_unique([&]() {executor_->spin();}); + thread_ = std::make_unique( + [&]() { + executor_->spin(); + }); } NodeThread::~NodeThread() diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index e5415b81ad..993eaf53b6 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -89,4 +90,17 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix) return rclcpp::Node::make_shared("_", options); } +void setSoftRealTimePriority() +{ + sched_param sch; + sch.sched_priority = 49; + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + std::string errmsg( + "Cannot set as real-time thread. Users must set: hard rtprio 99 and " + " soft rtprio 99 in /etc/security/limits.conf to enable " + "realtime prioritization! Error: "); + throw std::runtime_error(errmsg + std::strerror(errno)); + } +} + } // namespace nav2_util diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index ff85d2bd41..bf94527635 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,35 +94,3 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } - -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/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c86659aae..d3dd5c2a99 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -133,6 +133,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1)); + declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; }