From aef1c766574127fb5b7db4bfcac2e3fe09e0883b Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Thu, 18 Jan 2024 18:15:24 +0300 Subject: [PATCH] update --- .../src/reaction_analyzer_node.cpp | 49 ++++++++++++++----- 1 file changed, 38 insertions(+), 11 deletions(-) diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 6ea209f226445..c98b3c7d1ee00 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -118,7 +118,6 @@ void ReactionAnalyzerNode::pointcloud2OutputCallback( // reacted return; } - std::cout << "1 " << std::endl; // transform pointcloud geometry_msgs::msg::TransformStamped transform_stamped{}; @@ -267,7 +266,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options) entity_pointcloud_ptr_ = std::make_shared(); -// // initialization of test environment + // // initialization of test environment if (!loadChainModules()) { RCLCPP_ERROR( get_logger(), "Modules in chain are invalid. Node couldn't be initialized. Failed."); @@ -432,15 +431,43 @@ void ReactionAnalyzerNode::onTimer() printResults(message_buffers); } } else { - // if ( - // !initialization_state_ptr || - // initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { - std::unique_ptr msg_cloud_pub = - std::make_unique(); - *msg_cloud_pub = *msg_cloud_empty_; - msg_cloud_pub->header.stamp = this->get_clock()->now(); - pub_pointcloud_->publish(std::move(msg_cloud_pub)); - // } + if ( + !initialization_state_ptr || + initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { + std::unique_ptr msg_cloud_pub = std::make_unique(); + *msg_cloud_pub = *msg_cloud_empty_; + msg_cloud_pub->header.stamp = this->now(); + pub_pointcloud_->publish(std::move(msg_cloud_pub)); + } else { + if (!last_test_environment_init_time_) { + last_test_environment_init_time_ = this->now(); + std::unique_ptr msg_cloud_pub = std::make_unique(); + *msg_cloud_pub = *msg_cloud_empty_; + msg_cloud_pub->header.stamp = this->now(); + pub_pointcloud_->publish(std::move(msg_cloud_pub)); + return; + } else { + rclcpp::Duration time_diff = this->now() - last_test_environment_init_time_.value(); + if (time_diff > rclcpp::Duration::from_seconds(10.0)) { + if (!spawn_cmd_time) { + spawn_cmd_time = this->now(); + mutex_.lock(); + spawn_cmd_time_ = spawn_cmd_time; + mutex_.unlock(); + std::cout << "SPAWNED" << std::endl; + } + std::unique_ptr msg_cloud_pub = std::make_unique(); + *msg_cloud_pub = *msg_cloud_with_obj_; + msg_cloud_pub->header.stamp = this->now(); + pub_pointcloud_->publish(std::move(msg_cloud_pub)); + } else{ + std::unique_ptr msg_cloud_pub = std::make_unique(); + *msg_cloud_pub = *msg_cloud_empty_; + msg_cloud_pub->header.stamp = this->now(); + pub_pointcloud_->publish(std::move(msg_cloud_pub)); + } + } + } } }