From 4ea500b8d5daafe349e9c387518111331bf2ad29 Mon Sep 17 00:00:00 2001 From: KeiNakazato Date: Tue, 8 Oct 2024 18:06:04 +0900 Subject: [PATCH] minor fix --- .../calibration_adapter/calibration_adapter_node.hpp | 6 +++--- .../calibration_adapter/src/calibration_adapter_node.cpp | 4 ++-- .../config/parameter_estimator_param.yaml | 2 +- .../parameter_estimator/parameter_estimator_node.hpp | 1 + .../launch/parameter_estimator.launch.xml | 2 ++ .../parameter_estimator/src/parameter_estimator_node.cpp | 4 ++-- 6 files changed, 11 insertions(+), 8 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 index fcb422dd9..a72755312 100644 --- 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 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::SharedPtr pub_steering_angle_cmd_; rclcpp::Publisher::SharedPtr pub_vehicle_twist_; rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; template T getNearestTimeDataFromVec( const T base_data, const double back_time, const std::vector & vec); @@ -57,7 +57,7 @@ class CalibrationAdapterNode : public CalibrationAdapterNodeBase template void pushDataToVec(const T data, const std::size_t max_size, std::vector * 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_ diff --git a/vehicle/calibration_adapter/src/calibration_adapter_node.cpp b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp index 53f1bccfa..6cade1737 100644 --- a/vehicle/calibration_adapter/src/calibration_adapter_node.cpp +++ b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp @@ -44,7 +44,7 @@ CalibrationAdapterNode::CalibrationAdapterNode() sub_control_cmd_ = create_subscription( "~/input/control_cmd", queue_size, std::bind(&CalibrationAdapterNode::callbackControlCmd, this, _1)); - sub_twist_ = create_subscription( + sub_twist_ = create_subscription( "~/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; diff --git a/vehicle/parameter_estimator/config/parameter_estimator_param.yaml b/vehicle/parameter_estimator/config/parameter_estimator_param.yaml index 565946353..6649efc98 100644 --- a/vehicle/parameter_estimator/config/parameter_estimator_param.yaml +++ b/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 \ No newline at end of file diff --git a/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp b/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp index 2e721f817..29f05799f 100644 --- 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 index 2f21695d2..9eba3bb2d 100644 --- a/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml +++ b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml @@ -4,6 +4,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/vehicle/parameter_estimator/src/parameter_estimator_node.cpp b/vehicle/parameter_estimator/src/parameter_estimator_node.cpp index b288d1135..a768c537f 100644 --- 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("select_steer_offset_estimator", true); select_wheel_base_estimator = this->declare_parameter("select_wheel_base_estimator", true); + invert_imu_z = this->declare_parameter("invert_imu_z", true); params_.valid_max_steer_rad = this->declare_parameter("valid_max_steer_rad", 0.05); params_.valid_min_velocity = this->declare_parameter("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; }