Skip to content

Commit

Permalink
Revert "Merge pull request #21 from logivations/collision-monitor-add…
Browse files Browse the repository at this point in the history
…-frequency"

This reverts commit 4cf839b, reversing
changes made to 8996e15.
  • Loading branch information
Tony Najjar committed Jun 27, 2023
1 parent de2cc96 commit 5ec9c35
Show file tree
Hide file tree
Showing 2 changed files with 4 additions and 12 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -105,14 +105,12 @@ 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,
double & frequency);
std::string & cmd_vel_out_topic);
/**
* @brief Supporting routine creating and configuring all polygons
* @param base_frame_id Robot base frame ID
Expand Down
12 changes: 3 additions & 9 deletions nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,9 @@ 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, frequency)) {
if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) {
return nav2_util::CallbackReturn::FAILURE;
}

Expand All @@ -73,7 +72,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
for (std::shared_ptr<Polygon> polygon : polygons_) {
if (polygon->getActionType() == PUBLISH) {
timer_ = this->create_wall_timer(
std::chrono::duration<double>{1.0 / frequency},
100ms,
std::bind(&CollisionMonitor::process_without_vel, this));
break;
}
Expand Down Expand Up @@ -188,8 +187,7 @@ void CollisionMonitor::publishVelocity(const Action & robot_action)

bool CollisionMonitor::getParameters(
std::string & cmd_vel_in_topic,
std::string & cmd_vel_out_topic,
double & frequency)
std::string & cmd_vel_out_topic)
{
std::string base_frame_id, odom_frame_id;
tf2::Duration transform_tolerance;
Expand All @@ -204,10 +202,6 @@ 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();
Expand Down

0 comments on commit 5ec9c35

Please sign in to comment.