merge branch main into autoware_msg

Signed-off-by: liu cui <cynthia.liu@autocore.ai>

Author: cyn-liu

.pre-commit-config.yaml

@@ -3,7 +3,7 @@ ci:
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v4.4.0
     hooks:
       - id: check-json
       - id: check-merge-conflict
@@ -18,18 +18,18 @@ repos:
         args: [--markdown-linebreak-ext=md]
 
   - repo: https://github.com/igorshubovych/markdownlint-cli
-    rev: v0.39.0
+    rev: v0.33.0
     hooks:
       - id: markdownlint
         args: [-c, .markdownlint.yaml, --fix]
 
   - repo: https://github.com/pre-commit/mirrors-prettier
-    rev: v4.0.0-alpha.8
+    rev: v3.0.0-alpha.6
     hooks:
       - id: prettier
 
   - repo: https://github.com/adrienverge/yamllint
-    rev: v1.35.1
+    rev: v1.30.0
     hooks:
       - id: yamllint
 
@@ -44,29 +44,29 @@ repos:
       - id: sort-package-xml
 
   - repo: https://github.com/shellcheck-py/shellcheck-py
-    rev: v0.10.0.1
+    rev: v0.9.0.2
     hooks:
       - id: shellcheck
 
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.8.0-1
+    rev: v3.6.0-2
     hooks:
       - id: shfmt
         args: [-w, -s, -i=4]
 
   - repo: https://github.com/pycqa/isort
-    rev: 5.13.2
+    rev: 5.12.0
     hooks:
       - id: isort
 
   - repo: https://github.com/psf/black
-    rev: 24.4.0
+    rev: 23.3.0
     hooks:
       - id: black
         args: [--line-length=100]
 
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.3
+    rev: v16.0.0
     hooks:
       - id: clang-format
         types_or: [c++, c, cuda]

build_depends.repos

@@ -3,6 +3,10 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware_msgs.git
     version: main
+  core/autoware_internal_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_internal_msgs.git
+    version: main
   core/common:
     type: git
     url: https://github.com/autowarefoundation/autoware_common.git

pacmod_interface/include/pacmod_interface/pacmod_interface.hpp

@@ -45,10 +45,6 @@
 #include <tier4_vehicle_msgs/msg/steering_wheel_status_stamped.hpp>
 #include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>
 
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
-
 #include <algorithm>
 #include <cmath>
 #include <cstdlib>
@@ -66,12 +62,6 @@ class PacmodInterface : public rclcpp::Node
   PacmodInterface();
 
 private:
-  typedef message_filters::sync_policies::ApproximateTime<
-    pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::WheelSpeedRpt,
-    pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::SystemRptFloat,
-    pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::GlobalRpt>
-    PacmodFeedbacksSyncPolicy;
-
   /* subscribers */
   // From Autoware
   rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr
@@ -87,16 +77,13 @@ class PacmodInterface : public rclcpp::Node
   // From Pacmod
   rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr rear_door_rpt_sub_;
 
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>
-    steer_wheel_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>
-    wheel_speed_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> accel_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> brake_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> shift_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> turn_rpt_sub_;
-  std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>> global_rpt_sub_;
-  std::unique_ptr<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>> pacmod_feedbacks_sync_;
+  rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr steer_wheel_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::WheelSpeedRpt>::SharedPtr wheel_speed_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr accel_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr brake_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr shift_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr turn_rpt_sub_;
+  rclcpp::Subscription<pacmod3_msgs::msg::GlobalRpt>::SharedPtr global_rpt_sub_;
 
   /* publishers */
   // To Pacmod
@@ -129,6 +116,14 @@ class PacmodInterface : public rclcpp::Node
   /* ros param */
   std::string base_frame_id_;
   int command_timeout_ms_;  // vehicle_cmd timeout [ms]
+  bool is_steer_wheel_rpt_received_ = false;
+  bool is_wheel_speed_rpt_received_ = false;
+  bool is_accel_rpt_received_ = false;
+  bool is_brake_rpt_received_ = false;
+  bool is_shift_rpt_received_ = false;
+  bool is_turn_rpt_received_ = false;
+  bool is_global_rpt_received_ = false;
+
   bool is_pacmod_rpt_received_ = false;
   bool is_pacmod_enabled_ = false;
   bool is_clear_override_needed_ = false;
