From d4c3faa23037417b5b520373a7a6fa31a0585a9b Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 20 Sep 2024 20:41:15 +0200 Subject: [PATCH 01/29] fix(simple-action-server): info log instead of warn on cancel (#4684) Cancelling a goal is nominal behavior and therefore it should not log warning. Signed-off-by: Rein Appeldoorn --- nav2_util/include/nav2_util/simple_action_server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index c8b1660840f..342f930ab9f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -582,7 +582,7 @@ class SimpleActionServer if (is_active(handle)) { if (handle->is_canceling()) { - warn_msg("Client requested to cancel the goal. Cancelling."); + info_msg("Client requested to cancel the goal. Cancelling."); handle->canceled(result); } else { warn_msg("Aborting handle."); From 5dd7ad106986914cef2f25e3ab39fb976c869456 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Fri, 20 Sep 2024 21:12:57 +0200 Subject: [PATCH 02/29] feat(controller-server): `publish_zero_velocity` parameter (#4675) * feat(controller-server): `publish_zero_velocity` parameter For optionally publishing a zero velocity command reference on goal exit. Publishing a zero velocity is not desired when we are following consecutive path segments that end with a velocity. Signed-off-by: Rein Appeldoorn * processed comments Signed-off-by: Rein Appeldoorn * comments Steve Signed-off-by: Rein Appeldoorn --------- Signed-off-by: Rein Appeldoorn --- .../nav2_controller/controller_server.hpp | 5 +++ nav2_controller/src/controller_server.cpp | 31 ++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index e2d3fd497a1..6e30e17f140 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -176,6 +176,10 @@ class ControllerServer : public nav2_util::LifecycleNode * @brief Calls velocity publisher to publish zero velocity */ void publishZeroVelocity(); + /** + * @brief Called on goal exit + */ + void onGoalExit(); /** * @brief Checks if goal is reached * @return true or false @@ -267,6 +271,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; bool use_realtime_priority_; + bool publish_zero_velocity_; rclcpp::Duration costmap_update_timeout_; // Whether we've published the single controller warning yet diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index a990c3a4005..ec9dc3d81b8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -64,6 +64,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); + declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true)); declare_parameter("costmap_update_timeout", 0.30); // 300ms // The costmap node is used in the implementation of the controller @@ -130,6 +131,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); get_parameter("use_realtime_priority", use_realtime_priority_); + get_parameter("publish_zero_velocity", publish_zero_velocity_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -476,7 +478,7 @@ void ControllerServer::computeControl() if (controllers_[current_controller_]->cancel()) { RCLCPP_INFO(get_logger(), "Cancellation was successful. Stopping the robot."); action_server_->terminate_all(); - publishZeroVelocity(); + onGoalExit(); return; } else { RCLCPP_INFO_THROTTLE( @@ -513,63 +515,63 @@ void ControllerServer::computeControl() } } catch (nav2_core::InvalidController & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::INVALID_CONTROLLER; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::TF_ERROR; action_server_->terminate_current(result); return; } catch (nav2_core::NoValidControl & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::NO_VALID_CONTROL; action_server_->terminate_current(result); return; } catch (nav2_core::FailedToMakeProgress & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::FAILED_TO_MAKE_PROGRESS; action_server_->terminate_current(result); return; } catch (nav2_core::PatienceExceeded & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::PATIENCE_EXCEEDED; action_server_->terminate_current(result); return; } catch (nav2_core::InvalidPath & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::INVALID_PATH; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerTimedOut & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::CONTROLLER_TIMED_OUT; action_server_->terminate_current(result); return; } catch (nav2_core::ControllerException & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); return; } catch (std::exception & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - publishZeroVelocity(); + onGoalExit(); std::shared_ptr result = std::make_shared(); result->error_code = Action::Result::UNKNOWN; action_server_->terminate_current(result); @@ -578,7 +580,7 @@ void ControllerServer::computeControl() RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); - publishZeroVelocity(); + onGoalExit(); // TODO(orduno) #861 Handle a pending preemption and set controller name action_server_->succeeded_current(); @@ -746,6 +748,13 @@ void ControllerServer::publishZeroVelocity() velocity.header.frame_id = costmap_ros_->getBaseFrameID(); velocity.header.stamp = now(); publishVelocity(velocity); +} + +void ControllerServer::onGoalExit() +{ + if (publish_zero_velocity_) { + publishZeroVelocity(); + } // Reset the state of the controllers after the task has ended ControllerMap::iterator it; From 0ffc0fbd0d5f8ac532dc37f3d02697dc3045333d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 26 Sep 2024 15:50:13 +0200 Subject: [PATCH 03/29] Improvements in RemoveInCollisionGoals and adjacent features (#4676) * improvements Signed-off-by: Tony Najjar * ament_uncrustify Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * fix building Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar * add stamp Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 12 ++++++--- .../remove_in_collision_goals_action.cpp | 17 +++++++----- nav2_costmap_2d/src/costmap_2d_ros.cpp | 22 ++++++++++----- nav2_costmap_2d/src/layered_costmap.cpp | 8 +++--- .../test/unit/costmap_cost_service_test.cpp | 16 ++++++----- nav2_msgs/srv/GetCosts.srv | 2 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 27 +++++++++---------- 7 files changed, 62 insertions(+), 42 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 73aab1bd8cd..8992b4d516a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -52,17 +52,23 @@ class RemoveInCollisionGoals : public BtServiceNode static BT::PortsList providedPorts() { - return providedBasicPorts({ + return providedBasicPorts( + { BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("cost_threshold", 254.0, + BT::InputPort( + "cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), BT::InputPort("use_footprint", true, "Whether to use footprint cost"), + BT::InputPort( + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), - }); + }); } private: bool use_footprint_; + bool consider_unknown_as_obstacle_; double cost_threshold_; Goals input_goals_; }; diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 2fe8395ad4c..d6f9ee662d0 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -37,6 +37,7 @@ void RemoveInCollisionGoals::on_tick() getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); getInput("input_goals", input_goals_); + getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); if (input_goals_.empty()) { setOutput("output_goals", input_goals_); @@ -47,11 +48,7 @@ void RemoveInCollisionGoals::on_tick() request_->use_footprint = use_footprint_; for (const auto & goal : input_goals_) { - geometry_msgs::msg::Pose2D pose; - pose.x = goal.pose.position.x; - pose.y = goal.pose.position.y; - pose.theta = tf2::getYaw(goal.pose.orientation); - request_->poses.push_back(pose); + request_->poses.push_back(goal); } } @@ -60,10 +57,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( { Goals valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { - if (response->costs[i] < cost_threshold_) { + if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || + response->costs[i] < cost_threshold_) + { valid_goal_poses.push_back(input_goals_[i]); } } + // Inform if all goals have been removed + if (valid_goal_poses.empty()) { + RCLCPP_INFO( + node_->get_logger(), + "All goals are in collision and have been removed from the list"); + } setOutput("output_goals", valid_goal_poses); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index dcf6d50b7a0..fa11b60ebfe 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -254,7 +254,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // Service to get the cost at a point get_cost_service_ = create_service( "get_cost_" + getName(), - std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, + std::bind( + &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service @@ -835,12 +836,17 @@ void Costmap2DROS::getCostsCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); for (const auto & pose : request->poses) { - bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my); + geometry_msgs::msg::PoseStamped pose_transformed; + transformPoseToGlobalFrame(pose, pose_transformed); + bool in_bounds = costmap->worldToMap( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, mx, my); if (!in_bounds) { - response->costs.push_back(-1.0); + response->costs.push_back(NO_INFORMATION); continue; } + double yaw = tf2::getYaw(pose_transformed.pose.orientation); if (request->use_footprint) { Footprint footprint = layered_costmap_->getFootprint(); @@ -848,13 +854,17 @@ void Costmap2DROS::getCostsCallback( RCLCPP_DEBUG( get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", - pose.x, pose.y, pose.theta); + pose_transformed.pose.position.x, pose_transformed.pose.position.y, yaw); response->costs.push_back( - collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint)); + collision_checker.footprintCostAtPose( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, yaw, footprint)); } else { RCLCPP_DEBUG( - get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y); + get_logger(), "Received request to get cost at point (%f, %f)", + pose_transformed.pose.position.x, + pose_transformed.pose.position.y); // Get the cost at the map coordinates response->costs.push_back(static_cast(costmap->getCost(mx, my))); diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 33e9fc22aa5..2995ae03029 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -72,11 +72,11 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo footprint_(std::make_shared>()) { if (track_unknown) { - primary_costmap_.setDefaultValue(255); - combined_costmap_.setDefaultValue(255); + primary_costmap_.setDefaultValue(NO_INFORMATION); + combined_costmap_.setDefaultValue(NO_INFORMATION); } else { - primary_costmap_.setDefaultValue(0); - combined_costmap_.setDefaultValue(0); + primary_costmap_.setDefaultValue(FREE_SPACE); + combined_costmap_.setDefaultValue(FREE_SPACE); } } diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index 803e2a1f8e7..15780196b17 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -50,9 +50,9 @@ class GetCostServiceTest : public ::testing::Test TEST_F(GetCostServiceTest, TestWithoutFootprint) { auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = 0.5; - pose.y = 1.0; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0.5; + pose.pose.position.y = 1.0; request->poses.push_back(pose); request->use_footprint = false; @@ -72,10 +72,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) TEST_F(GetCostServiceTest, TestWithFootprint) { auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = 0.5; - pose.y = 1.0; - pose.theta = 0.5; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0.5; + pose.pose.position.y = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0.5); + pose.pose.orientation = tf2::toMsg(q); request->poses.push_back(pose); request->use_footprint = true; diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 0328d476397..55ebad4f0a2 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,6 +1,6 @@ # Get costmap costs at given poses bool use_footprint -geometry_msgs/Pose2D[] poses +geometry_msgs/PoseStamped[] poses --- float32[] costs \ No newline at end of file diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index d5ef5658745..03435a913e4 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -94,23 +94,28 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) void CostmapCostTool::callCostService(float x, float y) { + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // Create request for local costmap auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = x; - pose.y = y; + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = context_->getFixedFrame().toStdString(); + pose.header.stamp = node->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; request->poses.push_back(pose); request->use_footprint = false; // Call local costmap service if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { - local_cost_client_->async_send_request(request, + local_cost_client_->async_send_request( + request, std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1)); } // Call global costmap service if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) { - global_cost_client_->async_send_request(request, + global_cost_client_->async_send_request( + request, std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1)); } } @@ -120,11 +125,7 @@ void CostmapCostTool::handleLocalCostResponse( { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->costs[0] != -1) { - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost"); - } + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); } void CostmapCostTool::handleGlobalCostResponse( @@ -132,11 +133,7 @@ void CostmapCostTool::handleGlobalCostResponse( { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->costs[0] != -1) { - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost"); - } + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); } } // namespace nav2_rviz_plugins From 81739bfdcdeac4984c6f9229feadc2d5f04163eb Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 30 Sep 2024 13:50:30 -0700 Subject: [PATCH 04/29] Correct paper name for graceful controller Signed-off-by: Steve Macenski --- nav2_graceful_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 5c3f8a39fb5..24f1b851e7e 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,5 +1,5 @@ # Graceful Motion Controller -The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. +The graceful motion controller implements a controller based on the works of Jong Jin Park in "A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environment". In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. From 3233c84fe91a37f03aabe43a8d906dd1c46356b7 Mon Sep 17 00:00:00 2001 From: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Date: Tue, 1 Oct 2024 18:13:50 -0400 Subject: [PATCH 05/29] Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo --------- Signed-off-by: Tiwa Ojo --- .../nav2_simple_commander/robot_navigator.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 54d76044331..4b604d55613 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -136,6 +136,10 @@ def destroy_node(self): self.spin_client.destroy() self.backup_client.destroy() self.drive_on_heading_client.destroy() + self.assisted_teleop_client.destroy() + self.follow_gps_waypoints_client.destroy() + self.docking_client.destroy() + self.undocking_client.destroy() super().destroy_node() def setInitialPose(self, initial_pose): From fc7e08611a453889feb112bc4c2160687d320a91 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Oct 2024 11:25:37 -0700 Subject: [PATCH 06/29] Adding disengagement threshold to rotation shim controller (#4699) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski * change default to 22.5 deg Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../nav2_rotation_shim_controller.hpp | 4 ++-- .../src/nav2_rotation_shim_controller.cpp | 14 ++++++++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index f0da5e0fba2..d3807eeb952 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -168,10 +168,10 @@ class RotationShimController : public nav2_core::Controller nav2_core::Controller::Ptr primary_controller_; bool path_updated_; nav_msgs::msg::Path current_path_; - double forward_sampling_distance_, angular_dist_threshold_; + double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_; + bool rotate_to_goal_heading_, in_rotation_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 4e111edfda5..5947f5e36e4 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -34,7 +34,8 @@ namespace nav2_rotation_shim_controller RotationShimController::RotationShimController() : lp_loader_("nav2_core", "nav2_core::Controller"), primary_controller_(nullptr), - path_updated_(false) + path_updated_(false), + in_rotation_(false) { } @@ -56,6 +57,8 @@ void RotationShimController::configure( double control_frequency; nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785 / 2.0)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( @@ -70,6 +73,7 @@ void RotationShimController::configure( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); + node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); node->get_parameter( plugin_name_ + ".rotate_to_heading_angular_vel", @@ -111,6 +115,7 @@ void RotationShimController::activate() plugin_name_.c_str()); primary_controller_->activate(); + in_rotation_ = false; auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -197,10 +202,14 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); - if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + + double angular_thresh = + in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_; + if (abs(angular_distance_to_heading) > angular_thresh) { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); + in_rotation_ = true; return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); } else { RCLCPP_DEBUG( @@ -219,6 +228,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands } // If at this point, use the primary controller to path track + in_rotation_ = false; return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); } From 406a2f563542453f71b701078aa023ac44350b6b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 Oct 2024 15:00:58 -0400 Subject: [PATCH 07/29] Switch nav2_waypoint_follower to modern CMake idioms. (#4648) Signed-off-by: Chris Lalancette --- nav2_waypoint_follower/CMakeLists.txt | 168 ++++++++++++------ .../plugins/photo_at_waypoint.hpp | 8 - nav2_waypoint_follower/package.xml | 27 +-- nav2_waypoint_follower/test/CMakeLists.txt | 19 +- 4 files changed, 141 insertions(+), 81 deletions(-) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 0ea7eda8eba..e9624f36c84 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,87 +1,126 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) -# Try for OpenCV 4.X, but settle for whatever is installed -find_package(OpenCV 4 QUIET) -if(NOT OpenCV_FOUND) - find_package(OpenCV REQUIRED) -endif() -message(STATUS "Found OpenCV version ${OpenCV_VERSION}") - -find_package(image_transport REQUIRED) -find_package(cv_bridge REQUIRED) find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_transport REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(nav_msgs REQUIRED) +find_package(nav2_core REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav2_core REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(OpenCV REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(robot_localization REQUIRED) -find_package(geographic_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories( - include -) - set(executable_name waypoint_follower) -add_executable(${executable_name} - src/main.cpp -) - set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/waypoint_follower.cpp ) - -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - rclcpp_components - nav_msgs - nav2_msgs - nav2_util - tf2_ros - nav2_core - pluginlib - image_transport - cv_bridge - OpenCV - robot_localization +target_include_directories(${library_name} PUBLIC + "$" + "$" ) - -ament_target_dependencies(${executable_name} - ${dependencies} +target_link_libraries(${library_name} PUBLIC + ${geographic_msgs_TARGETS} + nav2_core::nav2_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + robot_localization::rl_lib + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component ) -target_link_libraries(${executable_name} ${library_name}) - -ament_target_dependencies(${library_name} - ${dependencies} +add_executable(${executable_name} + src/main.cpp +) +target_include_directories(${executable_name} PRIVATE + "$" + "$" +) +target_link_libraries(${executable_name} PRIVATE + ${library_name} + rclcpp::rclcpp ) add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp) -ament_target_dependencies(wait_at_waypoint ${dependencies}) +target_include_directories(wait_at_waypoint PUBLIC + "$" + "$" +) +target_link_libraries(wait_at_waypoint PUBLIC + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + nav2_core::nav2_core +) +target_link_libraries(wait_at_waypoint PRIVATE + pluginlib::pluginlib + nav2_util::nav2_util_core +) add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp) -ament_target_dependencies(photo_at_waypoint ${dependencies}) +target_include_directories(photo_at_waypoint PUBLIC + "$" + "$" +) +target_link_libraries(photo_at_waypoint PUBLIC + cv_bridge::cv_bridge + ${geometry_msgs_TARGETS} + image_transport::image_transport + nav2_core::nav2_core + opencv_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} +) +target_link_libraries(photo_at_waypoint PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib +) add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp) -ament_target_dependencies(input_at_waypoint ${dependencies}) +target_include_directories(input_at_waypoint PUBLIC + "$" + "$" +) +target_link_libraries(input_at_waypoint PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${std_msgs_TARGETS} +) +target_link_libraries(input_at_waypoint PRIVATE + pluginlib::pluginlib + nav2_util::nav2_util_core +) rclcpp_components_register_nodes(${library_name} "nav2_waypoint_follower::WaypointFollower") install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -92,19 +131,40 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint ${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + cv_bridge + geographic_msgs + geometry_msgs + image_transport + nav2_core + nav2_msgs + nav2_util + nav_msgs + OpenCV + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + robot_localization + sensor_msgs + std_msgs + tf2_ros +) +ament_export_targets(${PROJECT_NAME}) pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) ament_package() diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 1e6048aefca..8b5b2aaf150 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -15,20 +15,12 @@ #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ -/** - * While C++17 isn't the project standard. We have to force LLVM/CLang - * to ignore deprecated declarations - */ -#define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM - - #include #include #include #include #include "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" #include "sensor_msgs/msg/image.hpp" #include "nav2_core/waypoint_task_executor.hpp" diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 574a64bcc2c..aeb3d44009d 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -8,26 +8,29 @@ Apache-2.0 ament_cmake + nav2_common - nav2_common cv_bridge - pluginlib + geographic_msgs + geometry_msgs image_transport + nav2_core + nav2_msgs + nav2_util + nav_msgs + pluginlib rclcpp rclcpp_action + rclcpp_components rclcpp_lifecycle - nav_msgs - nav2_msgs - nav2_util - nav2_core - tf2_ros robot_localization - geographic_msgs - - ament_lint_common - ament_lint_auto + sensor_msgs + std_msgs + tf2_ros + ament_cmake_gtest - ament_cmake_pytest + ament_lint_auto + ament_lint_common ament_cmake diff --git a/nav2_waypoint_follower/test/CMakeLists.txt b/nav2_waypoint_follower/test/CMakeLists.txt index a6a7388eada..a8f498ca02c 100644 --- a/nav2_waypoint_follower/test/CMakeLists.txt +++ b/nav2_waypoint_follower/test/CMakeLists.txt @@ -2,20 +2,25 @@ ament_add_gtest(test_task_executors test_task_executors.cpp ) -ament_target_dependencies(test_task_executors - ${dependencies} -) target_link_libraries(test_task_executors - ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint + ${library_name} + wait_at_waypoint + photo_at_waypoint + input_at_waypoint + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} ) # Test dynamic parameters ament_add_gtest(test_dynamic_parameters test_dynamic_parameters.cpp ) -ament_target_dependencies(test_dynamic_parameters - ${dependencies} -) target_link_libraries(test_dynamic_parameters ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) From 1a3b637d90484a3d6cac5e2cd75836df8518a32b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 3 Oct 2024 11:39:25 -0700 Subject: [PATCH 08/29] Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski --- .../nav2_mppi_controller/tools/utils.hpp | 155 ++++-------------- nav2_smoother/src/savitzky_golay_smoother.cpp | 90 ++++------ 2 files changed, 63 insertions(+), 182 deletions(-) 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 ddde6077659..4ad0dbfbd78 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -461,9 +461,8 @@ inline void savitskyGolayFilter( xt::xarray filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; filter /= 231.0; - const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; - // Too short to smooth meaningfully + const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; if (num_sequences < 20) { return; } @@ -473,137 +472,49 @@ inline void savitskyGolayFilter( }; auto applyFilterOverAxis = - [&](xt::xtensor & sequence, + [&](xt::xtensor & sequence, const xt::xtensor & initial_sequence, const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void { - unsigned int idx = 0; - sequence(idx) = applyFilter( - { - hist_0, - hist_1, - hist_2, - hist_3, - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_1, - hist_2, - hist_3, - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_2, - hist_3, - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - idx++; - sequence(idx) = applyFilter( - { - hist_3, - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); - - for (idx = 4; idx != num_sequences - 4; idx++) { - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 4)}); + float pt_m4 = hist_0; + float pt_m3 = hist_1; + float pt_m2 = hist_2; + float pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1); + float pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3); + float pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter({pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + pt_p3 = pt_p4; + + if (idx + 5 < num_sequences) { + pt_p4 = initial_sequence(idx + 5); + } else { + // Return the last point + pt_p4 = initial_sequence(num_sequences); + } } - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 3), - sequence(idx + 3)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 2), - sequence(idx + 2), - sequence(idx + 2)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx + 1), - sequence(idx + 1), - sequence(idx + 1), - sequence(idx + 1)}); - - idx++; - sequence(idx) = applyFilter( - { - sequence(idx - 4), - sequence(idx - 3), - sequence(idx - 2), - sequence(idx - 1), - sequence(idx), - sequence(idx), - sequence(idx), - sequence(idx), - sequence(idx)}); }; // Filter trajectories + const models::ControlSequence initial_control_sequence = control_sequence; applyFilterOverAxis( - control_sequence.vx, control_history[0].vx, + control_sequence.vx, initial_control_sequence.vx, control_history[0].vx, control_history[1].vx, control_history[2].vx, control_history[3].vx); applyFilterOverAxis( - control_sequence.vy, control_history[0].vy, + control_sequence.vy, initial_control_sequence.vy, control_history[0].vy, control_history[1].vy, control_history[2].vy, control_history[3].vy); applyFilterOverAxis( - control_sequence.wz, control_history[0].wz, + control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, control_history[1].wz, control_history[2].wz, control_history[3].wz); // Update control history diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 17fe63a926b..2da176b12f3 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -118,74 +118,44 @@ bool SavitzkyGolaySmoother::smoothImpl( }; auto applyFilterOverAxes = - [&](std::vector & plan_pts) -> void + [&](std::vector & plan_pts, + const std::vector & init_plan_pts) -> void { - // Handle initial boundary conditions, first point is fixed - unsigned int idx = 1; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 1].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); - - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 2].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); - - // Apply nominal filter - for (idx = 3; idx < path_size - 4; ++idx) { - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 3].pose.position}); + auto pt_m3 = init_plan_pts[0].pose.position; + auto pt_m2 = init_plan_pts[0].pose.position; + auto pt_m1 = init_plan_pts[0].pose.position; + auto pt = init_plan_pts[1].pose.position; + auto pt_p1 = init_plan_pts[2].pose.position; + auto pt_p2 = init_plan_pts[3].pose.position; + auto pt_p3 = init_plan_pts[4].pose.position; + + // First point is fixed + for (unsigned int idx = 1; idx != path_size - 1; idx++) { + plan_pts[idx].pose.position = applyFilter({pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3}); + pt_m3 = pt_m2; + pt_m2 = pt_m1; + pt_m1 = pt; + pt = pt_p1; + pt_p1 = pt_p2; + pt_p2 = pt_p3; + + if (idx + 4 < path_size - 1) { + pt_p3 = init_plan_pts[idx + 4].pose.position; + } else { + // Return the last point + pt_p3 = init_plan_pts[path_size - 1].pose.position; + } } - - // Handle terminal boundary conditions, last point is fixed - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 2].pose.position, - plan_pts[idx + 2].pose.position}); - - idx++; - plan_pts[idx].pose.position = applyFilter( - { - plan_pts[idx - 3].pose.position, - plan_pts[idx - 2].pose.position, - plan_pts[idx - 1].pose.position, - plan_pts[idx].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 1].pose.position, - plan_pts[idx + 1].pose.position}); }; - applyFilterOverAxes(path.poses); + const auto initial_path_poses = path.poses; + applyFilterOverAxes(path.poses, initial_path_poses); // Lets do additional refinement, it shouldn't take more than a couple milliseconds if (do_refinement_) { for (int i = 0; i < refinement_num_; i++) { - applyFilterOverAxes(path.poses); + const auto reined_initial_path_poses = path.poses; + applyFilterOverAxes(path.poses, reined_initial_path_poses); } } From 4e62a89d01aeb3cc88562a559cd142688305272b Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Fri, 4 Oct 2024 02:03:07 +0200 Subject: [PATCH 09/29] Feat/migrate gps nav2 system test (#4682) * include missing docking station parameters Signed-off-by: stevedan * fix crach RL Signed-off-by: stevedan * lintering Signed-off-by: stevedan * Change naming Signed-off-by: stevedan * update submodule Signed-off-by: stevedan * minor changes Signed-off-by: stevedan * fix issue with caching Signed-off-by: stevedan * fix issue with caching, increase version number Signed-off-by: stevedan * Pin git ref via sha to bust underlay workspace cache --------- Signed-off-by: stevedan Co-authored-by: ruffsl --- .circleci/config.yml | 6 +- nav2_system_tests/CMakeLists.txt | 2 +- .../src/gps_navigation/CMakeLists.txt | 4 +- .../dual_ekf_navsat_params.yaml | 2 +- .../gps_navigation/nav2_no_map_params.yaml | 174 ++++++++++++++---- .../src/gps_navigation/test_case_py.launch.py | 118 +++++------- .../src/gps_navigation/tester.py | 6 +- tools/underlay.repos | 2 +- 8 files changed, 193 insertions(+), 121 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 99a7a13929e..9cf5261e8d0 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v26\ + - "<< parameters.key >>-v28\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v26\ + - "<< parameters.key >>-v28\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v26\ + key: "<< parameters.key >>-v28\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 851a1e2db40..e3152c0bde0 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -113,7 +113,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) - # add_subdirectory(src/gps_navigation) + add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/spin) add_subdirectory(src/behaviors/backup) diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt index 3342b6cd580..742d004f115 100644 --- a/nav2_system_tests/src/gps_navigation/CMakeLists.txt +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -5,7 +5,5 @@ ament_add_test(test_gps_waypoint_follower TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) + diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml index 6783a7b1db7..e46ce5d1a8d 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml @@ -121,7 +121,7 @@ navsat_transform: magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998 yaw_offset: 0.0 zero_altitude: true - broadcast_utm_transform: true + broadcast_cartesian_transform: true publish_filtered_gps: true use_odometry_yaw: true wait_for_datum: false diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 96dc593d824..0911902aee9 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 @@ -53,46 +53,93 @@ controller_server: 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 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -151,6 +198,8 @@ global_costmap: rolling_window: True width: 50 height: 50 + # origin_x: 0.0 + # origin_y: 0.0 track_unknown_space: true # no static map plugins: ["obstacle_layer", "inflation_layer"] @@ -290,3 +339,48 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + docks: ['home_dock'] # Input your docks here + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 \ No newline at end of file diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 69f19dd6ac0..3dd082c1918 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -14,6 +14,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -21,6 +22,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,100 +35,77 @@ def generate_launch_description(): - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_empty_world.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle_gps.sdf.xacro') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle_gps.urdf') + + with open(urdf, 'r') as infp: + robot_description = infp.read() + + # use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml') - bringup_dir = get_package_share_directory('nav2_bringup') configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True, ) return LaunchDescription( [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '--x', '0', - '--y', '0', - '--z', '0', - '--roll', '0', - '--pitch', '0', - '--yaw', '0', - '--frame-id', 'base_footprint', - '--child-frame-id', 'base_link' - ], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()), ), - Node( - package='tf2_ros', - executable='static_transform_publisher', + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], output='screen', - arguments=[ - '--x', '0', - '--y', '0', - '--z', '0', - '--roll', '0', - '--pitch', '0', - '--yaw', '0', - '--frame-id', 'base_link', - '--child-frame-id', 'base_scan' - ], ), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '--x', '-0.32', - '--y', '0', - '--z', '0.068', - '--roll', '0', - '--pitch', '0', - '--yaw', '0', - '--frame-id', 'base_link', - '--child-frame-id', 'imu_link' - ], + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3_gps.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '0.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=[ - '--x', '0', - '--y', '0', - '--z', '0', - '--roll', '0', - '--pitch', '0', - '--yaw', '0', - '--frame-id', 'base_link', - '--child-frame-id', 'gps_link' + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'navigation_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py') ), launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', 'use_sim_time': 'True', 'params_file': configured_params, + 'use_composition': 'False', 'autostart': 'True', }.items(), ), @@ -152,7 +131,8 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index b9fb4725efb..56064aa9ace 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -180,9 +180,9 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # set waypoint outside of map + # set waypoint outside of map but not outide the utm zone time.sleep(2) - test.setWaypoints([[35.0, -118.0]]) + test.setWaypoints([[55.929834, -3.184343]]) result = test.run(True, False) assert not result result = not result @@ -193,7 +193,7 @@ def main(argv=sys.argv[1:]): # stop on failure test with bogous waypoint test.setStopFailureParam(True) - bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]] + bwps = [[55.944831, -3.186998], [55.929834, -3.184343], [55.944782, -3.187060]] test.setWaypoints(bwps) result = test.run(True, False) assert not result diff --git a/tools/underlay.repos b/tools/underlay.repos index f28dc3a0cf4..09a6da9a92b 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -34,4 +34,4 @@ repositories: ros-navigation/nav2_minimal_turtlebot_simulation: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - version: 1.0.1 + version: f0eeedbc95d9f7cc8a513f7d46a84b3d08a3d395 From 6311978e0902b807a05883b3764e72f731d446f8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Oct 2024 19:06:49 -0700 Subject: [PATCH 10/29] fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas * adding support for rest of servers + review comments Signed-off-by: Steve Macenski * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski * fix vel smoother unit tests Signed-off-by: Steve Macenski * fixing docking server unit testing Signed-off-by: Steve Macenski * fixing last bits Signed-off-by: Steve Macenski --------- Signed-off-by: Kemal Bektas Signed-off-by: Steve Macenski Co-authored-by: Kemal Bektas --- nav2_behaviors/src/behavior_server.cpp | 3 +- nav2_bt_navigator/src/bt_navigator.cpp | 6 ++- .../src/collision_detector_node.cpp | 3 +- .../src/collision_monitor_node.cpp | 4 +- nav2_controller/src/controller_server.cpp | 6 ++- .../opennav_docking/src/docking_server.cpp | 3 +- .../test/test_docking_server_unit.cpp | 50 ++++++++++++++++++- nav2_planner/src/planner_server.cpp | 3 +- nav2_smoother/src/nav2_smoother.cpp | 5 +- .../src/velocity_smoother.cpp | 49 ++++++++++++------ .../test/test_velocity_smoother.cpp | 15 +++--- .../src/waypoint_follower.cpp | 3 +- 12 files changed, 117 insertions(+), 33 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 1017bf595a3..a04aea40315 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -78,7 +78,7 @@ BehaviorServer::~BehaviorServer() } nav2_util::CallbackReturn -BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -91,6 +91,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } setupResourcesForBehaviorPlugins(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9ae64016743..c49fae0e2b9 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -56,7 +56,7 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create navigator id %s." " Exception: %s", navigator_ids[i].c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -141,11 +142,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); for (size_t i = 0; i != navigators_.size(); i++) { if (!navigators_[i]->on_activate()) { + on_deactivate(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index d8e03e19ed4..7f87bc1bb92 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector() } nav2_util::CallbackReturn -CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index c0bd15cb63a..2b0991c9941 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -42,7 +42,7 @@ CollisionMonitor::~CollisionMonitor() } nav2_util::CallbackReturn -CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ec9dc3d81b8..c221c2db17b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -82,7 +82,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -152,6 +152,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -178,6 +179,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create goal checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -206,6 +208,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -242,6 +245,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0d5034bd60c..b350cb40262 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) } nav2_util::CallbackReturn -DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +DockingServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring %s", get_name()); auto node = shared_from_this(); @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 144a612f3bd..f12741774c8 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions) EXPECT_TRUE(false); } } + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, getateGoalDock) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); std::shared_ptr goal = std::make_shared(); auto dock = node->generateGoalDock(goal); - EXPECT_EQ(dock->plugin, nullptr); + EXPECT_NE(dock->plugin, nullptr); EXPECT_EQ(dock->frame, std::string()); node->stashDockData(false, dock, true); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } TEST(DockingServerTests, testDynamicParams) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); @@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + + node->on_deactivate(rclcpp_lifecycle::State()); + node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); + node.reset(); } } // namespace opennav_docking diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index bb59b0b0235..7233a7ba53d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -79,7 +79,7 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -120,6 +120,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index e7baa5e4839..fe462cb1c6b 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -63,7 +63,7 @@ SmootherServer::~SmootherServer() } nav2_util::CallbackReturn -SmootherServer::on_configure(const rclcpp_lifecycle::State &) +SmootherServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring smoother server"); @@ -100,6 +100,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &) *costmap_sub_, *footprint_sub_, this->get_name()); if (!loadSmootherPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -162,7 +163,7 @@ bool SmootherServer::loadSmootherPlugins() } nav2_util::CallbackReturn -SmootherServer::on_activate(const rclcpp_lifecycle::State &) +SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 535b6b08f21..9519ab94b61 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother() } nav2_util::CallbackReturn -VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) +VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring velocity smoother"); auto node = shared_from_this(); @@ -76,24 +76,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of deceleration! These should be negative to slow down!"); + RCLCPP_ERROR( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_accels_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of acceleration! These should be positive to speed up!"); + RCLCPP_ERROR( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > 0.0) { - throw std::runtime_error( - "Positive values set of min_velocities! These should be negative!"); + RCLCPP_ERROR( + get_logger(), "Positive values set of min_velocities! These should be negative!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (max_velocities_[i] < 0.0) { - throw std::runtime_error( - "Negative values set of max_velocities! These should be positive!"); + RCLCPP_ERROR( + get_logger(), "Negative values set of max_velocities! These should be positive!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } if (min_velocities_[i] > max_velocities_[i]) { - throw std::runtime_error( - "Min velocities are higher than max velocities!"); + RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!"); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } } @@ -112,9 +123,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) { - throw std::runtime_error( - "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 representing (x, y, theta)."); + RCLCPP_ERROR( + get_logger(), + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 representing (x, y, theta)."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Get control type @@ -124,7 +138,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) open_loop_ = false; odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); } else { - throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + RCLCPP_ERROR( + get_logger(), + "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Setup inputs / outputs @@ -144,6 +162,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 25754d0f355..76c5e0498b6 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -40,7 +40,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother public: VelSmootherShim() : VelocitySmoother() {} - void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + nav2_util::CallbackReturn configure(const rclcpp_lifecycle::State & state) + { + return this->on_configure(state); + } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} @@ -594,10 +597,10 @@ TEST(VelocitySmootherTest, testInvalidParams) std::vector max_vels{0.0, 0.0}; // invalid size smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) @@ -613,13 +616,13 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testDynamicParameter) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 02a4158d280..0014e79a15d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -54,7 +54,7 @@ WaypointFollower::~WaypointFollower() } nav2_util::CallbackReturn -WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -123,6 +123,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", e.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS; From fa9aaa935c5eae3dbcbd3f6a9652afbcde57f3f4 Mon Sep 17 00:00:00 2001 From: Saitama Date: Tue, 15 Oct 2024 19:55:35 +0200 Subject: [PATCH 11/29] [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5947f5e36e4..5b2bf306df0 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -253,7 +253,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() From 682e08915d0c2cc3fa3ad1bac110dc72590732db Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 15 Oct 2024 23:32:08 +0200 Subject: [PATCH 12/29] Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela * Improved dock_panel state machine Signed-off-by: Alberto Tudela * Added loading dock plugins log Signed-off-by: Alberto Tudela * Redo UI Signed-off-by: Alberto Tudela * Update tooltips Signed-off-by: Alberto Tudela * Fix null-dereference Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- .../nav2_rviz_plugins/docking_panel.hpp | 110 ++++-- nav2_rviz_plugins/src/docking_panel.cpp | 315 +++++++++++++----- nav2_rviz_plugins/src/utils.cpp | 4 +- 3 files changed, 317 insertions(+), 112 deletions(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index 868c644e4b1..f73f21952be 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -23,6 +23,7 @@ #include // ROS +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" @@ -37,6 +38,9 @@ class QPushButton; namespace nav2_rviz_plugins { +class InitialDockThread; + +/// Panel to interface to the docking server class DockingPanel : public rviz_common::Panel { Q_OBJECT @@ -44,11 +48,20 @@ class DockingPanel : public rviz_common::Panel public: explicit DockingPanel(QWidget * parent = 0); virtual ~DockingPanel(); + void onInitialize() override; + /// Load and save configuration data + void load(const rviz_common::Config & config) override; + void save(rviz_common::Config config) const override; + private Q_SLOTS: + void startThread(); + void onStartup(); void onDockingButtonPressed(); void onUndockingButtonPressed(); + void onCancelDocking(); + void onCancelUndocking(); void dockIdCheckbox(); private: @@ -57,14 +70,6 @@ private Q_SLOTS: using DockGoalHandle = rclcpp_action::ClientGoalHandle; using UndockGoalHandle = rclcpp_action::ClientGoalHandle; - // Start the actions - void startDocking(); - void startUndocking(); - - // Cancel the actions - void cancelDocking(); - void cancelUndocking(); - // The (non-spinning) client node used to invoke the action client void timerEvent(QTimerEvent * event) override; @@ -84,14 +89,33 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action + QBasicTimer action_timer_; + + // The Dock and Undock action client + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + + // Docking / Undocking action feedback subscribers + rclcpp::Subscription::SharedPtr docking_feedback_sub_; + rclcpp::Subscription::SharedPtr undocking_feedback_sub_; + rclcpp::Subscription::SharedPtr docking_goal_status_sub_; + rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; + + // Goal related state + DockGoalHandle::SharedPtr dock_goal_handle_; + UndockGoalHandle::SharedPtr undock_goal_handle_; + // Flags to indicate if the plugins have been loaded bool plugins_loaded_ = false; bool server_failed_ = false; - bool tried_once_ = false; - QBasicTimer timer_; QVBoxLayout * main_layout_{nullptr}; QHBoxLayout * info_layout_{nullptr}; @@ -121,20 +145,62 @@ private Q_SLOTS: bool undocking_in_progress_ = false; bool use_dock_id_ = false; - // The Dock and Undock action client - rclcpp_action::Client::SharedPtr dock_client_; - rclcpp_action::Client::SharedPtr undock_client_; - DockGoalHandle::SharedPtr dock_goal_handle_; - UndockGoalHandle::SharedPtr undock_goal_handle_; + QStateMachine state_machine_; + InitialDockThread * initial_thread_; - // The Node pointer that we need to keep alive for the duration of this plugin. - std::shared_ptr node_ptr_; + QState * pre_initial_{nullptr}; + QState * idle_{nullptr}; + QState * docking_{nullptr}; + QState * undocking_{nullptr}; + QState * canceled_docking_{nullptr}; + QState * canceled_undocking_{nullptr}; +}; - // Docking / Undocking action feedback subscribers - rclcpp::Subscription::SharedPtr docking_feedback_sub_; - rclcpp::Subscription::SharedPtr undocking_feedback_sub_; - rclcpp::Subscription::SharedPtr docking_goal_status_sub_; - rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; +class InitialDockThread : public QThread +{ + Q_OBJECT + +public: + explicit InitialDockThread( + rclcpp_action::Client::SharedPtr & dock_client, + rclcpp_action::Client::SharedPtr & undock_client) + : dock_client_(dock_client), undock_client_(undock_client) + {} + + void run() override + { + while (!dock_active_) { + dock_active_ = dock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + while (!undock_active_) { + undock_active_ = undock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + if (dock_active_) { + emit dockingActive(); + } else { + emit dockingInactive(); + } + + if (undock_active_) { + emit undockingActive(); + } else { + emit undockingInactive(); + } + } + +signals: + void dockingActive(); + void dockingInactive(); + void undockingActive(); + void undockingInactive(); + +private: + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + bool dock_active_ = false; + bool undock_active_ = false; }; } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 7daa327079c..e43f718ebee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -29,6 +29,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_rviz_plugins/docking_panel.hpp" +#include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "nav2_rviz_plugins/utils.hpp" using namespace std::chrono_literals; @@ -40,8 +41,7 @@ DockingPanel::DockingPanel(QWidget * parent) : Panel(parent), server_timeout_(100) { - client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); - + // Create the control buttons and its tooltip main_layout_ = new QVBoxLayout; info_layout_ = new QHBoxLayout; feedback_layout_ = new QVBoxLayout; @@ -50,8 +50,8 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_ = new QHBoxLayout; nav_stage_layout_ = new QHBoxLayout; dock_type_ = new QComboBox; - docking_button_ = new QPushButton("Dock robot"); - undocking_button_ = new QPushButton("Undock robot"); + docking_button_ = new QPushButton; + undocking_button_ = new QPushButton; docking_goal_status_indicator_ = new QLabel; docking_feedback_indicator_ = new QLabel; docking_result_indicator_ = new QLabel; @@ -62,27 +62,147 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_y_ = new QLineEdit; dock_pose_yaw_ = new QLineEdit; - docking_button_->setEnabled(false); - undocking_button_->setEnabled(false); - use_dock_id_checkbox_->setEnabled(false); - nav_to_staging_checkbox_->setEnabled(false); - dock_id_->setEnabled(false); - dock_pose_x_->setEnabled(false); - dock_pose_y_->setEnabled(false); - dock_pose_yaw_->setEnabled(false); + // Create the state machine used to present the proper control button states in the UI + const char * nav_to_stage_msg = "Navigate to the staging pose before docking"; + const char * use_dock_id_msg = "Use the dock id or the dock pose to dock the robot"; + const char * dock_msg = "Dock the robot at the specified docking station"; + const char * undock_msg = "Undock the robot from the docking station"; + const char * cancel_dock_msg = "Cancel the current docking action"; + const char * cancel_undock_msg = "Cancel the current undocking action"; + docking_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); docking_feedback_indicator_->setText(getDockFeedbackLabel()); docking_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); docking_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); - nav_to_staging_checkbox_->setFixedWidth(150); + pre_initial_ = new QState(); + pre_initial_->setObjectName("pre_initial"); + pre_initial_->assignProperty(docking_button_, "text", "Dock robot"); + pre_initial_->assignProperty(docking_button_, "enabled", false); + + pre_initial_->assignProperty(undocking_button_, "text", "Undock robot"); + pre_initial_->assignProperty(undocking_button_, "enabled", false); + + pre_initial_->assignProperty(nav_to_staging_checkbox_, "enabled", false); + pre_initial_->assignProperty(nav_to_staging_checkbox_, "checked", true); + pre_initial_->assignProperty(use_dock_id_checkbox_, "enabled", false); + pre_initial_->assignProperty(use_dock_id_checkbox_, "checked", true); + pre_initial_->assignProperty(dock_id_, "enabled", false); + pre_initial_->assignProperty(dock_type_, "enabled", false); + pre_initial_->assignProperty(dock_pose_x_, "enabled", false); + pre_initial_->assignProperty(dock_pose_y_, "enabled", false); + pre_initial_->assignProperty(dock_pose_yaw_, "enabled", false); + + // State entered when the docking / undocking action is not active + idle_ = new QState(); + idle_->setObjectName("idle"); + idle_->assignProperty(docking_button_, "text", "Dock robot"); + idle_->assignProperty(docking_button_, "toolTip", dock_msg); + idle_->assignProperty(docking_button_, "enabled", true); + + idle_->assignProperty(undocking_button_, "text", "Undock robot"); + idle_->assignProperty(undocking_button_, "toolTip", undock_msg); + idle_->assignProperty(undocking_button_, "enabled", true); + + idle_->assignProperty(nav_to_staging_checkbox_, "enabled", true); + idle_->assignProperty(nav_to_staging_checkbox_, "toolTip", nav_to_stage_msg); + idle_->assignProperty(use_dock_id_checkbox_, "enabled", true); + idle_->assignProperty(use_dock_id_checkbox_, "toolTip", use_dock_id_msg); + idle_->assignProperty(dock_id_, "enabled", true); + idle_->assignProperty(dock_type_, "enabled", true); + + // State entered to cancel the docking action + canceled_docking_ = new QState(); + canceled_docking_->setObjectName("canceled_docking"); + + // State entered to cancel the undocking action + canceled_undocking_ = new QState(); + canceled_undocking_->setObjectName("canceled_undocking"); + + // State entered while the docking action is active + docking_ = new QState(); + docking_->setObjectName("docking"); + docking_->assignProperty(docking_button_, "text", "Cancel docking"); + docking_->assignProperty(docking_button_, "toolTip", cancel_dock_msg); + + docking_->assignProperty(undocking_button_, "enabled", false); + + // State entered while the undocking action is active + undocking_ = new QState(); + undocking_->setObjectName("undocking"); + undocking_->assignProperty(docking_button_, "enabled", false); + + undocking_->assignProperty(undocking_button_, "text", "Cancel undocking"); + undocking_->assignProperty(undocking_button_, "toolTip", cancel_undock_msg); + + QObject::connect(docking_, SIGNAL(entered()), this, SLOT(onDockingButtonPressed())); + QObject::connect(undocking_, SIGNAL(entered()), this, SLOT(onUndockingButtonPressed())); + QObject::connect(canceled_docking_, SIGNAL(exited()), this, SLOT(onCancelDocking())); + QObject::connect(canceled_undocking_, SIGNAL(exited()), this, SLOT(onCancelUndocking())); + + // Start/Cancel button click transitions + idle_->addTransition(docking_button_, SIGNAL(clicked()), docking_); + idle_->addTransition(undocking_button_, SIGNAL(clicked()), undocking_); + docking_->addTransition(docking_button_, SIGNAL(clicked()), canceled_docking_); + undocking_->addTransition(undocking_button_, SIGNAL(clicked()), canceled_undocking_); + + // Internal state transitions + canceled_docking_->addTransition(canceled_docking_, SIGNAL(entered()), idle_); + canceled_undocking_->addTransition(canceled_undocking_, SIGNAL(entered()), idle_); + + // ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc) + // the state of the application will also update. This means that if in the processing + // states and then goes inactive, move back to the idle state. Vise versa as well. + ROSActionQTransition * idleDockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleDockTransition->setTargetState(docking_); + idle_->addTransition(idleDockTransition); + + ROSActionQTransition * idleUndockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleUndockTransition->setTargetState(undocking_); + idle_->addTransition(idleUndockTransition); + + ROSActionQTransition * dockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + dockingTransition->setTargetState(idle_); + docking_->addTransition(dockingTransition); + + ROSActionQTransition * undockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + undockingTransition->setTargetState(idle_); + undocking_->addTransition(undockingTransition); + + client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + + state_machine_.addState(pre_initial_); + state_machine_.addState(idle_); + state_machine_.addState(docking_); + state_machine_.addState(undocking_); + state_machine_.addState(canceled_docking_); + state_machine_.addState(canceled_undocking_); + + state_machine_.setInitialState(pre_initial_); + + // Delay starting initial thread until state machine has started or a race occurs + QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread())); + state_machine_.start(); + + // Create the layout for the panel info_layout_->addWidget(docking_goal_status_indicator_); info_layout_->addWidget(docking_result_indicator_); feedback_layout_->addWidget(docking_feedback_indicator_); - dock_id_layout_->addWidget(new QLabel("Dock id")); + + QLabel * nav_stage_label = new QLabel("Nav. to staging pose"); + QLabel * dock_id_label = new QLabel("Dock id"); + QLabel * dock_type_label = new QLabel("Dock type"); + + nav_stage_label->setFixedWidth(150); + dock_id_label->setFixedWidth(150); + dock_type_label->setFixedWidth(170); + + nav_stage_layout_->addWidget(nav_stage_label); + nav_stage_layout_->addWidget(nav_to_staging_checkbox_); + dock_id_layout_->addWidget(dock_id_label); dock_id_layout_->addWidget(use_dock_id_checkbox_); dock_id_layout_->addWidget(dock_id_); - dock_type_layout_->addWidget(new QLabel("Dock type")); + dock_type_layout_->addWidget(dock_type_label); dock_type_layout_->addWidget(dock_type_); dock_pose_layout_->addWidget(new QLabel("Dock pose {X")); dock_pose_layout_->addWidget(dock_pose_x_); @@ -91,28 +211,53 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_->addWidget(new QLabel("θ")); dock_pose_layout_->addWidget(dock_pose_yaw_); dock_pose_layout_->addWidget(new QLabel("}")); - nav_stage_layout_->addWidget(nav_to_staging_checkbox_); - nav_stage_layout_->addWidget(new QLabel("Navigate to staging pose")); + + QGroupBox * group_box = new QGroupBox(); + QVBoxLayout * group_box_layout = new QVBoxLayout; + group_box_layout->addLayout(nav_stage_layout_); + group_box_layout->addLayout(dock_id_layout_); + group_box_layout->addLayout(dock_type_layout_); + group_box_layout->addLayout(dock_pose_layout_); + group_box->setLayout(group_box_layout); main_layout_->setContentsMargins(10, 10, 10, 10); main_layout_->addLayout(info_layout_); main_layout_->addLayout(feedback_layout_); - main_layout_->addLayout(nav_stage_layout_); - main_layout_->addLayout(dock_id_layout_); - main_layout_->addLayout(dock_type_layout_); - main_layout_->addLayout(dock_pose_layout_); + main_layout_->addWidget(group_box); main_layout_->addWidget(docking_button_); main_layout_->addWidget(undocking_button_); setLayout(main_layout_); - timer_.start(200, this); + action_timer_.start(200, this); dock_client_ = rclcpp_action::create_client(client_node_, "dock_robot"); undock_client_ = rclcpp_action::create_client(client_node_, "undock_robot"); + initial_thread_ = new InitialDockThread(dock_client_, undock_client_); + connect(initial_thread_, &InitialDockThread::finished, initial_thread_, &QObject::deleteLater); + + QSignalTransition * activeDockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::dockingActive); + activeDockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeDockSignal); + + QSignalTransition * activeUndockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::undockingActive); + activeUndockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeUndockSignal); + + QObject::connect( + initial_thread_, &InitialDockThread::dockingActive, + [this] { + // Load the plugins if not already loaded + if (!plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + plugins_loaded_ = true; + } + }); // Conect buttons with functions - QObject::connect(docking_button_, SIGNAL(clicked()), this, SLOT(onDockingButtonPressed())); - QObject::connect(undocking_button_, SIGNAL(clicked()), this, SLOT(onUndockingButtonPressed())); QObject::connect( use_dock_id_checkbox_, &QCheckBox::stateChanged, this, &DockingPanel::dockIdCheckbox); } @@ -135,18 +280,6 @@ void DockingPanel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const Dock::Impl::FeedbackMessage::SharedPtr msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); - docking_button_->setText("Cancel docking"); - undocking_button_->setEnabled(false); - docking_in_progress_ = true; - }); - - undocking_feedback_sub_ = node->create_subscription( - "undock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), - [this](const Undock::Impl::FeedbackMessage::SharedPtr /*msg*/) { - docking_button_->setEnabled(false); - undocking_button_->setText("Cancel undocking"); - undocking_in_progress_ = true; }); // Create action goal status subscribers @@ -156,11 +289,6 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setText("Dock robot"); - undocking_button_->setEnabled(true); - docking_in_progress_ = false; - } // Reset values when action is completed if (msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { docking_feedback_indicator_->setText(getDockFeedbackLabel()); @@ -173,37 +301,30 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setEnabled(true); - undocking_button_->setText("Undock robot"); - undocking_in_progress_ = false; - } }); } +void DockingPanel::startThread() +{ + // start initial thread now that state machine is started + initial_thread_->start(); +} + DockingPanel::~DockingPanel() { } -void DockingPanel::onDockingButtonPressed() +void DockingPanel::load(const rviz_common::Config & config) { - if (!docking_in_progress_) { - startDocking(); - } else { - cancelDocking(); - } + Panel::load(config); } -void DockingPanel::onUndockingButtonPressed() +void DockingPanel::save(rviz_common::Config config) const { - if (!undocking_in_progress_) { - startUndocking(); - } else { - cancelUndocking(); - } + Panel::save(config); } -void DockingPanel::startDocking() +void DockingPanel::onDockingButtonPressed() { auto is_action_server_ready = dock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -286,10 +407,10 @@ void DockingPanel::startDocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } -void DockingPanel::startUndocking() +void DockingPanel::onUndockingButtonPressed() { auto is_action_server_ready = undock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -344,7 +465,7 @@ void DockingPanel::startUndocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } void DockingPanel::dockIdCheckbox() @@ -364,7 +485,7 @@ void DockingPanel::dockIdCheckbox() } } -void DockingPanel::cancelDocking() +void DockingPanel::onCancelDocking() { if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); @@ -377,9 +498,11 @@ void DockingPanel::cancelDocking() dock_goal_handle_.reset(); } } + + action_timer_.stop(); } -void DockingPanel::cancelUndocking() +void DockingPanel::onCancelUndocking() { if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); @@ -392,35 +515,53 @@ void DockingPanel::cancelUndocking() undock_goal_handle_.reset(); } } + + action_timer_.stop(); } void DockingPanel::timerEvent(QTimerEvent * event) { - if (event->timerId() == timer_.timerId()) { - if (!plugins_loaded_) { - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); - plugins_loaded_ = true; - docking_button_->setEnabled(true); - undocking_button_->setEnabled(true); - use_dock_id_checkbox_->setEnabled(true); - use_dock_id_checkbox_->setChecked(true); - nav_to_staging_checkbox_->setEnabled(true); - nav_to_staging_checkbox_->setChecked(true); - dock_id_->setEnabled(true); - } + if (event->timerId() == action_timer_.timerId()) { + // Check the status of the action servers + if (state_machine_.configuration().contains(docking_)) { + if (!dock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } - // Restart the timer if the one of the server fails - if (server_failed_ && !tried_once_) { - RCLCPP_INFO(client_node_->get_logger(), "Retrying to connect to the failed server."); - server_failed_ = false; - plugins_loaded_ = false; - tried_once_ = true; - timer_.start(200, this); - return; - } + rclcpp::spin_some(client_node_); + auto status = dock_goal_handle_->get_status(); - timer_.stop(); + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } else if (state_machine_.configuration().contains(undocking_)) { + if (!undock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } + + rclcpp::spin_some(client_node_); + auto status = undock_goal_handle_->get_status(); + + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } } } diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 6c7fcbbad03..96cf7784bf1 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -38,9 +38,7 @@ void pluginLoader( RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO( - node->get_logger(), - "%s service not available", server_name.c_str()); + RCLCPP_INFO(node->get_logger(), "%s service not available", server_name.c_str()); server_unavailable = true; server_failed = true; break; From c7ef3be69339bfa13ab76f74ecce5e7b70ae9b3f Mon Sep 17 00:00:00 2001 From: Daniil Khaninaev Date: Wed, 16 Oct 2024 23:39:12 +0300 Subject: [PATCH 13/29] Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev --- .../nav2_rotation_shim_controller.hpp | 9 ++- .../src/nav2_rotation_shim_controller.cpp | 18 +++++- .../test/test_shim_controller.cpp | 60 ++++++++++++++++++- 3 files changed, 84 insertions(+), 3 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3807eeb952..abaf02c9393 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -148,6 +148,13 @@ class RotationShimController : public nav2_core::Controller const double & angular_distance_to_heading, const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Checks if the goal has changed based on the given path. + * @param path The path to compare with the current goal. + * @return True if the goal has changed, false otherwise. + */ + bool isGoalChanged(const nav_msgs::msg::Path & path); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -171,7 +178,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_, in_rotation_; + bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5b2bf306df0..bb77879c7f7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -71,6 +71,8 @@ void RotationShimController::configure( node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); @@ -86,6 +88,7 @@ void RotationShimController::configure( control_duration_ = 1.0 / control_frequency; node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -340,9 +343,20 @@ void RotationShimController::isCollisionFree( } } +bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) +{ + // Return true if rotating or if the current path is empty + if (in_rotation_ || current_path_.poses.empty()) { + return true; + } + + // Check if the last pose of the current and new paths differ + return current_path_.poses.back().pose != path.poses.back().pose; +} + void RotationShimController::setPlan(const nav_msgs::msg::Path & path) { - path_updated_ = true; + path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); } @@ -377,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".rotate_to_heading_once") { + rotate_to_heading_once_ = parameter.as_bool(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1160a5a98af..1d63a77b47e 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return getSampledPathPt(); } + bool isGoalChangedWrapper(const nav_msgs::msg::Path & path) + { + return isGoalChanged(path); + } + geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt) { return transformPoseToBaseFrame(pt); @@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } +TEST(RotationShimControllerTest, isGoalChangedTest) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "PathFollower.rotate_to_heading_once", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(2); + path.poses.back().pose.position.x = 2.0; + path.poses.back().pose.position.y = 2.0; + + // Test: Current path is empty, should return true + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); + + // Test: Last pose of the current path is the same, should return false + controller->setPlan(path); + EXPECT_EQ(controller->isGoalChangedWrapper(path), false); + + // Test: Last pose of the current path differs, should return true + path.poses.back().pose.position.x = 3.0; + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), rclcpp::Parameter("test.primary_controller", std::string("HI")), - rclcpp::Parameter("test.rotate_to_goal_heading", true)}); + rclcpp::Parameter("test.rotate_to_goal_heading", true), + rclcpp::Parameter("test.rotate_to_heading_once", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); } From 44a69ccde93514ed1a962b7f18cb101ec671d93a Mon Sep 17 00:00:00 2001 From: Saitama Date: Thu, 17 Oct 2024 18:16:50 +0200 Subject: [PATCH 14/29] [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bb77879c7f7..f5fcf4c5b27 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -269,6 +269,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() } auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; goal.header.stamp = clock_->now(); return goal; } From 0db5f0708193fd3319c24b4f0781e9d0b863d67e Mon Sep 17 00:00:00 2001 From: Adi Vardi <57910756+adivardi@users.noreply.github.com> Date: Mon, 4 Nov 2024 19:09:55 +0100 Subject: [PATCH 15/29] [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi * Use single quotes Signed-off-by: Adi Vardi --------- Signed-off-by: Adi Vardi --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 +- nav2_loopback_sim/README.md | 10 ++++-- .../nav2_loopback_sim/loopback_simulator.py | 13 ++++++++ nav2_smac_planner/README.md | 32 +++++++++---------- 4 files changed, 37 insertions(+), 21 deletions(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 6321305dd79..3ba1c97f08a 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -225,8 +225,7 @@ void ObstacleLayer::onInitialize() source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; - custom_qos_profile.depth = 50; + const auto custom_qos_profile = rclcpp::SensorDataQoS().keep_last(50); // create a callback for the topic if (data_type == "LaserScan") { diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 73399040902..6c8c599aa94 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -8,7 +8,7 @@ This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org) It is drop-in replacable with AMR simulators and global localization by providing: - Map -> Odom transform -- Odom -> Base Link transform, `nav_msgs/Odometry` odometry +- Odom -> Base Link transform, `nav_msgs/Odometry` odometry - Accepts the standard `/initialpose` topic for transporting the robot to another location Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles. @@ -33,12 +33,15 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na ### Parameters -- `update_duration`: the duration between updates (default 0.01 -- 100hz) +- `update_duration`: The duration between updates (default 0.01 -- 100hz) - `base_frame_id`: The base frame to use (default `base_link`) - `odom_frame_id`: The odom frame to use (default `odom`) - `map_frame_id`: The map frame to use (default `map`) - `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) - `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`. +- `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) +- `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) +- `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) ### Topics @@ -47,6 +50,7 @@ This node subscribes to: - `cmd_vel`: Nav2's output twist to get the commanded velocity This node publishes: +- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True` - `odom`: To publish odometry from twist - `tf`: To publish map->odom and odom->base_link transforms -- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy +- `scan`: To publish a range laser scan sensor based on the static map diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 7902affb13b..71560c5aa80 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -24,6 +24,7 @@ from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations @@ -84,6 +85,9 @@ def __init__(self): self.publish_map_odom_tf = self.get_parameter( 'publish_map_odom_tf').get_parameter_value().bool_value + self.declare_parameter('publish_clock', True) + self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -112,6 +116,10 @@ def __init__(self): depth=10) self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) + if self.publish_clock: + self.clock_timer = self.create_timer(0.1, self.clockTimerCallback) + self.clock_pub = self.create_publisher(Clock, '/clock', 10) + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) self.map_client = self.create_client(GetMap, '/map_server/map') @@ -139,6 +147,11 @@ def setupTimerCallback(self): if self.mat_base_to_laser is None: self.getBaseToLaserTf() + def clockTimerCallback(self): + msg = Clock() + msg.clock = self.get_clock().now().to_msg() + self.clock_pub.publish(msg) + def cmdVelCallback(self, msg): self.debug('Received cmd_vel') if self.initial_pose is None: diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index ebb25bb52be..49ab250123f 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -43,7 +43,7 @@ The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots - Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. -## Features +## Features We further improve on Hybrid-A\* in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). @@ -90,7 +90,7 @@ We provide 3 nodes by default currently. The 2D node template (`Node2D`) which d In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. -We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. +We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. ## Parameters @@ -111,13 +111,13 @@ planner_server: max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp + motion_model_for_search: "DUBIN" # For Hybrid Dubin, Reeds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius - analytic_expansion_max_cost: true # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) - analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + analytic_expansion_max_cost: 200 # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) + analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required). If expansion is within 2*pi*min_r of the goal, then it will override the max cost if ``false``. minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 @@ -126,10 +126,10 @@ planner_server: retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 @@ -155,19 +155,19 @@ sudo apt-get install ros--nav2-smac-planner ### Potential Fields -Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. -Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. +Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. -So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. +So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. ### Hybrid-A* and State Lattice Turning Radius' A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. -I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. +I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities. @@ -177,11 +177,11 @@ We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costm I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. -Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. +Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. ### Penalty Function Tuning -The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. +The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. **However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements. @@ -197,13 +197,13 @@ Note that change penalty must be greater than 0.0. The non-straight, reverse, an Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. -In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. +In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! -As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. ![](media/A.png) ![](media/B.png) From a64dde1fb147c23e4131606ace3e862190992f1f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 4 Nov 2024 13:59:38 -0500 Subject: [PATCH 16/29] Remove nav2_loopback_sim dependency on transforms3d. (#4738) The package never uses it, so don't declare it. Signed-off-by: Chris Lalancette --- nav2_loopback_sim/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index a2a253def98..9376cfb50c7 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -12,7 +12,6 @@ nav_msgs tf_transformations tf2_ros - python3-transforms3d ament_copyright ament_flake8 From be73c241e8c7e27cbbf929b40d98ebf67303b873 Mon Sep 17 00:00:00 2001 From: Ryan <25047695+Ryanf55@users.noreply.github.com> Date: Wed, 6 Nov 2024 08:48:55 -0700 Subject: [PATCH 17/29] Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 005dd205d80..927e5806122 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -150,8 +150,7 @@ class Costmap2D /** * @brief Get the cost of a cell in the costmap - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell + * @param index The cell index * @return The cost of the cell */ unsigned char getCost(unsigned int index) const; From 6b2e244bf501e3a97d5dbc36eab09ed4ed949926 Mon Sep 17 00:00:00 2001 From: Ryan <25047695+Ryanf55@users.noreply.github.com> Date: Wed, 6 Nov 2024 08:49:57 -0700 Subject: [PATCH 18/29] Fix missing dependency on nav2_costmap_2d (#4742) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- nav2_controller/CMakeLists.txt | 1 + nav2_controller/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8a96b0bad17..76725b8d47f 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_2d_msgs REQUIRED) diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index b83ae3a214c..7eb51ca459c 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -14,6 +14,7 @@ geometry_msgs lifecycle_msgs nav2_core + nav2_costmap_2d nav2_msgs nav2_util nav_2d_msgs From 4b413204e87fa67db9a7754da93b29b946b25e06 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 7 Nov 2024 16:51:45 +0100 Subject: [PATCH 19/29] Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski * linting Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_smac_planner/src/collision_checker.cpp | 24 ++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 385f484615d..5afb919662f 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -54,6 +54,16 @@ void GridCollisionChecker::setFootprint( const double & possible_collision_cost) { possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + footprint_is_radius_ = radius; // Use radius, no caching required @@ -114,18 +124,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5f), static_cast(y + 0.5f))); - if (footprint_cost_ < possible_collision_cost_) { - if (possible_collision_cost_ > 0.0f) { - return false; - } else { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } + if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; } // If its inscribed, in collision, or unknown in the middle, From fbd1d3e7964dc220c0861d5e8bdcfcc7b9ec2812 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 7 Nov 2024 17:30:11 +0000 Subject: [PATCH 20/29] [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_map_server/src/map_io.cpp | 150 +++++++++++++++++++-------------- 1 file changed, 87 insertions(+), 63 deletions(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 6fb82c1af2a..b97342cf820 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -133,8 +133,10 @@ std::string expand_user_home_dir_if_needed( return yaml_filename; } if (home_variable_value.empty()) { - std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" - << "[INFO] [map_io] User home dir will be not expanded \n"; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"); return yaml_filename; } const std::string prefix{home_variable_value}; @@ -182,15 +184,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename) load_parameters.negate = yaml_get_value(doc, "negate"); } - std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl; - std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl; - std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl; - std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh << - std::endl; - std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl; - std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "occupied_thresh: " << load_parameters.occupied_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "mode: " << map_mode_to_string(load_parameters.mode)); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate); return load_parameters; } @@ -202,8 +207,9 @@ void loadMapFromFile( Magick::InitializeMagick(nullptr); nav_msgs::msg::OccupancyGrid msg; - std::cout << "[INFO] [map_io]: Loading image_file: " << - load_parameters.image_file_name << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), "Loading image_file: " << + load_parameters.image_file_name); Magick::Image img(load_parameters.image_file_name); // Copy the image data into the map structure @@ -291,9 +297,11 @@ void loadMapFromFile( msg.header.frame_id = "map"; msg.header.stamp = clock.now(); - std::cout << - "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << - " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Read map " << load_parameters.image_file_name + << ": " << msg.info.width << " X " << msg.info.height << " map @ " + << msg.info.resolution << " m/cell"); map = msg; } @@ -303,30 +311,32 @@ LOAD_MAP_STATUS loadMapFromYaml( nav_msgs::msg::OccupancyGrid & map) { if (yaml_file.empty()) { - std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!"); return MAP_DOES_NOT_EXIST; } - std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file); LoadParameters load_parameters; try { load_parameters = loadMapYaml(yaml_file); } catch (YAML::Exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" << - e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed processing YAML file " << yaml_file << " at position (" << + e.mark.line << ":" << e.mark.column << ") for reason: " << e.what()); return INVALID_MAP_METADATA; } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file << + " for reason: " << e.what()); return INVALID_MAP_METADATA; } try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed to load image file " << load_parameters.image_file_name << + " for reason: " << e.what()); return INVALID_MAP_DATA; } @@ -351,40 +361,46 @@ void checkSaveParameters(SaveParameters & save_parameters) rclcpp::Clock clock(RCL_SYSTEM_TIME); save_parameters.map_file_name = "map_" + std::to_string(static_cast(clock.now().seconds())); - std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " << - save_parameters.map_file_name << " file" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " << + save_parameters.map_file_name << " file"); } // Checking thresholds if (save_parameters.occupied_thresh == 0.0) { save_parameters.occupied_thresh = 0.65; - std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " << - save_parameters.occupied_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Occupied threshold unspecified. Setting it to default value: " << + save_parameters.occupied_thresh); } if (save_parameters.free_thresh == 0.0) { save_parameters.free_thresh = 0.25; - std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " << - save_parameters.free_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " << + save_parameters.free_thresh); } if (1.0 < save_parameters.occupied_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.free_thresh < 0.0) { - std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.occupied_thresh <= save_parameters.free_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" << - std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Threshold_free must be smaller than threshold_occupied"); throw std::runtime_error("Incorrect thresholds"); } // Checking image format if (save_parameters.image_format == "") { save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm"; - std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " << - save_parameters.image_format << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " << + save_parameters.image_format); } std::transform( @@ -407,24 +423,25 @@ void checkSaveParameters(SaveParameters & save_parameters) ss << "'" << format_name << "'"; first = false; } - std::cout << - "[WARN] [map_io]: Requested image format '" << save_parameters.image_format << - "' is not one of the recommended formats: " << ss.str() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format << + "' is not one of the recommended formats: " << ss.str()); } const std::string FALLBACK_FORMAT = "png"; try { Magick::CoderInfo info(save_parameters.image_format); if (!info.isWritable()) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << - "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format << + "' is not writable. Using '" << FALLBACK_FORMAT << "' instead"); save_parameters.image_format = FALLBACK_FORMAT; } } catch (Magick::ErrorOption & e) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" << - FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" << + FALLBACK_FORMAT << "' instead:" << std::endl << e.what()); save_parameters.image_format = FALLBACK_FORMAT; } @@ -435,10 +452,10 @@ void checkSaveParameters(SaveParameters & save_parameters) save_parameters.image_format == "jpg" || save_parameters.image_format == "jpeg")) { - std::cout << - "[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" << - save_parameters.image_format << - "' does not support it. Consider switching image format to 'png'." << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" << + save_parameters.image_format << + "' does not support it. Consider switching image format to 'png'."); } } @@ -452,9 +469,10 @@ void tryWriteMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters) { - std::cout << - "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " << - map.info.resolution << " m/pix" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " << + map.info.resolution << " m/pix"); std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format; { @@ -510,14 +528,18 @@ void tryWriteMapToFile( pixel = Magick::Color(q, q, q); break; default: - std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Map mode should be Trinary, Scale or Raw"); throw std::runtime_error("Invalid map mode"); } image.pixelColor(x, y, pixel); } } - std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "Writing map occupancy data to " << mapdatafile); image.write(mapdatafile); } @@ -546,15 +568,15 @@ void tryWriteMapToFile( e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; if (!e.good()) { - std::cout << - "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() << - ". The map metadata may be invalid." << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() << + ". The map metadata may be invalid."); } - std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile); std::ofstream(mapmetadatafile) << e.c_str(); } - std::cout << "[INFO] [map_io]: Map saved" << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved"); } bool saveMapToFile( @@ -570,7 +592,9 @@ bool saveMapToFile( tryWriteMapToFile(map, save_parameters_loc); } catch (std::exception & e) { - std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), + "Failed to write map for reason: " << e.what()); return false; } return true; From f426bdaadaa1a124bea0da5f32961cef6d679ae2 Mon Sep 17 00:00:00 2001 From: Vince Reda <60265874+redvinaa@users.noreply.github.com> Date: Mon, 11 Nov 2024 22:22:11 +0100 Subject: [PATCH 21/29] Pass IDLE to on_tick, use that for initialize condition (#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa * Fix battery sub creation bug Signed-off-by: redvinaa --------- Signed-off-by: redvinaa --- .../nav2_behavior_tree/bt_action_node.hpp | 6 +-- .../plugins/action/assisted_teleop_action.hpp | 1 - .../plugins/action/back_up_action.hpp | 3 -- .../action/remove_passed_goals_action.hpp | 1 - .../plugins/action/spin_action.hpp | 1 - .../plugins/action/wait_action.hpp | 3 -- .../condition/distance_traveled_condition.hpp | 1 - .../condition/goal_reached_condition.hpp | 1 - .../condition/is_battery_low_condition.hpp | 1 - .../condition/is_path_valid_condition.hpp | 1 - .../condition/time_expired_condition.hpp | 1 - .../transform_available_condition.hpp | 1 - .../plugins/decorator/rate_controller.hpp | 1 - .../plugins/action/assisted_teleop_action.cpp | 6 +-- .../plugins/action/back_up_action.cpp | 6 +-- .../action/remove_passed_goals_action.cpp | 8 +--- .../plugins/action/spin_action.cpp | 9 +---- .../plugins/action/wait_action.cpp | 6 +-- .../condition/distance_traveled_condition.cpp | 6 +-- .../condition/goal_reached_condition.cpp | 7 +--- .../condition/is_battery_low_condition.cpp | 39 +++++++++++-------- .../condition/is_path_valid_condition.cpp | 6 +-- .../condition/time_expired_condition.cpp | 6 +-- .../transform_available_condition.cpp | 6 +-- .../plugins/decorator/rate_controller.cpp | 6 +-- 25 files changed, 47 insertions(+), 86 deletions(-) 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 4e723f1ac5f..505a7895c51 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 @@ -191,15 +191,15 @@ class BtActionNode : public BT::ActionNodeBase { // first step to be done only at the beginning of the Action if (!BT::isStatusActive(status())) { - // setting the status to RUNNING to notify the BT Loggers (if any) - setStatus(BT::NodeStatus::RUNNING); - // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; // user defined callback, may modify "should_send_goal_". on_tick(); + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + if (!should_send_goal_) { return BT::NodeStatus::FAILURE; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index 97511608d50..6466948b313 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode "error_code_id", "The back up behavior server error code") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 33552a24f06..fcfade6d17b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase double transform_tolerance_; std::shared_ptr tf_; std::string robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4f9b300e445..cad5e8ce4a8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -86,7 +86,6 @@ class SpinAction : public BtActionNode private: bool is_recovery_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index fdc320c16b0..1749a032cb1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -61,9 +61,6 @@ class WaitAction : public BtActionNode BT::InputPort("wait_duration", 1.0, "Wait time") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 77a80728ddf..dd200f318b3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -80,7 +80,6 @@ class DistanceTraveledCondition : public BT::ConditionNode double distance_; double transform_tolerance_; std::string global_frame_, robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 44e582c5f53..ff65da09a18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; std::shared_ptr tf_; - bool initialized_; double goal_reached_tol_; double transform_tolerance_; std::string robot_base_frame_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 59a023ff3c2..40f0ac5804b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -86,7 +86,6 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 5a9f255a9b7..a22942f9a56 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -73,7 +73,6 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a responce from the // is path valid service std::chrono::milliseconds server_timeout_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 26a3431b5db..2e34689c56a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -68,7 +68,6 @@ class TimeExpiredCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; rclcpp::Time start_; double period_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 511c381321b..732e862ff4c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -80,7 +80,6 @@ class TransformAvailableCondition : public BT::ConditionNode std::string child_frame_; std::string parent_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index c390357b342..ccc346eb16d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode std::chrono::time_point start_; double period_; bool first_time_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index e876d2ce40b..7354b05f370 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) {} void AssistedTeleopAction::initialize() @@ -36,12 +35,11 @@ void AssistedTeleopAction::initialize() // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void AssistedTeleopAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index e17580fe71c..3df77f98d7c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,8 +24,7 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -44,12 +43,11 @@ void nav2_behavior_tree::BackUpAction::initialize() goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void BackUpAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 665bf81bc3f..723e704e437 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -28,8 +28,7 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5), - initialized_(false) + viapoint_achieved_radius_(0.5) {} void RemovePassedGoals::initialize() @@ -42,14 +41,11 @@ void RemovePassedGoals::initialize() robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); - initialized_ = true; } inline BT::NodeStatus RemovePassedGoals::tick() { - setStatus(BT::NodeStatus::RUNNING); - - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d10bb83f32b..cb3459006fe 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,10 +23,7 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) {} void SpinAction::initialize() { @@ -37,13 +34,11 @@ void SpinAction::initialize() goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); - - initialized_ = true; } void SpinAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b607d026e59..03a18c9f947 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,8 +25,7 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -42,12 +41,11 @@ void WaitAction::initialize() } goal_.time = rclcpp::Duration::from_seconds(duration); - initialized_ = true; } void WaitAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 991e0ab7cf2..2ebf9c486ea 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -29,8 +29,7 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1), - initialized_(false) + transform_tolerance_(0.1) { } @@ -46,12 +45,11 @@ void DistanceTraveledCondition::initialize() node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); - initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e93ba5cc360..4b5e20a733e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -27,8 +27,7 @@ namespace nav2_behavior_tree GoalReachedCondition::GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { auto node = config().blackboard->get("node"); @@ -52,13 +51,11 @@ void GoalReachedCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - - initialized_ = true; } BT::NodeStatus GoalReachedCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index a0e3761f28c..eb0a8be4301 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,35 +27,40 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false), - initialized_(false) + is_battery_low_(false) { } void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); - getInput("battery_topic", battery_topic_); + std::string battery_topic_new; + getInput("battery_topic", battery_topic_new); getInput("is_voltage", is_voltage_); - node_ = config().blackboard->get("node"); - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - battery_sub_ = node_->create_subscription( - battery_topic_, - rclcpp::SystemDefaultsQoS(), - std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), - sub_option); - initialized_ = true; + // Only create a new subscriber if the topic has changed or subscriber is empty + if (battery_topic_new != battery_topic_ || !battery_sub_) { + battery_topic_ = battery_topic_new; + + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), + sub_option); + } } BT::NodeStatus IsBatteryLowCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 7274743e1e9..cd76df97495 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,8 +23,7 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); - initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index f03c9711aa8..23fd614cb7c 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,8 +27,7 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0), - initialized_(false) + period_(1.0) { } @@ -37,12 +36,11 @@ void TimeExpiredCondition::initialize() getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); - initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 1afe0e14331..ab589048c82 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -31,8 +31,7 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false), - initialized_(false) + was_found_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); @@ -56,12 +55,11 @@ void TransformAvailableCondition::initialize() } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); - initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 9592d119c96..5263da09eb2 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,8 +24,7 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false), - initialized_(false) + first_time_(false) { } @@ -34,12 +33,11 @@ void RateController::initialize() double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; - initialized_ = true; } BT::NodeStatus RateController::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } From e52ea5d73bc3e081cecc2fb86d379e44b2e7b080 Mon Sep 17 00:00:00 2001 From: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Date: Mon, 18 Nov 2024 20:32:31 +0100 Subject: [PATCH 22/29] nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer --- nav2_costmap_2d/src/costmap_2d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 9cb7ce19816..9ddbcf5e9d6 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -219,6 +219,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) resolution_ = map.resolution_; origin_x_ = map.origin_x_; origin_y_ = map.origin_y_; + default_value_ = map.default_value_; // initialize our various maps initMaps(size_x_, size_y_); From f45d05b809064f27ecd9d141043a01fd8b41f063 Mon Sep 17 00:00:00 2001 From: aosmw <116058035+aosmw@users.noreply.github.com> Date: Fri, 22 Nov 2024 02:27:08 +1100 Subject: [PATCH 23/29] mppi parameters_handler: Improve verbose handling (#4704) (#4711) * mppi parameters_handler: Improve verbose handling (#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake * Address review comments. (#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake * mppi parameters_handler: Improve static/dynamic/not defined logging (#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake * mppi parameters_handler: populate SetParametersResult (#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake --------- Signed-off-by: Mike Wake --- .../tools/parameters_handler.hpp | 56 +++++--- .../src/critics/cost_critic.cpp | 5 +- .../src/parameters_handler.cpp | 22 ++- .../test/parameter_handler_test.cpp | 133 ++++++++++++++++-- 4 files changed, 182 insertions(+), 34 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp index fbb6760268f..43bcc0b540c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp @@ -36,12 +36,13 @@ enum class ParameterType { Dynamic, Static }; /** * @class mppi::ParametersHandler - * @brief Handles getting parameters and dynamic parmaeter changes + * @brief Handles getting parameters and dynamic parameter changes */ class ParametersHandler { public: - using get_param_func_t = void (const rclcpp::Parameter & param); + using get_param_func_t = void (const rclcpp::Parameter & param, + rcl_interfaces::msg::SetParametersResult & result); using post_callback_t = void (); using pre_callback_t = void (); @@ -100,9 +101,11 @@ class ParametersHandler * @brief Set a parameter to a dynamic parameter callback * @param setting Parameter * @param name Name of parameter + * @param param_type Type of parameter (dynamic or static) */ template - void setDynamicParamCallback(T & setting, const std::string & name); + void setParamCallback( + T & setting, const std::string & name, ParameterType param_type = ParameterType::Dynamic); /** * @brief Get mutex lock for changing parameters @@ -114,12 +117,20 @@ class ParametersHandler } /** - * @brief Set a parameter to a dynamic parameter callback + * @brief register a function to be called when setting a parameter + * + * The callback funciton is expected to behave as follows. + * Successful parameter changes should not interfere with + * the result parameter. + * Unsuccessful parameter changes should set the result.successful = false + * The result.reason should have "\n" appended if not empty before + * appending the reason that setting THIS parameter has failed. + * * @param name Name of parameter * @param callback Parameter callback */ template - void addDynamicParamCallback(const std::string & name, T && callback); + void addParamCallback(const std::string & name, T && callback); protected: /** @@ -160,8 +171,7 @@ class ParametersHandler bool verbose_{false}; - std::unordered_map> - get_param_callbacks_; + std::unordered_map> get_param_callbacks_; std::vector> pre_callbacks_; std::vector> post_callbacks_; @@ -179,7 +189,7 @@ inline auto ParametersHandler::getParamGetter(const std::string & ns) } template -void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback) +void ParametersHandler::addParamCallback(const std::string & name, T && callback) { get_param_callbacks_[name] = callback; } @@ -208,10 +218,7 @@ void ParametersHandler::getParam( node, name, rclcpp::ParameterValue(default_value)); setParam(setting, name, node); - - if (param_type == ParameterType::Dynamic) { - setDynamicParamCallback(setting, name); - } + setParamCallback(setting, name, param_type); } template @@ -224,24 +231,37 @@ void ParametersHandler::setParam( } template -void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name) +void ParametersHandler::setParamCallback( + T & setting, const std::string & name, ParameterType param_type) { if (get_param_callbacks_.find(name) != get_param_callbacks_.end()) { return; } - auto callback = [this, &setting, name](const rclcpp::Parameter & param) { + auto dynamic_callback = + [this, &setting, name]( + const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & /*result*/) { setting = as(param); - if (verbose_) { RCLCPP_INFO(logger_, "Dynamic parameter changed: %s", std::to_string(param).c_str()); } }; - addDynamicParamCallback(name, callback); + auto static_callback = + [this, &setting, name]( + const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & result) { + std::string reason = "Rejected change to static parameter: " + std::to_string(param); + result.successful = false; + if (!result.reason.empty()) { + result.reason += "\n"; + } + result.reason += reason; + }; - if (verbose_) { - RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str()); + if (param_type == ParameterType::Dynamic) { + addParamCallback(name, dynamic_callback); + } else { + addParamCallback(name, static_callback); } } diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index f348d7780a1..e75c68f6c29 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -35,10 +35,11 @@ void CostCritic::initialize() weight_ /= 254.0f; // Normalize weight when parameter is changed dynamically as well - auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { weight_ = weight.as_double() / 254.0f; }; - parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); collision_checker_.setCostmap(costmap_); possible_collision_cost_ = findCircumscribedCost(costmap_ros_); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index c66d5725615..4dd20556303 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -38,13 +38,14 @@ ParametersHandler::~ParametersHandler() void ParametersHandler::start() { auto node = node_.lock(); + + auto get_param = getParamGetter(node_name_); + get_param(verbose_, "verbose", false); + on_set_param_handler_ = node->add_on_set_parameters_callback( std::bind( &ParametersHandler::dynamicParamsCallback, this, std::placeholders::_1)); - - auto get_param = getParamGetter(node_name_); - get_param(verbose_, "verbose", false); } rcl_interfaces::msg::SetParametersResult @@ -52,6 +53,9 @@ ParametersHandler::dynamicParamsCallback( std::vector parameters) { rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = ""; + std::lock_guard lock(parameters_change_mutex_); for (auto & pre_cb : pre_callbacks_) { @@ -64,9 +68,13 @@ ParametersHandler::dynamicParamsCallback( if (auto callback = get_param_callbacks_.find(param_name); callback != get_param_callbacks_.end()) { - callback->second(param); + callback->second(param, result); } else { - RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str()); + result.successful = false; + if (!result.reason.empty()) { + result.reason += "\n"; + } + result.reason += "get_param_callback func for '" + param_name + "' not found.\n"; } } @@ -74,7 +82,9 @@ ParametersHandler::dynamicParamsCallback( post_cb(); } - result.successful = true; + if (!result.successful) { + RCLCPP_ERROR(logger_, result.reason.c_str()); + } return result; } diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index bcc3208e784..27ef61d90c7 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -93,7 +93,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) post_triggered = true; }; - auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) { + auto dynamicCb = [&](const rclcpp::Parameter & /*param*/, + rcl_interfaces::msg::SetParametersResult & /*result*/) { dynamic_triggered = true; }; @@ -104,8 +105,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) ParametersHandlerWrapper a; a.addPreCallback(preCb); a.addPostCallback(postCb); - a.addDynamicParamCallback("use_sim_time", dynamicCb); - a.setDynamicParamCallback(val, "blah_blah"); + a.addParamCallback("use_sim_time", dynamicCb); + a.setParamCallback(val, "blah_blah"); // Dynamic callback should not trigger, wrong parameter, but val should be updated a.dynamicParamsCallback(std::vector{random_param}); @@ -146,6 +147,52 @@ TEST(ParameterHandlerTest, GetSystemParamsTest) } TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) +{ + auto node = std::make_shared("my_node"); + + node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("static_int", rclcpp::ParameterValue(7)); + ParametersHandlerWrapper handler(node); + handler.start(); + + // Get parameters and check they have initial values + auto getParamer = handler.getParamGetter(""); + int p1 = 0, p2 = 0; + getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic); + getParamer(p2, "static_int", 0, ParameterType::Static); + EXPECT_EQ(p1, 7); + EXPECT_EQ(p2, 7); + + // Now change them both via dynamic parameters + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + std::shared_future result_future = + rec_param->set_parameters_atomically({ + rclcpp::Parameter("my_node.verbose", true), + rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10) + }); + + auto rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") + + "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + + // Now, only param1 should change, param 2 should be the same + EXPECT_EQ(p1, 10); + EXPECT_EQ(p2, 7); +} + +TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) { auto node = std::make_shared("my_node"); node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); @@ -167,15 +214,85 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) node->get_node_graph_interface(), node->get_node_services_interface()); - auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10)}); + std::shared_future result_future = + rec_param->set_parameters_atomically({ + // Don't set default param rclcpp::Parameter("my_node.verbose", false), + rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10) + }); - rclcpp::spin_until_future_complete( + auto rc = rclcpp::spin_until_future_complete( node->get_node_base_interface(), - results); + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") + + "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same EXPECT_EQ(p1, 10); EXPECT_EQ(p2, 7); } + +TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) +{ + auto node = std::make_shared("my_node"); + + node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("static_int", rclcpp::ParameterValue(7)); + ParametersHandlerWrapper handler(node); + handler.start(); + + // Set verbose true to get more information about bad parameter usage + auto getParamer = handler.getParamGetter(""); + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + std::shared_future + result_future = rec_param->set_parameters_atomically({ + rclcpp::Parameter("my_node.verbose", true), + }); + + auto rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, true); + EXPECT_TRUE(result.reason.empty()); + + // Try to access some parameters that have not been declared + int p1 = 0, p2 = 0; + EXPECT_THROW(getParamer(p1, "not_declared", 8, ParameterType::Dynamic), + rclcpp::exceptions::InvalidParameterValueException); + EXPECT_THROW(getParamer(p2, "not_declared2", 9, ParameterType::Static), + rclcpp::exceptions::InvalidParameterValueException); + + // Try to set some parameters that have not been declared via the service client + result_future = rec_param->set_parameters_atomically({ + rclcpp::Parameter("static_int", 10), + rclcpp::Parameter("not_declared", true), + rclcpp::Parameter("not_declared2", true), + }); + + rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + // The ParameterNotDeclaredException handler in rclcpp/parameter_service.cpp + // overrides any other reasons and does not provide details to the service client. + EXPECT_EQ(result.reason, std::string("One or more parameters were not declared before setting")); + + EXPECT_EQ(p1, 0); + EXPECT_EQ(p2, 0); +} From 90a6c8d80096469a22e1a56e5447d58fb8e66ef6 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 21 Nov 2024 20:20:02 +0100 Subject: [PATCH 24/29] Added collision detection for docking (#4752) * Added collision detection for docking Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alberto Tudela * Improve collision while undocking and test Signed-off-by: Alberto Tudela * Fix smoke testing Signed-off-by: Alberto Tudela * Rename dock_collision_threshold Signed-off-by: Alberto Tudela * Added docs and params Signed-off-by: Alberto Tudela * Minor changes in README Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- nav2_bringup/params/nav2_params.yaml | 7 + nav2_docking/README.md | 23 +- .../include/opennav_docking/controller.hpp | 71 ++- .../opennav_docking/docking_server.hpp | 3 +- .../opennav_docking/src/controller.cpp | 140 +++++- .../opennav_docking/src/docking_server.cpp | 17 +- .../opennav_docking/test/test_controller.cpp | 475 +++++++++++++++++- .../test/test_docking_server.py | 14 +- 8 files changed, 717 insertions(+), 33 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2027f4c0e1c..69fe42195eb 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -421,6 +421,13 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "/local_costmap/costmap_raw" + footprint_topic: "/local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 loopback_simulator: ros__parameters: diff --git a/nav2_docking/README.md b/nav2_docking/README.md index fba8ca11394..2a9ba502ae9 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -208,14 +208,21 @@ For debugging purposes, there are several publishers which can be used with RVIZ | dock_database | The filepath to the dock database to use for this environment | string | N/A | | docks | Instead of `dock_database`, the set of docks specified in the params file itself | vector | N/A | | navigator_bt_xml | BT XML to use for Navigator, if non-default | string | "" | -| controller.k_phi | TODO | double | 3.0 | -| controller.k_delta | TODO | double | 2.0 | -| controller.beta | TODO | double | 0.4 | -| controller.lambda | TODO | double | 2.0 | -| controller.v_linear_min | TODO | double | 0.1 | -| controller.v_linear_max | TODO | double | 0.25 | -| controller.v_angular_max | TODO | double | 0.75 | -| controller.slowdown_radius | TODO | double | 0.25 | +| controller.k_phi | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem | double | 3.0 | +| controller.k_delta | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem | double | 2.0 | +| controller.beta | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases | double | 0.4 | +| controller.lambda | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves | double | 2.0 | +| controller.v_linear_min | Minimum linear velocity (m/s) | double | 0.1 | +| controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25 | +| controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75 | +| controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25 | +| controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true | +| controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw" | +| controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint" | +| controller.transform_tolerance | Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. | double | 0.1 | +| controller.projection_time | Time to look ahead for collisions (s). | double | 5.0 | +| controller.simulation_time_step | Time step for projections (s). | double | 0.1 | +| controller.dock_collision_threshold | Distance (m) from the dock pose to ignore collisions. | double | 0.3 | Note: `dock_plugins` and either `docks` or `dock_database` are required. diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index afae5965d22..142b4beaf7d 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,11 +17,16 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include +#include #include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_graceful_controller/smooth_control_law.hpp" +#include "nav_msgs/msg/path.hpp" #include "nav2_util/lifecycle_node.hpp" namespace opennav_docking @@ -34,36 +40,91 @@ class Controller public: /** * @brief Create a controller instance. Configure ROS 2 parameters. + * + * @param node Lifecycle node + * @param tf tf2_ros TF buffer + * @param fixed_frame Fixed frame + * @param base_frame Robot base frame */ - explicit Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); + Controller( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame); + + /** + * @brief A destructor for opennav_docking::Controller + */ + ~Controller(); /** * @brief Compute a velocity command using control law. * @param pose Target pose, in robot centric coordinates. * @param cmd Command velocity. + * @param is_docking If true, robot is docking. If false, robot is undocking. * @param backward If true, robot will drive backwards to goal. * @returns True if command is valid, false otherwise. */ bool computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, bool backward = false); +protected: + /** + * @brief Check if a trajectory is collision free. + * + * @param target_pose Target pose, in robot centric coordinates. + * @param is_docking If true, robot is docking. If false, robot is undocking. + * @param backward If true, robot will drive backwards to goal. + * @return True if trajectory is collision free. + */ + bool isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false); + /** - * @brief Callback executed when a parameter change is detected + * @brief Callback executed when a parameter change is detected. + * * @param event ParameterEvent message */ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + /** + * @brief Configure the collision checker. + * + * @param node Lifecycle node + * @param costmap_topic Costmap topic + * @param footprint_topic Footprint topic + * @param transform_tolerance Transform tolerance + */ + void configureCollisionChecker( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string costmap_topic, std::string footprint_topic, double transform_tolerance); + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::mutex dynamic_params_lock_; -protected: - std::unique_ptr control_law_; + rclcpp::Logger logger_{rclcpp::get_logger("Controller")}; + rclcpp::Clock::SharedPtr clock_; + // Smooth control law + std::unique_ptr control_law_; double k_phi_, k_delta_, beta_, lambda_; double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + + // The trajectory of the robot while dock / undock for visualization / debug purposes + rclcpp::Publisher::SharedPtr trajectory_pub_; + + // Used for collision checking + bool use_collision_detection_; + double projection_time_; + double simulation_time_step_; + double dock_collision_threshold_; + double transform_tolerance_; + std::shared_ptr tf2_buffer_; + std::unique_ptr costmap_sub_; + std::unique_ptr footprint_sub_; + std::shared_ptr collision_checker_; + std::string fixed_frame_, base_frame_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 08c3cc1f7c2..9e03c5b154c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -112,12 +112,13 @@ class DockingServer : public nav2_util::LifecycleNode * @param pose The pose to command towards. * @param linear_tolerance Pose is reached when linear distance is within this tolerance. * @param angular_tolerance Pose is reached when angular distance is within this tolerance. + * @param is_docking If true, the robot is docking. If false, the robot is undocking. * @param backward If true, the robot will drive backwards. * @returns True if pose is reached. */ bool getCommandToPose( geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose, - double linear_tolerance, double angular_tolerance, bool backward); + double linear_tolerance, double angular_tolerance, bool is_docking, bool backward); /** * @brief Get the robot pose (aka base_frame pose) in another frame. diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 8613f2d27fc..c0e45ee4223 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,13 +17,22 @@ #include "rclcpp/rclcpp.hpp" #include "opennav_docking/controller.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "tf2/utils.h" namespace opennav_docking { -Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +Controller::Controller( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame) +: tf2_buffer_(tf), fixed_frame_(fixed_frame), base_frame_(base_frame) { + logger_ = node->get_logger(); + clock_ = node->get_clock(); + nav2_util::declare_parameter_if_not_declared( node, "controller.k_phi", rclcpp::ParameterValue(3.0)); nav2_util::declare_parameter_if_not_declared( @@ -39,6 +49,22 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) node, "controller.v_angular_max", rclcpp::ParameterValue(0.75)); nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.use_collision_detection", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", + rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue( + std::string("local_costmap/published_footprint"))); + nav2_util::declare_parameter_if_not_declared( + node, "controller.transform_tolerance", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(5.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); node->get_parameter("controller.k_phi", k_phi_); node->get_parameter("controller.k_delta", k_delta_); @@ -55,16 +81,120 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); + + node->get_parameter("controller.use_collision_detection", use_collision_detection_); + node->get_parameter("controller.projection_time", projection_time_); + node->get_parameter("controller.simulation_time_step", simulation_time_step_); + node->get_parameter("controller.transform_tolerance", transform_tolerance_); + + if (use_collision_detection_) { + std::string costmap_topic, footprint_topic; + node->get_parameter("controller.costmap_topic", costmap_topic); + node->get_parameter("controller.footprint_topic", footprint_topic); + node->get_parameter("controller.dock_collision_threshold", dock_collision_threshold_); + configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_); + } + + trajectory_pub_ = + node->create_publisher("docking_trajectory", 1); +} + +Controller::~Controller() +{ + control_law_.reset(); + trajectory_pub_.reset(); + collision_checker_.reset(); + costmap_sub_.reset(); + footprint_sub_.reset(); } bool Controller::computeVelocityCommand( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward) + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, + bool backward) { std::lock_guard lock(dynamic_params_lock_); cmd = control_law_->calculateRegularVelocity(pose, backward); + return isTrajectoryCollisionFree(pose, is_docking, backward); +} + +bool Controller::isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward) +{ + // Visualization of the trajectory + nav_msgs::msg::Path trajectory; + trajectory.header.frame_id = base_frame_; + trajectory.header.stamp = clock_->now(); + + // First pose + geometry_msgs::msg::PoseStamped next_pose; + next_pose.header.frame_id = base_frame_; + trajectory.poses.push_back(next_pose); + + // Get the transform from base_frame to fixed_frame + geometry_msgs::msg::TransformStamped base_to_fixed_transform; + try { + base_to_fixed_transform = tf2_buffer_->lookupTransform( + fixed_frame_, base_frame_, trajectory.header.stamp, + tf2::durationFromSec(transform_tolerance_)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not get transform from %s to %s: %s", + base_frame_.c_str(), fixed_frame_.c_str(), ex.what()); + return false; + } + + // Generate path + for (double t = 0; t < projection_time_; t += simulation_time_step_) { + // Apply velocities to calculate next pose + next_pose.pose = control_law_->calculateNextPose( + simulation_time_step_, target_pose, next_pose.pose, backward); + + // Add the pose to the trajectory for visualization + trajectory.poses.push_back(next_pose); + + // Transform pose from base_frame into fixed_frame + geometry_msgs::msg::PoseStamped local_pose = next_pose; + local_pose.header.stamp = trajectory.header.stamp; + tf2::doTransform(local_pose, local_pose, base_to_fixed_transform); + + // Determine the distance at which to check for collisions + // Skip the final segment of the trajectory for docking + // and the initial segment for undocking + // This avoids false positives when the robot is at the dock + double dock_collision_distance = is_docking ? + nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose) : + std::hypot(next_pose.pose.position.x, next_pose.pose.position.y); + + // If this distance is greater than the dock_collision_threshold, check for collisions + if (use_collision_detection_ && + dock_collision_distance > dock_collision_threshold_ && + !collision_checker_->isCollisionFree(nav_2d_utils::poseToPose2D(local_pose.pose))) + { + RCLCPP_WARN( + logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s", + local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z, + local_pose.header.frame_id.c_str()); + trajectory_pub_->publish(trajectory); + return false; + } + } + + trajectory_pub_->publish(trajectory); + return true; } +void Controller::configureCollisionChecker( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string costmap_topic, std::string footprint_topic, double transform_tolerance) +{ + costmap_sub_ = std::make_unique(node, costmap_topic); + footprint_sub_ = std::make_unique( + node, footprint_topic, *tf2_buffer_, base_frame_, transform_tolerance); + collision_checker_ = std::make_shared( + *costmap_sub_, *footprint_sub_, node->get_name()); +} + rcl_interfaces::msg::SetParametersResult Controller::dynamicParametersCallback(std::vector parameters) { @@ -92,6 +222,12 @@ Controller::dynamicParametersCallback(std::vector parameters) v_angular_max_ = parameter.as_double(); } else if (name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); + } else if (name == "controller.projection_time") { + projection_time_ = parameter.as_double(); + } else if (name == "controller.simulation_time_step") { + simulation_time_step_ = parameter.as_double(); + } else if (name == "controller.dock_collision_threshold") { + dock_collision_threshold_ = parameter.as_double(); } // Update the smooth control law with the new params diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index b350cb40262..d7af36c9ebc 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -86,7 +86,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) // Create composed utilities mutex_ = std::make_shared(); - controller_ = std::make_unique(node); + controller_ = std::make_unique(node, tf2_buffer_, fixed_frame_, base_frame_); navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { @@ -248,8 +248,7 @@ void DockingServer::dockRobot() // Send robot to its staging pose publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE); const auto initial_staging_pose = dock->getStagingPose(); - const auto robot_pose = getRobotPoseInFrame( - initial_staging_pose.header.frame_id); + const auto robot_pose = getRobotPoseInFrame(initial_staging_pose.header.frame_id); if (!goal->navigate_to_staging_pose || utils::l2Norm(robot_pose.pose, initial_staging_pose.pose) < dock_prestaging_tolerance_) { @@ -450,7 +449,9 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now(); - if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, dock_backwards_)) { + if (!controller_->computeVelocityCommand(target_pose.pose, command->twist, true, + dock_backwards_)) + { throw opennav_docking_core::FailedToControl("Failed to get control"); } vel_publisher_->publish(std::move(command)); @@ -516,7 +517,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin auto command = std::make_unique(); command->header.stamp = now(); if (getCommandToPose( - command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, + command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backwards_)) { return true; @@ -534,7 +535,7 @@ bool DockingServer::resetApproach(const geometry_msgs::msg::PoseStamped & stagin bool DockingServer::getCommandToPose( geometry_msgs::msg::Twist & cmd, const geometry_msgs::msg::PoseStamped & pose, - double linear_tolerance, double angular_tolerance, bool backward) + double linear_tolerance, double angular_tolerance, bool is_docking, bool backward) { // Reset command to zero velocity cmd.linear.x = 0; @@ -557,7 +558,7 @@ bool DockingServer::getCommandToPose( tf2_buffer_->transform(target_pose, target_pose, base_frame_); // Compute velocity command - if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) { + if (!controller_->computeVelocityCommand(target_pose.pose, cmd, is_docking, backward)) { throw opennav_docking_core::FailedToControl("Failed to get control"); } @@ -644,7 +645,7 @@ void DockingServer::undockRobot() auto command = std::make_unique(); command->header.stamp = now(); if (getCommandToPose( - command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, + command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backwards_)) { RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index c2f1e25f1c1..e0156cf727f 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2024 Open Navigation LLC +// Copyright (c) 2024 Alberto J. Tudela Roldán // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,6 +18,10 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose.hpp" #include "opennav_docking/controller.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2_ros/buffer.h" #include "ament_index_cpp/get_package_share_directory.hpp" // Testing the controller at high level; the nav2_graceful_controller @@ -33,21 +38,187 @@ RosLockGuard g_rclcpp; namespace opennav_docking { +class ControllerFixture : public opennav_docking::Controller +{ +public: + ControllerFixture( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::shared_ptr tf, + std::string fixed_frame, std::string base_frame) + : Controller(node, tf, fixed_frame, base_frame) + { + } + + ~ControllerFixture() = default; + + bool isTrajectoryCollisionFree( + const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward = false) + { + return opennav_docking::Controller::isTrajectoryCollisionFree( + target_pose, is_docking, backward); + } + + void setCollisionTolerance(double tolerance) + { + dock_collision_threshold_ = tolerance; + } +}; + +class TestCollisionChecker : public nav2_util::LifecycleNode +{ +public: + explicit TestCollisionChecker(std::string name) + : LifecycleNode(name) + { + } + + ~TestCollisionChecker() + { + footprint_pub_.reset(); + costmap_pub_.reset(); + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Configuring"); + + costmap_ = std::make_shared(100, 100, 0.1, -5.0, -5.0); + + footprint_pub_ = create_publisher( + "test_footprint", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + costmap_pub_ = std::make_shared( + shared_from_this(), costmap_.get(), "test_base_frame", "test_costmap", true); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Activating"); + costmap_pub_->on_activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_INFO(this->get_logger(), "Deactivating"); + costmap_pub_->on_deactivate(); + costmap_.reset(); + return nav2_util::CallbackReturn::SUCCESS; + } + + void publishFootprint( + const double radius, const double center_x, const double center_y, + std::string base_frame, const rclcpp::Time & stamp) + { + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = base_frame; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + + p.x = center_x + radius; + p.y = center_y + radius; + msg->polygon.points.push_back(p); + + p.x = center_x + radius; + p.y = center_y - radius; + msg->polygon.points.push_back(p); + + p.x = center_x - radius; + p.y = center_y - radius; + msg->polygon.points.push_back(p); + + p.x = center_x - radius; + p.y = center_y + radius; + msg->polygon.points.push_back(p); + + footprint_pub_->publish(std::move(msg)); + } + + void publishCostmap() + { + costmap_pub_->publishCostmap(); + } + + geometry_msgs::msg::Pose setPose(double x, double y, double theta) + { + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + return pose; + } + + void setRectangle( + double width, double height, double center_x, double center_y, unsigned char cost) + { + unsigned int mx, my; + if (!costmap_->worldToMap(center_x, center_y, mx, my)) { + RCLCPP_ERROR(this->get_logger(), "Failed to convert world coordinates to map coordinates"); + return; + } + + unsigned int width_cell = static_cast(width / costmap_->getResolution()); + unsigned int height_cell = static_cast(height / costmap_->getResolution()); + + for (unsigned int i = 0; i < width_cell; ++i) { + for (unsigned int j = 0; j < height_cell; ++j) { + costmap_->setCost(mx + i, my + j, cost); + } + } + } + + void clearCostmap() + { + if (!costmap_) { + RCLCPP_ERROR(this->get_logger(), "Costmap is not initialized"); + return; + } + + unsigned int size_x = costmap_->getSizeInCellsX(); + unsigned int size_y = costmap_->getSizeInCellsY(); + + for (unsigned int i = 0; i < size_x; ++i) { + for (unsigned int j = 0; j < size_y; ++j) { + costmap_->setCost(i, j, nav2_costmap_2d::FREE_SPACE); + } + } + } + +private: + std::shared_ptr costmap_; + + rclcpp::Publisher::SharedPtr footprint_pub_; + std::shared_ptr costmap_pub_; +}; + TEST(ControllerTests, ObjectLifecycle) { auto node = std::make_shared("test"); - auto controller = std::make_unique(node); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + // Skip collision detection + nav2_util::declare_parameter_if_not_declared( + node, "controller.use_collision_detection", rclcpp::ParameterValue(false)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); geometry_msgs::msg::Pose pose; geometry_msgs::msg::Twist cmd_out, cmd_init; - EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out)); + EXPECT_TRUE(controller->computeVelocityCommand(pose, cmd_out, true)); EXPECT_NE(cmd_init, cmd_out); controller.reset(); } TEST(ControllerTests, DynamicParameters) { auto node = std::make_shared("test"); - auto controller = std::make_shared(node); + auto controller = std::make_unique( + node, nullptr, "test_base_frame", "test_base_frame"); auto params = std::make_shared( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -63,7 +234,10 @@ TEST(ControllerTests, DynamicParameters) { rclcpp::Parameter("controller.v_linear_min", 5.0), rclcpp::Parameter("controller.v_linear_max", 6.0), rclcpp::Parameter("controller.v_angular_max", 7.0), - rclcpp::Parameter("controller.slowdown_radius", 8.0)}); + rclcpp::Parameter("controller.slowdown_radius", 8.0), + rclcpp::Parameter("controller.projection_time", 9.0), + rclcpp::Parameter("controller.simulation_time_step", 10.0), + rclcpp::Parameter("controller.dock_collision_threshold", 11.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); @@ -77,6 +251,299 @@ TEST(ControllerTests, DynamicParameters) { EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0); EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0); EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0); + EXPECT_EQ(node->get_parameter("controller.projection_time").as_double(), 9.0); + EXPECT_EQ(node->get_parameter("controller.simulation_time_step").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("controller.dock_collision_threshold").as_double(), 11.0); } +TEST(ControllerTests, TFException) +{ + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + auto controller = std::make_unique( + node, tf, "test_fixed_frame", "test_base_frame"); + + geometry_msgs::msg::Pose pose; + EXPECT_FALSE(controller->isTrajectoryCollisionFree(pose, false)); + controller.reset(); +} + +TEST(ControllerTests, CollisionCheckerDockForward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the pose of the dock at 1.75m in front of the robot + auto dock_pose = collision_tester->setPose(1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot + // It should hit the dock because the robot is 0.5m wide and the dock pose is at 1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set an object between the robot and the dock + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerDockBackward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the pose of the dock at 1.75m behind the robot + auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot + // It should hit the dock because the robot is 0.5m wide and the dock pose is at -1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set an object between the robot and the dock + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 2m behind the robot + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerUndockBackward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the staging pose at 1.75m behind the robot + auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" at origin + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked + // It should hit the dock because the robot is 0.5m wide and the robot pose is at 1.75 + // But it does not hit because the collision tolerance is 0.3m + collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set an object beyond the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set an object between the robot and the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true)); + + collision_tester->deactivate(); +} + +TEST(ControllerTests, CollisionCheckerUndockForward) { + auto collision_tester = std::make_shared("collision_test"); + auto node = std::make_shared("test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + nav2_util::declare_parameter_if_not_declared( + node, "controller.footprint_topic", rclcpp::ParameterValue("test_footprint")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.costmap_topic", rclcpp::ParameterValue("test_costmap_raw")); + nav2_util::declare_parameter_if_not_declared( + node, "controller.projection_time", rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.simulation_time_step", rclcpp::ParameterValue(0.1)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.dock_collision_threshold", rclcpp::ParameterValue(0.3)); + + auto controller = std::make_unique( + node, tf, "test_base_frame", "test_base_frame"); + collision_tester->configure(); + collision_tester->activate(); + + // Set the staging pose at 1.75m in the front of the robot + auto staging_pose = collision_tester->setPose(1.75, 0.0, 0.0); + + // Publish a footprint of 0.5m "radius" + auto radius = 0.5; + collision_tester->publishFootprint(radius, 0.0, 0.0, "test_base_frame", node->now()); + + // Publish an empty costmap + // It should not hit anything in an empty costmap + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked + // It should not hit anything because the robot is docked and the trajectory is backward + collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set an object beyond the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set an object between the robot and the staging pose + // It should hit the object + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + // Set the collision tolerance to 0 to ensure all obstacles in the path are detected + controller->setCollisionTolerance(0.0); + + // Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked + // Now it should hit the dock + collision_tester->clearCostmap(); + collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE); + collision_tester->publishCostmap(); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false)); + + collision_tester->deactivate(); +} + + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 563e810ce36..5b76f6095ce 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -41,15 +41,19 @@ def generate_test_description(): executable='opennav_docking', name='docking_server', parameters=[{'wait_charge_timeout': 1.0, + 'controller': { + 'use_collision_detection': False, + 'transform_tolerance': 0.5, + }, 'dock_plugins': ['test_dock_plugin'], 'test_dock_plugin': { - 'plugin': 'opennav_docking::SimpleChargingDock', - 'use_battery_status': True}, + 'plugin': 'opennav_docking::SimpleChargingDock', + 'use_battery_status': True}, 'docks': ['test_dock'], 'test_dock': { - 'type': 'test_dock_plugin', - 'frame': 'odom', - 'pose': [10.0, 0.0, 0.0] + 'type': 'test_dock_plugin', + 'frame': 'odom', + 'pose': [10.0, 0.0, 0.0] }}], output='screen', ), From dc26fa46d9240763af0cfc17f5b8a124f085eb41 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 27 Nov 2024 01:27:52 +0100 Subject: [PATCH 25/29] Use BT.CPP Tree::sleep (#4761) * Use BT.cpp sleep Signed-off-by: Tony Najjar * Implement BT Loop Rate Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * move to nav2_behavior_tree Signed-off-by: Tony Najjar * fix Signed-off-by: Tony Najjar * fix lint Signed-off-by: Tony Najjar * cache Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .circleci/config.yml | 6 +- .../nav2_behavior_tree/utils/loop_rate.hpp | 82 +++++++++++++++++++ .../src/behavior_tree_engine.cpp | 3 +- .../plugins/action/test_bt_action_node.cpp | 3 +- 4 files changed, 89 insertions(+), 5 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp diff --git a/.circleci/config.yml b/.circleci/config.yml index 9cf5261e8d0..385950c5f01 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v28\ + - "<< parameters.key >>-v29\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v28\ + - "<< parameters.key >>-v29\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v28\ + key: "<< parameters.key >>-v29\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp new file mode 100644 index 00000000000..d7ebd5b4ac9 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2024 Angsa Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ +#define NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" + +namespace nav2_behavior_tree +{ + +class LoopRate +{ +public: + LoopRate(const rclcpp::Duration & period, BT::Tree * tree) + : clock_(std::make_shared(RCL_STEADY_TIME)), period_(period), + last_interval_(clock_->now()), tree_(tree) + {} + + // Similar to rclcpp::WallRate::sleep() but using tree_->sleep() + bool sleep() + { + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + std::chrono::nanoseconds time_to_sleep_ns(time_to_sleep.nanoseconds()); + // Sleep (can get interrupted by emitWakeUpSignal()) + tree_->sleep(time_to_sleep_ns); + return true; + } + + std::chrono::nanoseconds period() const + { + return std::chrono::nanoseconds(period_.nanoseconds()); + } + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::Time last_interval_; + BT::Tree * tree_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__UTILS__LOOP_RATE_HPP_ diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 70a192d7196..7e8dc0a09cc 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/utils/shared_library.h" +#include "nav2_behavior_tree/utils/loop_rate.hpp" namespace nav2_behavior_tree { @@ -49,7 +50,7 @@ BehaviorTreeEngine::run( std::function cancelRequested, std::chrono::milliseconds loopTimeout) { - rclcpp::WallRate loopRate(loopTimeout); + nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree); BT::NodeStatus result = BT::NodeStatus::RUNNING; // Loop until something happens with ROS or the node completes 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 d22ed27c7a2..56a66a4602c 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 @@ -25,6 +25,7 @@ #include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_behavior_tree/utils/loop_rate.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -260,7 +261,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) BT::NodeStatus result = BT::NodeStatus::RUNNING; // BT loop execution rate - rclcpp::WallRate loopRate(10ms); + nav2_behavior_tree::LoopRate loopRate(10ms, tree_.get()); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { From bea6b6d125b3ffeb2f1e440ae2afda8ada5834a4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 2 Dec 2024 21:54:08 +0100 Subject: [PATCH 26/29] Add IsStoppedBTNode (#4764) * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- nav2_behavior_tree/CMakeLists.txt | 3 + .../condition/is_stopped_condition.hpp | 90 +++++++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 5 + .../condition/is_stopped_condition.cpp | 80 ++++++++++++ .../plugins/decorator/speed_controller.cpp | 2 - .../test/plugins/condition/CMakeLists.txt | 2 + .../plugins/condition/test_is_stopped.cpp | 123 ++++++++++++++++++ .../include/nav2_util/odometry_utils.hpp | 59 ++++++++- nav2_util/src/odometry_utils.cpp | 3 +- nav2_util/test/test_odometry_utils.cpp | 45 +++++++ 10 files changed, 407 insertions(+), 5 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 30955681deb..3aaec265445 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -95,6 +95,9 @@ list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp) list(APPEND plugin_libs nav2_is_stuck_condition_bt_node) +add_library(nav2_is_stopped_condition_bt_node SHARED plugins/condition/is_stopped_condition.cpp) +list(APPEND plugin_libs nav2_is_stopped_condition_bt_node) + add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp) list(APPEND plugin_libs nav2_transform_available_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp new file mode 100644 index 00000000000..40197939d1f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 2024 Angsa Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "nav_msgs/msg/odometry.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + + +using namespace std::chrono_literals; // NOLINT + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is considered stopped for long enough, RUNNING if stopped but not for long enough and FAILURE otherwise + */ +class IsStoppedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsStoppedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsStoppedCondition() = delete; + + /** + * @brief A destructor for nav2_behavior_tree::IsStoppedCondition + */ + ~IsStoppedCondition() override; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("velocity_threshold", 0.01, + "Velocity threshold below which robot is considered stopped"), + BT::InputPort("duration_stopped", 1000ms, + "Duration (ms) the velocity must remain below the threshold"), + }; + } + +private: + rclcpp::Node::SharedPtr node_; + + double velocity_threshold_; + std::chrono::milliseconds duration_stopped_; + rclcpp::Time stopped_stamp_; + + std::shared_ptr odom_smoother_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 71c20ee4ecc..67059def0eb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -227,6 +227,11 @@ + + Velocity threshold below which robot is considered stopped + Duration (ms) the velocity must remain below the threshold + + Child frame for transform Parent frame for transform diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp new file mode 100644 index 00000000000..f0b32d4c1c0 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2024 Angsa Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsStoppedCondition::IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + velocity_threshold_(0.01), + duration_stopped_(1000ms), + stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) +{ + node_ = config().blackboard->get("node"); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); +} + +IsStoppedCondition::~IsStoppedCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); +} + +BT::NodeStatus IsStoppedCondition::tick() +{ + getInput("velocity_threshold", velocity_threshold_); + getInput("duration_stopped", duration_stopped_); + + auto twist = odom_smoother_->getRawTwistStamped(); + + // if there is no timestamp, set it to now + if (twist.header.stamp.sec == 0 && twist.header.stamp.nanosec == 0) { + twist.header.stamp = node_->get_clock()->now(); + } + + if (abs(twist.twist.linear.x) < velocity_threshold_ && + abs(twist.twist.linear.y) < velocity_threshold_ && + abs(twist.twist.angular.z) < velocity_threshold_) + { + if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { + stopped_stamp_ = rclcpp::Time(twist.header.stamp); + } + + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::RUNNING; + } + + } else { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + return BT::NodeStatus::FAILURE; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsStopped"); +} diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 87c9eb5bf16..f5b4eb3bd86 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,8 +50,6 @@ SpeedController::SpeedController( d_rate_ = max_rate_ - min_rate_; d_speed_ = max_speed_ - min_speed_; - std::string odom_topic; - node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); odom_smoother_ = config().blackboard->get>( "odom_smoother"); } diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index de380c769af..106c4a96f27 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -16,6 +16,8 @@ plugin_add_test(test_condition_transform_available test_transform_available.cpp plugin_add_test(test_condition_is_stuck test_is_stuck.cpp nav2_is_stuck_condition_bt_node) +plugin_add_test(test_condition_is_stopped test_is_stopped.cpp nav2_is_stopped_condition_bt_node) + plugin_add_test(test_condition_is_battery_charging test_is_battery_charging.cpp nav2_is_battery_charging_condition_bt_node) plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_battery_low_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp new file mode 100644 index 00000000000..26ae239a180 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -0,0 +1,123 @@ +// Copyright (c) 2024 Angsa Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + odom_smoother_ = std::make_shared(node_); + config_->blackboard->set( + "odom_smoother", odom_smoother_); // NOLINT + // shorten duration_stopped from default to make the test faster + std::chrono::milliseconds duration = 100ms; + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; + bt_node_ = std::make_shared("is_stopped", *config_); + } + + void TearDown() + { + bt_node_.reset(); + odom_smoother_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr odom_smoother_; +}; + +std::shared_ptr +IsStoppedTestFixture::bt_node_ = nullptr; +std::shared_ptr +IsStoppedTestFixture::odom_smoother_ = nullptr; + + +TEST_F(IsStoppedTestFixture, test_behavior) +{ + auto odom_pub = node_->create_publisher("odom", + rclcpp::SystemDefaultsQoS()); + nav_msgs::msg::Odometry odom_msg; + + // Test FAILURE when robot is moving + auto time = node_->now(); + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test RUNNING when robot is stopped but not long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(90ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + // Test SUCCESS when robot is stopped for long enough + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test FAILURE immediately after robot starts moving + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.angular.z = 0.1; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test SUCCESS when robot is stopped for long with a back-dated timestamp + odom_msg.header.stamp = node_->now() - rclcpp::Duration(2, 0); + odom_msg.twist.twist.angular.z = 0; + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test SUCCESS when robot is stopped for long enough without a stamp for odometry + odom_msg.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + // In the first tick, the timestamp is set + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + std::this_thread::sleep_for(110ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 185a86d1471..ab9132b5ffe 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -68,13 +68,67 @@ class OdomSmoother * @brief Get twist msg from smoother * @return twist Twist msg */ - inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + inline geometry_msgs::msg::Twist getTwist() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } + return vel_smooth_.twist; + } /** * @brief Get twist stamped msg from smoother * @return twist TwistStamped msg */ - inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} + inline geometry_msgs::msg::TwistStamped getTwistStamped() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::TwistStamped twist_stamped; + return twist_stamped; + } + return vel_smooth_; + } + + /** + * @brief Get raw twist msg from smoother (without smoothing) + * @return twist Twist msg + */ + inline geometry_msgs::msg::Twist getRawTwist() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } + return odom_history_.back().twist.twist; + } + + /** + * @brief Get raw twist stamped msg from smoother (without smoothing) + * @return twist TwistStamped msg + */ + inline geometry_msgs::msg::TwistStamped getRawTwistStamped() + { + std::lock_guard lock(odom_mutex_); + geometry_msgs::msg::TwistStamped twist_stamped; + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + return twist_stamped; + } + twist_stamped.header = odom_history_.back().header; + twist_stamped.twist = odom_history_.back().twist.twist; + return twist_stamped; + } protected: /** @@ -88,6 +142,7 @@ class OdomSmoother */ void updateState(); + bool received_odom_; rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; geometry_msgs::msg::TwistStamped vel_smooth_; diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 9bf585bdfe2..6b50c332bcc 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -27,7 +27,7 @@ OdomSmoother::OdomSmoother( const rclcpp::Node::WeakPtr & parent, double filter_duration, const std::string & odom_topic) -: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +: received_odom_(false), odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); odom_sub_ = node->create_subscription( @@ -66,6 +66,7 @@ OdomSmoother::OdomSmoother( void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(odom_mutex_); + received_odom_ = true; // update cumulated odom only if history is not empty if (!odom_history_.empty()) { diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 7d5b8bf9fe7..6871949303e 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -32,6 +32,34 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +TEST(OdometryUtils, test_uninitialized) +{ + auto node = std::make_shared("test_node"); + nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::TwistStamped twist_stamped_msg; + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_msg = odom_smoother.getRawTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getRawTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); +} + TEST(OdometryUtils, test_smoothed_velocity) { auto node = std::make_shared("test_node"); @@ -41,6 +69,7 @@ TEST(OdometryUtils, test_smoothed_velocity) nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::Twist twist_raw_msg; auto time = node->now(); @@ -67,9 +96,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 1.5); EXPECT_EQ(twist_msg.linear.y, 1.5); EXPECT_EQ(twist_msg.angular.z, 1.5); + EXPECT_EQ(twist_raw_msg.linear.x, 2.0); + EXPECT_EQ(twist_raw_msg.linear.y, 2.0); + EXPECT_EQ(twist_raw_msg.angular.z, 2.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2); odom_msg.twist.twist.linear.x = 3.0; @@ -81,9 +114,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 2.0); EXPECT_EQ(twist_msg.linear.y, 2.0); EXPECT_EQ(twist_msg.angular.z, 2.0); + EXPECT_EQ(twist_raw_msg.linear.x, 3.0); + EXPECT_EQ(twist_raw_msg.linear.y, 3.0); + EXPECT_EQ(twist_raw_msg.angular.z, 3.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45); odom_msg.twist.twist.linear.x = 4.0; @@ -95,9 +132,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 3.5); EXPECT_EQ(twist_msg.linear.y, 3.5); EXPECT_EQ(twist_msg.angular.z, 3.5); + EXPECT_EQ(twist_raw_msg.linear.x, 4.0); + EXPECT_EQ(twist_raw_msg.linear.y, 4.0); + EXPECT_EQ(twist_raw_msg.angular.z, 4.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0); odom_msg.twist.twist.linear.x = 5.0; @@ -109,7 +150,11 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 5.0); EXPECT_EQ(twist_msg.linear.y, 5.0); EXPECT_EQ(twist_msg.angular.z, 5.0); + EXPECT_EQ(twist_raw_msg.linear.x, 5.0); + EXPECT_EQ(twist_raw_msg.linear.y, 5.0); + EXPECT_EQ(twist_raw_msg.angular.z, 5.0); } From 3ee384df622ed964680452dcd53da6d0e9a15514 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 3 Dec 2024 09:55:55 +0800 Subject: [PATCH 27/29] Simplify namespaced bringups and multirobot sim (#4715) * WIP single robot namespacing working Signed-off-by: Luca Della Vedova * Remove manual namespace substitution Signed-off-by: Luca Della Vedova * Remove all multirobot specific configs Signed-off-by: Luca Della Vedova * Refactor parsing function to common, add for rest of layers Signed-off-by: Luca Della Vedova * Fix dwb_critics test Signed-off-by: Luca Della Vedova * Add alternative API for costmap construction Signed-off-by: Luca Della Vedova * Address review feedback Signed-off-by: Luca Della Vedova * Remove remaining usage of `use_namespace` parameter Signed-off-by: Luca Della Vedova * Always join with parent namespace Signed-off-by: Luca Della Vedova * Use private parameter for parent namespace Signed-off-by: Luca Della Vedova * Fix integration test Signed-off-by: Luca Della Vedova * Revert "Use private parameter for parent namespace" This reverts commit 0c958dc994f5e618fef2471c0851a2bc3b19b1d5. Signed-off-by: Luca Della Vedova * Revert "Fix integration test" This reverts commit 137d57759b700f00c7548b997abcc8dc36d9cca9. Signed-off-by: Luca Della Vedova * Remove global map_topic parameter Signed-off-by: Luca Della Vedova * Simplify Costmap2DROS constructor Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Signed-off-by: Luca Della Vedova --- nav2_bringup/launch/bringup_launch.py | 22 +- .../cloned_multi_tb3_simulation_launch.py | 6 +- nav2_bringup/launch/rviz_launch.py | 42 +- .../launch/tb3_loopback_simulation.launch.py | 10 - nav2_bringup/launch/tb3_simulation_launch.py | 10 - .../launch/tb4_loopback_simulation.launch.py | 10 - nav2_bringup/launch/tb4_simulation_launch.py | 10 - .../unique_multi_tb3_simulation_launch.py | 8 +- .../params/nav2_multirobot_params_1.yaml | 316 --------------- .../params/nav2_multirobot_params_2.yaml | 315 --------------- .../params/nav2_multirobot_params_all.yaml | 377 ----------------- nav2_bringup/params/nav2_params.yaml | 20 +- nav2_bringup/rviz/nav2_default_view.rviz | 48 +-- nav2_bringup/rviz/nav2_namespaced_view.rviz | 379 ------------------ nav2_controller/src/controller_server.cpp | 2 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 17 +- .../include/nav2_costmap_2d/costmap_layer.hpp | 20 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 1 + .../plugins/range_sensor_layer.cpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 12 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 11 +- nav2_costmap_2d/src/costmap_layer.cpp | 16 + .../integration/test_costmap_2d_publisher.cpp | 1 - .../test/obstacle_footprint_test.cpp | 18 +- .../test/critic_manager_test.cpp | 4 +- nav2_mppi_controller/test/critics_tests.cpp | 18 +- .../test/optimizer_unit_tests.cpp | 24 +- .../test/path_handler_test.cpp | 4 +- nav2_mppi_controller/test/utils_test.cpp | 2 +- nav2_planner/src/planner_server.cpp | 2 +- .../launch/assisted_teleop_example_launch.py | 2 +- .../launch/follow_path_example_launch.py | 2 +- .../launch/inspection_demo_launch.py | 2 +- .../nav_through_poses_example_launch.py | 2 +- .../launch/nav_to_pose_example_launch.py | 2 +- .../launch/picking_demo_launch.py | 2 +- .../launch/recoveries_example_launch.py | 2 +- .../launch/security_demo_launch.py | 2 +- .../waypoint_follower_example_launch.py | 2 +- .../costmap_filters/test_keepout_launch.py | 1 - .../src/costmap_filters/test_speed_launch.py | 1 - .../src/gps_navigation/test_case_py.launch.py | 1 - .../src/system/test_multi_robot_launch.py | 4 +- .../src/system/test_system_launch.py | 1 - .../test_system_with_obstacle_launch.py | 1 - .../src/system/test_wrong_init_pose_launch.py | 1 - .../test_system_failure_launch.py | 1 - .../planning_benchmark_bringup.py | 2 +- .../smoother_benchmark_bringup.py | 2 +- 49 files changed, 146 insertions(+), 1613 deletions(-) delete mode 100644 nav2_bringup/params/nav2_multirobot_params_1.yaml delete mode 100644 nav2_bringup/params/nav2_multirobot_params_2.yaml delete mode 100644 nav2_bringup/params/nav2_multirobot_params_all.yaml delete mode 100644 nav2_bringup/rviz/nav2_namespaced_view.rviz diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index eee19f6b362..a5f6b9c3dc4 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -29,7 +29,7 @@ from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ParameterFile -from nav2_common.launch import ReplaceString, RewrittenYaml +from nav2_common.launch import RewrittenYaml def generate_launch_description(): @@ -39,7 +39,6 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') @@ -58,16 +57,6 @@ def generate_launch_description(): # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Only it applys when `use_namespace` is True. - # '' keyword shall be replaced by 'namespace' launch argument - # in config file 'nav2_multirobot_params.yaml' as a default & example. - # User defined config file should contain '' keyword for the replacements. - params_file = ReplaceString( - source_file=params_file, - replacements={'': ('/', namespace)}, - condition=IfCondition(use_namespace), - ) - configured_params = ParameterFile( RewrittenYaml( source_file=params_file, @@ -86,12 +75,6 @@ def generate_launch_description(): 'namespace', default_value='', description='Top-level namespace' ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_slam_cmd = DeclareLaunchArgument( 'slam', default_value='False', description='Whether run a SLAM' ) @@ -142,7 +125,7 @@ def generate_launch_description(): # Specify the actions bringup_cmd_group = GroupAction( [ - PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), + PushROSNamespace(namespace), Node( condition=IfCondition(use_composition), name='nav2_container', @@ -207,7 +190,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index bd001412d58..6bd6bdddcab 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -80,7 +80,7 @@ def generate_launch_description(): declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_all.yaml' + bringup_dir, 'params', 'nav2_params.yaml' ), description='Full path to the ROS2 parameters file to use for all launched nodes', ) @@ -93,7 +93,7 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use.', ) @@ -144,7 +144,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': TextSubstitution(text=robot_name), - 'use_namespace': 'True', 'rviz_config': rviz_config_file, }.items(), ), @@ -154,7 +153,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': robot_name, - 'use_namespace': 'True', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 8728581df12..425dac4edee 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -18,12 +18,10 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.events import Shutdown from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from nav2_common.launch import ReplaceString def generate_launch_description(): @@ -32,7 +30,6 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') use_sim_time = LaunchConfiguration('use_sim_time') @@ -46,12 +43,6 @@ def generate_launch_description(): ), ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -65,68 +56,37 @@ def generate_launch_description(): # Launch rviz start_rviz_cmd = Node( - condition=UnlessCondition(use_namespace), package='rviz2', executable='rviz2', + namespace=namespace, arguments=['-d', rviz_config_file], output='screen', parameters=[{'use_sim_time': use_sim_time}], - ) - - namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}, - ) - - start_namespaced_rviz_cmd = Node( - condition=IfCondition(use_namespace), - package='rviz2', - executable='rviz2', - namespace=namespace, - arguments=['-d', namespaced_rviz_config_file], - parameters=[{'use_sim_time': use_sim_time}], - output='screen', remappings=[ - ('/map', 'map'), ('/tf', 'tf'), ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose'), ], ) exit_event_handler = RegisterEventHandler( - condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), ), ) - exit_event_handler_namespaced = RegisterEventHandler( - condition=IfCondition(use_namespace), - event_handler=OnProcessExit( - target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), - ), - ) - # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_sim_time_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) - ld.add_action(start_namespaced_rviz_cmd) # Add other nodes and processes we need ld.add_action(exit_event_handler) - ld.add_action(exit_event_handler_namespaced) return ld diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 9e598e854cf..3553852597c 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -42,7 +42,6 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') @@ -61,12 +60,6 @@ def generate_launch_description(): 'namespace', default_value='', description='Top-level namespace' ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), @@ -134,7 +127,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'use_sim_time': 'True', 'rviz_config': rviz_config_file, }.items(), @@ -144,7 +136,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, @@ -203,7 +194,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index db1964f955e..532d8ea24fa 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -44,7 +44,6 @@ def generate_launch_description(): # Create the launch configuration variables slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -77,12 +76,6 @@ def generate_launch_description(): 'namespace', default_value='', description='Top-level namespace' ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_slam_cmd = DeclareLaunchArgument( 'slam', default_value='False', description='Whether run a SLAM' ) @@ -186,7 +179,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'use_sim_time': use_sim_time, 'rviz_config': rviz_config_file, }.items(), @@ -196,7 +188,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'slam': slam, 'map': map_yaml_file, 'use_sim_time': use_sim_time, @@ -255,7 +246,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py index 0ad911ddc74..5373efbfab3 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -42,7 +42,6 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') @@ -61,12 +60,6 @@ def generate_launch_description(): 'namespace', default_value='', description='Top-level namespace' ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! @@ -132,7 +125,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'use_sim_time': 'True', 'rviz_config': rviz_config_file, }.items(), @@ -142,7 +134,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, @@ -210,7 +201,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 463f0c17f34..0765ceb4341 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -48,7 +48,6 @@ def generate_launch_description(): # Create the launch configuration variables slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -81,12 +80,6 @@ def generate_launch_description(): 'namespace', default_value='', description='Top-level namespace' ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_slam_cmd = DeclareLaunchArgument( 'slam', default_value='False', description='Whether run a SLAM' ) @@ -187,7 +180,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'use_sim_time': use_sim_time, 'rviz_config': rviz_config_file, }.items(), @@ -197,7 +189,6 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={ 'namespace': namespace, - 'use_namespace': use_namespace, 'slam': slam, 'map': map_yaml_file, 'use_sim_time': use_sim_time, @@ -261,7 +252,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index ae5551d179a..862a0083799 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): declare_robot1_params_file_cmd = DeclareLaunchArgument( 'robot1_params_file', default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_1.yaml' + bringup_dir, 'params', 'nav2_params.yaml' ), description='Full path to the ROS2 parameters file to use for robot1 launched nodes', ) @@ -107,7 +107,7 @@ def generate_launch_description(): declare_robot2_params_file_cmd = DeclareLaunchArgument( 'robot2_params_file', default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_2.yaml' + bringup_dir, 'params', 'nav2_params.yaml' ), description='Full path to the ROS2 parameters file to use for robot2 launched nodes', ) @@ -120,7 +120,7 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use.', ) @@ -162,7 +162,6 @@ def generate_launch_description(): condition=IfCondition(use_rviz), launch_arguments={ 'namespace': TextSubstitution(text=robot['name']), - 'use_namespace': 'True', 'rviz_config': rviz_config_file, }.items(), ), @@ -172,7 +171,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': robot['name'], - 'use_namespace': 'True', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml deleted file mode 100644 index 319ebd41acd..00000000000 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ /dev/null @@ -1,316 +0,0 @@ -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 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["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 - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - stateful: True - # 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 - 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 - 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: 0.0 - GoalAlign.scale: 0.0 - 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: - global_frame: odom - 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: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /robot1/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: - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - robot_radius: 0.22 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /robot1/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 - -map_server: - ros__parameters: - yaml_filename: "tb3_sandbox.yaml" - save_map_timeout: 5.0 - -planner_server: - ros__parameters: - planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -behavior_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -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 - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml deleted file mode 100644 index 86e2b78e5ec..00000000000 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ /dev/null @@ -1,315 +0,0 @@ -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 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["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 - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - stateful: True - # 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 - 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 - 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: 0.0 - GoalAlign.scale: 0.0 - 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: - global_frame: odom - 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: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /robot2/scan - max_obstacle_height: 2.0 - clearing: True - marking: True - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - robot_radius: 0.22 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /robot2/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 - -map_server: - ros__parameters: - yaml_filename: "tb3_sandbox.yaml" - save_map_timeout: 5.0 - -planner_server: - ros__parameters: - planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -behavior_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -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 - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml deleted file mode 100644 index b6b27887a38..00000000000 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ /dev/null @@ -1,377 +0,0 @@ -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 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - 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 - 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 - costmap_update_timeout: 1.0 - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -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 - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 69fe42195eb..a5e4a539c02 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -42,7 +42,7 @@ bt_navigator: ros__parameters: global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 @@ -211,7 +211,13 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /scan + # A relative topic will be appended to the parent of the local_costmap namespace. + # For example: + # * User chosen namespace is `tb4`. + # * User chosen topic is `scan`. + # * Topic will be remapped to `/tb4/scan` without `local_costmap`. + # * Use global topic `/scan` if you do not wish the node namespace to apply + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -241,7 +247,13 @@ global_costmap: enabled: True observation_sources: scan scan: - topic: /scan + # A relative topic will be appended to the parent of the global_costmap namespace. + # For example: + # * User chosen namespace is `tb4`. + # * User chosen topic is `scan`. + # * Topic will be remapped to `/tb4/scan` without `global_costmap`. + # * Use global topic `/scan` if you do not wish the node namespace to apply + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -362,7 +374,7 @@ collision_monitor: FootprintApproach: type: "polygon" action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" + footprint_topic: "local_costmap/published_footprint" time_before_collision: 1.2 simulation_time_step: 0.1 min_points: 6 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index c8dd3768583..df1bc58d5dc 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -58,7 +58,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: robot_description Enabled: false Links: All Links Enabled: true @@ -148,7 +148,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /scan + Value: scan Use Fixed Frame: true Use rainbow: true Value: true @@ -181,7 +181,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /mobile_base/sensors/bumper_pointcloud + Value: mobile_base/sensors/bumper_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -199,13 +199,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /map + Value: map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /map_updates + Value: map_updates Use Timestamp: false Value: true - Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /particle_cloud + Value: particle_cloud Value: true - Class: rviz_common/Group Displays: @@ -240,13 +240,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap + Value: global_costmap/costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap_updates + Value: global_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 0.30000001192092896 @@ -263,13 +263,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /downsampled_costmap + Value: downsampled_costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /downsampled_costmap_updates + Value: downsampled_costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -298,7 +298,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /plan + Value: plan Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -329,7 +329,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/voxel_marked_cloud + Value: global_costmap/voxel_marked_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -344,7 +344,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/published_footprint + Value: global_costmap/published_footprint Value: false Enabled: true Name: Global Planner @@ -364,13 +364,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap + Value: local_costmap/costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap_updates + Value: local_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -399,7 +399,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_plan + Value: local_plan Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -411,7 +411,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /marker + Value: marker Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon @@ -424,7 +424,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/published_footprint + Value: local_costmap/published_footprint Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -455,7 +455,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/voxel_marked_cloud + Value: local_costmap/voxel_marked_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -475,7 +475,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /intel_realsense_r200_depth/image_raw + Value: intel_realsense_r200_depth/image_raw Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -506,7 +506,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /intel_realsense_r200_depth/points + Value: intel_realsense_r200_depth/points Use Fixed Frame: true Use rainbow: true Value: true @@ -522,7 +522,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /waypoints + Value: waypoints Value: true Enabled: true Global Options: @@ -545,7 +545,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /initialpose + Value: initialpose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: @@ -553,7 +553,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /clicked_point + Value: clicked_point - Class: nav2_rviz_plugins/GoalTool Transformation: Current: diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz deleted file mode 100644 index 4dcd23e24b4..00000000000 --- a/nav2_bringup/rviz/nav2_namespaced_view.rviz +++ /dev/null @@ -1,379 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 195 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /RobotModel1/Status1 - - /TF1/Frames1 - - /TF1/Tree1 - - /Global Planner1/Global Costmap1 - Splitter Ratio: 0.5833333134651184 - Tree Height: 464 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: nav2_rviz_plugins/Navigation 2 - Name: Navigation 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Bumper Hit - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /mobile_base/sensors/bumper_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Map - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Use Timestamp: false - Value: true - - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloud - Color: 0; 180; 0 - Enabled: true - Max Arrow Length: 0.3 - Min Arrow Length: 0.02 - Name: Amcl Particle Swarm - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /particle_cloud - Value: true - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Global Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.019999999552965164 - Head Length: 0.019999999552965164 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.004999999888241291 - Shaft Length: 0.019999999552965164 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /plan - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/published_footprint - Value: false - Enabled: true - Name: Global Planner - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Local Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Local Plan - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_plan - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Trajectories - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /marker - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/published_footprint - Value: true - Enabled: true - Name: Controller - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /waypoints - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: nav2_rviz_plugins/GoalTool - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: -1.5708 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 160 - Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 914 - Hide Left Dock: false - Hide Right Dock: true - Navigation 2: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1545 - X: 186 - Y: 117 - diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c221c2db17b..93e66539392 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -69,7 +69,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( - "local_costmap", std::string{get_namespace()}, "local_costmap", + "local_costmap", std::string{get_namespace()}, get_parameter("use_sim_time").as_bool()); } 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 33feaa700df..3d71c501ece 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 @@ -81,26 +81,16 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - /** - * @brief Constructor for the wrapper, the node will - * be placed in a namespace equal to the node's name - * @param name Name of the costmap ROS node - * @param use_sim_time Whether to use simulation or real time - */ - explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); - /** * @brief Constructor for the wrapper - * @param name Name of the costmap ROS node + * @param name Name of the costmap ROS node which will also be used as a local namespace * @param parent_namespace Absolute namespace of the node hosting the costmap node - * @param local_namespace Namespace to append to the parent namespace * @param use_sim_time Whether to use simulation or real time */ explicit Costmap2DROS( const std::string & name, - const std::string & parent_namespace, - const std::string & local_namespace, - const bool & use_sim_time); + const std::string & parent_namespace = "/", + const bool & use_sim_time = false); /** * @brief Common initialization for constructors @@ -372,7 +362,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::unique_ptr layered_costmap_{nullptr}; std::string name_; - std::string parent_namespace_; /** * @brief Function on timer for costmap update diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 43a82e0cde1..34f0314b76d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -38,6 +38,8 @@ #ifndef NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_ #define NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_ +#include + #include #include #include @@ -192,6 +194,24 @@ class CostmapLayer : public Layer, public Costmap2D */ CombinationMethod combination_method_from_int(const int value); + /** + * Joins the specified topic with the parent namespace of the costmap node. + * If the topic has an absolute path, it is returned instead. + * + * This is necessary for user defined relative topics to work as expected since costmap layers + * add a an additional `costmap_name` namespace to the topic. + * For example: + * * User chosen namespace is `tb4`. + * * User chosen topic is `scan`. + * * Costmap node namespace will be `/tb4/global_costmap`. + * * Without this function, the topic would be `/tb4/global_costmap/scan`. + * * With this function, topic will be remapped to `/tb4/scan`. + * Use global topic `/scan` if you do not wish the node namespace to apply + * + * @param topic the topic to parse + */ + std::string joinWithParentNamespace(const std::string & topic); + private: double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; }; diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 3ba1c97f08a..7021bfb4595 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -189,6 +189,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range); node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range); + topic = joinWithParentNamespace(topic); RCLCPP_DEBUG( logger_, diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index ffbe8a3ef8b..dc28d4f4342 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -130,6 +130,7 @@ void RangeSensorLayer::onInitialize() // Traverse the topic names list subscribing to all of them with the same callback method for (auto & topic_name : topic_names) { + topic_name = joinWithParentNamespace(topic_name); if (input_sensor_type == InputSensorType::VARIABLE) { processRangeMessageFunc_ = std::bind( &RangeSensorLayer::processVariableRangeMsg, this, diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 67b7fbd5ec9..896dfc77a5f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -136,7 +136,7 @@ StaticLayer::getParameters() declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); - declareParameter("map_topic", rclcpp::ParameterValue("")); + declareParameter("map_topic", rclcpp::ParameterValue("map")); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); auto node = node_.lock(); @@ -147,14 +147,8 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - std::string private_map_topic, global_map_topic; - node->get_parameter(name_ + "." + "map_topic", private_map_topic); - node->get_parameter("map_topic", global_map_topic); - if (!private_map_topic.empty()) { - map_topic_ = private_map_topic; - } else { - map_topic_ = global_map_topic; - } + node->get_parameter(name_ + "." + "map_topic", map_topic_); + map_topic_ = joinWithParentNamespace(map_topic_); node->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index fa11b60ebfe..5413fd3d2c1 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,9 +58,6 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) -: Costmap2DROS(name, "/", name, use_sim_time) {} - Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) : nav2_util::LifecycleNode("costmap", "", options), name_("costmap"), @@ -70,7 +67,6 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} { - declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); is_lifecycle_follower_ = false; init(); } @@ -78,7 +74,6 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace, const bool & use_sim_time) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line @@ -87,21 +82,17 @@ Costmap2DROS::Costmap2DROS( // of the namespaces rclcpp::NodeOptions().arguments({ "--ros-args", "-r", std::string("__ns:=") + - nav2_util::add_namespaces(parent_namespace, local_namespace), + nav2_util::add_namespaces(parent_namespace, name), "--ros-args", "-r", name + ":" + std::string("__node:=") + name, "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), })), name_(name), - parent_namespace_(parent_namespace), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} { - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); init(); } diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 8dbce5017f0..93f20f664dc 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -264,4 +264,20 @@ CombinationMethod CostmapLayer::combination_method_from_int(const int value) return CombinationMethod::Max; } } + +std::string CostmapLayer::joinWithParentNamespace(const std::string & topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (topic[0] != '/') { + std::string node_namespace = node->get_namespace(); + std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); + return parent_namespace + "/" + topic; + } + + return topic; +} } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 68a9875ef61..9b91adcf786 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -45,7 +45,6 @@ class CostmapRosLifecycleNode : public nav2_util::LifecycleNode costmap_ros_ = std::make_shared( name_, std::string{get_namespace()}, - name_, get_parameter("use_sim_time").as_bool()); costmap_thread_ = std::make_unique(costmap_ros_); diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp index b556a06be15..d3e18e2acbe 100644 --- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -125,11 +125,13 @@ TEST(ObstacleFootprint, Prepare) auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); - auto costmap_ros = std::make_shared("test_global_costmap"); + std::string ns = "/ns"; + std::string costmap_name = "test_global_costmap"; + auto costmap_ros = + std::make_shared(costmap_name, ns, false); costmap_ros->configure(); std::string name = "name"; - std::string ns = "ns"; critic->initialize(node, name, ns, costmap_ros); geometry_msgs::msg::Pose2D pose; @@ -185,11 +187,13 @@ TEST(ObstacleFootprint, PointCost) auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); - auto costmap_ros = std::make_shared("test_global_costmap"); + std::string ns = "/ns"; + std::string costmap_name = "test_global_costmap"; + auto costmap_ros = + std::make_shared(costmap_name, ns, false); costmap_ros->configure(); std::string name = "name"; - std::string ns = "ns"; critic->initialize(node, name, ns, costmap_ros); costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); @@ -208,11 +212,13 @@ TEST(ObstacleFootprint, LineCost) auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); - auto costmap_ros = std::make_shared("test_global_costmap"); + std::string ns = "/ns"; + std::string costmap_name = "test_global_costmap"; + auto costmap_ros = + std::make_shared(costmap_name, ns, false); costmap_ros->configure(); std::string name = "name"; - std::string ns = "ns"; critic->initialize(node, name, ns, costmap_ros); costmap_ros->getCostmap()->setCost(3, 3, nav2_costmap_2d::LETHAL_OBSTACLE); diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index f24d205bfbe..e2cf5d80034 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -102,7 +102,7 @@ TEST(CriticManagerTests, BasicCriticOperations) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -140,7 +140,7 @@ TEST(CriticManagerTests, CriticLoadingTest) "critic_manager.critics", rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 70417834df4..e4a28f3df1c 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -61,7 +61,7 @@ TEST(CriticTests, ConstraintsCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -133,7 +133,7 @@ TEST(CriticTests, GoalAngleCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -185,7 +185,7 @@ TEST(CriticTests, GoalCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -234,7 +234,7 @@ TEST(CriticTests, PathAngleCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -349,7 +349,7 @@ TEST(CriticTests, PreferForwardCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -402,7 +402,7 @@ TEST(CriticTests, TwirlingCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -462,7 +462,7 @@ TEST(CriticTests, PathFollowCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -510,7 +510,7 @@ TEST(CriticTests, PathAlignCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -614,7 +614,7 @@ TEST(CriticTests, VelocityDeadbandCritic) // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); auto getParam = param_handler.getParamGetter("critic"); std::vector deadband_velocities_; diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index c1b5f56799c..db28b904dd0 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -233,7 +233,7 @@ TEST(OptimizerTests, BasicInitializedFunctions) node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(3.0)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -269,7 +269,7 @@ TEST(OptimizerTests, TestOptimizerMotionModels) OptimizerTester optimizer_tester; node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -301,7 +301,7 @@ TEST(OptimizerTests, setOffsetTests) node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -324,7 +324,7 @@ TEST(OptimizerTests, resetTests) node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -344,7 +344,7 @@ TEST(OptimizerTests, FallbackTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -368,7 +368,7 @@ TEST(OptimizerTests, PrepareTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -395,7 +395,7 @@ TEST(OptimizerTests, shiftControlSequenceTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -438,7 +438,7 @@ TEST(OptimizerTests, SpeedLimitTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -478,7 +478,7 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests) node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.75)); node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -538,7 +538,7 @@ TEST(OptimizerTests, updateStateVelocitiesTests) node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(3.0)); node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -563,7 +563,7 @@ TEST(OptimizerTests, getControlFromSequenceAsTwistTests) node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60)); node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -598,7 +598,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1)); node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 0bcf5543924..54707c185bf 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -108,7 +108,7 @@ TEST(PathHandlerTests, TestBounds) auto node = std::make_shared("my_node"); node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); auto results = costmap_ros->set_parameters_atomically( {rclcpp::Parameter("global_frame", "odom"), rclcpp::Parameter("robot_base_frame", "base_link")}); @@ -159,7 +159,7 @@ TEST(PathHandlerTests, TestTransforms) auto node = std::make_shared("my_node"); node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); ParametersHandler param_handler(node); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 3603d4f3cf3..4c939bd72b0 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -289,7 +289,7 @@ TEST(UtilsTests, findPathCosts) std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( - "dummy_costmap", "", "dummy_costmap", true); + "dummy_costmap", "", true); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); auto * costmap = costmap_ros->getCostmap(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 7233a7ba53d..ccce9813544 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -64,7 +64,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Setup the global costmap costmap_ros_ = std::make_shared( - "global_costmap", std::string{get_namespace()}, "global_costmap", + "global_costmap", std::string{get_namespace()}, get_parameter("use_sim_time").as_bool()); } diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index 90abfcad8a7..ad3119ce441 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -114,7 +114,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index 84de9257a77..b07fef9ba68 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index 7394f494338..be98c14150d 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py index 8dc297d010f..593ee1e8816 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py index 52192c0a2d6..c3e22fb08d3 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index 65c49af0b52..f4bd8346013 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index 429d5d3bd37..c7afd73f31c 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index 4872c031c22..240f430b236 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index d87fd8ea11d..10f36ce7922 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) # start navigation 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 f0d7d924de7..bb43001b5b9 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -152,7 +152,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, 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 38492a53bc6..281aed36250 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -150,7 +150,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 3dd082c1918..cb11b2c36d7 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -102,7 +102,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'use_sim_time': 'True', 'params_file': configured_params, 'use_composition': 'False', diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 92f9a912dfd..87c7bb671e0 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -47,10 +47,10 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') robot1_params_file = os.path.join( # noqa: F841 - bringup_dir, 'params/nav2_multirobot_params_1.yaml' + bringup_dir, 'params/nav2_params.yaml' ) robot2_params_file = os.path.join( # noqa: F841 - bringup_dir, 'params/nav2_multirobot_params_2.yaml' + bringup_dir, 'params/nav2_params.yaml' ) # Names and poses of the robots diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 439a82e8962..fb4d3625880 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -129,7 +129,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 4f6cc09ffb5..b31f19bdd61 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -142,7 +142,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index c6fbe3f9548..31e1b8af411 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -128,7 +128,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 8adb3617977..0cbc76533e2 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -117,7 +117,6 @@ def generate_launch_description(): ), launch_arguments={ 'namespace': '', - 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': new_yaml, diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py index 8ed9f400168..05c61a6284d 100644 --- a/tools/planner_benchmarking/planning_benchmark_bringup.py +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -94,7 +94,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ), ] ) diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index dd1380f0b23..c19d1f2f703 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -107,7 +107,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') ), - launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + launch_arguments={'namespace': ''}.items(), ) metrics_cmd = ExecuteProcess( From 9284c8a9969947b99e821e0c85768cca886920ce Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 4 Dec 2024 02:30:34 +0100 Subject: [PATCH 28/29] Make RecoveryNode return Running (#4777) * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar * Make RecoveryNode return RUNNING Signed-off-by: Tony Najjar * PR review Signed-off-by: Tony Najjar * add halt at the end Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../plugins/control/recovery_node.cpp | 138 +++++++++--------- .../plugins/control/test_recovery_node.cpp | 9 +- 2 files changed, 76 insertions(+), 71 deletions(-) diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 6eb3c6e99e5..1ab9d2ff34c 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick() throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children."); } + if (retry_count_ > number_of_retries_) { + halt(); + return BT::NodeStatus::FAILURE; + } setStatus(BT::NodeStatus::RUNNING); - while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) { - TreeNode * child_node = children_nodes_[current_child_idx_]; - const BT::NodeStatus child_status = child_node->executeTick(); - - if (current_child_idx_ == 0) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - // If first child is skipped, the entire branch is considered skipped - halt(); - return BT::NodeStatus::SKIPPED; - - case BT::NodeStatus::SUCCESS: - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::FAILURE: - { - if (retry_count_ < number_of_retries_) { - // halt first child and tick second child in next iteration - ControlNode::haltChild(0); - current_child_idx_++; - break; - } else { - // reset node and return failure when max retries has been exceeded - halt(); - return BT::NodeStatus::FAILURE; - } - } - - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - - } else if (current_child_idx_ == 1) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - { - // if we skip the recovery (maybe a precondition fails), then we - // should assume that no recovery is possible. For this reason, - // we should return FAILURE and reset the index. - // This does not count as a retry. - current_child_idx_ = 0; - ControlNode::haltChild(1); + TreeNode * child_node = children_nodes_[current_child_idx_]; + const BT::NodeStatus child_status = child_node->executeTick(); + + if (current_child_idx_ == 0) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + + case BT::NodeStatus::SUCCESS: + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::FAILURE: + { + if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); + current_child_idx_ = 1; + return BT::NodeStatus::RUNNING; + } else { + // reset node and return failure when max retries has been exceeded + halt(); return BT::NodeStatus::FAILURE; } - case BT::NodeStatus::RUNNING: - return child_status; - - case BT::NodeStatus::SUCCESS: - { - // halt second child, increment recovery count, and tick first child in next iteration - ControlNode::haltChild(1); - retry_count_++; - current_child_idx_ = 0; - } - break; - - case BT::NodeStatus::FAILURE: - // reset node and return failure if second child fails - halt(); + } + + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + + } else if (current_child_idx_ == 1) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); return BT::NodeStatus::FAILURE; + } + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::SUCCESS: + { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); + retry_count_++; + current_child_idx_ = 0; + return BT::NodeStatus::RUNNING; + } - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - } - } // end while loop + case BT::NodeStatus::FAILURE: + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; - // reset node and return failure + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + } halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A recovery node has exactly 2 children and should never reach here"); } void RecoveryNode::halt() diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 82927031429..532fcad45b2 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -105,6 +105,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) first_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); second_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } @@ -119,8 +120,9 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } @@ -130,8 +132,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) // first child fails, second child fails first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); @@ -139,6 +141,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); @@ -158,6 +162,7 @@ TEST_F(RecoveryNodeTestFixture, test_skipping) // first child fails, second child skipped first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); From add09f60b17f6134416122f8285eba7268d275d7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 18:41:25 -0800 Subject: [PATCH 29/29] Migrating twist to twiststamped in simulations for recommended default bringups (#4779) * migrating from twist to twiststamped Signed-off-by: Steve Macenski * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski * fix assisted teleop test Signed-off-by: Steve Macenski * fixing docking server smoke test Signed-off-by: Steve Macenski * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski --- .circleci/config.yml | 6 +++--- .../test/collision_monitor_node_test.cpp | 1 + .../test/test_docking_server.py | 8 +++---- .../nav2_loopback_sim/loopback_simulator.py | 2 +- .../assisted_teleop_behavior_tester.cpp | 14 ++++++------- .../assisted_teleop_behavior_tester.hpp | 8 +++---- .../include/nav2_util/twist_publisher.hpp | 4 ++-- .../include/nav2_util/twist_subscriber.hpp | 6 +++--- nav2_util/test/test_twist_publisher.cpp | 1 + nav2_util/test/test_twist_subscriber.cpp | 1 + .../test/test_velocity_smoother.cpp | 21 +++++++++++-------- tools/underlay.repos | 2 +- 12 files changed, 40 insertions(+), 34 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 385950c5f01..c52e5a26e57 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v29\ + - "<< parameters.key >>-v31\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v29\ + - "<< parameters.key >>-v31\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v29\ + key: "<< parameters.key >>-v31\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index f297ca363a2..1e8c0bf013a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -220,6 +220,7 @@ class Tester : public ::testing::Test Tester::Tester() { cm_ = std::make_shared(); + cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); footprint_pub_ = cm_->create_publisher( FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 5b76f6095ce..43e3e2df329 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -17,7 +17,7 @@ import unittest from action_msgs.msg import GoalStatus -from geometry_msgs.msg import TransformStamped, Twist +from geometry_msgs.msg import TransformStamped, Twist, TwistStamped from launch import LaunchDescription from launch_ros.actions import Node import launch_testing @@ -90,8 +90,8 @@ def tearDownClass(cls): rclpy.shutdown() def command_velocity_callback(self, msg): - self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z)) - self.command = msg + self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z)) + self.command = msg.twist def timer_callback(self): # Propagate command @@ -155,7 +155,7 @@ def test_docking_server(self): # Subscribe to command velocity self.node.create_subscription( - Twist, + TwistStamped, 'cmd_vel', self.command_velocity_callback, 10 diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 71560c5aa80..4000ddfb16e 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -74,7 +74,7 @@ def __init__(self): self.declare_parameter('scan_frame_id', 'base_scan') self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value - self.declare_parameter('enable_stamped_cmd_vel', False) + self.declare_parameter('enable_stamped_cmd_vel', True) use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value self.declare_parameter('scan_publish_dur', 0.1) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp index bcc1950bde0..f846201cac1 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -54,13 +54,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() node_->create_publisher("preempt_teleop", 10); cmd_vel_pub_ = - node_->create_publisher("cmd_vel_teleop", 10); + node_->create_publisher("cmd_vel_teleop", 10); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); - filtered_vel_sub_ = node_->create_subscription( + filtered_vel_sub_ = node_->create_subscription( "cmd_vel", rclcpp::SystemDefaultsQoS(), std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1)); @@ -167,9 +167,9 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( counter_ = 0; auto start_time = std::chrono::system_clock::now(); while (rclcpp::ok()) { - geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist(); - cmd_vel.linear.x = lin_vel; - cmd_vel.angular.z = ang_vel; + geometry_msgs::msg::TwistStamped cmd_vel = geometry_msgs::msg::TwistStamped(); + cmd_vel.twist.linear.x = lin_vel; + cmd_vel.twist.angular.z = ang_vel; cmd_vel_pub_->publish(cmd_vel); if (counter_ > 1) { @@ -265,9 +265,9 @@ void AssistedTeleopBehaviorTester::amclPoseCallback( } void AssistedTeleopBehaviorTester::filteredVelCallback( - geometry_msgs::msg::Twist::SharedPtr msg) + geometry_msgs::msg::TwistStamped::SharedPtr msg) { - if (msg->linear.x == 0.0f) { + if (msg->twist.linear.x == 0.0f) { counter_++; } else { counter_ = 0; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp index 37bdd790c2c..626588bbd09 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -24,7 +24,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_msgs/action/assisted_teleop.hpp" @@ -68,7 +68,7 @@ class AssistedTeleopBehaviorTester void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); - void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg); + void filteredVelCallback(geometry_msgs::msg::TwistStamped::SharedPtr msg); unsigned int counter_; bool is_active_; @@ -83,11 +83,11 @@ class AssistedTeleopBehaviorTester // Publishers rclcpp::Publisher::SharedPtr initial_pose_pub_; rclcpp::Publisher::SharedPtr preempt_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; // Subscribers rclcpp::Subscription::SharedPtr subscription_; - rclcpp::Subscription::SharedPtr filtered_vel_sub_; + rclcpp::Subscription::SharedPtr filtered_vel_sub_; // Action client to call AssistedTeleop action rclcpp_action::Client::SharedPtr client_ptr_; diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index b86fdee5e78..025f3aabe68 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -61,7 +61,7 @@ class TwistPublisher using nav2_util::declare_parameter_if_not_declared; declare_parameter_if_not_declared( node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue{false}); + rclcpp::ParameterValue{true}); node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_pub_ = node->create_publisher( @@ -122,7 +122,7 @@ class TwistPublisher protected: std::string topic_; - bool is_stamped_; + bool is_stamped_{true}; rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_stamped_pub_; diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 01f9ca25fed..eec5a3e1ae4 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -92,7 +92,7 @@ class TwistSubscriber { nav2_util::declare_parameter_if_not_declared( node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); + rclcpp::ParameterValue(true)); node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_sub_ = node->create_subscription( @@ -125,7 +125,7 @@ class TwistSubscriber { nav2_util::declare_parameter_if_not_declared( node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); + rclcpp::ParameterValue(true)); node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_sub_ = node->create_subscription( @@ -140,7 +140,7 @@ class TwistSubscriber protected: //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel - bool is_stamped_{false}; + bool is_stamped_{true}; //! @brief The subscription when using Twist rclcpp::Subscription::SharedPtr twist_sub_ {nullptr}; //! @brief The subscription when using TwistStamped diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index 94361e94aa8..be185b47f74 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -29,6 +29,7 @@ TEST(TwistPublisher, Unstamped) rclcpp::init(0, nullptr); auto pub_node = std::make_shared("pub_node", ""); pub_node->configure(); + pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); auto vel_publisher = std::make_unique(pub_node, "cmd_vel", 1); ASSERT_EQ(vel_publisher->get_subscription_count(), 0); EXPECT_FALSE(vel_publisher->is_activated()); diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index 56728873682..c18553bc391 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -29,6 +29,7 @@ TEST(TwistSubscriber, Unstamped) auto sub_node = std::make_shared("sub_node", ""); sub_node->configure(); sub_node->activate(); + sub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); geometry_msgs::msg::TwistStamped sub_msg {}; auto vel_subscriber = std::make_unique( diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 76c5e0498b6..a5d34149bc4 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_velocity_smoother/velocity_smoother.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "nav2_util/twist_subscriber.hpp" using namespace std::chrono_literals; @@ -53,7 +53,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;} geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;} - void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);} + void sendCommandMsg(geometry_msgs::msg::TwistStamped::SharedPtr msg) + { + inputCommandStampedCallback(msg); + } }; TEST(VelocitySmootherTest, openLoopTestTimer) @@ -81,8 +84,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer) }); // Send a velocity command - auto cmd = std::make_shared(); - cmd->linear.x = 1.0; // Max is 0.5, so should threshold + auto cmd = std::make_shared(); + cmd->twist.linear.x = 1.0; // Max is 0.5, so should threshold smoother->sendCommandMsg(cmd); // Process velocity smoothing and send updated odometry based on commands @@ -147,8 +150,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) } // Send a velocity command - auto cmd = std::make_shared(); - cmd->linear.x = 1.0; // Max is 0.5, so should threshold + auto cmd = std::make_shared(); + cmd->twist.linear.x = 1.0; // Max is 0.5, so should threshold smoother->sendCommandMsg(cmd); // Process velocity smoothing and send updated odometry based on commands @@ -568,10 +571,10 @@ TEST(VelocitySmootherTest, testCommandCallback) smoother->configure(state); smoother->activate(state); - auto pub = smoother->create_publisher("cmd_vel", 1); + auto pub = smoother->create_publisher("cmd_vel", 1); pub->on_activate(); - auto msg = std::make_unique(); - msg->linear.x = 100.0; + auto msg = std::make_unique(); + msg->twist.linear.x = 100.0; pub->publish(std::move(msg)); rclcpp::spin_some(smoother->get_node_base_interface()); diff --git a/tools/underlay.repos b/tools/underlay.repos index 09a6da9a92b..005f11ec3f0 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -34,4 +34,4 @@ repositories: ros-navigation/nav2_minimal_turtlebot_simulation: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - version: f0eeedbc95d9f7cc8a513f7d46a84b3d08a3d395 + version: 091b5ff12436890a54de6325df3573ae110e912b