some fix on calibration_adapter and parameter_estimator

autowarefoundation · Oct 1, 2024 · 3e7f3df · 3e7f3df
1 parent 56d1758
commit 3e7f3df
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 7 deletions.
diff --git a/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp b/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp
@@ -30,7 +30,7 @@
 
 class CalibrationAdapterNode : public CalibrationAdapterNodeBase
 {
-  using Velocity = autoware_vehicle_msgs::msg::VelocityReport;
+  using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport;
   using ControlCommandStamped = autoware_control_msgs::msg::Control;
   using TwistStamped = geometry_msgs::msg::TwistStamped;
 
@@ -46,8 +46,9 @@ class CalibrationAdapterNode : public CalibrationAdapterNodeBase
   rclcpp::Publisher<Float32Stamped>::SharedPtr pub_acceleration_status_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr pub_acceleration_cmd_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr pub_steering_angle_cmd_;
+  rclcpp::Publisher<TwistStamped>::SharedPtr pub_vehicle_twist_;
   rclcpp::Subscription<ControlCommandStamped>::SharedPtr sub_control_cmd_;
-  rclcpp::Subscription<Velocity>::SharedPtr sub_twist_;
+  rclcpp::Subscription<VelocityReport>::SharedPtr sub_twist_;
   template <class T>
   T getNearestTimeDataFromVec(
     const T base_data, const double back_time, const std::vector<T> & vec);
@@ -56,7 +57,7 @@ class CalibrationAdapterNode : public CalibrationAdapterNodeBase
   template <class T>
   void pushDataToVec(const T data, const std::size_t max_size, std::vector<T> * vec);
   void callbackControlCmd(const ControlCommandStamped::ConstSharedPtr msg);
-  void callbackTwistStatus(const Velocity::ConstSharedPtr msg);
+  void callbackTwistStatus(const VelocityReport::ConstSharedPtr msg);
 };
 
 #endif  // CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_HPP_
diff --git a/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml b/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml
@@ -30,5 +30,6 @@
     <remap from="~/output/acceleration_status" to="/calibration/vehicle/acceleration_status"/>
     <remap from="~/output/steering_angle_cmd" to="/calibration/vehicle/steering_angle_cmd"/>
     <remap from="~/output/steering_angle_status" to="/calibration/vehicle/steering_angle_status"/>
+    <remap from="~/output/vehicle_twist" to="/calibration/vehicle/twist_status"/>
   </node>
 </launch>
diff --git a/vehicle/calibration_adapter/src/calibration_adapter_node.cpp b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp
@@ -38,11 +38,13 @@ CalibrationAdapterNode::CalibrationAdapterNode()
     create_publisher<Float32Stamped>("~/output/acceleration_status", durable_qos);
   pub_acceleration_cmd_ =
     create_publisher<Float32Stamped>("~/output/acceleration_cmd", durable_qos);
+  pub_vehicle_twist_ =
+    create_publisher<TwistStamped>("~/output/vehicle_twist", durable_qos);
 
   sub_control_cmd_ = create_subscription<ControlCommandStamped>(
     "~/input/control_cmd", queue_size,
     std::bind(&CalibrationAdapterNode::callbackControlCmd, this, _1));
-  sub_twist_ = create_subscription<Velocity>(
+  sub_twist_ = create_subscription<VelocityReport>(
     "~/input/twist_status", queue_size,
     std::bind(&CalibrationAdapterNode::callbackTwistStatus, this, _1));
 }
@@ -101,11 +103,14 @@ void CalibrationAdapterNode::callbackControlCmd(const ControlCommandStamped::Con
   pub_acceleration_cmd_->publish(accel_msg);
 }
 
-void CalibrationAdapterNode::callbackTwistStatus(const Velocity::ConstSharedPtr msg)
+void CalibrationAdapterNode::callbackTwistStatus(const VelocityReport::ConstSharedPtr msg)
 {
   TwistStamped twist;
   twist.header = msg->header;
   twist.twist.linear.x = msg->longitudinal_velocity;
+  twist.twist.linear.y = msg->lateral_velocity;
+  twist.twist.angular.z = msg->heading_rate;
+  pub_vehicle_twist_->publish(twist);
   if (!twist_vec_.empty()) {
     const auto past_msg = getNearestTimeDataFromVec(twist, dif_twist_time_, twist_vec_);
     const double dt =

diff --git a/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml
@@ -27,7 +27,7 @@
     <param name="select_wheel_base_estimator" value="$(var select_wheel_base_estimator)"/>
     <param name="use_auto_mode" value="$(var use_auto_mode)"/>
     <remap from="input/imu_twist" to="$(var imu_twist)"/>
-    <remap from="input/vehicle_twist" to="/vehicle/status/twist"/>
+    <remap from="input/vehicle_twist" to="/calibration/vehicle/twist_status"/>
     <remap from="input/control_mode" to="/vehicle/status/control_mode"/>
     <remap from="input/handle_status" to="/calibration/vehicle/handle_status"/>
     <remap from="input/steer" to="/calibration/vehicle/steering_angle_status"/>

diff --git a/vehicle/parameter_estimator/src/parameter_estimator_node.cpp b/vehicle/parameter_estimator/src/parameter_estimator_node.cpp
@@ -122,7 +122,8 @@ void ParameterEstimatorNode::timerCallback()
   {
     VehicleData v = {};
     v.velocity = vehicle_twist_ptr_->twist.linear.x;
-    v.angular_velocity = -imu_ptr_->angular_velocity.z;
+    //v.angular_velocity = -imu_ptr_->angular_velocity.z;
+    v.angular_velocity = imu_ptr_->angular_velocity.z;
     if (select_steer_offset_estimator || select_wheel_base_estimator) {
       v.steer = steer_ptr_->data;
     }