Remove unstamped twist subscribers + parameters (#1151)

christophfroehlich · web-flow · commit 64adf6701786 · 2024-06-05T19:14:54.000+01:00
diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml
@@ -6,7 +6,6 @@ test_ackermann_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
 
diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml
@@ -5,7 +5,6 @@ test_ackermann_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
     front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint]
     rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp
@@ -280,7 +280,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test
   bool open_loop_ = false;
   unsigned int velocity_rolling_window_size_ = 10;
   bool position_feedback_ = false;
-  bool use_stamped_vel_ = true;
   std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
   std::vector<std::string> front_wheels_names_ = {
     "front_right_steering_joint", "front_left_steering_joint"};
diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml
@@ -6,7 +6,6 @@ test_bicycle_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [rear_wheel_joint]
     front_wheels_names: [steering_axis_joint]
 
diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml
@@ -5,7 +5,6 @@ test_bicycle_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [pid_controller/rear_wheel_joint]
     front_wheels_names: [pid_controller/steering_axis_joint]
     rear_wheels_state_names: [rear_wheel_joint]
diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp
@@ -251,7 +251,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test
   bool open_loop_ = false;
   unsigned int velocity_rolling_window_size_ = 10;
   bool position_feedback_ = false;
-  bool use_stamped_vel_ = true;
   std::vector<std::string> rear_wheels_names_ = {"rear_wheel_joint"};
   std::vector<std::string> front_wheels_names_ = {"steering_axis_joint"};
   std::vector<std::string> joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]};
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp
@@ -98,7 +98,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   // Command subscribers and Controller State publisher
   rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
   rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_ackermann_ = nullptr;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr ref_subscriber_unstamped_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_;
   rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
 
@@ -143,7 +142,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   // callback for topic interface
   STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback(
     const std::shared_ptr<ControllerTwistReferenceMsg> msg);
-  void reference_callback_unstamped(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
 };
 
 }  // namespace steering_controllers_library
diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -114,22 +114,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
 
   // Reference Subscriber
   ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);
-  if (params_.use_stamped_vel)
-  {
-    RCLCPP_WARN(
-      get_node()->get_logger(),
-      "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated.");
-    ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
-      "~/reference", subscribers_qos,
-      std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));
-  }
-  else
-  {
-    ref_subscriber_unstamped_ = get_node()->create_subscription<geometry_msgs::msg::Twist>(
-      "~/reference_unstamped", subscribers_qos,
-      std::bind(
-        &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1));
-  }
+  ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
+    "~/reference", subscribers_qos,
+    std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));
 
   std::shared_ptr<ControllerTwistReferenceMsg> msg =
     std::make_shared<ControllerTwistReferenceMsg>();
@@ -244,42 +231,6 @@ void SteeringControllersLibrary::reference_callback(
   }
 }
 
-void SteeringControllersLibrary::reference_callback_unstamped(
-  const std::shared_ptr<geometry_msgs::msg::Twist> msg)
-{
-  RCLCPP_WARN(
-    get_node()->get_logger(),
-    "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle "
-    "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the "
-    "future.");
-  auto twist_stamped = *(input_ref_.readFromNonRT());
-  twist_stamped->header.stamp = get_node()->now();
-  // if no timestamp provided use current time for command timestamp
-  if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u)
-  {
-    RCLCPP_WARN(
-      get_node()->get_logger(),
-      "Timestamp in header is missing, using current time as command timestamp.");
-    twist_stamped->header.stamp = get_node()->now();
-  }
-
-  const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp;
-
-  if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
-  {
-    twist_stamped->twist = *msg;
-  }
-  else
-  {
-    RCLCPP_ERROR(
-      get_node()->get_logger(),
-      "Received message has timestamp %.10f older for %.10f which is more then allowed timeout "
-      "(%.4f).",
-      rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(),
-      ref_timeout_.seconds());
-  }
-}
-
 controller_interface::InterfaceConfiguration
 SteeringControllersLibrary::command_interface_configuration() const
 {
diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml
@@ -112,11 +112,3 @@ steering_controllers_library:
     position_feedback is true then ``HW_IF_POSITION`` is taken as interface type",
     read_only: false,
   }
-
-  use_stamped_vel: {
-    type: bool,
-    default_value: false,
-    description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if
-    use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type",
-    read_only: false,
-  }
diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml
@@ -6,7 +6,6 @@ test_steering_controllers_library:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
 
diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp
@@ -301,7 +301,6 @@ class SteeringControllersLibraryFixture : public ::testing::Test
   bool open_loop_ = false;
   unsigned int velocity_rolling_window_size_ = 10;
   bool position_feedback_ = false;
-  bool use_stamped_vel_ = true;
   std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
   std::vector<std::string> front_wheels_names_ = {
     "front_right_steering_joint", "front_left_steering_joint"};
diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml
@@ -36,7 +36,6 @@ tricycle_controller:
 
     # cmd_vel input
     cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
-    use_stamped_vel: false # Set to True if using TwistStamped.
 
     # Debug
     publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.
diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
@@ -140,8 +140,6 @@ class TricycleController : public controller_interface::ControllerInterface
 
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
-    velocity_command_unstamped_subscriber_ = nullptr;
 
   realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
 
diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp
@@ -239,7 +239,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
   cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout};
   params_.publish_ackermann_command =
     get_node()->get_parameter("publish_ackermann_command").as_bool();
-  params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool();
 
   try
   {
@@ -291,52 +290,25 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
   }
 
   // initialize command subscriber
-  if (params_.use_stamped_vel)
-  {
-    velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
-      DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
-      [this](const std::shared_ptr<TwistStamped> msg) -> void
+  velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
+    DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
+    [this](const std::shared_ptr<TwistStamped> msg) -> void
+    {
+      if (!subscriber_is_active_)
       {
-        if (!subscriber_is_active_)
-        {
-          RCLCPP_WARN(
-            get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
-          return;
-        }
-        if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
-        {
-          RCLCPP_WARN_ONCE(
-            get_node()->get_logger(),
-            "Received TwistStamped with zero timestamp, setting it to current "
-            "time, this message will only be shown once");
-          msg->header.stamp = get_node()->get_clock()->now();
-        }
-        received_velocity_msg_ptr_.set(std::move(msg));
-      });
-  }
-  else
-  {
-    RCLCPP_WARN(
-      get_node()->get_logger(),
-      "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated.");
-    velocity_command_unstamped_subscriber_ = get_node()->create_subscription<Twist>(
-      DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
-      [this](const std::shared_ptr<Twist> msg) -> void
+        RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
+        return;
+      }
+      if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
       {
-        if (!subscriber_is_active_)
-        {
-          RCLCPP_WARN(
-            get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
-          return;
-        }
-
-        // Write fake header in the stored stamped command
-        std::shared_ptr<TwistStamped> twist_stamped;
-        received_velocity_msg_ptr_.get(twist_stamped);
-        twist_stamped->twist = *msg;
-        twist_stamped->header.stamp = get_node()->get_clock()->now();
-      });
-  }
+        RCLCPP_WARN_ONCE(
+          get_node()->get_logger(),
+          "Received TwistStamped with zero timestamp, setting it to current "
+          "time, this message will only be shown once");
+        msg->header.stamp = get_node()->get_clock()->now();
+      }
+      received_velocity_msg_ptr_.set(std::move(msg));
+    });
 
   // initialize odometry publisher and message
   odometry_publisher_ = get_node()->create_publisher<nav_msgs::msg::Odometry>(
@@ -460,7 +432,6 @@ bool TricycleController::reset()
 
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
-  velocity_command_unstamped_subscriber_.reset();
 
   received_velocity_msg_ptr_.set(nullptr);
   is_halted = false;
diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml
@@ -84,11 +84,6 @@ tricycle_controller:
       gt<>: [0]
     }
   }
-  use_stamped_vel: {
-    type: bool,
-    default_value: true,
-    description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.",
-  }
   traction:
     # "The positive limit will be applied to both directions. Setting different limits for positive "
     # "and negative directions is not supported. Actuators are "
diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp
@@ -265,7 +265,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test
   bool open_loop_ = false;
   unsigned int velocity_rolling_window_size_ = 10;
   bool position_feedback_ = false;
-  bool use_stamped_vel_ = true;
   std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
   std::vector<std::string> front_wheels_names_ = {"steering_axis_joint"};
   std::vector<std::string> joint_names_ = {
diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml
@@ -6,7 +6,6 @@ test_tricycle_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     front_wheels_names: [steering_axis_joint]
 
diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml
@@ -5,7 +5,6 @@ test_tricycle_steering_controller:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
-    use_stamped_vel: true
     rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint]
     front_wheels_names: [pid_controller/steering_axis_joint]
     rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint]
