feat(radar_tracks_msgs_converter): change twist topic (#1195)

Signed-off-by: yutaka <purewater0901@gmail.com>

perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp

@@ -38,7 +38,7 @@ using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
-using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
 using radar_msgs::msg::RadarTracks;
 
 class RadarTracksMsgsConverterNode : public rclcpp::Node
@@ -56,16 +56,16 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
 private:
   // Subscriber
   rclcpp::Subscription<RadarTracks>::SharedPtr sub_radar_{};
-  rclcpp::Subscription<TwistStamped>::SharedPtr sub_twist_{};
+  rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_{};
   std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
 
   // Callback
   void onRadarTracks(const RadarTracks::ConstSharedPtr msg);
-  void onTwist(const TwistStamped::ConstSharedPtr msg);
+  void onTwist(const Odometry::ConstSharedPtr msg);
 
   // Data Buffer
   RadarTracks::ConstSharedPtr radar_data_{};
-  TwistStamped::ConstSharedPtr twist_data_{};
+  Odometry::ConstSharedPtr odometry_data_{};
   geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
 
   // Publisher

perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml

@@ -1,13 +1,13 @@
 <launch>
   <arg name="input/radar_objects" default="input/radar_objects"/>
-  <arg name="input/twist" default="input/twist"/>
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="output/radar_objects" default="output/radar_objects"/>
   <arg name="update_rate_hz" default="20.0"/>
   <arg name="use_twist_compensation" default="false"/>
 
   <node pkg="radar_tracks_msgs_converter" exec="radar_tracks_msgs_converter_node" name="radar_tracks_msgs_converter" output="screen">
     <remap from="~/input/radar_objects" to="$(var input/radar_objects)"/>
-    <remap from="~/input/twist" to="$(var input/twist)"/>
+    <remap from="~/input/odometry" to="$(var input/odometry)"/>
     <remap from="~/output/radar_objects" to="$(var output/radar_objects)"/>
     <param name="update_rate_hz" value="$(var update_rate_hz)"/>
     <param name="use_twist_compensation" value="$(var use_twist_compensation)"/>

perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp

@@ -82,8 +82,9 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt
   sub_radar_ = create_subscription<RadarTracks>(
     "~/input/radar_objects", rclcpp::QoS{1},
     std::bind(&RadarTracksMsgsConverterNode::onRadarTracks, this, _1));
-  sub_twist_ = create_subscription<TwistStamped>(
-    "~/input/twist", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1));
+  sub_odometry_ = create_subscription<Odometry>(
+    "~/input/odometry", rclcpp::QoS{1},
+    std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1));
   transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
 
   // Publisher
@@ -100,9 +101,9 @@ void RadarTracksMsgsConverterNode::onRadarTracks(const RadarTracks::ConstSharedP
   radar_data_ = msg;
 }
 
-void RadarTracksMsgsConverterNode::onTwist(const TwistStamped::ConstSharedPtr msg)
+void RadarTracksMsgsConverterNode::onTwist(const Odometry::ConstSharedPtr msg)
 {
-  twist_data_ = msg;
+  odometry_data_ = msg;
 }
 
 rcl_interfaces::msg::SetParametersResult RadarTracksMsgsConverterNode::onSetParam(
@@ -201,12 +202,12 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
 
     // twist compensation
     if (node_param_.use_twist_compensation) {
-      if (twist_data_) {
-        kinematics.twist_with_covariance.twist.linear.x += twist_data_->twist.linear.x;
-        kinematics.twist_with_covariance.twist.linear.y += twist_data_->twist.linear.y;
-        kinematics.twist_with_covariance.twist.linear.z += twist_data_->twist.linear.z;
+      if (odometry_data_) {
+        kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x;
+        kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y;
+        kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z;
       } else {
-        RCLCPP_INFO(get_logger(), "Twist data is not coming");
+        RCLCPP_INFO(get_logger(), "Odometry data is not coming");
       }
     }