From 0c5fb8b1d1f7badcfb92eafca6acf08d8a978507 Mon Sep 17 00:00:00 2001 From: koki suzuki Date: Thu, 1 Feb 2024 18:03:40 +0900 Subject: [PATCH] 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