From c4a673d4f275d0eb3741d7faa16b17d4bc9e80fc Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 15 Feb 2024 16:16:08 +0900 Subject: [PATCH] =?UTF-8?q?=E9=80=94=E4=B8=AD=E7=B5=8C=E9=81=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mock/cpp_mock_scenarios/CMakeLists.txt | 26 +++++++++---------- .../synchronized_action.cpp | 1 + .../src/entity/entity_base.cpp | 22 +++++++++++----- .../src/entity/entity_manager.cpp | 4 +++ .../src/entity/vehicle_entity.cpp | 2 ++ simulation/traffic_simulator/src/job/job.cpp | 11 ++++++++ .../traffic_simulator/src/job/job_list.cpp | 7 +++++ 7 files changed, 53 insertions(+), 20 deletions(-) diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 0a669497827..a65c755663d 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/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() diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp index 028dea17f23..5c234ab5f23 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/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 { diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index d6cef2da7a4..d79086555df 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/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(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; } diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index baf99026b2e..1aa6d324bcf 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/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); } diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index f2718d6ff2f..9b7de59313c 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/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) diff --git a/simulation/traffic_simulator/src/job/job.cpp b/simulation/traffic_simulator/src/job/job.cpp index 10ed0495204..9c03049b811 100644 --- a/simulation/traffic_simulator/src/job/job.cpp +++ b/simulation/traffic_simulator/src/job/job.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include 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: diff --git a/simulation/traffic_simulator/src/job/job_list.cpp b/simulation/traffic_simulator/src/job/job_list.cpp index e292125998d..27d6724bbfe 100644 --- a/simulation/traffic_simulator/src/job/job_list.cpp +++ b/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