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 32befa12..a7275531 100644 --- 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::SharedPtr pub_acceleration_status_; rclcpp::Publisher::SharedPtr pub_acceleration_cmd_; rclcpp::Publisher::SharedPtr pub_steering_angle_cmd_; + rclcpp::Publisher::SharedPtr pub_vehicle_twist_; rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_twist_; template diff --git a/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml b/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml index 9b4f006e..000319e3 100644 --- a/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml +++ b/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml @@ -30,5 +30,6 @@ + diff --git a/vehicle/calibration_adapter/src/calibration_adapter_node.cpp b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp index 650b8d29..41e9205c 100644 --- 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("~/output/acceleration_status", durable_qos); pub_acceleration_cmd_ = create_publisher("~/output/acceleration_cmd", durable_qos); + pub_vehicle_twist_ = create_publisher("~/output/vehicle_twist", durable_qos); sub_control_cmd_ = create_subscription( "~/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 index 2e721f81..29f05799 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 5cdffa7c..9eba3bb2 100644 --- a/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml +++ b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml @@ -4,6 +4,7 @@ + @@ -26,8 +27,9 @@ + - + diff --git a/vehicle/parameter_estimator/src/parameter_estimator_node.cpp b/vehicle/parameter_estimator/src/parameter_estimator_node.cpp index 60bf2f2d..a768c537 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,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; }