feat(external_cmd_converter): replace polling takeData function with the callback function

vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp

@@ -15,6 +15,8 @@
 #ifndef EXTERNAL_CMD_CONVERTER__NODE_HPP_
 #define EXTERNAL_CMD_CONVERTER__NODE_HPP_
 
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
+
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <raw_vehicle_cmd_converter/accel_map.hpp>
 #include <raw_vehicle_cmd_converter/brake_map.hpp>
@@ -33,12 +35,12 @@
 namespace external_cmd_converter
 {
 using GearCommand = autoware_vehicle_msgs::msg::GearCommand;
-using Control = autoware_control_msgs::msg::Control;
+using autoware_control_msgs::msg::Control;
 using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped;
-using Odometry = nav_msgs::msg::Odometry;
+using nav_msgs::msg::Odometry;
 using raw_vehicle_cmd_converter::AccelMap;
 using raw_vehicle_cmd_converter::BrakeMap;
-using Odometry = nav_msgs::msg::Odometry;
+using tier4_control_msgs::msg::GateMode;
 
 class ExternalCmdConverterNode : public rclcpp::Node
 {
@@ -52,28 +54,32 @@ class ExternalCmdConverterNode : public rclcpp::Node
     pub_current_cmd_;
 
   // Subscriber
-  rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
   rclcpp::Subscription<tier4_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
     sub_control_cmd_;
-  rclcpp::Subscription<GearCommand>::SharedPtr sub_shift_cmd_;
   rclcpp::Subscription<tier4_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
   rclcpp::Subscription<tier4_external_api_msgs::msg::Heartbeat>::SharedPtr
     sub_emergency_stop_heartbeat_;
 
-  void onVelocity(const Odometry::ConstSharedPtr msg);
-  void onExternalCmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr);
-  void onGearCommand(const GearCommand::ConstSharedPtr msg);
-  void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
-  void onEmergencyStopHeartbeat(const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg);
+  // Polling Subscriber
+  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> velocity_sub_{this, "in/odometry"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<GearCommand> shift_cmd_sub_{
+    this, "in/shift_cmd"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<GateMode> gate_mode_sub_{
+    this, "in/current_gate_mode"};
+
+  void on_external_cmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr);
+  void on_emergency_stop_heartbeat(
+    const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg);
+
+  Odometry::ConstSharedPtr current_velocity_ptr_{nullptr};  // [m/s]
+  GearCommand::ConstSharedPtr current_shift_cmd_{nullptr};
+  GateMode::ConstSharedPtr current_gate_mode_{nullptr};
 
-  std::shared_ptr<double> current_velocity_ptr_;  // [m/s]
   std::shared_ptr<rclcpp::Time> latest_emergency_stop_heartbeat_received_time_;
   std::shared_ptr<rclcpp::Time> latest_cmd_received_time_;
-  GearCommand::ConstSharedPtr current_shift_cmd_;
-  tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_;
 
   // Timer
-  void onTimer();
+  void on_timer();
   rclcpp::TimerBase::SharedPtr rate_check_timer_;
 
   // Parameter
@@ -85,18 +91,18 @@ class ExternalCmdConverterNode : public rclcpp::Node
   // Diagnostics
   diagnostic_updater::Updater updater_{this};
 
-  void checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void checkEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  bool checkEmergencyStopTopicTimeout();
-  bool checkRemoteTopicRate();
+  void check_topic_status(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void check_emergency_stop(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  bool check_emergency_stop_topic_timeout();
+  bool check_remote_topic_rate();
 
   // Algorithm
   AccelMap accel_map_;
   BrakeMap brake_map_;
   bool acc_map_initialized_;
 
-  double calculateAcc(const ExternalControlCommand & cmd, const double vel);
-  double getShiftVelocitySign(const GearCommand & cmd);
+  double calculate_acc(const ExternalControlCommand & cmd, const double vel);
+  double get_shift_velocity_sign(const GearCommand & cmd);
 };
 
 }  // namespace external_cmd_converter

vehicle/external_cmd_converter/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The external_cmd_converter package</description>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
+  <maintainer email="[email protected]">Eiki Nagata</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Takamasa Horibe</author>

vehicle/external_cmd_converter/src/node.cpp

@@ -29,17 +29,11 @@
   pub_cmd_ = create_publisher<Control>("out/control_cmd", rclcpp::QoS{1});
   pub_current_cmd_ =
     create_publisher<ExternalControlCommand>("out/latest_external_control_cmd", rclcpp::QoS{1});
-  sub_velocity_ = create_subscription<Odometry>(
-    "in/odometry", 1, std::bind(&ExternalCmdConverterNode::onVelocity, this, _1));
   sub_control_cmd_ = create_subscription<ExternalControlCommand>(
-    "in/external_control_cmd", 1, std::bind(&ExternalCmdConverterNode::onExternalCmd, this, _1));
-  sub_shift_cmd_ = create_subscription<GearCommand>(
-    "in/shift_cmd", 1, std::bind(&ExternalCmdConverterNode::onGearCommand, this, _1));
-  sub_gate_mode_ = create_subscription<tier4_control_msgs::msg::GateMode>(
-    "in/current_gate_mode", 1, std::bind(&ExternalCmdConverterNode::onGateMode, this, _1));
+    "in/external_control_cmd", 1, std::bind(&ExternalCmdConverterNode::on_external_cmd, this, _1));
   sub_emergency_stop_heartbeat_ = create_subscription<tier4_external_api_msgs::msg::Heartbeat>(
     "in/emergency_stop", 1,
-    std::bind(&ExternalCmdConverterNode::onEmergencyStopHeartbeat, this, _1));
+    std::bind(&ExternalCmdConverterNode::on_emergency_stop_heartbeat, this, _1));
 
   // Parameter
   ref_vel_gain_ = declare_parameter<double>("ref_vel_gain");
