diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index 79275858ee0e..a16cd5945801 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -1,6 +1,6 @@ # radar_tracks_msgs_converter -This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). +This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). - Calculation cost is O(n). - n: The number of radar objects @@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic - Output - - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message + - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message + - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message ### Parameters 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 a45d7c8fc8c7..d5738c80dcca 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 @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" #include "autoware_auto_perception_msgs/msg/shape.hpp" #include "autoware_auto_perception_msgs/msg/tracked_object.hpp" @@ -33,6 +34,9 @@ namespace radar_tracks_msgs_converter { +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; +using autoware_auto_perception_msgs::msg::DetectedObjects; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_perception_msgs::msg::TrackedObject; @@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; // Publisher - rclcpp::Publisher::SharedPtr pub_radar_{}; + rclcpp::Publisher::SharedPtr pub_tracked_objects_{}; + rclcpp::Publisher::SharedPtr pub_detected_objects_{}; // Timer rclcpp::TimerBase::SharedPtr timer_{}; @@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node NodeParam node_param_{}; // Core + geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance(); TrackedObjects convertRadarTrackToTrackedObjects(); + DetectedObjects convertRadarTrackToDetectedObjects(); uint8_t convertClassification(const uint16_t classification); }; 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 ba4cc4762ea7..c9f4a3135424 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,14 +1,16 @@ - + + - + + 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 cfe6ca3e4cea..eb4a497e7049 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 @@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt transform_listener_ = std::make_shared(this); // Publisher - pub_radar_ = create_publisher("~/output/radar_objects", 1); + pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1); + pub_detected_objects_ = create_publisher("~/output/radar_detected_objects", 1); // Timer const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period(); @@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer() node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects(); + DetectedObjects detected_objects = convertRadarTrackToDetectedObjects(); if (!tracked_objects.objects.empty()) { - pub_radar_->publish(tracked_objects); + pub_tracked_objects_->publish(tracked_objects); + pub_detected_objects_->publish(detected_objects); } } +DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects() +{ + DetectedObjects detected_objects; + detected_objects.header = radar_data_->header; + detected_objects.header.frame_id = node_param_.new_frame_id; + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + + for (auto & radar_track : radar_data_->tracks) { + DetectedObject detected_object; + + detected_object.existence_probability = 1.0; + + detected_object.shape.type = Shape::BOUNDING_BOX; + detected_object.shape.dimensions = radar_track.size; + + // kinematics + DetectedObjectKinematics kinematics; + kinematics.has_twist = true; + kinematics.has_twist_covariance = true; + + // convert by tf + geometry_msgs::msg::PoseStamped radar_pose_stamped{}; + radar_pose_stamped.pose.position = radar_track.position; + geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); + kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + + { + auto & pose_cov = kinematics.pose_with_covariance.covariance; + auto & radar_position_cov = radar_track.position_covariance; + pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X]; + pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y]; + pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y]; + pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z]; + pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z]; + pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z]; + } + + // convert by tf + geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{}; + radar_velocity_stamped.vector = radar_track.velocity; + geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{}; + tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_); + kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector; + + // twist compensation + if (node_param_.use_twist_compensation) { + 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(), "Odometry data is not coming"); + } + } + + { + auto & twist_cov = kinematics.twist_with_covariance.covariance; + auto & radar_vel_cov = radar_track.velocity_covariance; + twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X]; + twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y]; + twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y]; + twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z]; + twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z]; + twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z]; + } + detected_object.kinematics = kinematics; + + // classification + ObjectClassification classification; + classification.probability = 1.0; + classification.label = convertClassification(radar_track.classification); + detected_object.classification.emplace_back(classification); + + detected_objects.objects.emplace_back(detected_object); + } + return detected_objects; +} + TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() { TrackedObjects tracked_objects;