diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index e25c46dfd7c0d..7554457d37149 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -20,7 +20,8 @@ This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to ## Parameters -| Name | Type | Description | -| ------------ | ------ | ----------------------------------------- | -| `frame_id` | string | frame id for output message | -| `covariance` | double | set covariance value to the twist message | +| Name | Type | Description | +| ---------------------------- | ------ | ------------------------------- | +| `frame_id` | string | frame id for output message | +| `velocity_stddev_xx` | double | standard deviation for vx | +| `angular_velocity_stddev_zz` | double | standard deviation for yaw rate | diff --git a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml index 4c15d1caf6274..7dd04823453c5 100644 --- a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml +++ b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml @@ -1,12 +1,5 @@ /**: ros__parameters: + velocity_stddev_xx: 0.2 # [m/s] + angular_velocity_stddev_zz: 0.1 # [rad/s] frame_id: base_link - twist_covariance: - [ - 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 10000.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 10000.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, - ] diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 043087a3afdf2..bad4d4bc06326 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -41,6 +41,8 @@ class VehicleVelocityConverter : public rclcpp::Node twist_with_covariance_pub_; std::string frame_id_; + double stddev_vx_; + double stddev_wz_; std::array twist_covariance_; }; diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index d4cc91a2b0ae9..844a17df4715b 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -17,10 +17,8 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") { // set covariance value for twist with covariance msg - std::vector covariance = declare_parameter>("twist_covariance"); - for (std::size_t i = 0; i < covariance.size(); ++i) { - twist_covariance_[i] = covariance[i]; - } + stddev_vx_ = declare_parameter("velocity_stddev_xx", 0.2); + stddev_wz_ = declare_parameter("angular_velocity_stddev_zz", 0.1); frame_id_ = declare_parameter("frame_id", "base_link"); vehicle_report_sub_ = create_subscription( @@ -44,7 +42,12 @@ void VehicleVelocityConverter::callbackVelocityReport( twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity; twist_with_covariance_msg.twist.twist.linear.y = msg->lateral_velocity; twist_with_covariance_msg.twist.twist.angular.z = msg->heading_rate; - twist_with_covariance_msg.twist.covariance = twist_covariance_; + twist_with_covariance_msg.twist.covariance[0 + 0 * 6] = stddev_vx_ * stddev_vx_; + twist_with_covariance_msg.twist.covariance[1 + 1 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[2 + 2 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[3 + 3 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[4 + 4 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[5 + 5 * 6] = stddev_wz_ * stddev_wz_; twist_with_covariance_pub_->publish(twist_with_covariance_msg); }