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