From 457422767401fd8df6e9b3673e55afe9a02543e9 Mon Sep 17 00:00:00 2001
From: scepter914 <scepter914@gmail.com>
Date: Tue, 30 Aug 2022 23:14:56 +0900
Subject: [PATCH] fix(lidar_centerpoint): fix has_twist condition

Signed-off-by: scepter914 <scepter914@gmail.com>
---
 .../launch/lidar_centerpoint.launch.xml          |  2 ++
 perception/lidar_centerpoint/lib/ros_utils.cpp   | 16 +++++++++-------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
index 326964eefdeae..fb2be1dd610ba 100644
--- 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
index 57237609139e5..8bc5f955efab4 100644
--- 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)