fix(vehicle_velocity_converter): simpify parameter inputs (#1727)

* fix(vehicle_velocity_converter): simpify parameter inputs Signed-off-by: kminoda <koji.m.minoda@gmail.com> * fix readme and default value Signed-off-by: kminoda <koji.m.minoda@gmail.com> * fix default value Signed-off-by: kminoda <koji.m.minoda@gmail.com> Signed-off-by: kminoda <koji.m.minoda@gmail.com>
autowarefoundation · Aug 30, 2022 · fbcfee1 · fbcfee1
1 parent 687be97
commit fbcfee1
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 18 deletions.
diff --git 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
@@ -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/...icle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/...icle_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<double, 36> twist_covariance_;
 };
 

diff --git 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<double> covariance = declare_parameter<std::vector<double>>("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<autoware_auto_vehicle_msgs::msg::VelocityReport>(
@@ -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);
 }