@@ -52,7 +46,7 @@
 
   const auto period_ns = rclcpp::Rate(timer_rate).period();
   rate_check_timer_ = rclcpp::create_timer(
-    this, get_clock(), period_ns, std::bind(&ExternalCmdConverterNode::onTimer, this));
+    this, get_clock(), period_ns, std::bind(&ExternalCmdConverterNode::on_timer, this));
 
   // Parameter for accel/brake map
   const std::string csv_path_accel_map = declare_parameter<std::string>("csv_path_accel_map");
@@ -73,35 +67,25 @@
 
   // Diagnostics
   updater_.setHardwareID("external_cmd_converter");
-  updater_.add("remote_control_topic_status", this, &ExternalCmdConverterNode::checkTopicStatus);
+  updater_.add("remote_control_topic_status", this, &ExternalCmdConverterNode::check_topic_status);
 
   // Set default values
   current_shift_cmd_ = std::make_shared<GearCommand>();
 }
 
-void ExternalCmdConverterNode::onTimer()
+void ExternalCmdConverterNode::on_timer()
 {
   updater_.force_update();
 }
 
-void ExternalCmdConverterNode::onVelocity(const Odometry::ConstSharedPtr msg)
-{
-  current_velocity_ptr_ = std::make_shared<double>(msg->twist.twist.linear.x);
-}
-
-void ExternalCmdConverterNode::onGearCommand(const GearCommand::ConstSharedPtr msg)
-{
-  current_shift_cmd_ = msg;
-}
-
-void ExternalCmdConverterNode::onEmergencyStopHeartbeat(
+void ExternalCmdConverterNode::on_emergency_stop_heartbeat(
   [[maybe_unused]] const tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg)
 {
   latest_emergency_stop_heartbeat_received_time_ = std::make_shared<rclcpp::Time>(this->now());
   updater_.force_update();
 }
 
-void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr)
+void ExternalCmdConverterNode::on_external_cmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr)
 {
   // Echo back received command
   {
@@ -113,14 +97,19 @@
   // Save received time for rate check
   latest_cmd_received_time_ = std::make_shared<rclcpp::Time>(this->now());
 
+  // take data from subscribers
+  current_velocity_ptr_ = velocity_sub_.takeData();
+  current_shift_cmd_ = shift_cmd_sub_.takeData();
+
   // Wait for input data
   if (!current_velocity_ptr_ || !acc_map_initialized_) {
     return;
   }
 
   // Calculate reference velocity and acceleration
-  const double sign = getShiftVelocitySign(*current_shift_cmd_);
-  const double ref_acceleration = calculateAcc(*cmd_ptr, std::fabs(*current_velocity_ptr_));
+  const double sign = get_shift_velocity_sign(*current_shift_cmd_);
+  const double ref_acceleration =
+    calculate_acc(*cmd_ptr, std::fabs(current_velocity_ptr_->twist.twist.linear.x));
 
   if (ref_acceleration > 0.0 && sign == 0.0) {
     RCLCPP_WARN_THROTTLE(
@@ -129,7 +118,8 @@
       ref_acceleration, current_shift_cmd_->command);
   }
 
-  double ref_velocity = *current_velocity_ptr_ + ref_acceleration * ref_vel_gain_ * sign;
+  double ref_velocity =
+    current_velocity_ptr_->twist.twist.linear.x + ref_acceleration * ref_vel_gain_ * sign;
   if (current_shift_cmd_->command == GearCommand::REVERSE) {
     ref_velocity = std::min(0.0, ref_velocity);
   } else if (
@@ -144,15 +134,16 @@
   // Publish ControlCommand
   autoware_control_msgs::msg::Control output;
   output.stamp = cmd_ptr->stamp;
-  output.lateral.steering_tire_angle = cmd_ptr->control.steering_angle;
-  output.lateral.steering_tire_rotation_rate = cmd_ptr->control.steering_angle_velocity;
-  output.longitudinal.velocity = ref_velocity;
-  output.longitudinal.acceleration = ref_acceleration;
+  output.lateral.steering_tire_angle = static_cast<float>(cmd_ptr->control.steering_angle);
+  output.lateral.steering_tire_rotation_rate =
+    static_cast<float>(cmd_ptr->control.steering_angle_velocity);
+  output.longitudinal.velocity = static_cast<float>(ref_velocity);
+  output.longitudinal.acceleration = static_cast<float>(ref_acceleration);
 
   pub_cmd_->publish(output);
 }
 
-double ExternalCmdConverterNode::calculateAcc(const ExternalControlCommand & cmd, const double vel)
+double ExternalCmdConverterNode::calculate_acc(const ExternalControlCommand & cmd, const double vel)
 {
   const double desired_throttle = cmd.control.throttle;
   const double desired_brake = cmd.control.brake;
@@ -168,7 +159,7 @@
   return ref_acceleration;
 }
 
-double ExternalCmdConverterNode::getShiftVelocitySign(const GearCommand & cmd)
+double ExternalCmdConverterNode::get_shift_velocity_sign(const GearCommand & cmd)
 {
   if (cmd.command == GearCommand::DRIVE) {
     return 1.0;
@@ -183,15 +174,15 @@
   return 0.0;
 }
 
-void ExternalCmdConverterNode::checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
+void ExternalCmdConverterNode::check_topic_status(
+  diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
   using diagnostic_msgs::msg::DiagnosticStatus;
-
   DiagnosticStatus status;
-  if (!checkEmergencyStopTopicTimeout()) {
+  if (!check_emergency_stop_topic_timeout()) {
     status.level = DiagnosticStatus::ERROR;
     status.message = "emergency stop topic is timeout";
-  } else if (!checkRemoteTopicRate()) {
+  } else if (!check_remote_topic_rate()) {
     status.level = DiagnosticStatus::ERROR;
     status.message = "low topic rate for remote vehicle_cmd";
   } else {
@@ -202,42 +193,26 @@
   stat.summary(status.level, status.message);
 }
 
-void ExternalCmdConverterNode::onGateMode(
-  const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
-{
-  current_gate_mode_ = msg;
-}
-
-bool ExternalCmdConverterNode::checkEmergencyStopTopicTimeout()
+bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout()
 {
   if (!latest_emergency_stop_heartbeat_received_time_) {
-    if (wait_for_first_topic_) {
-      return true;
-    } else {
-      return false;
-    }
+    return wait_for_first_topic_;
   }
 
   const auto duration = (this->now() - *latest_emergency_stop_heartbeat_received_time_);
-  if (duration.seconds() > emergency_stop_timeout_) {
-    return false;
-  }
-
-  return true;
+  return duration.seconds() <= emergency_stop_timeout_;
 }
 
-bool ExternalCmdConverterNode::checkRemoteTopicRate()
+bool ExternalCmdConverterNode::check_remote_topic_rate()
 {
+  current_gate_mode_ = gate_mode_sub_.takeData();
+
   if (!current_gate_mode_) {
     return true;
   }
 
   if (!latest_cmd_received_time_) {
-    if (wait_for_first_topic_) {
-      return true;
-    } else {
-      return false;
-    }
+    return wait_for_first_topic_;
   }
 
   if (current_gate_mode_->data == tier4_control_msgs::msg::GateMode::EXTERNAL) {