Add current speed to collision monitor

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2/time.h"
@@ -98,6 +99,11 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @param msg Input cmd_vel message
    */
   void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg);
+  /**
+   * @brief Callback for input odom
+   * @param msg Input odom message
+   */
+  void odomCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg);
   /**
    * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
    * quit to publish 0-velocity.
@@ -107,13 +113,15 @@ class CollisionMonitor : public nav2_util::LifecycleNode
 
   /**
    * @brief Supporting routine obtaining all ROS-parameters
+   * @param odom_in_topic
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
    * is required.
    * @param state_topic topic name for publishing collision monitor state
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
+    std::string & odom_in_topic_,
     std::string & cmd_vel_in_topic,
     std::string & cmd_vel_out_topic,
     std::string & state_topic);
@@ -201,12 +209,19 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /// @brief Polygons array
   std::vector<std::shared_ptr<Polygon>> polygons_;
 
+  /// @brief Last twist from odometry
+  geometry_msgs::msg::Twist last_odom_msg_;
+
   /// @brief Data sources array
   std::vector<std::shared_ptr<Source>> sources_;
 
   // Input/output speed controls
-  /// @beirf Input cmd_vel subscriber
+  /// @brief Input cmd_vel subscriber
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
+
+  /// @brief Input odom subscriber
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_in_sub_;
+
   /// @brief Output cmd_vel publisher
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
 

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -54,18 +54,22 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
   tf_buffer_->setCreateTimerInterface(timer_interface);
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
+  std::string odom_in_topic;
   std::string cmd_vel_in_topic;
   std::string cmd_vel_out_topic;
   std::string state_topic;
 
   // Obtaining ROS parameters
-  if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
+  if (!getParameters(odom_in_topic, cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
   cmd_vel_in_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
     cmd_vel_in_topic, 1,
     std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
+  odom_in_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    odom_in_topic, 1,
+    std::bind(&CollisionMonitor::odomCallback, this, std::placeholders::_1));
   cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
     cmd_vel_out_topic, 1);
 
@@ -159,6 +163,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
   cmd_vel_in_sub_.reset();
+  odom_in_sub_.reset();
   cmd_vel_out_pub_.reset();
   state_pub_.reset();
   collision_points_marker_pub_.reset();
@@ -191,6 +196,11 @@ void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPt
   process({msg->linear.x, msg->linear.y, msg->angular.z});
 }
 
+void CollisionMonitor::odomCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
+{
+  last_odom_msg_ = msg->twist.twist;
+}
+
 void CollisionMonitor::publishVelocity(const Action & robot_action)
 {
   if (robot_action.req_vel.isZero()) {
@@ -215,6 +225,7 @@ void CollisionMonitor::publishVelocity(const Action & robot_action)
 }
 
 bool CollisionMonitor::getParameters(
+  std::string & odom_in_topic,
   std::string & cmd_vel_in_topic,
   std::string & cmd_vel_out_topic,
   std::string & state_topic)
@@ -228,6 +239,9 @@ bool CollisionMonitor::getParameters(
   nav2_util::declare_parameter_if_not_declared(
     node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw"));
   cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "odom_in_topic", rclcpp::ParameterValue("odom"));
+  odom_in_topic = get_parameter("odom_in_topic").as_string();
   nav2_util::declare_parameter_if_not_declared(
     node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel"));
   cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string();
@@ -462,7 +476,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
 
     // Update polygon coordinates
-    polygon->updatePolygon(cmd_vel_in);
+    polygon->updatePolygon({last_odom_msg_.linear.x, last_odom_msg_.linear.y, last_odom_msg_.angular.z});
 
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {