From 370ffe2f7804dbf42d527c35a75180f146a1e74c Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 3 Jan 2024 15:50:53 +0300 Subject: [PATCH] structure changes update --- .../include/reaction_analyzer_node.hpp | 11 +- .../launch/reaction_analyzer.launch.xml | 10 +- .../src/reaction_analyzer_node.cpp | 421 +++++++++--------- 3 files changed, 232 insertions(+), 210 deletions(-) diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index f2bd900afebc6..3caa2b62076de 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -146,6 +146,7 @@ class ReactionAnalyzerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_{tf_buffer_}; // Functions + rclcpp::SubscriptionOptions createSubscriptionOptions(); bool loadChainModules(); bool initObservers(const ChainModules & modules); void initAnalyzerVariables(); @@ -157,6 +158,11 @@ class ReactionAnalyzerNode : public rclcpp::Node void setControlCommandToBuffer( std::vector & buffer, const AckermannControlCommand & cmd); std::optional findFirstBrakeIdx(const std::vector & cmd_array); + void spawnObstacle(const geometry_msgs::msg::Point & ego_pose); + void searchForReactions( + const std::unordered_map & message_buffers, + const geometry_msgs::msg::Point & ego_pose); + void printResults(const std::unordered_map & message_buffers); void onTimer(); // Callbacks @@ -173,7 +179,7 @@ class ReactionAnalyzerNode : public rclcpp::Node // Variables std::vector subscribers_; - std::unordered_map messageBuffers_; + std::unordered_map message_buffers_; std::optional last_test_environment_init_time_; std::optional spawn_cmd_time_; bool is_test_environment_created_{false}; @@ -187,8 +193,7 @@ class ReactionAnalyzerNode : public rclcpp::Node // Client rclcpp::Client::SharedPtr client_change_to_autonomous_; - void callOperationModeServiceWithoutResponse( - const rclcpp::Client::SharedPtr client); + void callOperationModeServiceWithoutResponse(); // Pointers PointCloud2::SharedPtr entity_pointcloud_ptr_; diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml index eaeecc439ae3f..79e3464526ffa 100644 --- a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -5,11 +5,13 @@ + + + - - - - + + + diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index b9758c46b07bb..dee2a189b797d 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -31,91 +31,35 @@ namespace reaction_analyzer { -double createDurationMs(const rclcpp::Time & start_time, const rclcpp::Time & end_time) -{ - return static_cast((end_time - start_time).nanoseconds()) / 1e6; -} - -rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) -{ - rclcpp::CallbackGroup::SharedPtr callback_group = - node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group; - - return sub_opt; -} - -void ReactionAnalyzerNode::callOperationModeServiceWithoutResponse( - const rclcpp::Client::SharedPtr client) -{ - auto req = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), "client request"); - - if (!client->service_is_ready()) { - RCLCPP_INFO(this->get_logger(), "client is unavailable"); - return; - } +// Callbacks - client->async_send_request( - req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( - this->get_logger(), "Status: %d, %s", result.get()->status.code, - result.get()->status.message.c_str()); - }); -} - -std::vector split(const std::string & str, const char delim) +void ReactionAnalyzerNode::controlCommandOutputCallback( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr) { - std::vector elems; - std::stringstream ss(str); - std::string item; - while (std::getline(ss, item, delim)) { - elems.push_back(item); + std::lock_guard lock(mutex_); + auto & variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + variant = buffer; } - return elems; + setControlCommandToBuffer(std::get(variant).first, *msg_ptr); } -MessageType stringToMessageType(const std::string & input) +void ReactionAnalyzerNode::planningOutputCallback( + const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr) { - if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") { - return MessageType::AckermannControlCommand; - } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") { - return MessageType::Trajectory; + std::lock_guard lock(mutex_); + auto & variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + // If the variant doesn't hold a Trajectory message yet, initialize it + TrajectoryBuffer buffer(*msg_ptr, std::nullopt); + variant = buffer; } else { - return MessageType::Unknown; - } -} - -bool ReactionAnalyzerNode::loadChainModules() -{ - // get the topic addresses and message types of the modules in chain - const auto param_key = std::string("chain"); - const auto module_names = this->list_parameters({param_key}, 3).prefixes; - ChainModules chain_modules; - for (const auto & module_name : module_names) { - const auto splitted_name = split(module_name, '.'); - TopicConfig tmp; - tmp.node_name = splitted_name.back(); - const auto topic_info = this->list_parameters({module_name}, 2).names; - tmp.topic_address = this->get_parameter(module_name + ".topic_name").as_string(); - tmp.message_type = - stringToMessageType(this->get_parameter(module_name + ".message_type").as_string()); - if (tmp.message_type != MessageType::Unknown) { - chain_modules.emplace_back(tmp); - } else { - RCLCPP_WARN( - this->get_logger(), "Unknown message type for module name: %s, skipping..", - tmp.node_name.c_str()); - } + std::get(variant).first = *msg_ptr; } - return (initObservers(chain_modules)); } -// --- - void ReactionAnalyzerNode::operationModeCallback(const OperationModeState::ConstSharedPtr msg_ptr) { std::lock_guard lock(mutex_); @@ -187,37 +131,32 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options) if (!loadChainModules()) { RCLCPP_ERROR( get_logger(), "Modules in chain are invalid. Node couldn't be initialized. Failed."); + return; } initAnalyzerVariables(); + sub_kinematics_ = create_subscription( + "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::vehiclePoseCallback, this, _1), + createSubscriptionOptions()); sub_localization_init_state_ = create_subscription( - "/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), + "input/localization_initialization_state", rclcpp::QoS(1).transient_local(), std::bind(&ReactionAnalyzerNode::initializationStateCallback, this, _1), - createSubscriptionOptions(this)); + createSubscriptionOptions()); sub_route_state_ = create_subscription( - "/api/routing/state", rclcpp::QoS{1}.transient_local(), - std::bind(&ReactionAnalyzerNode::routeStateCallback, this, _1), - createSubscriptionOptions(this)); + "input/routing_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::routeStateCallback, this, _1), createSubscriptionOptions()); sub_operation_mode_ = create_subscription( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&ReactionAnalyzerNode::operationModeCallback, this, _1), - createSubscriptionOptions(this)); - - client_change_to_autonomous_ = - create_client("/api/operation_mode/change_to_autonomous"); - - pub_initial_pose_ = - create_publisher("/initialpose", rclcpp::QoS(1)); - pub_goal_pose_ = create_publisher( - "/planning/mission_planning/goal", rclcpp::QoS(1)); + "input/operation_mode_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::operationModeCallback, this, _1), createSubscriptionOptions()); + pub_initial_pose_ = create_publisher( + "output/initialpose", rclcpp::QoS(1)); + pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); pub_pointcloud_ = create_publisher("output/points_raw", rclcpp::QoS(1)); pub_predicted_objects_ = create_publisher("output/objects", rclcpp::QoS(1)); - sub_kinematics_ = create_subscription( - "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::vehiclePoseCallback, this, _1), - createSubscriptionOptions(this)); + client_change_to_autonomous_ = create_client("service/change_to_autonomous"); using namespace std::literals::chrono_literals; const auto period_ns = std::chrono::duration_cast( @@ -233,7 +172,7 @@ void ReactionAnalyzerNode::onTimer() const auto initialization_state_ptr = initialization_state_ptr_; const auto route_state_ptr = current_route_state_ptr_; const auto operation_mode_ptr = operation_mode_ptr_; - const auto messageBuffers = messageBuffers_; + const auto message_buffers = message_buffers_; mutex_.unlock(); if (!is_test_environment_created_) { @@ -250,24 +189,42 @@ void ReactionAnalyzerNode::onTimer() if ( operation_mode_ptr->mode != OperationModeState::AUTONOMOUS && operation_mode_ptr->is_autonomous_mode_available) { - callOperationModeServiceWithoutResponse(client_change_to_autonomous_); + callOperationModeServiceWithoutResponse(); + } + + spawnObstacle(current_odometry_ptr->pose.pose.position); + + if (!spawn_cmd_time_) return; + + if (!all_topics_reacted_) { + searchForReactions(message_buffers, current_odometry_ptr->pose.pose.position); + return; } + if (!is_output_printed_) { + printResults(message_buffers); + } +} + +void ReactionAnalyzerNode::spawnObstacle(const geometry_msgs::msg::Point & ego_pose) +{ + const auto current_time = this->now(); + if ( - tier4_autoware_utils::calcDistance3d( - current_odometry_ptr->pose.pose.position, entity_pose_.position) >= + tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) >= node_params_.spawn_distance_threshold) { // do not spawn it, send empty pointcloud pcl::PointCloud pcl_empty; PointCloud2 empty_pointcloud; pcl::toROSMsg(pcl_empty, empty_pointcloud); empty_pointcloud.header.frame_id = "base_link"; + empty_pointcloud.header.stamp = current_time; + PredictedObjects empty_predicted_objects; empty_predicted_objects.header.frame_id = "map"; + empty_predicted_objects.header.stamp = current_time; - empty_pointcloud.header.stamp = this->now(); pub_pointcloud_->publish(empty_pointcloud); - empty_predicted_objects.header.stamp = this->now(); pub_predicted_objects_->publish(empty_predicted_objects); } else { @@ -281,110 +238,155 @@ void ReactionAnalyzerNode::onTimer() return; } - const auto now = this->now(); - if (!spawn_cmd_time_) { - spawn_cmd_time_ = now; - std::cout << "Spawned!" << std::endl; - } - // transform by using eigen matrix PointCloud2 transformed_points{}; const Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(affine_matrix, *entity_pointcloud_ptr_, transformed_points); + transformed_points.header.frame_id = "base_link"; + transformed_points.header.stamp = current_time; - // predicted objects predicted_objects_ptr_->header.frame_id = "map"; + predicted_objects_ptr_->header.stamp = current_time; - transformed_points.header.stamp = now; pub_pointcloud_->publish(transformed_points); - predicted_objects_ptr_->header.stamp = now; pub_predicted_objects_->publish(*predicted_objects_ptr_); + + if (!spawn_cmd_time_) { + spawn_cmd_time_ = current_time; + std::cout << "Spawned!" << std::endl; + } } +} - if (spawn_cmd_time_ && !all_topics_reacted_) { - all_topics_reacted_ = true; - const auto current_time = this->now(); - for (const auto & [key, variant] : messageBuffers) { - if (auto * control_message = std::get_if(&variant)) { - if (!control_message->second) { - all_topics_reacted_ = false; - const auto brake_start_idx = findFirstBrakeIdx(control_message->first); - if (brake_start_idx) { - mutex_.lock(); - auto * tmp = std::get_if(&messageBuffers_[key]); - if (tmp) tmp->second = control_message->first.at(brake_start_idx.value()); - mutex_.unlock(); - } +void ReactionAnalyzerNode::searchForReactions( + const std::unordered_map & message_buffers, + const geometry_msgs::msg::Point & ego_pose) +{ + all_topics_reacted_ = true; + for (const auto & [key, variant] : message_buffers) { + if (auto * control_message = std::get_if(&variant)) { + if (!control_message->second) { + all_topics_reacted_ = false; + const auto brake_start_idx = findFirstBrakeIdx(control_message->first); + if (brake_start_idx) { + mutex_.lock(); + auto * tmp = std::get_if(&message_buffers_[key]); + if (tmp) tmp->second = control_message->first.at(brake_start_idx.value()); + mutex_.unlock(); } - } else if (auto * planning_message = std::get_if(&variant)) { - if (!planning_message->second) { - all_topics_reacted_ = false; - const auto nearest_idx = motion_utils::findNearestIndex( - planning_message->first.points, current_odometry_ptr->pose.pose.position); - - const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( - planning_message->first.points, nearest_idx, planning_message->first.points.size() - 1); - - if (zero_vel_idx) { - mutex_.lock(); - auto * tmp = std::get_if(&messageBuffers_[key]); - if (tmp) tmp->second = planning_message->first; - mutex_.unlock(); - } + } + } else if (auto * planning_message = std::get_if(&variant)) { + if (!planning_message->second) { + all_topics_reacted_ = false; + const auto nearest_idx = + motion_utils::findNearestIndex(planning_message->first.points, ego_pose); + + const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + planning_message->first.points, nearest_idx, planning_message->first.points.size() - 1); + + if (zero_vel_idx) { + mutex_.lock(); + auto * tmp = std::get_if(&message_buffers_[key]); + if (tmp) tmp->second = planning_message->first; + mutex_.unlock(); } } - // Add similar blocks for other types } + // Add similar blocks for other types } - if (all_topics_reacted_ && !is_output_printed_) { - is_output_printed_ = true; - std::vector> reaction_times; - for (const auto & [key, variant] : messageBuffers) { - if (auto * control_message = std::get_if(&variant)) { - if (control_message->second) { - reaction_times.emplace_back(key, rclcpp::Time(control_message->second->stamp)); - } - } else if (auto * planning_message = std::get_if(&variant)) { - if (planning_message->second) { - reaction_times.emplace_back(key, rclcpp::Time(planning_message->second->header.stamp)); - } +} + +void ReactionAnalyzerNode::printResults( + const std::unordered_map & message_buffers) +{ + auto createDurationMs = [](const rclcpp::Time & start_time, const rclcpp::Time & end_time) { + return static_cast((end_time - start_time).nanoseconds()) / 1e6; + }; + + is_output_printed_ = true; + std::vector> reaction_times; + for (const auto & [key, variant] : message_buffers) { + if (auto * control_message = std::get_if(&variant)) { + if (control_message->second) { + reaction_times.emplace_back(key, rclcpp::Time(control_message->second->stamp)); + } + } else if (auto * planning_message = std::get_if(&variant)) { + if (planning_message->second) { + reaction_times.emplace_back(key, rclcpp::Time(planning_message->second->header.stamp)); } } + } - // sort the reaction times - std::sort( - reaction_times.begin(), reaction_times.end(), - []( - const std::pair & a, - const std::pair & b) { return a.second < b.second; }); + if (reaction_times.empty()) return; - // 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; + // sort the reaction times + std::sort( + reaction_times.begin(), reaction_times.end(), + []( + const std::pair & a, + const std::pair & b) { return a.second < b.second; }); - if (reaction_times.size() < 2) return; + // 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; - for (size_t i = 0; i < reaction_times.size() - 1; i++) { - const auto & first_reaction = reaction_times.at(i); - const auto & second_reaction = reaction_times.at(i + 1); + if (reaction_times.size() < 2) return; - std::cout << first_reaction.first << " to " << second_reaction.first << ": " - << createDurationMs(first_reaction.second, second_reaction.second) << std::endl; - } + for (size_t i = 0; i < reaction_times.size() - 1; i++) { + const auto & first_reaction = reaction_times.at(i); + 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; } } -unique_identifier_msgs::msg::UUID generateUUIDMsg(const std::string & input) +bool ReactionAnalyzerNode::loadChainModules() { - static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); - const auto uuid = generate_uuid(input); + auto split = [](const std::string & str, const char delim) { + std::vector elems; + std::stringstream ss(str); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + return elems; + }; + + auto stringToMessageType = [](const std::string & input) { + if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") { + return MessageType::AckermannControlCommand; + } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") { + return MessageType::Trajectory; + } else { + return MessageType::Unknown; + } + }; - unique_identifier_msgs::msg::UUID uuid_msg; - std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); - return uuid_msg; + // get the topic addresses and message types of the modules in chain + const auto param_key = std::string("chain"); + const auto module_names = this->list_parameters({param_key}, 3).prefixes; + ChainModules chain_modules; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + TopicConfig tmp; + tmp.node_name = splitted_name.back(); + const auto topic_info = this->list_parameters({module_name}, 2).names; + tmp.topic_address = this->get_parameter(module_name + ".topic_name").as_string(); + tmp.message_type = + stringToMessageType(this->get_parameter(module_name + ".message_type").as_string()); + if (tmp.message_type != MessageType::Unknown) { + chain_modules.emplace_back(tmp); + } else { + RCLCPP_WARN( + this->get_logger(), "Unknown message type for module name: %s, skipping..", + tmp.node_name.c_str()); + } + } + return (initObservers(chain_modules)); } void ReactionAnalyzerNode::initAnalyzerVariables() @@ -472,33 +474,6 @@ void ReactionAnalyzerNode::initPointcloud() pcl::toROSMsg(point_cloud, *entity_pointcloud_ptr_); } -void ReactionAnalyzerNode::controlCommandOutputCallback( - const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr) -{ - std::lock_guard lock(mutex_); - auto & variant = messageBuffers_[node_name]; - if (!std::holds_alternative(variant)) { - // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it - ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); - variant = buffer; - } - setControlCommandToBuffer(std::get(variant).first, *msg_ptr); -} - -void ReactionAnalyzerNode::planningOutputCallback( - const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr) -{ - std::lock_guard lock(mutex_); - auto & variant = messageBuffers_[node_name]; - if (!std::holds_alternative(variant)) { - // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it - TrajectoryBuffer buffer(*msg_ptr, std::nullopt); - variant = buffer; - } else { - std::get(variant).first = *msg_ptr; - } -} - bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & modules) { if (modules.empty()) { @@ -512,7 +487,7 @@ bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & this->controlCommandOutputCallback(module.node_name, msg); }; auto subscriber = this->create_subscription( - module.topic_address, 1, callback, createSubscriptionOptions(this)); + module.topic_address, 1, callback, createSubscriptionOptions()); subscribers_.push_back(subscriber); break; } @@ -521,7 +496,7 @@ bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & this->planningOutputCallback(module.node_name, msg); }; auto subscriber = this->create_subscription( - module.topic_address, 1, callback, createSubscriptionOptions(this)); + module.topic_address, 1, callback, createSubscriptionOptions()); subscribers_.push_back(subscriber); break; } @@ -547,6 +522,15 @@ bool ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & void ReactionAnalyzerNode::initPredictedObjects() { + auto generateUUIDMsg = [](const std::string & input) { + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(input); + + unique_identifier_msgs::msg::UUID uuid_msg; + std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); + return uuid_msg; + }; + PredictedObject obj; geometry_msgs::msg::Vector3 dimension; @@ -675,6 +659,37 @@ void ReactionAnalyzerNode::initEgoForTest( return; } } + +rclcpp::SubscriptionOptions ReactionAnalyzerNode::createSubscriptionOptions() +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} + +void ReactionAnalyzerNode::callOperationModeServiceWithoutResponse() +{ + auto req = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), "client request"); + + if (!client_change_to_autonomous_->service_is_ready()) { + RCLCPP_INFO(this->get_logger(), "client is unavailable"); + return; + } + + client_change_to_autonomous_->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + this->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); +} + } // namespace reaction_analyzer #include