diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8544429466..2cc7b19c73 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) ### Header ### @@ -35,6 +36,7 @@ set(dependencies tf2_geometry_msgs nav2_util nav2_costmap_2d + nav2_msgs ) set(executable_name collision_monitor) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 97423dc411..d5843dd606 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -86,6 +86,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon maximum points to enter inside polygon causing no action * @return Maximum points to enter to current polygon and take no action @@ -161,6 +166,14 @@ class Polygon * @param point Given point to check * @return True if given point is inside polygon, otherwise false */ + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + bool isPointInside(const Point & point) const; // ----- Variables ----- @@ -169,7 +182,9 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; - + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Basic parameters /// @brief Name of polygon std::string polygon_name_; @@ -185,6 +200,8 @@ class Polygon double simulation_time_step_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Whether polygon is enabled + bool enabled_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc71..30d58d53bc 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -74,7 +74,19 @@ class Source const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +103,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of data source @@ -116,6 +137,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 1b0c36529e..a84d74c659 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -22,6 +22,7 @@ collision_monitor: max_points: 3 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -38,12 +40,15 @@ collision_monitor: simulation_time_step: 0.1 max_points: 5 visualize: False + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "/scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 57125501cf..4cc6ef3600 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -355,7 +355,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } } // By default - there is no action @@ -364,6 +366,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) std::shared_ptr action_polygon; for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -481,7 +486,9 @@ void CollisionMonitor::printAction( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index d4d2ea1adf..7e84d8890a 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -48,6 +48,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 2469fff71a..272582d56e 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -44,6 +44,7 @@ Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -82,6 +83,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -109,6 +114,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMaxPoints() const { return max_points_; @@ -244,6 +254,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3)); max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int(); @@ -350,6 +364,26 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index ae15bd488c..8f75049096 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -48,6 +48,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 50f670cb13..8ac45904a2 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -45,6 +45,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 6ad41224b5..41b6f647ee 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -46,6 +46,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -57,6 +68,10 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); } bool Source::sourceValid( @@ -78,4 +93,29 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index c05b2b05e7..503c7559a7 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -22,7 +22,7 @@ #include #include #include - +#include "nav2_msgs/msg/collision_monitor_state.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" #include "sensor_msgs/msg/laser_scan.hpp" @@ -46,6 +46,7 @@ static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; +static const char STATE_TOPIC[]{"collision_monitor_state"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; @@ -73,6 +74,15 @@ enum SourceType RANGE = 3 }; +enum ActionType +{ + DO_NOTHING = 0, + STOP = 1, + SLOWDOWN = 2, + APPROACH = 3, + LIMIT = 4, +}; + class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor { public: @@ -147,9 +157,14 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); + bool waitFuture( + rclcpp::Client::SharedFuture, + const std::chrono::nanoseconds & timeout); + bool waitActionState(const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); + void actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg); // CollisionMonitor node std::shared_ptr cm_; @@ -167,6 +182,13 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr cmd_vel_out_sub_; geometry_msgs::msg::Twist::SharedPtr cmd_vel_out_; + + // CollisionMonitor Action state + rclcpp::Subscription::SharedPtr action_state_sub_; + nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + + // Service client for setting CollisionMonitor parameters + rclcpp::Client::SharedPtr parameters_client_; }; // Tester Tester::Tester() @@ -188,6 +210,14 @@ Tester::Tester() cmd_vel_out_sub_ = cm_->create_subscription( CMD_VEL_OUT_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::cmdVelOutCallback, this, std::placeholders::_1)); + + action_state_sub_ = cm_->create_subscription( + STATE_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + parameters_client_ = + cm_->create_client( + std::string( + cm_->get_name()) + "/set_parameters"); } Tester::~Tester() @@ -279,6 +309,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "unknown")); } + cm_->declare_parameter( + polygon_name + ".enabled", rclcpp::ParameterValue(true)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".enabled", true)); + cm_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(at)); cm_->set_parameter( @@ -539,11 +574,45 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitFuture( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (action_state_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) { cmd_vel_out_ = msg; } +void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg) +{ + action_state_ = msg; +} + TEST_F(Tester, testProcessStopSlowdown) { rclcpp::Time curr_time = cm_->now(); @@ -850,6 +919,116 @@ TEST_F(Tester, testCeasePublishZeroVel) cm_->stop(); } +TEST_F(Tester, testPolygonNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when polygon is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable polygon by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = "Stop.enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when polygon is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when source is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + // ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable source by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = std::string(SCAN_NAME) + ".enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when source is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessNonActive) { rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index b2cbccec48..b9895f07a4 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(action_msgs REQUIRED) nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} + "msg/CollisionMonitorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" "msg/CostmapFilterInfo.msg" diff --git a/nav2_msgs/msg/CollisionMonitorState.msg b/nav2_msgs/msg/CollisionMonitorState.msg new file mode 100644 index 0000000000..e470df57eb --- /dev/null +++ b/nav2_msgs/msg/CollisionMonitorState.msg @@ -0,0 +1,9 @@ +# Action type for robot in Collision Monitor +uint8 DO_NOTHING=0 # No action +uint8 STOP=1 # Stop the robot +uint8 SLOWDOWN=2 # Slowdown in percentage from current operating speed +uint8 APPROACH=3 # Keep constant time interval before collision +uint8 action_type + +# Name of triggered polygon +string polygon_name \ No newline at end of file