From 5cc65835e3336b61c8b5e95b5373a23017d119f0 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 24 Jan 2024 16:11:28 +0300 Subject: [PATCH] update for detected objects update reaction analyzer update --- .../include/reaction_analyzer_node.hpp | 2 +- .../param/reaction_analyzer.param.yaml | 41 ++++++------ .../src/reaction_analyzer_node.cpp | 67 +++++++++++++++---- 3 files changed, 75 insertions(+), 35 deletions(-) diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 73252b1b79cfe..244a233daa6cd 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -117,6 +117,7 @@ struct NodeParams double control_cmd_buffer_time_interval; double min_jerk_for_brake_cmd; int min_number_descending_order_control_cmd; + bool run_from_bag; }; class ReactionAnalyzerNode : public rclcpp::Node @@ -137,7 +138,6 @@ class ReactionAnalyzerNode : public rclcpp::Node double entity_search_radius_; // TEMP - bool run_from_bag_{true}; PointCloud2::SharedPtr msg_cloud_empty_; PointCloud2::SharedPtr msg_cloud_with_obj_; diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index ca5a334c57e9d..1e52224368f35 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: use_sim_time: true + run_from_bag: true path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag-without-car-full/rosbag2_2024_01_17-16_23_07_0.db3 path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag-with-car-full/rosbag2_2024_01_17-17_27_14_0.db3 topic_cloud: /sensing/lidar/top/pointcloud_raw_ex @@ -54,30 +55,30 @@ # vehicle_cmd_gate: # topic_name: /control/command/control_cmd # message_type: autoware_auto_control_msgs::msg::AckermannControlCommand - ground_filtered_pointcloud: - topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw - message_type: sensor_msgs::msg::PointCloud2 - obstacle_seg_pointcloud: +# ground_filtered_pointcloud: +# topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw +# message_type: sensor_msgs::msg::PointCloud2 + occupancy_grid_map_outlier: topic_name: /perception/obstacle_segmentation/pointcloud message_type: sensor_msgs::msg::PointCloud2 voxel_based_compare_map_filter: topic_name: /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud message_type: sensor_msgs::msg::PointCloud2 -# cropbox_filter_self: -# topic_name: /sensing/lidar/top/self_cropped/pointcloud_ex -# message_type: sensor_msgs::msg::PointCloud2 -# cropbox_filter_mirror: -# topic_name: /sensing/lidar/top/mirror_cropped/pointcloud_ex -# message_type: sensor_msgs::msg::PointCloud2 -# distortion_corrector: -# topic_name: /sensing/lidar/top/rectified/pointcloud_ex -# message_type: sensor_msgs::msg::PointCloud2 -# outlier_filter: -# topic_name: /sensing/lidar/top/outlier_filtered/pointcloud -# message_type: sensor_msgs::msg::PointCloud2 -# concat_pointcloud: -# topic_name: /sensing/lidar/top/concatenated/pointcloud -# message_type: sensor_msgs::msg::PointCloud2 - predicted_objects_topic: + lidar_centerpoint: + topic_name: /perception/object_recognition/detection/centerpoint/objects + message_type: autoware_auto_perception_msgs::msg::DetectedObjects + obstacle_pointcloud_based_validator: + topic_name: /perception/object_recognition/detection/centerpoint/validation/objects + message_type: autoware_auto_perception_msgs::msg::DetectedObjects + detected_object_feature_remover: + topic_name: /perception/object_recognition/detection/clustering/objects + message_type: autoware_auto_perception_msgs::msg::DetectedObjects + detection_by_tracker: + topic_name: /perception/object_recognition/detection/detection_by_tracker/objects + message_type: autoware_auto_perception_msgs::msg::DetectedObjects + object_lanelet_filter: + topic_name: /perception/object_recognition/detection/objects + message_type: autoware_auto_perception_msgs::msg::DetectedObjects + map_based_prediction: topic_name: /perception/object_recognition/objects message_type: autoware_auto_perception_msgs::msg::PredictedObjects diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index d5ce39c72a9b5..11feea7b72df9 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -110,16 +110,19 @@ void ReactionAnalyzerNode::pointcloud2OutputCallback( const auto current_odometry_ptr = odometry_; // const auto is_spawned = spawn_cmd_time_; mutex_.unlock(); - if (!current_odometry_ptr) return; if (!std::holds_alternative(variant)) { // If the variant doesn't hold a Trajectory message yet, initialize it PointCloud2Buffer buffer(std::nullopt); variant = buffer; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); } else if (std::get(variant).has_value()) { // reacted return; } + if (!current_odometry_ptr) return; // transform pointcloud geometry_msgs::msg::TransformStamped transform_stamped{}; @@ -156,6 +159,7 @@ void ReactionAnalyzerNode::predictedObjectsOutputCallback( { mutex_.lock(); auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; // const auto is_spawned = spawn_cmd_time_; mutex_.unlock(); @@ -163,10 +167,15 @@ void ReactionAnalyzerNode::predictedObjectsOutputCallback( // If the variant doesn't hold a Trajectory message yet, initialize it PredictedObjectsBuffer buffer(std::nullopt); variant = buffer; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); } else if (std::get(variant).has_value()) { // reacted return; } + if (!current_odometry_ptr || msg_ptr->objects.empty()) return; + if (searchPredictedObjectsNearEntity(*msg_ptr)) { std::get(variant) = *msg_ptr; std::cout << "Reacted " << node_name << std::endl; @@ -182,6 +191,7 @@ void ReactionAnalyzerNode::detectedObjectsOutputCallback( { mutex_.lock(); auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; // const auto is_spawned = spawn_cmd_time_; mutex_.unlock(); @@ -189,12 +199,37 @@ void ReactionAnalyzerNode::detectedObjectsOutputCallback( // If the variant doesn't hold a Trajectory message yet, initialize it DetectedObjectsBuffer buffer(std::nullopt); variant = buffer; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); } else if (std::get(variant).has_value()) { // reacted return; } - if (searchDetectedObjectsNearEntity(*msg_ptr)) { + + if (!current_odometry_ptr || msg_ptr->objects.empty()) return; + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "map", msg_ptr->header.frame_id, this->now(), rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped, input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (searchDetectedObjectsNearEntity(output_objs)) { std::get(variant) = *msg_ptr; + std::cout << "Reacted " << node_name << std::endl; } // if (!is_spawned) return; mutex_.lock(); @@ -241,6 +276,9 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options) get_parameter("observer.min_jerk_for_brake_cmd").as_double(); node_params_.min_number_descending_order_control_cmd = get_parameter("observer.min_number_descending_order_control_cmd").as_int(); + node_params_.run_from_bag = + get_parameter("run_from_bag").as_bool(); + // prepare the object which will be spawned entity_params_.x = get_parameter("test_manager.entity.x").as_double(); @@ -295,7 +333,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options) pub_initial_pose_ = create_publisher( "output/initialpose", rclcpp::QoS(1)); pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); - if (!run_from_bag_) { + if (!node_params_.run_from_bag) { pub_pointcloud_ = create_publisher( "/perception/obstacle_segmentation/pointcloud", rclcpp::SensorDataQoS()); } @@ -303,7 +341,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options) pub_predicted_objects_ = create_publisher("output/objects", rclcpp::QoS(1)); client_change_to_autonomous_ = create_client("service/change_to_autonomous"); - if (run_from_bag_) { + if (node_params_.run_from_bag) { // init pointcloud in bags const std::string path_bag_without_object = get_parameter("path_bag_without_object").as_string(); @@ -408,7 +446,7 @@ void ReactionAnalyzerNode::onTimer() const auto message_buffers = message_buffers_; auto spawn_cmd_time = spawn_cmd_time_; mutex_.unlock(); - if (!run_from_bag_) { + if (!node_params_.run_from_bag) { if (!is_test_environment_created_) { initEgoForTest(initialization_state_ptr, route_state_ptr); return; @@ -451,7 +489,7 @@ void ReactionAnalyzerNode::onTimer() return; } else { rclcpp::Duration time_diff = this->now() - last_test_environment_init_time_.value(); - if (time_diff > rclcpp::Duration::from_seconds(10.0)) { + if (time_diff > rclcpp::Duration::from_seconds(50.0)) { if (!spawn_cmd_time) { spawn_cmd_time = this->now(); mutex_.lock(); @@ -591,18 +629,18 @@ void ReactionAnalyzerNode::printResults( const std::pair & b) { return a.second < b.second; }); // first print spawn_cmd_time to first reaction - std::cout << "spawn_cmd_time to " << reaction_times.begin()->first << ": " - << createDurationMs(spawn_cmd_time_.value(), reaction_times.begin()->second) - << std::endl; +// std::cout << "spawn_cmd_time to " << reaction_times.begin()->first << ": " +// << createDurationMs(spawn_cmd_time_.value(), reaction_times.begin()->second) +// << std::endl; - if (reaction_times.size() < 2) return; +// if (reaction_times.size() < 2) return; - for (size_t i = 0; i < reaction_times.size() - 1; i++) { + for (size_t i = 0; i < reaction_times.size(); i++) { const auto & first_reaction = reaction_times.at(i); - const auto & second_reaction = reaction_times.at(i + 1); +// const auto & second_reaction = reaction_times.at(i + 1); - std::cout << first_reaction.first << " to " << second_reaction.first << ": " - << createDurationMs(first_reaction.second, second_reaction.second) << std::endl; + std::cout << "spawn_cmd_time to " << first_reaction.first << ": " + << createDurationMs(spawn_cmd_time_.value(), first_reaction.second) << std::endl; } is_output_printed_ = true; } @@ -1049,4 +1087,5 @@ bool ReactionAnalyzerNode::searchDetectedObjectsNearEntity(const DetectedObjects #include #include + RCLCPP_COMPONENTS_REGISTER_NODE(reaction_analyzer::ReactionAnalyzerNode)