minor fix

vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp

@@ -30,7 +30,7 @@
 
 class CalibrationAdapterNode : public CalibrationAdapterNodeBase
 {
-  using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport;
+  using Velocity = autoware_vehicle_msgs::msg::VelocityReport;
   using ControlCommandStamped = autoware_control_msgs::msg::Control;
   using TwistStamped = geometry_msgs::msg::TwistStamped;
 
@@ -48,7 +48,7 @@ class CalibrationAdapterNode : public CalibrationAdapterNodeBase
   rclcpp::Publisher<Float32Stamped>::SharedPtr pub_steering_angle_cmd_;
   rclcpp::Publisher<TwistStamped>::SharedPtr pub_vehicle_twist_;
   rclcpp::Subscription<ControlCommandStamped>::SharedPtr sub_control_cmd_;
-  rclcpp::Subscription<VelocityReport>::SharedPtr sub_twist_;
+  rclcpp::Subscription<Velocity>::SharedPtr sub_twist_;
   template <class T>
   T getNearestTimeDataFromVec(
     const T base_data, const double back_time, const std::vector<T> & vec);
@@ -57,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 VelocityReport::ConstSharedPtr msg);
+  void callbackTwistStatus(const Velocity::ConstSharedPtr msg);
 };
 
 #endif  // CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_HPP_

vehicle/calibration_adapter/src/calibration_adapter_node.cpp

@@ -44,7 +44,7 @@ CalibrationAdapterNode::CalibrationAdapterNode()
   sub_control_cmd_ = create_subscription<ControlCommandStamped>(
     "~/input/control_cmd", queue_size,
     std::bind(&CalibrationAdapterNode::callbackControlCmd, this, _1));
-  sub_twist_ = create_subscription<VelocityReport>(
+  sub_twist_ = create_subscription<Velocity>(
     "~/input/twist_status", queue_size,
     std::bind(&CalibrationAdapterNode::callbackTwistStatus, this, _1));
 }
@@ -103,7 +103,7 @@ void CalibrationAdapterNode::callbackControlCmd(const ControlCommandStamped::Con
   pub_acceleration_cmd_->publish(accel_msg);
 }
 
-void CalibrationAdapterNode::callbackTwistStatus(const VelocityReport::ConstSharedPtr msg)
+void CalibrationAdapterNode::callbackTwistStatus(const Velocity::ConstSharedPtr msg)
 {
   TwistStamped twist;
   twist.header = msg->header;

vehicle/parameter_estimator/config/parameter_estimator_param.yaml

@@ -6,4 +6,4 @@ parameter_estimator:
     valid_max_steer_rad: 0.05 # Used as steer data validation, the data should be less than this value
     valid_min_velocity: 0.5 # Used as velocity validation, the data should be more than this value
     valid_min_angular_velocity: 0.1 # Used in gear ratio estimator, the angular should be more than this value
-    gear_ratio: [15.7, 0.053, 0.047] # Initial estimated gear ratio
+    gear_ratio: [15.7, 0.053, 0.047] # Initial estimated gear ratio

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;

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,6 +27,7 @@
     <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="/calibration/vehicle/twist_status"/>
     <remap from="input/control_mode" to="/vehicle/status/control_mode"/>

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,8 +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;
+    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;
     }