diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 4622ad1c2d2..f3ca1cc8892 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -105,12 +105,14 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Supporting routine obtaining all ROS-parameters * @param cmd_vel_in_topic Output name of cmd_vel_in topic * @param cmd_vel_out_topic Output name of cmd_vel_out topic + * @param frequency Frequency of the loop running process_without_vel * is required. * @return True if all parameters were obtained or false in failure case */ bool getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic); + std::string & cmd_vel_out_topic, + double & frequency); /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 6e5f0b728de..0558bb0eb24 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -57,9 +57,10 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string cmd_vel_in_topic; std::string cmd_vel_out_topic; + double frequency; // Obtaining ROS parameters - if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { + if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, frequency)) { return nav2_util::CallbackReturn::FAILURE; } @@ -72,7 +73,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) for (std::shared_ptr polygon : polygons_) { if (polygon->getActionType() == PUBLISH) { timer_ = this->create_wall_timer( - 100ms, + std::chrono::duration{1.0 / frequency}, std::bind(&CollisionMonitor::process_without_vel, this)); break; } @@ -187,7 +188,8 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) bool CollisionMonitor::getParameters( std::string & cmd_vel_in_topic, - std::string & cmd_vel_out_topic) + std::string & cmd_vel_out_topic, + double & frequency) { std::string base_frame_id, odom_frame_id; tf2::Duration transform_tolerance; @@ -202,6 +204,10 @@ bool CollisionMonitor::getParameters( node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel")); cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "frequency", rclcpp::ParameterValue(10.0)); + frequency = get_parameter("frequency").as_double(); + nav2_util::declare_parameter_if_not_declared( node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); base_frame_id = get_parameter("base_frame_id").as_string();