update

brkay54 · Jan 18, 2024 · aef1c76 · aef1c76
1 parent 5465962
commit aef1c76
Showing 1 changed file with 38 additions and 11 deletions.
diff --git 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<PointCloud2>();
 
-//  // 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<PointCloud2> msg_cloud_pub =
-      std::make_unique<PointCloud2>();
-    *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<PointCloud2> msg_cloud_pub = std::make_unique<PointCloud2>();
+      *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<PointCloud2> msg_cloud_pub = std::make_unique<PointCloud2>();
+        *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<PointCloud2> msg_cloud_pub = std::make_unique<PointCloud2>();
+          *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<PointCloud2> msg_cloud_pub = std::make_unique<PointCloud2>();
+          *msg_cloud_pub = *msg_cloud_empty_;
+          msg_cloud_pub->header.stamp = this->now();
+          pub_pointcloud_->publish(std::move(msg_cloud_pub));
+        }
+      }
+    }
   }
 }