@@ -206,17 +201,19 @@ class PacmodInterface : public rclcpp::Node
     const autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg);
   void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg);
   void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt);
-  void callbackPacmodRpt(
-    const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
-    const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
-    const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
-    const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
-    const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_cmd_rpt,
-    const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
-    const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);
+  void callbackSteerWheelRpt(
+    const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt);
+  void callbackWheelSpeedRpt(
+    const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt);
+  void callbackAccelRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt);
+  void callbackBrakeRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt);
+  void callbackGearRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt);
+  void callbackTurnRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt);
+  void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);
 
   /*  functions */
   void publishCommands();
+  void publishVehicleStatus();
   double calculateVehicleVelocity(
     const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt,
     const pacmod3_msgs::msg::SystemRptInt & shift_rpt);

pacmod_interface/launch/pacmod_steer_test.launch.xml

@@ -4,6 +4,7 @@
 
   <arg name="vehicle_model" description="vehicle model name"/>
   <arg name="pacmod_param_path" default="$(find-pkg-share pacmod_interface)/config/pacmod_steer_test.param.yaml"/>
+  <arg name="socketcan_device" default="canVehicle"/>
 
   <!-- vehicle info param -->
   <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
@@ -17,6 +18,9 @@
 
   <!-- pacmod3 -->
   <group>
-    <include file="$(find-pkg-share $(var vehicle_model)_description)/launch/pacmod3.launch.py"/>
+    <include file="$(find-pkg-share pacmod3)/launch/pacmod3.launch.xml">
+      <arg name="use_socketcan" value="true"/>
+      <arg name="socketcan_device" value="$(var socketcan_device)"/>
+    </include>
   </group>
 </launch>

pacmod_interface/src/pacmod_interface/pacmod_interface.cpp

@@ -96,32 +96,20 @@ PacmodInterface::PacmodInterface()
   rear_door_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
     "/pacmod/rear_pass_door_rpt", 1, std::bind(&PacmodInterface::callbackRearDoor, this, _1));
 
-  steer_wheel_rpt_sub_ =
-    std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
-      this, "/pacmod/steering_rpt");
-  wheel_speed_rpt_sub_ =
-    std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>(
-      this, "/pacmod/wheel_speed_rpt");
-  accel_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
-    this, "/pacmod/accel_rpt");
-  brake_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
-    this, "/pacmod/brake_rpt");
-  shift_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
-    this, "/pacmod/shift_rpt");
-  turn_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
-    this, "/pacmod/turn_rpt");
-  global_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>>(
-    this, "/pacmod/global_rpt");
-
-  pacmod_feedbacks_sync_ =
-    std::make_unique<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>>(
-      PacmodFeedbacksSyncPolicy(10), *steer_wheel_rpt_sub_, *wheel_speed_rpt_sub_, *accel_rpt_sub_,
-      *brake_rpt_sub_, *shift_rpt_sub_, *turn_rpt_sub_, *global_rpt_sub_);
-
-  pacmod_feedbacks_sync_->registerCallback(std::bind(
-    &PacmodInterface::callbackPacmodRpt, this, std::placeholders::_1, std::placeholders::_2,
-    std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6,
-    std::placeholders::_7));
+  steer_wheel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
+    "/pacmod/steering_rpt", 1, std::bind(&PacmodInterface::callbackSteerWheelRpt, this, _1));
+  wheel_speed_rpt_sub_ = create_subscription<pacmod3_msgs::msg::WheelSpeedRpt>(
+    "/pacmod/wheel_speed_rpt", 1, std::bind(&PacmodInterface::callbackWheelSpeedRpt, this, _1));
+  accel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
+    "/pacmod/accel_rpt", 1, std::bind(&PacmodInterface::callbackAccelRpt, this, _1));
+  brake_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
+    "/pacmod/brake_rpt", 1, std::bind(&PacmodInterface::callbackBrakeRpt, this, _1));
+  shift_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
+    "/pacmod/shift_rpt", 1, std::bind(&PacmodInterface::callbackGearRpt, this, _1));
+  turn_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
+    "/pacmod/turn_rpt", 1, std::bind(&PacmodInterface::callbackTurnRpt, this, _1));
+  global_rpt_sub_ = create_subscription<pacmod3_msgs::msg::GlobalRpt>(
+    "/pacmod/global_rpt", 1, std::bind(&PacmodInterface::callbackGlobalRpt, this, _1));
 
   /* publisher */
   // To pacmod
