feat(vehicle_velocity_converter): add speed_scale_factor (autowaref…

…oundation#2641)

* feat(vehicle_velocity_converter): add speed_scale_factor

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* add parameter description in readme

* ci(pre-commit): autofix

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

sensing/vehicle_velocity_converter/README.md

@@ -20,8 +20,9 @@ This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to
 
 ## Parameters
 
-| 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 |
+| Name                         | Type   | Description                             |
+| ---------------------------- | ------ | --------------------------------------- |
+| `speed_scale_factor`         | double | speed scale factor (ideal value is 1.0) |
+| `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         |

sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    speed_scale_factor: 1.0  # [.]
     velocity_stddev_xx: 0.2  # [m/s]
     angular_velocity_stddev_zz: 0.1  # [rad/s]
     frame_id: base_link

sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp

@@ -43,6 +43,7 @@ class VehicleVelocityConverter : public rclcpp::Node
   std::string frame_id_;
   double stddev_vx_;
   double stddev_wz_;
+  double speed_scale_factor_;
   std::array<double, 36> twist_covariance_;
 };
 

sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp

@@ -20,6 +20,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co
   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");
+  speed_scale_factor_ = declare_parameter("speed_scale_factor", 1.0);
 
   vehicle_report_sub_ = create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
     "velocity_status", rclcpp::QoS{100},
@@ -39,7 +40,7 @@ void VehicleVelocityConverter::callbackVelocityReport(
   // set twist with covariance msg from vehicle report msg
   geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance_msg;
   twist_with_covariance_msg.header = msg->header;
-  twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity;
+  twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity * speed_scale_factor_;
   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[0 + 0 * 6] = stddev_vx_ * stddev_vx_;