diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 0343f4e39f470..6421061ac884d 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/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::SharedPtr sub_radar_{}; - rclcpp::Subscription::SharedPtr sub_twist_{}; + rclcpp::Subscription::SharedPtr sub_odometry_{}; std::shared_ptr 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 diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index a467f54bc079f..ba4cc4762ea7d 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -1,13 +1,13 @@ - + - + diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index c3f64424b22a5..368e6a343fd06 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/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( "~/input/radar_objects", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onRadarTracks, this, _1)); - sub_twist_ = create_subscription( - "~/input/twist", rclcpp::QoS{1}, std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); + sub_odometry_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, + std::bind(&RadarTracksMsgsConverterNode::onTwist, this, _1)); transform_listener_ = std::make_shared(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"); } }