move odom sub thresholds to the controller server for no change in behavior...

move odom sub thresholds to the controller server for no change in behavior but prior change in authority (#1528)

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -167,6 +167,31 @@ protected:
    */
   bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
 
+  /**
+   * @brief get the thresholded velocity
+   * @param velocity The current velocity from odometry
+   * @param threshold The minimum velocity to return non-zero
+   * @return double velocity value
+   */
+  double getThresholdedVelocity(double velocity, double threshold)
+  {
+    return (std::abs(velocity) > threshold) ? velocity : 0.0;
+  }
+
+  /**
+   * @brief get the thresholded Twist
+   * @param Twist The current Twist from odometry
+   * @return Twist Twist after thresholds applied
+   */
+  nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist)
+  {
+    nav_2d_msgs::msg::Twist2D twist_thresh;
+    twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_);
+    twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_);
+    twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_);
+    return twist_thresh;
+  }
+
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
@@ -184,6 +209,9 @@ protected:
   std::unique_ptr<ProgressChecker> progress_checker_;
 
   double controller_frequency_;
+  double min_x_velocity_threshold_;
+  double min_y_velocity_threshold_;
+  double min_theta_velocity_threshold_;
 
   // Whether we've published the single controller warning yet
   bool single_controller_warning_given_{false};

nav2_controller/src/nav2_controller.cpp

@@ -41,6 +41,10 @@ ControllerServer::ControllerServer()
   declare_parameter("controller_plugin_ids", default_id);
   declare_parameter("controller_plugin_types", default_type);
 
+  declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
+  declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
+  declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()}, "local_costmap");
@@ -62,6 +66,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("controller_plugin_ids", controller_ids_);
   get_parameter("controller_plugin_types", controller_types_);
   get_parameter("controller_frequency", controller_frequency_);
+
+  get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
+  get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
+  get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
   costmap_ros_->on_configure(state);
@@ -306,10 +314,12 @@ void ControllerServer::computeAndPublishVelocity()
 
   progress_checker_->check(pose);
 
+  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+
   auto cmd_vel_2d =
     controllers_[current_controller_]->computeVelocityCommands(
     pose,
-    nav_2d_utils::twist2Dto3D(odom_sub_->getTwist()));
+    nav_2d_utils::twist2Dto3D(twist));
 
   RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
   publishVelocity(cmd_vel_2d);
@@ -360,7 +370,8 @@ bool ControllerServer::isGoalReached()
     return false;
   }
 
-  geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
+  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
   return controllers_[current_controller_]->isGoalReached(pose, velocity);
 }
 

nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp

@@ -72,20 +72,6 @@ public:
       odom_topic,
       rclcpp::SystemDefaultsQoS(),
       std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1));
-
-    nav2_util::declare_parameter_if_not_declared(
-      nh,
-      "min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
-    nav2_util::declare_parameter_if_not_declared(
-      nh,
-      "min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
-    nav2_util::declare_parameter_if_not_declared(
-      nh,
-      "min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
-
-    nh->get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
-    nh->get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
-    nh->get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   }
 
   inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;}
@@ -97,26 +83,14 @@ protected:
     // ROS_INFO_ONCE("odom received!");
     std::lock_guard<std::mutex> lock(odom_mutex_);
     odom_vel_.header = msg->header;
-    odom_vel_.velocity.x =
-      thresholded_velocity(msg->twist.twist.linear.x, min_x_velocity_threshold_);
-    odom_vel_.velocity.y =
-      thresholded_velocity(msg->twist.twist.linear.y, min_y_velocity_threshold_);
-    odom_vel_.velocity.theta =
-      thresholded_velocity(msg->twist.twist.angular.z, min_theta_velocity_threshold_);
-  }
-
-  double thresholded_velocity(double velocity, double threshold)
-  {
-    return (std::abs(velocity) > threshold) ? velocity : 0.0;
+    odom_vel_.velocity.x = msg->twist.twist.linear.x;
+    odom_vel_.velocity.y = msg->twist.twist.linear.y;
+    odom_vel_.velocity.theta = msg->twist.twist.angular.z;
   }
 
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
   nav_2d_msgs::msg::Twist2DStamped odom_vel_;
   std::mutex odom_mutex_;
-
-  double min_x_velocity_threshold_;
-  double min_y_velocity_threshold_;
-  double min_theta_velocity_threshold_;
 };
 
 }  // namespace nav_2d_utils