From 45db78d989f44657e7420e76b2e4a4eace028f14 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Tue, 16 Jan 2024 16:15:19 +0900 Subject: [PATCH 01/18] Add draft functions for synchronized action --- .../src/entity/entity_base.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index fff0eae220d..c0c9437c703 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -818,5 +818,32 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double return traveled_distance_; } +auto EntityBase::getDistanceToSyncronizationPosition( + const lanelet::Id & + intersection_id // dont know about this, maybe i can get the other infomation? + const double stop_position // may be if the info givven is only the intersection, perhaps needs + // info about how far the stop position is from the intersection + ) -> double +{ + // return distance to the nearest intersection on the route from ego + return entity_distance_to_syncronization_position_; +} + +auto EntityBase::adjustElementVelocityByIntersection( + const double ego_distance_to_intersection, const double max_acceleration, const double max_jerk, ) +{ + double calculated_velocity = 0.0; + // get ego info inside the function + // adjusting velocity of the element by the distance to the intersection + requestSpeedChange(calculated_velocity, false); +} + +void EntityBase::setSynchronizedAction() +{ + // appending synchronized action to job list + // job_list_.append( + // adjustElemetVelocityByIntersection(getDistanceToSyncronizationPosition(hoge,fuga), hoge, piyo), +} + } // namespace entity } // namespace traffic_simulator From aa28c60666641630bfe1ec787debf5b92dda65c9 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Tue, 16 Jan 2024 17:35:24 +0900 Subject: [PATCH 02/18] changed the function and made it simple --- .../src/entity/entity_base.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index c0c9437c703..47439ad7015 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -818,31 +818,25 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double return traveled_distance_; } -auto EntityBase::getDistanceToSyncronizationPosition( - const lanelet::Id & - intersection_id // dont know about this, maybe i can get the other infomation? +auto EntityBase::getDistanceToTargetLaneletPose( + const CanonicalizedLaneletPose intersection_id // dont know about this, maybe i can get the other infomation? const double stop_position // may be if the info givven is only the intersection, perhaps needs // info about how far the stop position is from the intersection ) -> double { // return distance to the nearest intersection on the route from ego - return entity_distance_to_syncronization_position_; + return entity_distance_to_target_position_; } -auto EntityBase::adjustElementVelocityByIntersection( - const double ego_distance_to_intersection, const double max_acceleration, const double max_jerk, ) +void EntityBase::requestSynchronize() { - double calculated_velocity = 0.0; - // get ego info inside the function - // adjusting velocity of the element by the distance to the intersection - requestSpeedChange(calculated_velocity, false); -} - -void EntityBase::setSynchronizedAction() -{ - // appending synchronized action to job list // job_list_.append( - // adjustElemetVelocityByIntersection(getDistanceToSyncronizationPosition(hoge,fuga), hoge, piyo), + // [this]() { + // auto entity_distance = getDistanceToTargetLaneletPose(hoge,piyo); + // calculating all things and may also set speed + // requestSpeedChange(entity_speed, false); + // }, + // [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); } } // namespace entity From bdb284d270c129872c67c2c48932ea2898af19e3 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 18 Jan 2024 11:57:01 +0900 Subject: [PATCH 03/18] Implemented a draft of getDistanceToTargetLaneletPose and requestSynchronize methods --- .../src/entity/entity_base.cpp | 55 +++++++++++++++---- 1 file changed, 43 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 47439ad7015..064b18182b1 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -819,24 +819,55 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double } auto EntityBase::getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose intersection_id // dont know about this, maybe i can get the other infomation? + const CanonicalizedLaneletPose target_lanelet_pose, const double stop_position // may be if the info givven is only the intersection, perhaps needs // info about how far the stop position is from the intersection - ) -> double + ) -> std::optional { - // return distance to the nearest intersection on the route from ego - return entity_distance_to_target_position_; + // what does lanelet look like in the target, is it point? + // check if the argument lanelt is point + if(hoge){ + // give error + } + // get the lanelet pose of the entity + const auto entity_lanelet_pose = getLaneletPose(); + // check if the entity is on the lanelet + if(entity_lanelet_pose){ + const auto entity_distance_to_intersection = hdmap_utils_ptr_->getLongitudinalDistance( + entity_lanelet_pose.value().lanelet_id, LaneletPose(target_lanelet_pose).value().lanelet_id,false,false); + } + + // may be give error here too? + return false; } -void EntityBase::requestSynchronize() +void EntityBase::requestSynchronize( + const CanonicalizedLaneletPose ego_target, + const CanonicalizedLaneletPose entity_target, + const double ego_target_lanelet_stop_position, //may be any data type? + const double entity_target_lanelet_stop_position //may be any data type? +) { - // job_list_.append( - // [this]() { - // auto entity_distance = getDistanceToTargetLaneletPose(hoge,piyo); - // calculating all things and may also set speed - // requestSpeedChange(entity_speed, false); - // }, - // [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); + job_list_.append( + [this]() { + const auto entity_distance = getDistanceToTargetLaneletPose(entity_target, entity_target_lanelet_stop_position); + const auto ego_distance = getDistanceToTargetLaneletPose(ego_target, ego_target_lanelet_stop_position); + const auto entity_velocity = getCurrentTwist().linear.x; + const auto ego_velocity = other_status_.at("ego").getCurrentTwist().linear.x; + // be better to use acceleration,jerk to estimate the arrival time + + // estimate ego's arrival time to the target point + // can estimate it more precisely by using accel,jerk + const auto ego_arrival_time = ego_distance / ego_velocity; + + // calculate the speed of entity to synchronize with ego + // realy just a simple example, kind of like a joke + const auto entity_velocity_to_synchronize = entity_distance / ego_arrival_time; + + this.requestSpeedChange(entity_velocity_to_synchronize, false); + }, + // after this im not sure it is correct, just an draft + [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); } } // namespace entity From 435e9868fdb96863c6a5924f364ee132790b7737 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 18 Jan 2024 17:26:13 +0900 Subject: [PATCH 04/18] fixed revert and errors --- .../traffic_simulator/entity/entity_base.hpp | 7 ++++ .../src/entity/entity_base.cpp | 38 +++++++++---------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 68c49c9b560..c04aedf5095 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -229,6 +229,13 @@ class EntityBase /* */ auto updateTraveledDistance(const double step_time) -> double; + /* */ auto getDistanceToTargetLaneletPose( + const CanonicalizedLaneletPose target_lanelet_pose) + -> std::optional; + + /* */ void requestSynchronize(const CanonicalizedLaneletPose ego_target, + const CanonicalizedLaneletPose entity_target); + virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 064b18182b1..01bc39e23f1 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -819,22 +819,24 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double } auto EntityBase::getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose target_lanelet_pose, - const double stop_position // may be if the info givven is only the intersection, perhaps needs - // info about how far the stop position is from the intersection - ) -> std::optional -{ - // what does lanelet look like in the target, is it point? - // check if the argument lanelt is point - if(hoge){ + const CanonicalizedLaneletPose target_lanelet_pose)-> std::optional +{ + constexpr double matching_distance = 100.0; // may be better to use a parameter + + // check if the argument lanelet is point + if(true){ // give error } - // get the lanelet pose of the entity - const auto entity_lanelet_pose = getLaneletPose(); + + const auto entity_lanelet_pose_ = getLaneletPose(matching_distance).LaneletPose(matching_distance); + const auto target_lanelet_pose_ = target_lanelet_pose.LaneletPose(matching_distance); // check if the entity is on the lanelet if(entity_lanelet_pose){ const auto entity_distance_to_intersection = hdmap_utils_ptr_->getLongitudinalDistance( - entity_lanelet_pose.value().lanelet_id, LaneletPose(target_lanelet_pose).value().lanelet_id,false,false); + entity_lanelet_pose_. + target_lanelet_pose_.value().lanelet_id, + false, + false); } // may be give error here too? @@ -843,17 +845,15 @@ auto EntityBase::getDistanceToTargetLaneletPose( void EntityBase::requestSynchronize( const CanonicalizedLaneletPose ego_target, - const CanonicalizedLaneletPose entity_target, - const double ego_target_lanelet_stop_position, //may be any data type? - const double entity_target_lanelet_stop_position //may be any data type? + const CanonicalizedLaneletPose entity_target ) { job_list_.append( - [this]() { - const auto entity_distance = getDistanceToTargetLaneletPose(entity_target, entity_target_lanelet_stop_position); - const auto ego_distance = getDistanceToTargetLaneletPose(ego_target, ego_target_lanelet_stop_position); + [this,ego_target,entitiy_target]() { + const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); + const auto ego_distance = getDistanceToTargetLaneletPose(ego_target); const auto entity_velocity = getCurrentTwist().linear.x; - const auto ego_velocity = other_status_.at("ego").getCurrentTwist().linear.x; + const auto ego_velocity = other_status_.find("ego").getCurrentTwist().linear.x; // be better to use acceleration,jerk to estimate the arrival time // estimate ego's arrival time to the target point @@ -861,7 +861,7 @@ void EntityBase::requestSynchronize( const auto ego_arrival_time = ego_distance / ego_velocity; // calculate the speed of entity to synchronize with ego - // realy just a simple example, kind of like a joke + // really just a simple example, kind of like a joke const auto entity_velocity_to_synchronize = entity_distance / ego_arrival_time; this.requestSpeedChange(entity_velocity_to_synchronize, false); From cbde06e14c885a17d6e8e92c466b2fbccb80c1c7 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 18 Jan 2024 18:01:15 +0900 Subject: [PATCH 05/18] error is not fixed but pushing today's implements --- .../src/entity/entity_base.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 01bc39e23f1..b2ab6d23dc9 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -823,20 +823,15 @@ auto EntityBase::getDistanceToTargetLaneletPose( { constexpr double matching_distance = 100.0; // may be better to use a parameter - // check if the argument lanelet is point - if(true){ - // give error - } + const auto entity_lanelet_pose_ = getLaneletPose(matching_distance); + const auto target_lanelet_pose_ = target_lanelet_pose; - const auto entity_lanelet_pose_ = getLaneletPose(matching_distance).LaneletPose(matching_distance); - const auto target_lanelet_pose_ = target_lanelet_pose.LaneletPose(matching_distance); // check if the entity is on the lanelet - if(entity_lanelet_pose){ - const auto entity_distance_to_intersection = hdmap_utils_ptr_->getLongitudinalDistance( - entity_lanelet_pose_. - target_lanelet_pose_.value().lanelet_id, - false, - false); + if(entity_lanelet_pose_){ + const auto entity_distance_to_intersection = + hdmap_utils_ptr_->getLongitudinalDistance( + entity_lanelet_pose_.value().operator traffic_simulator::LaneletPose().lanelet_id, + target_lanelet_pose_.operator traffic_simulator::LaneletPose().lanelet_id); } // may be give error here too? From bced89ebbeab689b9e935eac76dbe5f939745843 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 18 Jan 2024 18:06:43 +0900 Subject: [PATCH 06/18] Fix distance calculation in EntityBase::getDistanceToTargetLaneletPose() --- simulation/traffic_simulator/src/entity/entity_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index b2ab6d23dc9..c8621f0a347 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -830,8 +830,8 @@ auto EntityBase::getDistanceToTargetLaneletPose( if(entity_lanelet_pose_){ const auto entity_distance_to_intersection = hdmap_utils_ptr_->getLongitudinalDistance( - entity_lanelet_pose_.value().operator traffic_simulator::LaneletPose().lanelet_id, - target_lanelet_pose_.operator traffic_simulator::LaneletPose().lanelet_id); + static_cast(entity_lanelet_pose_.value()), + static_cast(target_lanelet_pose_)); } // may be give error here too? From 4cf36b860f0b0f5d8d423b85d1c7e9aa90dae7fe Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Wed, 24 Jan 2024 10:30:18 +0900 Subject: [PATCH 07/18] Fix synchronization issue in EntityBase class --- .../traffic_simulator/src/entity/entity_base.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index c8621f0a347..afc01db70d3 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -828,10 +828,12 @@ auto EntityBase::getDistanceToTargetLaneletPose( // check if the entity is on the lanelet if(entity_lanelet_pose_){ - const auto entity_distance_to_intersection = + const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( static_cast(entity_lanelet_pose_.value()), static_cast(target_lanelet_pose_)); + + return entity_distance_to_target; } // may be give error here too? @@ -844,22 +846,22 @@ void EntityBase::requestSynchronize( ) { job_list_.append( - [this,ego_target,entitiy_target]() { + [this,ego_target,entity_target](double) { const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); const auto ego_distance = getDistanceToTargetLaneletPose(ego_target); const auto entity_velocity = getCurrentTwist().linear.x; - const auto ego_velocity = other_status_.find("ego").getCurrentTwist().linear.x; + const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x; // be better to use acceleration,jerk to estimate the arrival time // estimate ego's arrival time to the target point // can estimate it more precisely by using accel,jerk - const auto ego_arrival_time = ego_distance / ego_velocity; + const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0; // calculate the speed of entity to synchronize with ego // really just a simple example, kind of like a joke - const auto entity_velocity_to_synchronize = entity_distance / ego_arrival_time; + const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time; - this.requestSpeedChange(entity_velocity_to_synchronize, false); + this->requestSpeedChange(entity_velocity_to_synchronize, false); }, // after this im not sure it is correct, just an draft [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); From 36d4697c59e38593894632785054b63424ab3776 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Wed, 24 Jan 2024 11:42:46 +0900 Subject: [PATCH 08/18] implemented atleast functions with no error --- simulation/traffic_simulator/src/entity/entity_base.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index afc01db70d3..6e800075f41 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -846,10 +846,10 @@ void EntityBase::requestSynchronize( ) { job_list_.append( - [this,ego_target,entity_target](double) { + [this,ego_target,entity_target](double)->bool { const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); const auto ego_distance = getDistanceToTargetLaneletPose(ego_target); - const auto entity_velocity = getCurrentTwist().linear.x; + // const auto entity_velocity = getCurrentTwist().linear.x; const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x; // be better to use acceleration,jerk to estimate the arrival time @@ -862,6 +862,8 @@ void EntityBase::requestSynchronize( const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time; this->requestSpeedChange(entity_velocity_to_synchronize, false); + + return false; }, // after this im not sure it is correct, just an draft [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); From 4bb9f9234498b3d50e90c4433b094067e25ffac4 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Wed, 24 Jan 2024 14:06:46 +0900 Subject: [PATCH 09/18] tried to format --- .../src/entity/entity_base.cpp | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 6e800075f41..6a8f9a39bdd 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -818,20 +818,19 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double return traveled_distance_; } -auto EntityBase::getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose target_lanelet_pose)-> std::optional +auto EntityBase::getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose target_lanelet_pose) + -> std::optional { - constexpr double matching_distance = 100.0; // may be better to use a parameter + constexpr double matching_distance = 100.0; // may be better to use a parameter const auto entity_lanelet_pose_ = getLaneletPose(matching_distance); const auto target_lanelet_pose_ = target_lanelet_pose; // check if the entity is on the lanelet - if(entity_lanelet_pose_){ - const auto entity_distance_to_target = - hdmap_utils_ptr_->getLongitudinalDistance( - static_cast(entity_lanelet_pose_.value()), - static_cast(target_lanelet_pose_)); + if (entity_lanelet_pose_) { + const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( + static_cast(entity_lanelet_pose_.value()), + static_cast(target_lanelet_pose_)); return entity_distance_to_target; } @@ -841,12 +840,10 @@ auto EntityBase::getDistanceToTargetLaneletPose( } void EntityBase::requestSynchronize( - const CanonicalizedLaneletPose ego_target, - const CanonicalizedLaneletPose entity_target -) + const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target) { job_list_.append( - [this,ego_target,entity_target](double)->bool { + [this, ego_target, entity_target](double) -> bool { const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); const auto ego_distance = getDistanceToTargetLaneletPose(ego_target); // const auto entity_velocity = getCurrentTwist().linear.x; From 59ab9c786fce21f95bb52cd594394e6fec12a9f0 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Wed, 24 Jan 2024 17:18:38 +0900 Subject: [PATCH 10/18] Add requestSynchronize method to API and EntityManager and few bug fix --- .../include/traffic_simulator/api/api.hpp | 1 + .../include/traffic_simulator/entity/entity_base.hpp | 9 ++++----- .../include/traffic_simulator/entity/entity_manager.hpp | 1 + simulation/traffic_simulator/src/entity/entity_base.cpp | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index b66ec2b09a7..0bcc3de511e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -330,6 +330,7 @@ class API FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); + FORWARD_TO_ENTITY_MANAGER(requestSynchronize); FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index c04aedf5095..c1331c401c3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -229,12 +229,11 @@ class EntityBase /* */ auto updateTraveledDistance(const double step_time) -> double; - /* */ auto getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose target_lanelet_pose) - -> std::optional; + /* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose target_lanelet_pose) + -> std::optional; - /* */ void requestSynchronize(const CanonicalizedLaneletPose ego_target, - const CanonicalizedLaneletPose entity_target); + /* */ void requestSynchronize( + const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target); virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index aa1b4becaff..df7a2c690b9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -311,6 +311,7 @@ class EntityManager FORWARD_TO_ENTITY(requestAssignRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); + FORWARD_TO_ENTITY(requestSynchronize, ); FORWARD_TO_ENTITY(requestWalkStraight, ); FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 6a8f9a39bdd..f9f8b903900 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -860,7 +860,7 @@ void EntityBase::requestSynchronize( this->requestSpeedChange(entity_velocity_to_synchronize, false); - return false; + return true; }, // after this im not sure it is correct, just an draft [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); From a42a80178836bc8716323adafcbbc0b4c701e1ed Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Wed, 24 Jan 2024 17:52:02 +0900 Subject: [PATCH 11/18] Made draft mock scenarios --- mock/cpp_mock_scenarios/CMakeLists.txt | 3 +- .../src/synchronized_action/CMakeLists.txt | 14 +++ .../synchronized_action.cpp | 85 +++++++++++++++++++ 3 files changed, 101 insertions(+), 1 deletion(-) create mode 100644 mock/cpp_mock_scenarios/src/synchronized_action/CMakeLists.txt create mode 100644 mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 9d6e37a592a..9ff0edf7542 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -38,7 +38,7 @@ ament_auto_add_executable(traffic_simulation_demo ) target_link_libraries(traffic_simulation_demo cpp_scenario_node) -option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" OFF) +option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" ON) if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/behavior_plugin) add_subdirectory(src/collision) @@ -53,6 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/pedestrian) add_subdirectory(src/random_scenario) add_subdirectory(src/speed_planning) + add_subdirectory(src/synchronized_action) endif() install( diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/CMakeLists.txt b/mock/cpp_mock_scenarios/src/synchronized_action/CMakeLists.txt new file mode 100644 index 00000000000..62e2a15cb49 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/synchronized_action/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(synchronized_action + synchronized_action.cpp +) +target_link_libraries(synchronized_action cpp_scenario_node) + +install(TARGETS + synchronized_action + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "synchronized_action" "15.0") +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 new file mode 100644 index 00000000000..c0be72a59e0 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -0,0 +1,85 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +namespace cpp_mock_scenarios +{ +class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit SynchronizedAction(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "synchronized_action", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + bool requested = false; + void onUpdate() override + { + if (api_.requestSynchronize()) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_START + if (api_.getCurrentTime() >= 10.0) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if (api_.checkCollision("ego", "npc")) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + void onInitialize() override + { + api_.spawn( + "ego", + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34462, 15, 0, 0, 0, 0)), + getVehicleParameters()); + api_.setLinearVelocity("ego", 5); + api_.requestSpeedChange("ego", 5, true); + api_.requestLaneChange("ego", 34513); + + api_.spawn( + "npc", + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 0, 0, 0, 0, 0)), + getVehicleParameters()); + api_.setLinearVelocity("npc", 10); + } +}; +} // namespace cpp_mock_scenarios + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} From ab572de465bc5dabd51c691555fc3baa1092f091 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Mon, 29 Jan 2024 17:00:47 +0900 Subject: [PATCH 12/18] Disable building of C++ mock scenarios and update requestSynchronize function --- mock/cpp_mock_scenarios/CMakeLists.txt | 2 +- .../src/synchronized_action/synchronized_action.cpp | 13 ++++++++++++- .../traffic_simulator/entity/entity_base.hpp | 4 ++-- .../traffic_simulator/src/entity/entity_base.cpp | 5 +++-- 4 files changed, 18 insertions(+), 6 deletions(-) diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 9ff0edf7542..0a669497827 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -38,7 +38,7 @@ ament_auto_add_executable(traffic_simulation_demo ) target_link_libraries(traffic_simulation_demo cpp_scenario_node) -option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" ON) +option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" OFF) if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/behavior_plugin) add_subdirectory(src/collision) 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 c0be72a59e0..1936bc53076 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -44,7 +44,12 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.requestSynchronize()) { + const traffic_simulator::CanonicalizedLaneletPose ego_target = + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0, 0, 0, 0, 0)); + const traffic_simulator::CanonicalizedLaneletPose npc_target = + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34462, 15, 0, 0, 0, 0)); + + if (api_.requestSynchronize("npc", ego_target, npc_target)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START @@ -71,6 +76,12 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode getVehicleParameters()); api_.setLinearVelocity("npc", 10); } + + auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) + -> traffic_simulator::CanonicalizedLaneletPose + { + return api_.canonicalize(lanelet_pose); + } }; } // namespace cpp_mock_scenarios diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index c1331c401c3..c5674ac3be6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -232,8 +232,8 @@ class EntityBase /* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose target_lanelet_pose) -> std::optional; - /* */ void requestSynchronize( - const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target); + /* */ auto requestSynchronize( + const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target) -> bool; virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index f9f8b903900..5b13c0547f9 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -839,8 +839,8 @@ auto EntityBase::getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose t return false; } -void EntityBase::requestSynchronize( - const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target) +auto EntityBase::requestSynchronize( + const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target)->bool { job_list_.append( [this, ego_target, entity_target](double) -> bool { @@ -864,6 +864,7 @@ void EntityBase::requestSynchronize( }, // after this im not sure it is correct, just an draft [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); + return true; } } // namespace entity From 0c5fb8b1d1f7badcfb92eafca6acf08d8a978507 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 1 Feb 2024 18:03:40 +0900 Subject: [PATCH 13/18] Refactor synchronization logic and add new API method --- .../synchronized_action.cpp | 33 ++++++----- .../include/traffic_simulator/api/api.hpp | 1 + .../traffic_simulator/entity/entity_base.hpp | 8 ++- .../entity/entity_manager.hpp | 1 + .../src/entity/entity_base.cpp | 57 ++++++++++++++++--- 5 files changed, 76 insertions(+), 24 deletions(-) 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 1936bc53076..028dea17f23 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -42,18 +42,25 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode private: bool requested = false; + const traffic_simulator::CanonicalizedLaneletPose ego_target = + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34510, 0, 0, 0, 0, 0)); + const traffic_simulator::CanonicalizedLaneletPose npc_target = + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34495, 15, 0, 0, 0, 0)); + void onUpdate() override { - const traffic_simulator::CanonicalizedLaneletPose ego_target = - getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0, 0, 0, 0, 0)); - const traffic_simulator::CanonicalizedLaneletPose npc_target = - getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34462, 15, 0, 0, 0, 0)); - - if (api_.requestSynchronize("npc", ego_target, npc_target)) { + // SUCCESS + // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped + if ( + api_.requestSynchronize("npc", ego_target, npc_target, 0.2) && + // api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && + // api_.getCurrentTwist("npc").linear.x == 0.0) { + true) { stop(cpp_mock_scenarios::Result::SUCCESS); } - // LCOV_EXCL_START - if (api_.getCurrentTime() >= 10.0) { + + // FAILURES + if (api_.getCurrentTime() >= 20.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.checkCollision("ego", "npc")) { @@ -64,17 +71,17 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode { api_.spawn( "ego", - api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34462, 15, 0, 0, 0, 0)), + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 15, 0, 0, 0, 0)), getVehicleParameters()); api_.setLinearVelocity("ego", 5); - api_.requestSpeedChange("ego", 5, true); - api_.requestLaneChange("ego", 34513); + // api_.requestSpeedChange("ego", 5, true); + // api_.requestLaneChange("ego", 34513); api_.spawn( "npc", - api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 0, 0, 0, 0, 0)), + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34564, 5, 0, 0, 0, 0)), getVehicleParameters()); - api_.setLinearVelocity("npc", 10); + api_.setLinearVelocity("npc", 5); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 0bcc3de511e..80ee3b44720 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -322,6 +322,7 @@ class API FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); + FORWARD_TO_ENTITY_MANAGER(getIfArrivedToTargetLaneletPose); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isInLanelet); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index c5674ac3be6..bd8b0ca17ba 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -229,11 +229,15 @@ class EntityBase /* */ auto updateTraveledDistance(const double step_time) -> double; - /* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose target_lanelet_pose) + /* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose & target_lanelet_pose) -> std::optional; + /* */ auto getIfArrivedToTargetLaneletPose( + const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool; + /* */ auto requestSynchronize( - const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target) -> bool; + const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, + const double threshold) -> bool; virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index df7a2c690b9..74a977f5909 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -298,6 +298,7 @@ class EntityManager FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const); FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityType, const); + FORWARD_TO_ENTITY(getIfArrivedToTargetLaneletPose,); FORWARD_TO_ENTITY(fillLaneletPose, const); FORWARD_TO_ENTITY(getLaneletPose, const); FORWARD_TO_ENTITY(getLinearJerk, const); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 5b13c0547f9..d6cef2da7a4 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -818,11 +818,10 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double return traveled_distance_; } -auto EntityBase::getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose target_lanelet_pose) - -> std::optional +auto EntityBase::getDistanceToTargetLaneletPose( + const CanonicalizedLaneletPose & target_lanelet_pose) -> std::optional { constexpr double matching_distance = 100.0; // may be better to use a parameter - const auto entity_lanelet_pose_ = getLaneletPose(matching_distance); const auto target_lanelet_pose_ = target_lanelet_pose; @@ -836,20 +835,61 @@ auto EntityBase::getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose t } // may be give error here too? + return std::nullopt; +} + +auto EntityBase::getIfArrivedToTargetLaneletPose( + const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool +{ + if (!getDistanceToTargetLaneletPose(target_lanelet_pose).has_value()) { + // error here + } + const auto entity_lanelet_pose_ = getLaneletPose(); + const auto target_lanelet_pose_ = target_lanelet_pose; + + const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( + static_cast(entity_lanelet_pose_.value()), + static_cast(target_lanelet_pose_)); + + if (entity_distance_to_target <= threshold) { + return true; + } return false; } +/** WARNING*************** + * This should not be called from ego entity + */ auto EntityBase::requestSynchronize( - const CanonicalizedLaneletPose ego_target, const CanonicalizedLaneletPose entity_target)->bool + const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, + const double threshold) -> bool { + if (getIfArrivedToTargetLaneletPose(entity_target, threshold)) { + return true; + } + job_list_.append( [this, ego_target, entity_target](double) -> bool { + // WARNING**************** + // not checking job_duration_ at all const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); - const auto ego_distance = getDistanceToTargetLaneletPose(ego_target); - // const auto entity_velocity = getCurrentTwist().linear.x; + const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance( + static_cast(other_status_.find("ego")->second.getLaneletPose()), + static_cast(ego_target)); + + if (!entity_distance.has_value() ){ + THROW_SEMANTIC_ERROR( + "Failed to get entity's distance to target lanelet pose. Please check entity distance exists."); + } + if (!ego_distance.has_value() ){ + 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; @@ -858,13 +898,12 @@ auto EntityBase::requestSynchronize( // really just a simple example, kind of like a joke const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time; - this->requestSpeedChange(entity_velocity_to_synchronize, false); - + this->requestSpeedChange(entity_velocity_to_synchronize, true); return true; }, // after this im not sure it is correct, just an draft [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); - return true; + return false; } } // namespace entity From c4a673d4f275d0eb3741d7faa16b17d4bc9e80fc Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 15 Feb 2024 16:16:08 +0900 Subject: [PATCH 14/18] =?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 From f18fc6855eca868349f502951af22cdda9982265 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Tue, 20 Feb 2024 18:14:35 +0900 Subject: [PATCH 15/18] Update target lanelet poses and velocities --- .../synchronized_action.cpp | 43 +++++---- .../traffic_simulator/entity/entity_base.hpp | 3 +- .../src/entity/entity_base.cpp | 95 +++++++++++-------- 3 files changed, 86 insertions(+), 55 deletions(-) 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..01e95d357c1 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -43,45 +43,56 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode private: bool requested = false; const traffic_simulator::CanonicalizedLaneletPose ego_target = - getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34510, 0, 0, 0, 0, 0)); + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34585, 0, 0, 0, 0, 0)); const traffic_simulator::CanonicalizedLaneletPose npc_target = - getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34495, 15, 0, 0, 0, 0)); + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34570, 0, 0, 0, 0, 0)); void onUpdate() override { - // SUCCESS - // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped - if ( - api_.requestSynchronize("npc", ego_target, npc_target, 0.2) && - // api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && - // api_.getCurrentTwist("npc").linear.x == 0.0) { - true) { + api_.requestSynchronize("npc", ego_target, npc_target, 0.2); + // SUCCESS + // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped + if ( + // api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && + // api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && + // api_.getCurrentTwist("npc").linear.x <= 0.1 + false + ) + { stop(cpp_mock_scenarios::Result::SUCCESS); } // FAILURES - if (api_.getCurrentTime() >= 20.0) { + if (api_.getCurrentTime() >= 40.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.checkCollision("ego", "npc")) { stop(cpp_mock_scenarios::Result::FAILURE); } + } void onInitialize() override { api_.spawn( "ego", - api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 15, 0, 0, 0, 0)), + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34976, 10, 0, 0, 0, 0)), getVehicleParameters()); - api_.setLinearVelocity("ego", 5); - // api_.requestSpeedChange("ego", 5, true); - // api_.requestLaneChange("ego", 34513); + api_.setLinearVelocity("ego", 10); + api_.requestSpeedChange("ego", 10, true); + + std::vector goal_poses; + goal_poses.emplace_back(api_.toMapPose( + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34579, 10, 0, 0, 0, 0)))); + // goal_poses.emplace_back(api_.toMapPose( + // api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34579, 10, 0, 0, 0, 0)))); + api_.requestAssignRoute("ego", goal_poses); + api_.spawn( "npc", - api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34564, 5, 0, 0, 0, 0)), + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34576, 5, 0, 0, 0, 0)), getVehicleParameters()); - api_.setLinearVelocity("npc", 5); + api_.setLinearVelocity("npc", 0); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index bd8b0ca17ba..455041bc70b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -229,7 +229,8 @@ class EntityBase /* */ auto updateTraveledDistance(const double step_time) -> double; - /* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose & target_lanelet_pose) + /* */ auto getDistanceToTargetLaneletPose( + const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance) -> std::optional; /* */ auto getIfArrivedToTargetLaneletPose( diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index d6cef2da7a4..3b625173735 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -819,77 +819,91 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double } auto EntityBase::getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose & target_lanelet_pose) -> std::optional + const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance) + -> std::optional { - constexpr double matching_distance = 100.0; // may be better to use a parameter - const auto entity_lanelet_pose_ = getLaneletPose(matching_distance); + const double matching_distance_ = matching_distance; + const auto entity_lanelet_pose_ = getLaneletPose(matching_distance_); const auto target_lanelet_pose_ = target_lanelet_pose; - // check if the entity is on the lanelet - if (entity_lanelet_pose_) { - const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( - static_cast(entity_lanelet_pose_.value()), - static_cast(target_lanelet_pose_)); + if (!entity_lanelet_pose_) { + THROW_SEMANTIC_ERROR( + "Failed to get entity's lanelet pose. Please check entity lanelet pose exists."); + } + + const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( + static_cast(entity_lanelet_pose_.value()), + static_cast(target_lanelet_pose_)); - return entity_distance_to_target; + if (!entity_distance_to_target.has_value()) { + /** + * This is not an error, means that the entity is already over the target lanelet. + */ + return std::nullopt; } - // may be give error here too? - return std::nullopt; + return entity_distance_to_target; } auto EntityBase::getIfArrivedToTargetLaneletPose( const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool { - if (!getDistanceToTargetLaneletPose(target_lanelet_pose).has_value()) { - // error here - } - const auto entity_lanelet_pose_ = getLaneletPose(); - const auto target_lanelet_pose_ = target_lanelet_pose; + /** + * WARNING*****WIP********* + * mesuring distance is set to 100 manually + */ + const auto entity_distance_to_target = getDistanceToTargetLaneletPose(target_lanelet_pose, 100); - const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( - static_cast(entity_lanelet_pose_.value()), - static_cast(target_lanelet_pose_)); + if (!entity_distance_to_target.has_value()) { + /** + * @brief meaning that the entity is already over the target lanelet. + * + */ + return true; + } if (entity_distance_to_target <= threshold) { return true; + } else { + return false; } - return false; } -/** WARNING*************** - * This should not be called from ego entity - */ auto EntityBase::requestSynchronize( const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, const double threshold) -> bool { + if (name == "ego") { + /** + * *********************CHECK********************* + * is this correct to throw syntax error? + */ + THROW_SYNTAX_ERROR("The entity that is requested to synchronize is the ego. Please check."); + } if (getIfArrivedToTargetLaneletPose(entity_target, threshold)) { return true; } job_list_.append( - [this, ego_target, entity_target](double) -> bool { - // WARNING**************** - // not checking job_duration_ at all - const auto entity_distance = getDistanceToTargetLaneletPose(entity_target); + [this, ego_target, entity_target](double) { + /** + * @brief This is draft function for synchronization. + * It is not a good way to synchronize the entity with the ego. + * WARNING**************** + * not checking job_duration_ at all + */ + const auto entity_distance = getDistanceToTargetLaneletPose(entity_target, 100); const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance( static_cast(other_status_.find("ego")->second.getLaneletPose()), static_cast(ego_target)); - if (!entity_distance.has_value() ){ + if (!entity_distance.has_value()) { THROW_SEMANTIC_ERROR( - "Failed to get entity's distance to target lanelet pose. Please check entity distance exists."); + "Entity is already over the target lanelet."); } - if (!ego_distance.has_value() ){ - 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; @@ -898,13 +912,18 @@ auto EntityBase::requestSynchronize( // really just a simple example, kind of like a joke 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"), "speed change"); + // using this->requestSpeedChange here does not work in some kind of reason. + // it seems that after this function is called by some reason, func_on_cleanup will be deleted and become nullptr + // this->requestSpeedChange(entity_velocity_to_synchronize, true); + target_speed_ = entity_velocity_to_synchronize; return true; }, // after this im not sure it is correct, just an draft - [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); + [this]() { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "sync job done"); }, + job::Type::LINEAR_VELOCITY, false, job::Event::POST_UPDATE); return false; } } // namespace entity -} // namespace traffic_simulator +} // namespace traffic_simulator \ No newline at end of file From 6e4af62e44bdef01ea687960cb2c850251a30f96 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 22 Feb 2024 18:04:51 +0900 Subject: [PATCH 16/18] Update entity synchronization logic to consider acceleration limit --- .../synchronized_action.cpp | 11 ++-- .../traffic_simulator/entity/entity_base.hpp | 2 +- .../src/entity/entity_base.cpp | 58 ++++++++++++++----- .../src/entity/vehicle_entity.cpp | 2 + .../traffic_simulator/src/job/job_list.cpp | 3 + 5 files changed, 56 insertions(+), 20 deletions(-) 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 01e95d357c1..cf24bfff909 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -49,14 +49,13 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { - api_.requestSynchronize("npc", ego_target, npc_target, 0.2); + api_.requestSynchronize("npc", ego_target, npc_target, 0.2, 2.0); // SUCCESS // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped if ( - // api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && - // api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && - // api_.getCurrentTwist("npc").linear.x <= 0.1 - false + api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && + api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && + api_.getCurrentTwist("npc").linear.x <= 0.01 ) { stop(cpp_mock_scenarios::Result::SUCCESS); @@ -92,7 +91,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode "npc", api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34576, 5, 0, 0, 0, 0)), getVehicleParameters()); - api_.setLinearVelocity("npc", 0); + api_.setLinearVelocity("npc", 10); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 455041bc70b..a6a3b1d7192 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -238,7 +238,7 @@ class EntityBase /* */ auto requestSynchronize( const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, - const double threshold) -> bool; + const double threshold, const double accel_limit) -> bool; virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 3b625173735..2fb45a3899a 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -871,7 +872,7 @@ auto EntityBase::getIfArrivedToTargetLaneletPose( auto EntityBase::requestSynchronize( const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, - const double threshold) -> bool + const double threshold, const double accel_limit) -> bool { if (name == "ego") { /** @@ -885,7 +886,7 @@ auto EntityBase::requestSynchronize( } job_list_.append( - [this, ego_target, entity_target](double) { + [this, ego_target, entity_target, accel_limit](double job_duration) { /** * @brief This is draft function for synchronization. * It is not a good way to synchronize the entity with the ego. @@ -897,31 +898,62 @@ auto EntityBase::requestSynchronize( static_cast(other_status_.find("ego")->second.getLaneletPose()), static_cast(ego_target)); + /** + * *********************CHECK********************* + * there might be a better way if the car had passed away + */ if (!entity_distance.has_value()) { - THROW_SEMANTIC_ERROR( - "Entity is already over the target lanelet."); + return true; + } + if (!ego_distance.has_value()) { + return true; } const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x; - // be better to use acceleration,jerk to estimate the arrival time + const auto entity_velocity = this->getStatus().getTwist().linear.x; - // estimate ego's arrival time to the target point - // can estimate it more precisely by using accel,jerk + // be better to use acceleration,jerk to estimate the arrival time const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0; - // calculate the speed of entity to synchronize with ego - // really just a simple example, kind of like a joke - const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time; + // if(ego_arrival_time < std::sqrt(ego_distance.value()/accel_limit)){ + // THROW_SEMANTIC_ERROR( + // "The entity can not stop on the destination by time."); + // } + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "ego arrival time: %f", ego_arrival_time); + + auto entity_velocity_to_synchronize = entity_velocity; + + if ( + (entity_velocity * ego_arrival_time) - + (entity_velocity * entity_velocity / (2 * accel_limit)) <= + entity_distance.value()) { + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed up"); + // speed up + entity_velocity_to_synchronize = entity_velocity + accel_limit * job_duration; + } else { + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "keep or slow down"); + // keep or slow down + auto vel_keep_time = 2*entity_distance.value()/entity_velocity - ego_arrival_time; + if(vel_keep_time < 0){ + // slow down + entity_velocity_to_synchronize = entity_velocity - (entity_velocity / ego_arrival_time) * job_duration; + } else{ + // keep + entity_velocity_to_synchronize = entity_velocity; + } + } RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed change"); - // using this->requestSpeedChange here does not work in some kind of reason. - // it seems that after this function is called by some reason, func_on_cleanup will be deleted and become nullptr + /** + * using this->requestSpeedChange here does not work in some kind of reason. + * it seems that after this function is called by some reason, func_on_cleanup will be deleted and become nullptr + */ // this->requestSpeedChange(entity_velocity_to_synchronize, true); target_speed_ = entity_velocity_to_synchronize; return true; }, // after this im not sure it is correct, just an draft [this]() { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "sync job done"); }, - job::Type::LINEAR_VELOCITY, false, job::Event::POST_UPDATE); + job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); return false; } diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index f2718d6ff2f..18bad7a3896 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"), "##############END onUpdate###############"); } void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/src/job/job_list.cpp b/simulation/traffic_simulator/src/job/job_list.cpp index e292125998d..b85e7ceccdf 100644 --- a/simulation/traffic_simulator/src/job/job_list.cpp +++ b/simulation/traffic_simulator/src/job/job_list.cpp @@ -33,6 +33,9 @@ void JobList::append( void JobList::update(const double step_time, const job::Event event) { + // show numper of jobs in the list + RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "job list size: " << list_.size()); + for (auto & job : list_) { if (job.event == event) { job.onUpdate(step_time); From 01abaccdb9e754a5555d1306238b96431b74d8b7 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 28 Mar 2024 16:45:37 +0900 Subject: [PATCH 17/18] Update synchronized action behavior --- .../synchronized_action.cpp | 33 ++--- .../src/entity/entity_base.cpp | 115 ++++++++++++------ 2 files changed, 100 insertions(+), 48 deletions(-) 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 cf24bfff909..64245691074 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -45,19 +45,22 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode const traffic_simulator::CanonicalizedLaneletPose ego_target = getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34585, 0, 0, 0, 0, 0)); const traffic_simulator::CanonicalizedLaneletPose npc_target = - getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34570, 0, 0, 0, 0, 0)); + getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34756, 0, 0, 0, 0, 0)); void onUpdate() override { - api_.requestSynchronize("npc", ego_target, npc_target, 0.2, 2.0); - // SUCCESS - // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped - if ( - api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && - api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && - api_.getCurrentTwist("npc").linear.x <= 0.01 - ) - { + if (!api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.4)) { + api_.requestSynchronize("npc", ego_target, npc_target, 0.2, 3.0); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("synchronized action"), "current time: " << api_.getCurrentTime()); + } + + // SUCCESS + // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped + if ( + api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && + api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && + api_.getCurrentTwist("npc").linear.x <= 0.01) { stop(cpp_mock_scenarios::Result::SUCCESS); } @@ -68,7 +71,6 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode if (api_.checkCollision("ego", "npc")) { stop(cpp_mock_scenarios::Result::FAILURE); } - } void onInitialize() override { @@ -86,12 +88,15 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode // api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34579, 10, 0, 0, 0, 0)))); api_.requestAssignRoute("ego", goal_poses); - api_.spawn( "npc", - api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34576, 5, 0, 0, 0, 0)), + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34630, 14, 0, 0, 0, 0)), getVehicleParameters()); - api_.setLinearVelocity("npc", 10); + std::vector npc_goal_poses; + npc_goal_poses.emplace_back(api_.toMapPose( + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34696, 3, 0, 0, 0, 0)))); + api_.requestAssignRoute("npc", npc_goal_poses); + api_.setLinearVelocity("npc", 6); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 2fb45a3899a..eb99ff3d0cc 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -273,11 +273,13 @@ void EntityBase::onUpdate(double /*current_time*/, double step_time) speed_planner_ = std::make_unique( step_time, name); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "EntityBase::onUpdate" << name); } void EntityBase::onPostUpdate(double /*current_time*/, double step_time) { job_list_.update(step_time, job::Event::POST_UPDATE); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("JobList"), "EntityBase::onPostUpdate vihicle::" << name); } void EntityBase::resetDynamicConstraints() @@ -831,19 +833,41 @@ auto EntityBase::getDistanceToTargetLaneletPose( THROW_SEMANTIC_ERROR( "Failed to get entity's lanelet pose. Please check entity lanelet pose exists."); } + // RCLCPP_ERROR_STREAM( + // rclcpp::get_logger("traffic_simulator"), + // "getLogitudinalDistance" << static_cast(entity_lanelet_pose_.value()).lanelet_id + // << " " << static_cast(target_lanelet_pose_).lanelet_id); const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( static_cast(entity_lanelet_pose_.value()), static_cast(target_lanelet_pose_)); - if (!entity_distance_to_target.has_value()) { - /** - * This is not an error, means that the entity is already over the target lanelet. - */ + // RCLCPP_ERROR_STREAM( + // rclcpp::get_logger("traffic_simulator"), + // "getreverted Distance" << static_cast(target_lanelet_pose_).lanelet_id << " " + // << static_cast(entity_lanelet_pose_.value()).lanelet_id); + + const auto revert_entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance( + static_cast(target_lanelet_pose_), + static_cast(entity_lanelet_pose_.value())); + + if (!revert_entity_distance_to_target.has_value() && !entity_distance_to_target.has_value()) { return std::nullopt; } - return entity_distance_to_target; + if (!revert_entity_distance_to_target.has_value()) { + return entity_distance_to_target; + } + + if (!entity_distance_to_target.has_value()) { + return revert_entity_distance_to_target; + } + + if (revert_entity_distance_to_target.value() < entity_distance_to_target.value()) { + return revert_entity_distance_to_target; + } else { + return entity_distance_to_target; + } } auto EntityBase::getIfArrivedToTargetLaneletPose( @@ -886,63 +910,87 @@ auto EntityBase::requestSynchronize( } job_list_.append( - [this, ego_target, entity_target, accel_limit](double job_duration) { + [this, ego_target, entity_target, accel_limit](double) { /** * @brief This is draft function for synchronization. * It is not a good way to synchronize the entity with the ego. - * WARNING**************** - * not checking job_duration_ at all */ - const auto entity_distance = getDistanceToTargetLaneletPose(entity_target, 100); - const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance( + auto entity_distance = getDistanceToTargetLaneletPose(entity_target, 100); + auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance( static_cast(other_status_.find("ego")->second.getLaneletPose()), static_cast(ego_target)); - /** * *********************CHECK********************* * there might be a better way if the car had passed away */ if (!entity_distance.has_value()) { - return true; + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "entity has already passed away"); + entity_distance = 0; } if (!ego_distance.has_value()) { + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "ego has already passed away"); return true; } const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x; const auto entity_velocity = this->getStatus().getTwist().linear.x; + // better change 0.2 to parameter + // check if the entity has already arrived to the target lanelet and entity is nearly stopped + if (getIfArrivedToTargetLaneletPose(entity_target, 0.2) && entity_velocity < 0.8) { + target_speed_ = 0; + return true; + } + // be better to use acceleration,jerk to estimate the arrival time + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("traffic_simulator"), + "ego velocity: " << ego_velocity << " ego distance: " << ego_distance.value()); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("traffic_simulator"), + "entity velocity: " << entity_velocity << " entity distance: " << entity_distance.value()); const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0; - // if(ego_arrival_time < std::sqrt(ego_distance.value()/accel_limit)){ - // THROW_SEMANTIC_ERROR( - // "The entity can not stop on the destination by time."); - // } - RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "ego arrival time: %f", ego_arrival_time); + RCLCPP_ERROR( + rclcpp::get_logger("traffic_simulator"), "ego arrival time: %f", ego_arrival_time); auto entity_velocity_to_synchronize = entity_velocity; + if (accel_limit * ego_arrival_time < entity_velocity) + THROW_SEMANTIC_ERROR("The entity can not stop on the destination by time."); + + if (entity_velocity * entity_velocity / (accel_limit * 2) > entity_distance.value()) + THROW_SEMANTIC_ERROR("The entity will pass away the destination by time."); + if ( - (entity_velocity * ego_arrival_time) - - (entity_velocity * entity_velocity / (2 * accel_limit)) <= - entity_distance.value()) { - RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed up"); + entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / accel_limit + + accel_limit * ego_arrival_time * ego_arrival_time / 2 < + entity_distance.value()) + THROW_SEMANTIC_ERROR("The entity can not reach the destination by time."); + + auto accel_limit_ = accel_limit * 1.5; // 0.05 is a margin + auto const distance_mergin = 0.0; + + if ( + entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / (accel_limit * 2) < + entity_distance.value() - distance_mergin) { + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed up"); // speed up - entity_velocity_to_synchronize = entity_velocity + accel_limit * job_duration; + entity_velocity_to_synchronize = entity_velocity + accel_limit_ * 50 / 1000; + } else if ( + entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / (accel_limit * 2) > + entity_distance.value() - distance_mergin) { + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "slow down"); + // slow down + entity_velocity_to_synchronize = entity_velocity - accel_limit_ * 50 / 1000; } else { - RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "keep or slow down"); - // keep or slow down - auto vel_keep_time = 2*entity_distance.value()/entity_velocity - ego_arrival_time; - if(vel_keep_time < 0){ - // slow down - entity_velocity_to_synchronize = entity_velocity - (entity_velocity / ego_arrival_time) * job_duration; - } else{ - // keep - entity_velocity_to_synchronize = entity_velocity; - } + RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "keep speed"); + // keep speed + entity_velocity_to_synchronize = entity_velocity; } - RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed change"); + RCLCPP_ERROR( + rclcpp::get_logger("traffic_simulator"), "speed change: %f", + entity_velocity_to_synchronize); /** * using this->requestSpeedChange here does not work in some kind of reason. * it seems that after this function is called by some reason, func_on_cleanup will be deleted and become nullptr @@ -951,7 +999,6 @@ auto EntityBase::requestSynchronize( target_speed_ = entity_velocity_to_synchronize; return true; }, - // after this im not sure it is correct, just an draft [this]() { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "sync job done"); }, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE); return false; From c5436e230e91b6abf974f04006b530787fd04906 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Fri, 29 Mar 2024 17:54:27 +0900 Subject: [PATCH 18/18] Update entity_base.hpp and synchronized_action.cpp --- .../synchronized_action.cpp | 15 +-- .../traffic_simulator/entity/entity_base.hpp | 4 +- .../src/entity/entity_base.cpp | 111 +++++++++--------- 3 files changed, 62 insertions(+), 68 deletions(-) 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 64245691074..896575a0d70 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -49,23 +49,20 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { - if (!api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.4)) { - api_.requestSynchronize("npc", ego_target, npc_target, 0.2, 3.0); - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("synchronized action"), "current time: " << api_.getCurrentTime()); - } - + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("synchronized action"), "current time: " << api_.getCurrentTime()); // SUCCESS // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped if ( + api_.requestSynchronize("npc", ego_target, npc_target, 0.2, 3.0, 50) && api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) && api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) && - api_.getCurrentTwist("npc").linear.x <= 0.01) { + api_.getCurrentTwist("npc").linear.x <= 0.1) { stop(cpp_mock_scenarios::Result::SUCCESS); } // FAILURES - if (api_.getCurrentTime() >= 40.0) { + if (api_.getCurrentTime() >= 9.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.checkCollision("ego", "npc")) { @@ -96,7 +93,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode npc_goal_poses.emplace_back(api_.toMapPose( api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34696, 3, 0, 0, 0, 0)))); api_.requestAssignRoute("npc", npc_goal_poses); - api_.setLinearVelocity("npc", 6); + api_.setLinearVelocity("npc", 5.5); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index a6a3b1d7192..71df36d4da1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -231,14 +231,14 @@ class EntityBase /* */ auto getDistanceToTargetLaneletPose( const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance) - -> std::optional; + -> double; /* */ auto getIfArrivedToTargetLaneletPose( const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool; /* */ auto requestSynchronize( const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, - const double threshold, const double accel_limit) -> bool; + const double threshold, const double accel_limit, const double loop_period) -> bool; virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk) -> void final; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index eb99ff3d0cc..dbfa030d8f1 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -822,11 +822,9 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double } auto EntityBase::getDistanceToTargetLaneletPose( - const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance) - -> std::optional + const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance) -> double { - const double matching_distance_ = matching_distance; - const auto entity_lanelet_pose_ = getLaneletPose(matching_distance_); + const auto entity_lanelet_pose_ = getLaneletPose(matching_distance); const auto target_lanelet_pose_ = target_lanelet_pose; if (!entity_lanelet_pose_) { @@ -852,21 +850,33 @@ auto EntityBase::getDistanceToTargetLaneletPose( static_cast(entity_lanelet_pose_.value())); if (!revert_entity_distance_to_target.has_value() && !entity_distance_to_target.has_value()) { - return std::nullopt; + THROW_SIMULATION_ERROR("Failed to get distance between entity and target lanelet pose."); } if (!revert_entity_distance_to_target.has_value()) { - return entity_distance_to_target; + RCLCPP_WARN( + rclcpp::get_logger("traffic_simulator"), + "Failed to get reverted distance between entity and target lanelet pose."); + return entity_distance_to_target.value(); } if (!entity_distance_to_target.has_value()) { - return revert_entity_distance_to_target; + RCLCPP_WARN( + rclcpp::get_logger("traffic_simulator"), + "Failed to get forward distance between entity and target lanelet pose."); + return revert_entity_distance_to_target.value(); } + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("traffic_simulator"), + "entity_distance_to_target: " << entity_distance_to_target.value() + << " revert_entity_distance_to_target: " + << revert_entity_distance_to_target.value()); + if (revert_entity_distance_to_target.value() < entity_distance_to_target.value()) { - return revert_entity_distance_to_target; + return revert_entity_distance_to_target.value(); } else { - return entity_distance_to_target; + return entity_distance_to_target.value(); } } @@ -874,18 +884,10 @@ auto EntityBase::getIfArrivedToTargetLaneletPose( const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool { /** - * WARNING*****WIP********* - * mesuring distance is set to 100 manually + * CHECK************** + * matching distance is set to 500 manually */ - const auto entity_distance_to_target = getDistanceToTargetLaneletPose(target_lanelet_pose, 100); - - if (!entity_distance_to_target.has_value()) { - /** - * @brief meaning that the entity is already over the target lanelet. - * - */ - return true; - } + const auto entity_distance_to_target = getDistanceToTargetLaneletPose(target_lanelet_pose, 500); if (entity_distance_to_target <= threshold) { return true; @@ -896,37 +898,39 @@ auto EntityBase::getIfArrivedToTargetLaneletPose( auto EntityBase::requestSynchronize( const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target, - const double threshold, const double accel_limit) -> bool + const double threshold, const double accel_limit, const double loop_period) -> bool { - if (name == "ego") { + if (this-> /** * *********************CHECK********************* * is this correct to throw syntax error? */ THROW_SYNTAX_ERROR("The entity that is requested to synchronize is the ego. Please check."); } + + // check if the entity has already arrived to the target lanelet and entity is nearly stopped if (getIfArrivedToTargetLaneletPose(entity_target, threshold)) { + if (this->getStatus().getTwist().linear.x < accel_limit * loop_period / 1000) { + } else { + RCLCPP_WARN_ONCE( + rclcpp::get_logger("traffic_simulator"), + "The entity has already arrived to the target lanelet and the entity is not nearly " + "stopped."); + } + target_speed_ = 0; return true; } job_list_.append( - [this, ego_target, entity_target, accel_limit](double) { + [this, ego_target, entity_target, accel_limit, loop_period](double) { /** - * @brief This is draft function for synchronization. - * It is not a good way to synchronize the entity with the ego. + * @brief This is function for synchronization. */ auto entity_distance = getDistanceToTargetLaneletPose(entity_target, 100); auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance( static_cast(other_status_.find("ego")->second.getLaneletPose()), static_cast(ego_target)); - /** - * *********************CHECK********************* - * there might be a better way if the car had passed away - */ - if (!entity_distance.has_value()) { - RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "entity has already passed away"); - entity_distance = 0; - } + if (!ego_distance.has_value()) { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "ego has already passed away"); return true; @@ -934,20 +938,13 @@ auto EntityBase::requestSynchronize( const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x; const auto entity_velocity = this->getStatus().getTwist().linear.x; - // better change 0.2 to parameter - // check if the entity has already arrived to the target lanelet and entity is nearly stopped - if (getIfArrivedToTargetLaneletPose(entity_target, 0.2) && entity_velocity < 0.8) { - target_speed_ = 0; - return true; - } - // be better to use acceleration,jerk to estimate the arrival time RCLCPP_ERROR_STREAM( rclcpp::get_logger("traffic_simulator"), "ego velocity: " << ego_velocity << " ego distance: " << ego_distance.value()); RCLCPP_ERROR_STREAM( rclcpp::get_logger("traffic_simulator"), - "entity velocity: " << entity_velocity << " entity distance: " << entity_distance.value()); + "entity velocity: " << entity_velocity << " entity distance: " << entity_distance); const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0; RCLCPP_ERROR( @@ -955,33 +952,33 @@ auto EntityBase::requestSynchronize( auto entity_velocity_to_synchronize = entity_velocity; - if (accel_limit * ego_arrival_time < entity_velocity) - THROW_SEMANTIC_ERROR("The entity can not stop on the destination by time."); + // if (accel_limit * ego_arrival_time < entity_velocity) + // THROW_SEMANTIC_ERROR("The entity can not stop on the destination by time."); - if (entity_velocity * entity_velocity / (accel_limit * 2) > entity_distance.value()) - THROW_SEMANTIC_ERROR("The entity will pass away the destination by time."); + // if (entity_velocity * entity_velocity / (accel_limit * 2) > entity_distance.value()) + // THROW_SEMANTIC_ERROR("The entity will pass away the destination by time."); - if ( - entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / accel_limit + - accel_limit * ego_arrival_time * ego_arrival_time / 2 < - entity_distance.value()) - THROW_SEMANTIC_ERROR("The entity can not reach the destination by time."); + // if ( + // entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / accel_limit + + // accel_limit * ego_arrival_time * ego_arrival_time / 2 < + // entity_distance.value()) + // THROW_SEMANTIC_ERROR("The entity can not reach the destination by time."); - auto accel_limit_ = accel_limit * 1.5; // 0.05 is a margin - auto const distance_mergin = 0.0; + auto accel_limit_ = accel_limit * 1; + auto const distance_mergin = 1.0; if ( entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / (accel_limit * 2) < - entity_distance.value() - distance_mergin) { + entity_distance - distance_mergin) { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed up"); // speed up - entity_velocity_to_synchronize = entity_velocity + accel_limit_ * 50 / 1000; + entity_velocity_to_synchronize = entity_velocity + accel_limit_ * loop_period / 1000; } else if ( entity_velocity * ego_arrival_time - entity_velocity * entity_velocity / (accel_limit * 2) > - entity_distance.value() - distance_mergin) { + entity_distance - distance_mergin) { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "slow down"); // slow down - entity_velocity_to_synchronize = entity_velocity - accel_limit_ * 50 / 1000; + entity_velocity_to_synchronize = entity_velocity - accel_limit_ * loop_period / 1000; } else { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "keep speed"); // keep speed @@ -997,7 +994,7 @@ auto EntityBase::requestSynchronize( */ // this->requestSpeedChange(entity_velocity_to_synchronize, true); target_speed_ = entity_velocity_to_synchronize; - return true; + return false; }, [this]() { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "sync job done"); }, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE);