fix(lidar_centerpoint): fix has_twist condition

Signed-off-by: scepter914 <scepter914@gmail.com>
autowarefoundation · Aug 30, 2022 · 4574227 · 4574227
1 parent ae833f3
commit 4574227
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
@@ -6,6 +6,7 @@
   <arg name="model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
   <arg name="model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
   <arg name="score_threshold" default="0.45"/>
+  <arg name="has_twist" default="false"/>
 
   <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
     <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
@@ -14,6 +15,7 @@
     <param name="densification_world_frame_id" value="map"/>
     <param name="densification_num_past_frames" value="1"/>
     <param name="trt_precision" value="fp16"/>
+    <param name="has_twist" value="$(var has_twist)"/>
     <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
     <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
     <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>

diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp
@@ -71,13 +71,15 @@ void box3DToDetectedObject(
     tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height);
 
   // twist
-  float vel_x = box3d.vel_x;
-  float vel_y = box3d.vel_y;
-  geometry_msgs::msg::Twist twist;
-  twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2));
-  twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw);
-  obj.kinematics.twist_with_covariance.twist = twist;
-  obj.kinematics.has_twist = has_twist;
+  if (has_twist) {
+    float vel_x = box3d.vel_x;
+    float vel_y = box3d.vel_y;
+    geometry_msgs::msg::Twist twist;
+    twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2));
+    twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw);
+    obj.kinematics.twist_with_covariance.twist = twist;
+    obj.kinematics.has_twist = has_twist;
+  }
 }
 
 uint8_t getSemanticType(const std::string & class_name)