途中経過

mock/cpp_mock_scenarios/CMakeLists.txt

@@ -40,19 +40,19 @@ target_link_libraries(traffic_simulation_demo cpp_scenario_node)
 
 option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" OFF)
 if(BUILD_CPP_MOCK_SCENARIOS)
-  add_subdirectory(src/behavior_plugin)
-  add_subdirectory(src/collision)
-  add_subdirectory(src/crosswalk)
-  add_subdirectory(src/follow_front_entity)
-  add_subdirectory(src/follow_lane)
-  add_subdirectory(src/lane_change)
-  add_subdirectory(src/measurement)
-  add_subdirectory(src/merge)
-  add_subdirectory(src/metrics)
-  add_subdirectory(src/move_backward)
-  add_subdirectory(src/pedestrian)
-  add_subdirectory(src/random_scenario)
-  add_subdirectory(src/speed_planning)
+  # add_subdirectory(src/behavior_plugin)
+  # add_subdirectory(src/collision)
+  # add_subdirectory(src/crosswalk)
+  # add_subdirectory(src/follow_front_entity)
+  # add_subdirectory(src/follow_lane)
+  # add_subdirectory(src/lane_change)
+  # add_subdirectory(src/measurement)
+  # add_subdirectory(src/merge)
+  # add_subdirectory(src/metrics)
+  # add_subdirectory(src/move_backward)
+  # add_subdirectory(src/pedestrian)
+  # add_subdirectory(src/random_scenario)
+  # add_subdirectory(src/speed_planning)
   add_subdirectory(src/synchronized_action)
 endif()
 

mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp

@@ -66,6 +66,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode
     if (api_.checkCollision("ego", "npc")) {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
+
   }
   void onInitialize() override
   {

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -857,7 +857,8 @@ auto EntityBase::getIfArrivedToTargetLaneletPose(
   return false;
 }
 
-/** WARNING***************
+/**
+ * WARNING*****WIP*********
  * This should not be called from ego entity
  */
 auto EntityBase::requestSynchronize(
@@ -869,9 +870,17 @@ auto EntityBase::requestSynchronize(
   }
 
   job_list_.append(
-    [this, ego_target, entity_target](double) -> bool {
-      // WARNING****************
-      // not checking job_duration_ at all
+    [this, ego_target, entity_target](double) {
+      /**
+       * @brief This is a simple example of synchronization.
+       * It is not a good way to synchronize the entity with the ego.
+       * It is just a simple example.
+       * WARNING****************
+          not checking job_duration_ at all
+       */
+
+      RCLCPP_ERROR(
+        rclcpp::get_logger("traffic_simulator"), "fuck off");
       const auto entity_distance = getDistanceToTargetLaneletPose(entity_target);
       const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance(
         static_cast<LaneletPose>(other_status_.find("ego")->second.getLaneletPose()),
@@ -885,11 +894,9 @@ auto EntityBase::requestSynchronize(
         THROW_SEMANTIC_ERROR(
           "Failed to get ego's distance to target lanelet pose. Please check ego distance exists.");
       }
-
       const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x;
       // be better to use acceleration,jerk to estimate the arrival time
 
-      RCLCPP_ERROR(rclcpp::get_logger("simulation"), "#####################");
       // estimate ego's arrival time to the target point
       // can estimate it more precisely by using accel,jerk
       const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0;
@@ -899,10 +906,11 @@ auto EntityBase::requestSynchronize(
       const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time;
 
       this->requestSpeedChange(entity_velocity_to_synchronize, true);
+      RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "fuck off");
       return true;
     },
     // after this im not sure it is correct, just an draft
-    [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE);
+    [](){}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE);
   return false;
 }
 

simulation/traffic_simulator/src/entity/entity_manager.cpp

@@ -756,6 +756,8 @@ auto EntityManager::updateNpcLogic(
     std::cout << "update " << name << " behavior" << std::endl;
   }
   entities_[name]->setEntityTypeList(type_list);
+  RCLCPP_ERROR_STREAM(
+    rclcpp::get_logger("EntityManager"), "updateNpcLogic: " << name << " " << current_time_);
   entities_[name]->onUpdate(current_time_, step_time_);
   return entities_[name]->getStatus();
 }
@@ -781,9 +783,11 @@ void EntityManager::update(const double current_time, const double step_time)
     entity->setOtherStatus(all_status);
   }
   all_status.clear();
+
   for (auto && [name, entity] : entities_) {
     all_status.emplace(name, updateNpcLogic(name, type_list));
   }
+
   for (auto && [name, entity] : entities_) {
     entity->setOtherStatus(all_status);
   }

simulation/traffic_simulator/src/entity/vehicle_entity.cpp

@@ -171,6 +171,8 @@ void VehicleEntity::onUpdate(double current_time, double step_time)
     updateEntityStatusTimestamp(current_time);
   }
   EntityBase::onPostUpdate(current_time, step_time);
+  RCLCPP_ERROR_STREAM(
+    rclcpp::get_logger("VehicleEntity"), "#############ERROR SOLVED################");
 }
 
 void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose)

simulation/traffic_simulator/src/job/job.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <traffic_simulator/job/job.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace traffic_simulator
 {
@@ -34,18 +35,28 @@ Job::Job(
 void Job::inactivate()
 {
   status_ = Status::INACTIVE;
+  RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "inactivate start");
+  if(!func_on_cleanup_){
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "func_on_cleanup_ is null");
+    return;
+  }
   func_on_cleanup_();
+  RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "done inactivate");
 }
 
 Status Job::getStatus() const { return status_; }
 
 void Job::onUpdate(const double step_time)
 {
+  RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "onupdate job type: " << (int)type);
+  RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "onupdate job status: " << (int)status_);
   switch (status_) {
     case Status::ACTIVE:
       if (func_on_update_(job_duration_)) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "inactivate");
         inactivate();
       }
+      RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "victoiry");
       job_duration_ = job_duration_ + step_time;
       return;
     case Status::INACTIVE:

simulation/traffic_simulator/src/job/job_list.cpp

@@ -33,10 +33,17 @@ void JobList::append(
 
 void JobList::update(const double step_time, const job::Event event)
 {
+  // show all jobs in the listｚ
   for (auto & job : list_) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "job type: " << (int)job.type);
+  }
+
+  for (auto & job : list_) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "executing job type: " << (int)job.type);
     if (job.event == event) {
       job.onUpdate(step_time);
     }
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "job update done");
   }
 }
 }  // namespace job