@@ -241,24 +229,57 @@ void PacmodInterface::callbackRearDoor(
   door_status_pub_->publish(toAutowareDoorStatusMsg(*rear_door_rpt));
 }
 
-void PacmodInterface::callbackPacmodRpt(
-  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
-  const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
-  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
-  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
-  const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr shift_rpt,
-  const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
-  const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
+void PacmodInterface::callbackSteerWheelRpt(
+  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt)
 {
-  is_pacmod_rpt_received_ = true;
+  is_steer_wheel_rpt_received_ = true;
   steer_wheel_rpt_ptr_ = steer_wheel_rpt;
+}
+
+void PacmodInterface::callbackWheelSpeedRpt(
+  const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt)
+{
+  is_wheel_speed_rpt_received_ = true;
   wheel_speed_rpt_ptr_ = wheel_speed_rpt;
+}
+
+void PacmodInterface::callbackAccelRpt(
+  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt)
+{
+  is_accel_rpt_received_ = true;
   accel_rpt_ptr_ = accel_rpt;
+}
+
+void PacmodInterface::callbackBrakeRpt(
+  const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt)
+{
+  is_brake_rpt_received_ = true;
   brake_rpt_ptr_ = brake_rpt;
-  gear_cmd_rpt_ptr_ = shift_rpt;
-  global_rpt_ptr_ = global_rpt;
+}
+
+void PacmodInterface::callbackGearRpt(
+  const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt)
+{
+  is_shift_rpt_received_ = true;
+  gear_cmd_rpt_ptr_ = gear_rpt;
+}
+
+void PacmodInterface::callbackTurnRpt(
+  const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt)
+{
+  is_turn_rpt_received_ = true;
   turn_rpt_ptr_ = turn_rpt;
+}
 
+void PacmodInterface::callbackGlobalRpt(
+  const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
+{
+  is_global_rpt_received_ = true;
+  global_rpt_ptr_ = global_rpt;
+}
+
+void PacmodInterface::publishVehicleStatus()
+{
   is_pacmod_enabled_ =
     steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled;
   RCLCPP_DEBUG(
@@ -293,7 +314,7 @@ void PacmodInterface::callbackPacmodRpt(
     autoware_vehicle_msgs::msg::ControlModeReport control_mode_msg;
     control_mode_msg.stamp = header.stamp;
 
-    if (global_rpt->enabled && is_pacmod_enabled_) {
+    if (global_rpt_ptr_->enabled && is_pacmod_enabled_) {
       control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
     } else {
       control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL;
@@ -344,18 +365,23 @@ void PacmodInterface::callbackPacmodRpt(
   {
     autoware_vehicle_msgs::msg::TurnIndicatorsReport turn_msg;
     turn_msg.stamp = header.stamp;
-    turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt);
+    turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt_ptr_);
     turn_indicators_status_pub_->publish(turn_msg);
 
     autoware_vehicle_msgs::msg::HazardLightsReport hazard_msg;
     hazard_msg.stamp = header.stamp;
-    hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt);
+    hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt_ptr_);
     hazard_lights_status_pub_->publish(hazard_msg);
   }
 }
 
 void PacmodInterface::publishCommands()
 {
+  is_pacmod_rpt_received_ = is_steer_wheel_rpt_received_ && is_wheel_speed_rpt_received_ &&
+                            is_accel_rpt_received_ && is_brake_rpt_received_ &&
+                            is_shift_rpt_received_ && is_turn_rpt_received_ &&
+                            is_global_rpt_received_;
+
   /* guard */
   if (!actuation_cmd_ptr_ || !control_cmd_ptr_ || !is_pacmod_rpt_received_ || !gear_cmd_ptr_) {
     RCLCPP_INFO_THROTTLE(
@@ -365,6 +391,8 @@ void PacmodInterface::publishCommands()
     return;
   }
 
+  publishVehicleStatus();
+
   const rclcpp::Time current_time = get_clock()->now();
 
   double desired_throttle = actuation_cmd_ptr_->actuation.accel_cmd + accel_pedal_offset_;