fix: some fix on calibration_adapter and parameter_estimator (#127)

* some fix on calibration_adapter and parameter_estimator Signed-off-by: KeiNakazato <[email protected]> * minor fix Signed-off-by: KeiNakazato <[email protected]> * style(pre-commit): autofix Signed-off-by: KeiNakazato <[email protected]> --------- Signed-off-by: KeiNakazato <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yuxuan Liu <[email protected]>
autowarefoundation · Oct 25, 2024 · 5049ea7 · 5049ea7
1 parent d606f4b
commit 5049ea7
Show file tree

Hide file tree

Showing 6 changed files with 12 additions and 2 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
@@ -46,6 +46,7 @@ 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_;
   template <class T>

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,6 +38,7 @@ 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,
@@ -106,6 +107,9 @@ void CalibrationAdapterNode::callbackTwistStatus(const Velocity::ConstSharedPtr
   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/include/parameter_estimator/parameter_estimator_node.hpp b/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp
@@ -64,6 +64,7 @@ class ParameterEstimatorNode : public rclcpp::Node
   bool select_gear_ratio_estimator;
   bool select_steer_offset_estimator;
   bool select_wheel_base_estimator;
+  bool invert_imu_z;
 
   double auto_mode_duration_ = 0;
   double last_manual_time_ = 0;

diff --git a/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml
@@ -4,6 +4,7 @@
   <arg name="select_gear_ratio_estimator" default="true" description="Select gear ratio estimator"/>
   <arg name="select_steer_offset_estimator" default="true" description="Select steer offset estimator"/>
   <arg name="select_wheel_base_estimator" default="true" description="Select wheel base estimator"/>
+  <arg name="invert_imu_z" default="true" description="Invert z-angular velocity from imu"/>
 
   <!-- get wheel base from vehicle info -->
   <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
@@ -26,8 +27,9 @@
     <param name="select_steer_offset_estimator" value="$(var select_steer_offset_estimator)"/>
     <param name="select_wheel_base_estimator" value="$(var select_wheel_base_estimator)"/>
     <param name="use_auto_mode" value="$(var use_auto_mode)"/>
+    <param name="invert_imu_z" value="$(var invert_imu_z)"/>
     <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
@@ -41,6 +41,7 @@ ParameterEstimatorNode::ParameterEstimatorNode(const rclcpp::NodeOptions & node_
   select_steer_offset_estimator =
     this->declare_parameter<bool>("select_steer_offset_estimator", true);
   select_wheel_base_estimator = this->declare_parameter<bool>("select_wheel_base_estimator", true);
+  invert_imu_z = this->declare_parameter<bool>("invert_imu_z", true);
   params_.valid_max_steer_rad = this->declare_parameter<double>("valid_max_steer_rad", 0.05);
   params_.valid_min_velocity = this->declare_parameter<double>("valid_min_velocity", 0.5);
   params_.valid_min_angular_velocity =
@@ -122,7 +123,7 @@ 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 * (invert_imu_z ? -1 : 1);
     if (select_steer_offset_estimator || select_wheel_base_estimator) {
       v.steer = steer_ptr_->data;
     }