From 7ff62481a9fabdcb1b32b7110df8943e387ddbb7 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 13:16:52 +0200 Subject: [PATCH 01/49] Add getEntityOrThrow function to mimic forwarding functionality where exceptions were thrown Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/entity/entity_manager.hpp | 3 +++ .../traffic_simulator/src/entity/entity_manager.cpp | 10 ++++++++++ 2 files changed, 13 insertions(+) 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 7e5b9111e59..ee651a5b99b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -321,6 +321,9 @@ class EntityManager auto getEntity(const std::string & name) const -> std::shared_ptr; + auto getEntityOrThrow(const std::string & name) + -> std::shared_ptr; + auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &; auto getHdmapUtils() -> const std::shared_ptr &; diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 7a73bc62e4e..1e27cef13b6 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -162,6 +162,16 @@ auto EntityManager::getEntity(const std::string & name) const } }; +auto EntityManager::getEntityOrThrow(const std::string & name) + -> std::shared_ptr +{ + if (const auto entity = getEntity(name)) { + return entity; + } else { + THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); + } +} + auto EntityManager::getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus & { From aacd0b3f924d90cbfa83337907bb752254f2740a Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 13:32:32 +0200 Subject: [PATCH 02/49] Remove forwarding of getEntityType in EntityManager Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 1 file changed, 1 deletion(-) 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 ee651a5b99b..cd641a6ddd9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -264,7 +264,6 @@ class EntityManager FORWARD_TO_ENTITY(getCurrentTwist, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); - FORWARD_TO_ENTITY(getEntityType, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getRouteLanelets, const); From b972a16f4bf75781b884c1ba2503721a25c415d3 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 14:40:59 +0200 Subject: [PATCH 03/49] Forward getEntityOrThrow from API to EntityManager Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 73b1c2b265c..1e9ebf0854a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -353,6 +353,7 @@ class API FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); + FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getEntityStatus); FORWARD_TO_ENTITY_MANAGER(getEntityStatusBeforeUpdate); From 38d9552430ecb34ba08c218d7a2192f68910e426 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 14:42:56 +0200 Subject: [PATCH 04/49] Remove forwarding of getEntityStatus in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../follow_front_entity/accelerate_and_follow.cpp | 2 +- .../follow_front_entity/decelerate_and_follow.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic_simulator/entity/entity_manager.hpp | 2 -- simulation/traffic_simulator/src/api/api.cpp | 10 ++++++---- .../traffic_simulator/src/entity/entity_manager.cpp | 12 +----------- .../include/random_test_runner/test_executor.hpp | 9 +++++---- .../random_test_runner/test/test_test_executor.cpp | 12 +++++++----- 8 files changed, 21 insertions(+), 29 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index ae2633b1e19..28292a45c6c 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -41,7 +41,7 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode { double ego_accel = api_.getCurrentAccel("ego").linear.x; double ego_twist = api_.getCurrentTwist("ego").linear.x; - // double npc_accel = static_cast(api_.getEntityStatus("npc")).action_status.accel.linear.x; + // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; double npc_twist = api_.getCurrentTwist("npc").linear.x; ; // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index 01e0862c55b..1d5590e19ea 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -41,7 +41,7 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode { double ego_accel = api_.getCurrentAccel("ego").linear.x; double ego_twist = api_.getCurrentTwist("ego").linear.x; - // double npc_accel = static_cast(api_.getEntityStatus("npc")).action_status.accel.linear.x; + // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; double npc_twist = api_.getCurrentTwist("npc").linear.x; ; // LCOV_EXCL_START diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1e9ebf0854a..df66d21719f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -355,7 +355,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); FORWARD_TO_ENTITY_MANAGER(getEntityNames); - FORWARD_TO_ENTITY_MANAGER(getEntityStatus); FORWARD_TO_ENTITY_MANAGER(getEntityStatusBeforeUpdate); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getLinearJerk); 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 cd641a6ddd9..c126e036184 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -323,8 +323,6 @@ class EntityManager auto getEntityOrThrow(const std::string & name) -> std::shared_ptr; - auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &; - auto getHdmapUtils() -> const std::shared_ptr &; auto getNumberOfEgo() const -> std::size_t; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 5a84536742e..1666da9b67b 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -67,7 +67,8 @@ auto API::respawn( // read status from EntityManager, then send it to SimpleSensorSimulator simulation_api_schema::UpdateEntityStatusRequest req; simulation_interface::toProto( - static_cast(entity_manager_ptr_->getEntityStatus(name)), *req.add_status()); + static_cast(entity_manager_ptr_->getEntityOrThrow(name)->getStatus()), + *req.add_status()); req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(name)); entity_manager_ptr_->setControlledBySimulator(name, false); @@ -85,7 +86,8 @@ auto API::respawn( res_name + "\"."); } else { // if valid, set response in EntityManager, then plan path and engage - auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(name)); + auto entity_status = + static_cast(entity_manager_ptr_->getEntityOrThrow(name)->getStatus()); simulation_interface::toMsg(res_status->pose(), entity_status.pose); simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); setMapPose(name, entity_status.pose); @@ -283,7 +285,7 @@ bool API::updateEntitiesStatusInSim() req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { const auto entity_status = - static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); + static_cast(entity_manager_ptr_->getEntityOrThrow(entity_name)->getStatus()); simulation_interface::toProto(entity_status, *req.add_status()); if (entity_manager_ptr_->is(entity_name)) { req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); @@ -295,7 +297,7 @@ bool API::updateEntitiesStatusInSim() for (const auto & res_status : res.status()) { auto entity_name = res_status.name(); auto entity_status = - static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); + static_cast(entity_manager_ptr_->getEntityOrThrow(entity_name)->getStatus()); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 1e27cef13b6..762390fe142 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -172,16 +172,6 @@ auto EntityManager::getEntityOrThrow(const std::string & name) } } -auto EntityManager::getEntityStatus(const std::string & name) const - -> const CanonicalizedEntityStatus & -{ - if (const auto entity = getEntity(name)) { - return entity->getStatus(); - } else { - THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); - } -} - auto EntityManager::getHdmapUtils() -> const std::shared_ptr & { return hdmap_utils_ptr_; @@ -319,7 +309,7 @@ void EntityManager::requestLaneChange( void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { - const auto & status = getEntityStatus(name); + const auto & status = getEntityOrThrow(name)->getStatus(); const auto behavior_parameter = getBehaviorParameter(name); if (is(name)) { THROW_SEMANTIC_ERROR( diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 1a95f011592..84009ec77ee 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -151,13 +151,13 @@ class TestExecutor auto current_time = api_->getCurrentTime(); if (!std::isnan(current_time)) { - if (goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { + if (goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { scenario_completed_ = true; } bool timeout_reached = current_time >= test_timeout_; if (timeout_reached) { - if (!goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { + if (!goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Timeout reached"); error_reporter_.reportTimeout(); } @@ -177,9 +177,10 @@ class TestExecutor } } - if (almost_standstill_metric_.isAlmostStandingStill(api_->getEntityStatus(ego_name_))) { + if (almost_standstill_metric_.isAlmostStandingStill( + api_->getEntityOrThrow(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Standstill duration exceeded"); - if (goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { + if (goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Goal reached, standstill expected"); } else { error_reporter_.reportStandStill(); diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index d3e92a8ae82..9004f27cf02 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -75,7 +75,7 @@ class MockTrafficSimulatorAPI MOCK_METHOD(bool, despawn, (const std::string), ()); MOCK_METHOD(std::string, getEgoName, (), ()); MOCK_METHOD(double, getCurrentTime, (), ()); - MOCK_METHOD(void, getEntityStatusMock, (const std::string &), ()); + MOCK_METHOD(void, getEntityOrThrowMock, (const std::string &), ()); MOCK_METHOD(bool, entityExists, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); @@ -86,11 +86,13 @@ class MockTrafficSimulatorAPI return *field_operator_application_mock; } - traffic_simulator::CanonicalizedEntityStatus getEntityStatus(const std::string & name) + std::shared_ptr getEntityOrThrow(const std::string & name) { - getEntityStatusMock(name); + getEntityOrThrowMock(name); /// @note set invalid LaneletPose so pass std::nullopt - return traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt); + return std::make_shared( + "", traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt), nullptr, + getVehicleParameters()); } void setEntityStatusNecessaryValues( @@ -163,7 +165,7 @@ TEST(TestExecutor, UpdateNoNPCs) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(1.0)); - EXPECT_CALL(*MockAPI, getEntityStatusMock).Times(2).InSequence(sequence); + EXPECT_CALL(*MockAPI, getEntityOrThrowMock).Times(2).InSequence(sequence); EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); test_executor.update(); From 26ce1d1bc958d94e2e7b1d1e17de65bd288fd929 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 15:14:53 +0200 Subject: [PATCH 05/49] Add const Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/entity/entity_manager.hpp | 2 +- simulation/traffic_simulator/src/entity/entity_manager.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 c126e036184..c754b063c6e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -320,7 +320,7 @@ class EntityManager auto getEntity(const std::string & name) const -> std::shared_ptr; - auto getEntityOrThrow(const std::string & name) + auto getEntityOrThrow(const std::string & name) const -> std::shared_ptr; auto getHdmapUtils() -> const std::shared_ptr &; diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 762390fe142..89f2b6d0f49 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -162,7 +162,7 @@ auto EntityManager::getEntity(const std::string & name) const } }; -auto EntityManager::getEntityOrThrow(const std::string & name) +auto EntityManager::getEntityOrThrow(const std::string & name) const -> std::shared_ptr { if (const auto entity = getEntity(name)) { From cb0fbb218107582d33c20aa36e91704ba4d94400 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 15:20:31 +0200 Subject: [PATCH 06/49] Remove forwarding of getEntityStatusBeforeUpdate in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index df66d21719f..26878327d89 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -355,7 +355,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); FORWARD_TO_ENTITY_MANAGER(getEntityNames); - FORWARD_TO_ENTITY_MANAGER(getEntityStatusBeforeUpdate); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); 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 c754b063c6e..e8379bec2aa 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -263,7 +263,6 @@ class EntityManager FORWARD_TO_ENTITY(getCurrentAccel, const); FORWARD_TO_ENTITY(getCurrentTwist, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); - FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getRouteLanelets, const); From 9c60bb32b673e0f87773ca16349bd7b08f277d0c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 15:46:51 +0200 Subject: [PATCH 07/49] Remove forwarding of getBehaviorParameter in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../include/openscenario_interpreter/simulator_core.hpp | 4 ++-- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - simulation/traffic_simulator/src/entity/entity_manager.cpp | 2 +- 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 19cc3ec08a1..6117c1b39e7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -278,7 +278,7 @@ class SimulatorCore const EntityRef & entity_ref, const DynamicConstraints & dynamic_constraints) -> void { return core->setBehaviorParameter(entity_ref, [&]() { - auto behavior_parameter = core->getBehaviorParameter(entity_ref); + auto behavior_parameter = core->getEntityOrThrow(entity_ref)->getBehaviorParameter(); if (not std::isinf(dynamic_constraints.max_speed)) { behavior_parameter.dynamic_constraints.max_speed = dynamic_constraints.max_speed; @@ -317,7 +317,7 @@ class SimulatorCore "maxSpeed", std::numeric_limits::max())); core->setBehaviorParameter(entity_ref, [&]() { - auto message = core->getBehaviorParameter(entity_ref); + auto message = core->getEntityOrThrow(entity_ref)->getBehaviorParameter(); message.see_around = not controller.properties.template get("isBlind"); /// The default values written in https://github.com/tier4/scenario_simulator_v2/blob/master/simulation/traffic_simulator_msgs/msg/DynamicConstraints.msg message.dynamic_constraints.max_acceleration = diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 26878327d89..cf75aa19d91 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -344,7 +344,6 @@ class API FORWARD_TO_ENTITY_MANAGER(cancelRequest); FORWARD_TO_ENTITY_MANAGER(checkCollision); FORWARD_TO_ENTITY_MANAGER(entityExists); - FORWARD_TO_ENTITY_MANAGER(getBehaviorParameter); FORWARD_TO_ENTITY_MANAGER(getBoundingBox); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); 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 e8379bec2aa..282efb03645 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -258,7 +258,6 @@ class EntityManager FORWARD_TO_ENTITY(asFieldOperatorApplication, const); FORWARD_TO_ENTITY(get2DPolygon, const); - FORWARD_TO_ENTITY(getBehaviorParameter, const); FORWARD_TO_ENTITY(getBoundingBox, const); FORWARD_TO_ENTITY(getCurrentAccel, const); FORWARD_TO_ENTITY(getCurrentTwist, const); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 89f2b6d0f49..d596eb7c4c1 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -310,7 +310,7 @@ void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { const auto & status = getEntityOrThrow(name)->getStatus(); - const auto behavior_parameter = getBehaviorParameter(name); + const auto behavior_parameter = getEntityOrThrow(name)->getBehaviorParameter(); if (is(name)) { THROW_SEMANTIC_ERROR( "Entity :", name, "is EgoEntity.", "You cannot reset behavior plugin of EgoEntity."); From 1a0c2be74cf1d5a6454b94edb7cd5f03b84a27fc Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 16:24:24 +0200 Subject: [PATCH 08/49] Remove forwarding of getCurrentTwist in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../src/crosswalk/stop_at_crosswalk.cpp | 6 +++--- .../src/follow_front_entity/accelerate_and_follow.cpp | 4 ++-- .../src/follow_front_entity/decelerate_and_follow.cpp | 4 ++-- .../src/move_backward/move_backward.cpp | 2 +- .../src/pedestrian/walk_straight.cpp | 4 ++-- .../src/speed_planning/request_speed_change.cpp | 11 +++++++---- .../request_speed_change_continuous_false.cpp | 10 +++++++--- .../speed_planning/request_speed_change_relative.cpp | 9 ++++++--- .../src/speed_planning/request_speed_change_step.cpp | 5 +++-- .../request_speed_change_with_limit.cpp | 9 ++++++--- .../request_speed_change_with_time_constraint.cpp | 8 +++++--- ...quest_speed_change_with_time_constraint_linear.cpp | 4 +++- ...est_speed_change_with_time_constraint_relative.cpp | 8 +++++--- .../openscenario_interpreter/simulator_core.hpp | 7 +++---- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic_simulator/entity/entity_manager.hpp | 1 - simulation/traffic_simulator/src/api/api.cpp | 4 ++-- .../traffic_simulator/src/entity/entity_manager.cpp | 3 ++- 18 files changed, 59 insertions(+), 41 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index d32fcf6e9c9..dc0a6ea6cfb 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -50,17 +50,17 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P */ if (t == 1.0) { - if (t != api_.getCurrentTwist("bob").linear.x) { + if (t != api_.getEntityOrThrow("bob")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } if (t >= 6.6) { if (7.5 >= t) { - if (std::fabs(0.1) <= api_.getCurrentTwist("ego").linear.x) { + if (std::fabs(0.1) <= api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } else { - if (0.1 >= api_.getCurrentTwist("ego").linear.x) { + if (0.1 >= api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index 28292a45c6c..522bdc59efc 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -40,9 +40,9 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { double ego_accel = api_.getCurrentAccel("ego").linear.x; - double ego_twist = api_.getCurrentTwist("ego").linear.x; + double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getCurrentTwist("npc").linear.x; + double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; ; // LCOV_EXCL_START if (npc_twist > (ego_twist + 1) && ego_accel < 0) { diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index 1d5590e19ea..8a9286cf133 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -40,9 +40,9 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { double ego_accel = api_.getCurrentAccel("ego").linear.x; - double ego_twist = api_.getCurrentTwist("ego").linear.x; + double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getCurrentTwist("npc").linear.x; + double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; ; // LCOV_EXCL_START if (ego_twist > (npc_twist + 1) && ego_accel > 0) { diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 82c34135275..001edec148d 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -42,7 +42,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START - double ego_twist = api_.getCurrentTwist("ego").linear.x; + double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; if (ego_twist >= -2.9) { stop(cpp_mock_scenarios::Result::FAILURE); } diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 782f52ed496..9f999c8b4ed 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -50,13 +50,13 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P */ if (t == 1.0) { - const auto vel = api_.getCurrentTwist("bob").linear.x; + const auto vel = api_.getEntityOrThrow("bob")->getCurrentTwist().linear.x; if (t != vel) { stop(cpp_mock_scenarios::Result::FAILURE); } } if (t >= 6.65 && 7.25 >= t) { - const auto vel = api_.getCurrentTwist("ego").linear.x; + const auto vel = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; if (std::fabs(0.01) <= vel) { stop(cpp_mock_scenarios::Result::FAILURE); } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 54f0ec7987f..75dd3fa4958 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -39,15 +39,18 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTwist("front").linear.x < 10.0) { + if (api_.getEntityOrThrow("front")->getCurrentTwist().linear.x < 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } - if (api_.getCurrentTime() <= 0.9 && api_.getCurrentTwist("ego").linear.x > 10.0) { + if ( + api_.getCurrentTime() <= 0.9 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - api_.getCurrentTime() >= 1.0 && api_.getCurrentTwist("ego").linear.x <= 10.0 && - api_.getCurrentTwist("ego").linear.x >= 9.9) { + api_.getCurrentTime() >= 1.0 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 463d1729ae8..4f0a0d8ee81 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -44,16 +44,20 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp * @brief checking linear speed */ if (api_.getCurrentTime() <= 0.95) { - if (!equals(api_.getCurrentTime() * 10.0, api_.getCurrentTwist("ego").linear.x, 0.01)) { + if (!equals( + api_.getCurrentTime() * 10.0, api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, + 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } - if (api_.getCurrentTime() >= 1.0 && api_.getCurrentTwist("ego").linear.x <= 10.0) { + if ( + api_.getCurrentTime() >= 1.0 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0) { speed_reached = true; } if ( speed_reached && api_.getCurrentTime() >= 1.5 && - api_.getCurrentTwist("ego").linear.x >= 13.88) { + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 13.88) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index e32004c3fc7..f556c236e0d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -40,13 +40,16 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari void onUpdate() override { if (api_.getCurrentTime() <= 1.9) { - if (!equals(api_.getCurrentTime() + 3.0, api_.getCurrentTwist("front").linear.x, 0.01)) { + if (!equals( + api_.getCurrentTime() + 3.0, api_.getEntityOrThrow("front")->getCurrentTwist().linear.x, + 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } if ( - api_.getCurrentTime() >= 2.0 && api_.getCurrentTwist("front").linear.x <= 5.05 && - api_.getCurrentTwist("front").linear.x >= 4.95) { + api_.getCurrentTime() >= 2.0 && + api_.getEntityOrThrow("front")->getCurrentTwist().linear.x <= 5.05 && + api_.getEntityOrThrow("front")->getCurrentTwist().linear.x >= 4.95) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index d829ffb193e..a36e35929c1 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -40,8 +40,9 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod void onUpdate() override { if ( - api_.getCurrentTime() <= 0.999 && !equals(api_.getCurrentTwist("ego").linear.x, 10.0, 0.01) && - !equals(api_.getCurrentTwist("front").linear.x, 10.0, 0.01)) { + api_.getCurrentTime() <= 0.999 && + !equals(api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, 10.0, 0.01) && + !equals(api_.getEntityOrThrow("front")->getCurrentTwist().linear.x, 10.0, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 1.0) { diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 8033961f010..8619619a33c 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -39,12 +39,15 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar private: void onUpdate() override { - if (api_.getCurrentTime() <= 0.9 && api_.getCurrentTwist("ego").linear.x > 10.0) { + if ( + api_.getCurrentTime() <= 0.9 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - api_.getCurrentTime() >= 1.0 && api_.getCurrentTwist("ego").linear.x <= 5.1 && - api_.getCurrentTwist("ego").linear.x >= 4.9) { + api_.getCurrentTime() >= 1.0 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 5.1 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 4.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index f9078c865c9..5b0ef3e1fbd 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -39,13 +39,15 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: private: void onUpdate() override { - if (api_.getCurrentTime() <= 3.9 && api_.getCurrentTwist("ego").linear.x > 10.0) { + if ( + api_.getCurrentTime() <= 3.9 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.999) { if ( - api_.getCurrentTwist("ego").linear.x <= 10.0 && - api_.getCurrentTwist("ego").linear.x >= 9.9) { + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index abe467abba6..16065790d6e 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -41,7 +41,9 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario void onUpdate() override { if (api_.getCurrentTime() <= 3.999) { - if (!equals(api_.getCurrentTime() * 2.5, api_.getCurrentTwist("ego").linear.x, 0.01)) { + if (!equals( + api_.getCurrentTime() * 2.5, api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, + 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } else if (api_.getCurrentTime() >= 4.0) { diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index c9662d1b120..8bb70471986 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -40,13 +40,15 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario private: void onUpdate() override { - if (api_.getCurrentTime() <= 3.9 && api_.getCurrentTwist("ego").linear.x >= 3.0) { + if ( + api_.getCurrentTime() <= 3.9 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 3.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.9999) { if ( - api_.getCurrentTwist("ego").linear.x <= 3.1 && - api_.getCurrentTwist("ego").linear.x >= 2.9) { + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 3.1 && + api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 2.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 6117c1b39e7..f0ec28d6e83 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -458,10 +458,9 @@ class SimulatorCore class ConditionEvaluation : protected CoordinateSystemConversion { protected: - template - static auto evaluateAcceleration(Ts &&... xs) + static auto evaluateAcceleration(const std::string & name) { - return core->getCurrentAccel(std::forward(xs)...).linear.x; + return core->getEntityOrThrow(name)->getCurrentAccel().linear.x; } template @@ -501,7 +500,7 @@ class SimulatorCore template static auto evaluateSpeed(Ts &&... xs) { - return core->getCurrentTwist(std::forward(xs)...).linear.x; + return core->getEntityOrThrow(std::forward(xs)...)->getCurrentTwist().linear.x; } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index cf75aa19d91..e0347c5bc14 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -349,7 +349,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); FORWARD_TO_ENTITY_MANAGER(getCurrentAccel); FORWARD_TO_ENTITY_MANAGER(getCurrentAction); - FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); 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 282efb03645..faa0664dc1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -260,7 +260,6 @@ class EntityManager FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBoundingBox, const); FORWARD_TO_ENTITY(getCurrentAccel, const); - FORWARD_TO_ENTITY(getCurrentTwist, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 1666da9b67b..49503979ef9 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -137,8 +137,8 @@ std::optional API::getTimeHeadway( if (auto to_entity = getEntity(to_entity_name); to_entity) { if (auto relative_pose = relativePose(from_entity->getMapPose(), to_entity->getMapPose()); relative_pose && relative_pose->position.x <= 0) { - const double time_headway = - (relative_pose->position.x * -1) / getCurrentTwist(to_entity_name).linear.x; + const double time_headway = (relative_pose->position.x * -1) / + getEntityOrThrow(to_entity_name)->getCurrentTwist().linear.x; return std::isnan(time_headway) ? std::numeric_limits::infinity() : time_headway; } } diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index d596eb7c4c1..98df2b25a0c 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -263,7 +263,8 @@ bool EntityManager::isInLanelet( bool EntityManager::isStopping(const std::string & name) const { - return std::fabs(getCurrentTwist(name).linear.x) < std::numeric_limits::epsilon(); + return std::fabs(getEntityOrThrow(name)->getCurrentTwist().linear.x) < + std::numeric_limits::epsilon(); } bool EntityManager::reachPosition( From 421a18ef98fc1e14a841c5649429e4a09b3d26e1 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 17:38:28 +0200 Subject: [PATCH 09/49] Clean unused template Signed-off-by: Mateusz Palczuk --- .../include/openscenario_interpreter/simulator_core.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index f0ec28d6e83..443191a0efd 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -469,7 +469,6 @@ class SimulatorCore return core->checkCollision(std::forward(xs)...); } - template static auto evaluateBoundingBoxEuclideanDistance( const std::string & from_entity_name, const std::string & to_entity_name) // for RelativeDistanceCondition From d3e83a33cb4ed9d392a8dba887403849ef70cd11 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 17:39:03 +0200 Subject: [PATCH 10/49] Remove forwarding of getCurrentAccel in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../src/follow_front_entity/accelerate_and_follow.cpp | 2 +- .../src/follow_front_entity/decelerate_and_follow.cpp | 2 +- .../include/openscenario_interpreter/simulator_core.hpp | 5 ++--- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 5 files changed, 4 insertions(+), 7 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index 522bdc59efc..6f68e68ddaf 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -39,7 +39,7 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getCurrentAccel("ego").linear.x; + double ego_accel = api_.getEntityOrThrow("ego")->getCurrentAccel().linear.x; double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index 8a9286cf133..81e22862709 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -39,7 +39,7 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getCurrentAccel("ego").linear.x; + double ego_accel = api_.getEntityOrThrow("ego")->getCurrentAccel().linear.x; double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 443191a0efd..2a81542791e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -496,10 +496,9 @@ class SimulatorCore } } - template - static auto evaluateSpeed(Ts &&... xs) + static auto evaluateSpeed(const std::string & name) { - return core->getEntityOrThrow(std::forward(xs)...)->getCurrentTwist().linear.x; + return core->getEntityOrThrow(name)->getCurrentTwist().linear.x; } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index e0347c5bc14..82060bd2372 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -347,7 +347,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getBoundingBox); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); - FORWARD_TO_ENTITY_MANAGER(getCurrentAccel); FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); 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 faa0664dc1a..3802b0f863a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -259,7 +259,6 @@ class EntityManager FORWARD_TO_ENTITY(asFieldOperatorApplication, const); FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBoundingBox, const); - FORWARD_TO_ENTITY(getCurrentAccel, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); From 89ef194aca6d6e32eba0bd4c832c21303a90b281 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 17:42:32 +0200 Subject: [PATCH 11/49] Remove forwarding of getLinearJerk in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 82060bd2372..9f953efbcc4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -353,7 +353,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); - FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); 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 3802b0f863a..2527b867d35 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -261,7 +261,6 @@ class EntityManager FORWARD_TO_ENTITY(getBoundingBox, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); - FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); From 24e732108c2bd57940a3f0839cc8aceb9ffec512 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 17:46:38 +0200 Subject: [PATCH 12/49] Remove forwarding of getBoundingBox in EntityManager and API Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 9f953efbcc4..4f2755d4aa7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -344,7 +344,6 @@ class API FORWARD_TO_ENTITY_MANAGER(cancelRequest); FORWARD_TO_ENTITY_MANAGER(checkCollision); FORWARD_TO_ENTITY_MANAGER(entityExists); - FORWARD_TO_ENTITY_MANAGER(getBoundingBox); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); FORWARD_TO_ENTITY_MANAGER(getCurrentAction); 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 2527b867d35..76cf4c0fa78 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -258,7 +258,6 @@ class EntityManager FORWARD_TO_ENTITY(asFieldOperatorApplication, const); FORWARD_TO_ENTITY(get2DPolygon, const); - FORWARD_TO_ENTITY(getBoundingBox, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getRouteLanelets, const); From be319aab502c7bddea5eb51dace40aba1995f847 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 17:47:58 +0200 Subject: [PATCH 13/49] Remove forwarding of get2DPolygon in EntityManager Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 1 file changed, 1 deletion(-) 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 76cf4c0fa78..44ca4035f4f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -257,7 +257,6 @@ class EntityManager // clang-format on FORWARD_TO_ENTITY(asFieldOperatorApplication, const); - FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getRouteLanelets, const); From 9d942f96b731349ece41f4443ce77d965050cd84 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Jun 2024 18:00:01 +0200 Subject: [PATCH 14/49] Remove forwarding of getStandStillDuration in EntityManager and API Signed-off-by: Mateusz Palczuk --- mock/cpp_mock_scenarios/src/random_scenario/random001.cpp | 4 +++- .../include/openscenario_interpreter/simulator_core.hpp | 5 ++--- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/entity_manager.hpp | 1 - 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 98bb5b8ac1b..a2be32cd8f3 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -175,7 +175,9 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange(entity_name, speed, true); api_.setLinearVelocity(entity_name, speed); } - if (api_.entityExists(entity_name) && api_.getStandStillDuration(entity_name) >= 0.5) { + if ( + api_.entityExists(entity_name) && + api_.getEntityOrThrow(entity_name)->getStandStillDuration() >= 0.5) { api_.despawn(entity_name); } }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 2a81542791e..7deda4485eb 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -501,10 +501,9 @@ class SimulatorCore return core->getEntityOrThrow(name)->getCurrentTwist().linear.x; } - template - static auto evaluateStandStill(Ts &&... xs) + static auto evaluateStandStill(const std::string & name) { - return core->getStandStillDuration(std::forward(xs)...); + return core->getEntityOrThrow(name)->getStandStillDuration(); } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 4f2755d4aa7..8132647daad 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -352,7 +352,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); - FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); 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 44ca4035f4f..1172dee2ec0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -260,7 +260,6 @@ class EntityManager FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getRouteLanelets, const); - FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); FORWARD_TO_ENTITY(laneMatchingSucceed, const); FORWARD_TO_ENTITY(activateOutOfRangeJob, ); From 9953d8c4d6f142f248a5fc3015b465c7e5b76c5b Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 26 Jun 2024 14:15:11 +0200 Subject: [PATCH 15/49] feat(cpp_mock, traffic_simulator): change getEntity, use getEntityOrNullptr --- .../src/crosswalk/stop_at_crosswalk.cpp | 6 +- .../accelerate_and_follow.cpp | 8 +- .../decelerate_and_follow.cpp | 8 +- .../src/follow_lane/follow_with_offset.cpp | 11 ++- ...t_distance_in_lane_coordinate_distance.cpp | 47 +++++------ .../get_distance_to_lane_bound.cpp | 17 ++-- .../src/move_backward/move_backward.cpp | 2 +- .../src/pedestrian/walk_straight.cpp | 4 +- .../src/random_scenario/random001.cpp | 52 ++++++------ .../speed_planning/request_speed_change.cpp | 11 +-- .../request_speed_change_continuous_false.cpp | 8 +- .../request_speed_change_relative.cpp | 7 +- .../request_speed_change_step.cpp | 4 +- .../request_speed_change_with_limit.cpp | 9 +- ...uest_speed_change_with_time_constraint.cpp | 8 +- ...eed_change_with_time_constraint_linear.cpp | 3 +- ...d_change_with_time_constraint_relative.cpp | 8 +- .../define_traffic_source_delay.cpp | 15 ++-- .../define_traffic_source_high_rate.cpp | 13 ++- .../define_traffic_source_large.cpp | 11 ++- .../define_traffic_source_mixed.cpp | 14 ++-- .../define_traffic_source_multiple.cpp | 24 +++--- .../define_traffic_source_outside_lane.cpp | 11 ++- .../define_traffic_source_pedestrian.cpp | 11 ++- .../define_traffic_source_vehicle.cpp | 13 ++- .../simulator_core.hpp | 42 +++++----- .../include/traffic_simulator/api/api.hpp | 16 ++-- .../entity/entity_manager.hpp | 4 +- simulation/traffic_simulator/src/api/api.cpp | 66 ++++++--------- .../src/entity/entity_manager.cpp | 84 +++++++++---------- .../random_test_runner/test_executor.hpp | 8 +- .../test/test_test_executor.cpp | 8 +- 32 files changed, 260 insertions(+), 293 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index dc0a6ea6cfb..c9ac307a7ed 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -50,17 +50,17 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P */ if (t == 1.0) { - if (t != api_.getEntityOrThrow("bob")->getCurrentTwist().linear.x) { + if (t != api_.getEntity("bob")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } if (t >= 6.6) { if (7.5 >= t) { - if (std::fabs(0.1) <= api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x) { + if (std::fabs(0.1) <= api_.getEntity("ego")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } else { - if (0.1 >= api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x) { + if (0.1 >= api_.getEntity("ego")->getCurrentTwist().linear.x) { stop(cpp_mock_scenarios::Result::FAILURE); } } diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index 6f68e68ddaf..5edc604cef4 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -39,10 +39,10 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getEntityOrThrow("ego")->getCurrentAccel().linear.x; - double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; - // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; + double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x; + double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; + // double npc_accel = static_cast(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x; + double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x; ; // LCOV_EXCL_START if (npc_twist > (ego_twist + 1) && ego_accel < 0) { diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index 81e22862709..1d94bd72d92 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -39,10 +39,10 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getEntityOrThrow("ego")->getCurrentAccel().linear.x; - double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; - // double npc_accel = static_cast(api_.getEntityOrThrow("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getEntityOrThrow("npc")->getCurrentTwist().linear.x; + double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x; + double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; + // double npc_accel = static_cast(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x; + double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x; ; // LCOV_EXCL_START if (ego_twist > (npc_twist + 1) && ego_accel > 0) { diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index 3efd31a9089..3a306ffe1f7 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -40,13 +40,12 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34507, 0.1)) { - stop(cpp_mock_scenarios::Result::SUCCESS); - } - if (const auto entity = api_.getEntity("ego"); not entity) { - stop(cpp_mock_scenarios::Result::FAILURE); - } else if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + const auto entity = api_.getEntity("ego"); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { stop(cpp_mock_scenarios::Result::FAILURE); + } else if (traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34507, 0.1, api_.getHdmapUtils())) { + stop(cpp_mock_scenarios::Result::SUCCESS); } else if ( std::abs(static_cast(lanelet_pose.value()).offset) <= 2.8) { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 7b8af48d5d9..e98df3ae113 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -44,14 +44,12 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar auto lateralDistance(const std::string & from_entity_name, const std::string & to_entity_name) -> std::optional { - if (const auto from_entity = api_.getEntity(from_entity_name); - from_entity && from_entity->laneMatchingSucceed()) { - if (const auto to_entity = api_.getEntity(to_entity_name); - to_entity && to_entity->laneMatchingSucceed()) { - return traffic_simulator::distance::lateralDistance( - from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, api_.getHdmapUtils()); - } + const auto from_entity = api_.getEntity(from_entity_name); + const auto to_entity = api_.getEntity(to_entity_name); + if (from_entity->laneMatchingSucceed() && to_entity->laneMatchingSucceed()) { + return traffic_simulator::distance::lateralDistance( + from_entity->getCanonicalizedLaneletPose().value(), + to_entity->getCanonicalizedLaneletPose().value(), false, api_.getHdmapUtils()); } return std::nullopt; }; @@ -60,16 +58,14 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar const std::string & from_entity_name, const std::string & to_entity_name, const double matching_distance) -> std::optional { - if (const auto from_entity = api_.getEntity(from_entity_name)) { - if (const auto to_entity = api_.getEntity(to_entity_name)) { - auto from_entity_lanelet_pose = from_entity->getCanonicalizedLaneletPose(matching_distance); - auto to_entity_lanelet_pose = to_entity->getCanonicalizedLaneletPose(matching_distance); - if (from_entity_lanelet_pose && to_entity_lanelet_pose) { - return traffic_simulator::distance::lateralDistance( - from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), false, - api_.getHdmapUtils()); - } - } + auto from_entity_lanelet_pose = + api_.getEntity(from_entity_name)->getCanonicalizedLaneletPose(matching_distance); + auto to_entity_lanelet_pose = + api_.getEntity(to_entity_name)->getCanonicalizedLaneletPose(matching_distance); + if (from_entity_lanelet_pose && to_entity_lanelet_pose) { + return traffic_simulator::distance::lateralDistance( + from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), false, + api_.getHdmapUtils()); } return std::nullopt; }; @@ -78,15 +74,12 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar const std::string & from_entity_name, const std::string & to_entity_name) -> std::optional { - if (const auto from_entity = api_.getEntity(from_entity_name); - from_entity && from_entity->laneMatchingSucceed()) { - if (const auto to_entity = api_.getEntity(to_entity_name); - to_entity && to_entity->laneMatchingSucceed()) { - return traffic_simulator::distance::longitudinalDistance( - from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, true, false, - api_.getHdmapUtils()); - } + const auto from_entity = api_.getEntity(from_entity_name); + const auto to_entity = api_.getEntity(to_entity_name); + if (from_entity->laneMatchingSucceed() && to_entity->laneMatchingSucceed()) { + return traffic_simulator::distance::longitudinalDistance( + from_entity->getCanonicalizedLaneletPose().value(), + to_entity->getCanonicalizedLaneletPose().value(), false, true, false, api_.getHdmapUtils()); } return std::nullopt; } diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index b9caae26f42..2f1b41cd733 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -44,16 +44,15 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod if (api_.getCurrentTime() >= 10.0) { stop(cpp_mock_scenarios::Result::SUCCESS); } - if (auto ego_entity = api_.getEntity("ego")) { - const auto distance = traffic_simulator::distance::distanceToLaneBound( - ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets(), - api_.getHdmapUtils()); - // LCOV_EXCL_START - if (distance <= 0.4 && distance >= 0.52) { - stop(cpp_mock_scenarios::Result::FAILURE); - } - // LCOV_EXCL_STOP + const auto ego_entity = api_.getEntity("ego"); + const auto distance = traffic_simulator::distance::distanceToLaneBound( + ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets(), + api_.getHdmapUtils()); + // LCOV_EXCL_START + if (distance <= 0.4 && distance >= 0.52) { + stop(cpp_mock_scenarios::Result::FAILURE); } + // LCOV_EXCL_STOP } void onInitialize() override { diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 001edec148d..dc3ca614fa2 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -42,7 +42,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START - double ego_twist = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; + double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; if (ego_twist >= -2.9) { stop(cpp_mock_scenarios::Result::FAILURE); } diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 9f999c8b4ed..bfa867da610 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -50,13 +50,13 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P */ if (t == 1.0) { - const auto vel = api_.getEntityOrThrow("bob")->getCurrentTwist().linear.x; + const auto vel = api_.getEntity("bob")->getCurrentTwist().linear.x; if (t != vel) { stop(cpp_mock_scenarios::Result::FAILURE); } } if (t >= 6.65 && 7.25 >= t) { - const auto vel = api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x; + const auto vel = api_.getEntity("ego")->getCurrentTwist().linear.x; if (std::fabs(0.01) <= vel) { stop(cpp_mock_scenarios::Result::FAILURE); } diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index a2be32cd8f3..6b5f5666bd9 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -177,7 +177,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } if ( api_.entityExists(entity_name) && - api_.getEntityOrThrow(entity_name)->getStandStillDuration() >= 0.5) { + api_.getEntity(entity_name)->getStandStillDuration() >= 0.5) { api_.despawn(entity_name); } }; @@ -189,20 +189,19 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( 34621, 10, 0.0, api_.getHdmapUtils()); const auto entity_name = "spawn_nearby_ego"; - if (const auto ego = api_.getEntity("ego")) { - if (api_.reachPosition("ego", trigger_position, 20.0) && !api_.entityExists(entity_name)) { - api_.spawn( - entity_name, - traffic_simulator::pose::transformRelativePoseToGlobal( - ego->getMapPose(), - geometry_msgs::build() - .position(geometry_msgs::build().x(10).y(-5).z(0)) - .orientation(geometry_msgs::msg::Quaternion())), - getVehicleParameters(), - traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); - } else { - stop(cpp_mock_scenarios::Result::FAILURE); - } + const auto ego = api_.getEntity("ego"); + if (api_.reachPosition("ego", trigger_position, 20.0) && !api_.entityExists(entity_name)) { + api_.spawn( + entity_name, + traffic_simulator::pose::transformRelativePoseToGlobal( + ego->getMapPose(), + geometry_msgs::build() + .position(geometry_msgs::build().x(10).y(-5).z(0)) + .orientation(geometry_msgs::msg::Quaternion())), + getVehicleParameters(), + traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); } if (!api_.reachPosition("ego", trigger_position, 20.0) && api_.entityExists(entity_name)) { api_.despawn(entity_name); @@ -222,19 +221,16 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode {traffic_simulator::helper::constructCanonicalizedLaneletPose( 34606, 0.0, 0.0, api_.getHdmapUtils())}, getVehicleParameters()); - if (const auto ego = api_.getEntity("ego")) { - api_.spawn( - "parking_outside", - traffic_simulator::pose::transformRelativePoseToGlobal( - ego->getMapPose(), - geometry_msgs::build() - .position(geometry_msgs::build().x(10).y(15).z(0)) - .orientation(geometry_msgs::msg::Quaternion())), - getVehicleParameters(), - traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); - } else { - stop(cpp_mock_scenarios::Result::FAILURE); - } + const auto ego = api_.getEntity("ego"); + api_.spawn( + "parking_outside", + traffic_simulator::pose::transformRelativePoseToGlobal( + ego->getMapPose(), + geometry_msgs::build() + .position(geometry_msgs::build().x(10).y(15).z(0)) + .orientation(geometry_msgs::msg::Quaternion())), + getVehicleParameters(), + traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); } }; diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 75dd3fa4958..0b81cc33d70 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -39,18 +39,15 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getEntityOrThrow("front")->getCurrentTwist().linear.x < 10.0) { + if (api_.getEntity("front")->getCurrentTwist().linear.x < 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } - if ( - api_.getCurrentTime() <= 0.9 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { + if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - api_.getCurrentTime() >= 1.0 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 9.9) { + api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 && + api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 4f0a0d8ee81..167334f53a5 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -45,19 +45,17 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp */ if (api_.getCurrentTime() <= 0.95) { if (!equals( - api_.getCurrentTime() * 10.0, api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, + api_.getCurrentTime() * 10.0, api_.getEntity("ego")->getCurrentTwist().linear.x, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } - if ( - api_.getCurrentTime() >= 1.0 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0) { + if (api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0) { speed_reached = true; } if ( speed_reached && api_.getCurrentTime() >= 1.5 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 13.88) { + api_.getEntity("ego")->getCurrentTwist().linear.x >= 13.88) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index f556c236e0d..d82327ae941 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -41,15 +41,14 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari { if (api_.getCurrentTime() <= 1.9) { if (!equals( - api_.getCurrentTime() + 3.0, api_.getEntityOrThrow("front")->getCurrentTwist().linear.x, + api_.getCurrentTime() + 3.0, api_.getEntity("front")->getCurrentTwist().linear.x, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } if ( - api_.getCurrentTime() >= 2.0 && - api_.getEntityOrThrow("front")->getCurrentTwist().linear.x <= 5.05 && - api_.getEntityOrThrow("front")->getCurrentTwist().linear.x >= 4.95) { + api_.getCurrentTime() >= 2.0 && api_.getEntity("front")->getCurrentTwist().linear.x <= 5.05 && + api_.getEntity("front")->getCurrentTwist().linear.x >= 4.95) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index a36e35929c1..57e1033c903 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -41,8 +41,8 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod { if ( api_.getCurrentTime() <= 0.999 && - !equals(api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, 10.0, 0.01) && - !equals(api_.getEntityOrThrow("front")->getCurrentTwist().linear.x, 10.0, 0.01)) { + !equals(api_.getEntity("ego")->getCurrentTwist().linear.x, 10.0, 0.01) && + !equals(api_.getEntity("front")->getCurrentTwist().linear.x, 10.0, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 1.0) { diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 8619619a33c..2a0b21cde51 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -39,15 +39,12 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar private: void onUpdate() override { - if ( - api_.getCurrentTime() <= 0.9 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { + if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - api_.getCurrentTime() >= 1.0 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 5.1 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 4.9) { + api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 5.1 && + api_.getEntity("ego")->getCurrentTwist().linear.x >= 4.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 5b0ef3e1fbd..cb9b9c71aa3 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -39,15 +39,13 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: private: void onUpdate() override { - if ( - api_.getCurrentTime() <= 3.9 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x > 10.0) { + if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.999) { if ( - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 10.0 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 9.9) { + api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 && + api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index 16065790d6e..2baebbd6ff8 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -42,8 +42,7 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario { if (api_.getCurrentTime() <= 3.999) { if (!equals( - api_.getCurrentTime() * 2.5, api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x, - 0.01)) { + api_.getCurrentTime() * 2.5, api_.getEntity("ego")->getCurrentTwist().linear.x, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } else if (api_.getCurrentTime() >= 4.0) { diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index 8bb70471986..3496d077642 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -40,15 +40,13 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario private: void onUpdate() override { - if ( - api_.getCurrentTime() <= 3.9 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 3.0) { + if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x >= 3.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.9999) { if ( - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x <= 3.1 && - api_.getEntityOrThrow("ego")->getCurrentTwist().linear.x >= 2.9) { + api_.getEntity("ego")->getCurrentTwist().linear.x <= 3.1 && + api_.getEntity("ego")->getCurrentTwist().linear.x >= 2.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index cef7e329a57..db69ad583f7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -75,15 +75,18 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); + if (!valid_vehicle_lanelet || !is_vehicle) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 3079602a1f5..88ad79885a8 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -52,15 +52,20 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { + if (!valid_vehicle_lanelet || !is_vehicle) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 58811267ae3..5b92a0ef466 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -52,13 +52,12 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { - const bool is_vehicle = - entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; + const auto entity = api_.getEntity(name); + const bool is_vehicle = + entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; - if (!entity->laneMatchingSucceed() || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE - } + if (!entity->laneMatchingSucceed() || !is_vehicle) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index b27e8c549f0..ff98f2444e4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -53,15 +53,19 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; const bool is_pedestrian = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); if (is_vehicle) { ++vehicle_count; @@ -69,7 +73,7 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode ++pedestrian_count; } - if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet) { + if (!valid_vehicle_lanelet) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 759aa9ea92a..e3c70a82eab 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -53,18 +53,22 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; const bool is_pedestrian = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); + const bool valid_pedestrian_lanelet = traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34385, 10.0, api_.getHdmapUtils()); if (is_vehicle) { ++vehicle_count; @@ -73,12 +77,8 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode } if ( - // clang-format off - !entity->laneMatchingSucceed() || - (is_vehicle && !valid_vehicle_lanelet) || - (is_pedestrian && !valid_pedestrian_lanelet)) - // clang-format on - { + (is_vehicle && !valid_vehicle_lanelet) || + (is_pedestrian && !valid_pedestrian_lanelet)) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index f738806d4b9..31996cc9352 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -52,13 +52,12 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { - const bool is_pedestrian = - entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + const auto entity = api_.getEntity(name); + const bool is_pedestrian = + entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - if (entity->laneMatchingSucceed() || !is_pedestrian) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE - } + if (entity->laneMatchingSucceed() || !is_pedestrian) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 862d0442aa9..a02bd859d74 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -52,14 +52,17 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_pedestrian = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); + const bool valid_pedestrian_lanelet = traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34385, 10.0, api_.getHdmapUtils()); - if (!entity->laneMatchingSucceed() || !valid_pedestrian_lanelet || !is_pedestrian) { + if (!valid_pedestrian_lanelet || !is_pedestrian) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 19d717563db..cbda4c723f7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -52,15 +52,20 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - if (const auto entity = api_.getEntity(name)) { + const auto entity = api_.getEntity(name); + if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } else { const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || + traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { + if (!valid_vehicle_lanelet || !is_vehicle) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 7deda4485eb..9ed0e8a9df2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -106,8 +106,8 @@ class SimulatorCore static auto makeNativeRelativeWorldPosition( const std::string & from_entity_name, const std::string & to_entity_name) { - if (const auto from_entity = core->getEntity(from_entity_name)) { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if ( const auto relative_pose = traffic_simulator::pose::relativePose( from_entity->getMapPose(), to_entity->getMapPose())) @@ -120,7 +120,7 @@ class SimulatorCore static auto makeNativeRelativeWorldPosition( const std::string & from_entity_name, const NativeWorldPosition & to_map_pose) { - if (const auto from_entity = core->getEntity(from_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { if ( const auto relative_pose = traffic_simulator::pose::relativePose(from_entity->getMapPose(), to_map_pose)) { @@ -133,7 +133,7 @@ class SimulatorCore static auto makeNativeRelativeWorldPosition( const NativeWorldPosition & from_map_pose, const std::string & to_entity_name) { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if ( const auto relative_pose = traffic_simulator::pose::relativePose(from_map_pose, to_entity->getMapPose())) { @@ -148,7 +148,7 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if (const auto to_lanelet_pose = to_entity->getCanonicalizedLaneletPose()) { return makeNativeRelativeLanePosition( from_entity_name, to_lanelet_pose.value(), routing_algorithm); @@ -162,7 +162,7 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - if (const auto from_entity = core->getEntity(from_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { if (const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) { return makeNativeRelativeLanePosition( from_lanelet_pose.value(), to_lanelet_pose, routing_algorithm); @@ -185,8 +185,8 @@ class SimulatorCore const std::string & from_entity_name, const std::string & to_entity_name, const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) { - if (const auto from_entity = core->getEntity(from_entity_name)) { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if (const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) { if (const auto to_lanelet_pose = to_entity->getCanonicalizedLaneletPose()) { return makeNativeBoundingBoxRelativeLanePosition( @@ -203,7 +203,7 @@ class SimulatorCore const std::string & from_entity_name, const NativeLanePosition & to_lanelet_pose, const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) { - if (const auto from_entity = core->getEntity(from_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { if (const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) { return makeNativeBoundingBoxRelativeLanePosition( from_lanelet_pose.value(), from_entity->getBoundingBox(), to_lanelet_pose, @@ -230,8 +230,8 @@ class SimulatorCore static auto makeNativeBoundingBoxRelativeWorldPosition( const std::string & from_entity_name, const std::string & to_entity_name) { - if (const auto from_entity = core->getEntity(from_entity_name)) { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if ( const auto relative_pose = traffic_simulator::pose::boundingBoxRelativePose( from_entity->getMapPose(), from_entity->getBoundingBox(), to_entity->getMapPose(), @@ -246,7 +246,7 @@ class SimulatorCore static auto makeNativeBoundingBoxRelativeWorldPosition( const std::string & from_entity_name, const NativeWorldPosition & to_map_pose) { - if (const auto from_entity = core->getEntity(from_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { if ( const auto relative_pose = traffic_simulator::pose::boundingBoxRelativePose( from_entity->getMapPose(), from_entity->getBoundingBox(), to_map_pose, @@ -278,7 +278,7 @@ class SimulatorCore const EntityRef & entity_ref, const DynamicConstraints & dynamic_constraints) -> void { return core->setBehaviorParameter(entity_ref, [&]() { - auto behavior_parameter = core->getEntityOrThrow(entity_ref)->getBehaviorParameter(); + auto behavior_parameter = core->getEntity(entity_ref)->getBehaviorParameter(); if (not std::isinf(dynamic_constraints.max_speed)) { behavior_parameter.dynamic_constraints.max_speed = dynamic_constraints.max_speed; @@ -317,7 +317,7 @@ class SimulatorCore "maxSpeed", std::numeric_limits::max())); core->setBehaviorParameter(entity_ref, [&]() { - auto message = core->getEntityOrThrow(entity_ref)->getBehaviorParameter(); + auto message = core->getEntity(entity_ref)->getBehaviorParameter(); message.see_around = not controller.properties.template get("isBlind"); /// The default values written in https://github.com/tier4/scenario_simulator_v2/blob/master/simulation/traffic_simulator_msgs/msg/DynamicConstraints.msg message.dynamic_constraints.max_acceleration = @@ -460,7 +460,7 @@ class SimulatorCore protected: static auto evaluateAcceleration(const std::string & name) { - return core->getEntityOrThrow(name)->getCurrentAccel().linear.x; + return core->getEntity(name)->getCurrentAccel().linear.x; } template @@ -473,8 +473,8 @@ class SimulatorCore const std::string & from_entity_name, const std::string & to_entity_name) // for RelativeDistanceCondition { - if (const auto from_entity = core->getEntity(from_entity_name)) { - if (const auto to_entity = core->getEntity(to_entity_name)) { + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { if ( const auto distance = traffic_simulator::distance::boundingBoxDistance( from_entity->getMapPose(), from_entity->getBoundingBox(), to_entity->getMapPose(), @@ -498,12 +498,12 @@ class SimulatorCore static auto evaluateSpeed(const std::string & name) { - return core->getEntityOrThrow(name)->getCurrentTwist().linear.x; + return core->getEntity(name)->getCurrentTwist().linear.x; } static auto evaluateStandStill(const std::string & name) { - return core->getEntityOrThrow(name)->getStandStillDuration(); + return core->getEntity(name)->getStandStillDuration(); } template @@ -553,7 +553,7 @@ class SimulatorCore static auto evaluateRelativeHeading( const EntityRef & entity_ref, const OSCLanePosition & osc_lane_position) { - if (const auto entity = core->getEntity(entity_ref)) { + if (const auto entity = core->getEntityOrNullptr(entity_ref)) { const auto from_map_pose = entity->getMapPose(); const auto to_map_pose = static_cast(osc_lane_position); if ( @@ -569,7 +569,7 @@ class SimulatorCore template static auto evaluateRelativeHeading(const EntityRef & entity_ref) { - if (const auto entity = core->getEntity(entity_ref)) { + if (const auto entity = core->getEntityOrNullptr(entity_ref)) { if (const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) { return static_cast(std::abs( static_cast(canonicalized_lanelet_pose.value()).rpy.z)); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 8132647daad..06d36eb6b86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -68,13 +68,7 @@ class API entity_manager_ptr_(std::make_shared(node, configuration)), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, - [this](const auto & entity_name) { - if (const auto entity = getEntity(entity_name)) { - return entity->getMapPose(); - } else { - THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); - } - }, + [this](const auto & entity_name) { return getEntity(entity_name)->getMapPose(); }, [this](const auto & name) { return API::despawn(name); }, configuration.auto_sink)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), @@ -147,7 +141,7 @@ class API auto register_to_environment_simulator = [&]() { if (configuration.standalone_mode) { return true; - } else if (const auto entity = entity_manager_ptr_->getEntity(name); not entity) { + } else if (const auto entity = entity_manager_ptr_->getEntityOrNullptr(name); not entity) { throw common::SemanticError( "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); } else { @@ -181,7 +175,7 @@ class API auto register_to_environment_simulator = [&]() { if (configuration.standalone_mode) { return true; - } else if (const auto entity = entity_manager_ptr_->getEntity(name); not entity) { + } else if (const auto entity = entity_manager_ptr_->getEntityOrNullptr(name); not entity) { throw common::SemanticError( "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); } else { @@ -211,7 +205,7 @@ class API auto register_to_environment_simulator = [&]() { if (configuration.standalone_mode) { return true; - } else if (const auto entity = entity_manager_ptr_->getEntity(name); not entity) { + } else if (const auto entity = entity_manager_ptr_->getEntityOrNullptr(name); not entity) { throw common::SemanticError( "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); } else { @@ -349,7 +343,7 @@ class API FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); - FORWARD_TO_ENTITY_MANAGER(getEntityOrThrow); + FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); 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 1172dee2ec0..9eac169ded6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -309,10 +309,10 @@ class EntityManager auto getEntityNames() const -> const std::vector; - auto getEntity(const std::string & name) const + auto getEntityOrNullptr(const std::string & name) const -> std::shared_ptr; - auto getEntityOrThrow(const std::string & name) const + auto getEntity(const std::string & name) const -> std::shared_ptr; auto getHdmapUtils() -> const std::shared_ptr &; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 49503979ef9..77831c39b2f 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -67,7 +67,7 @@ auto API::respawn( // read status from EntityManager, then send it to SimpleSensorSimulator simulation_api_schema::UpdateEntityStatusRequest req; simulation_interface::toProto( - static_cast(entity_manager_ptr_->getEntityOrThrow(name)->getStatus()), + static_cast(entity_manager_ptr_->getEntity(name)->getStatus()), *req.add_status()); req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(name)); @@ -87,7 +87,7 @@ auto API::respawn( } else { // if valid, set response in EntityManager, then plan path and engage auto entity_status = - static_cast(entity_manager_ptr_->getEntityOrThrow(name)->getStatus()); + static_cast(entity_manager_ptr_->getEntity(name)->getStatus()); simulation_interface::toMsg(res_status->pose(), entity_status.pose); simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); setMapPose(name, entity_status.pose); @@ -106,39 +106,32 @@ auto API::setEntityStatus( const std::optional & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - if (const auto entity = getEntity(name)) { - auto status = static_cast(entity->getStatus()); - status.action_status = action_status; - if (canonicalized_lanelet_pose) { - status.pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose_valid = true; - } - entity->setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } else { - THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); + const auto entity = getEntity(name); + auto status = static_cast(entity->getStatus()); + status.action_status = action_status; + if (canonicalized_lanelet_pose) { + status.pose = static_cast(canonicalized_lanelet_pose.value()); + status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); + status.lanelet_pose_valid = true; } + entity->setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } auto API::setEntityStatus(const std::string & name, const EntityStatus & status) -> void { - if (const auto entity = getEntity(name)) { - entity->setStatus(status); - } else { - THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); - } + getEntity(name)->setStatus(status); } /// @todo it probably should be moved to SimulatorCore std::optional API::getTimeHeadway( const std::string & from_entity_name, const std::string & to_entity_name) { - if (auto from_entity = getEntity(from_entity_name); from_entity) { - if (auto to_entity = getEntity(to_entity_name); to_entity) { + if (auto from_entity = getEntityOrNullptr(from_entity_name); from_entity) { + if (auto to_entity = getEntityOrNullptr(to_entity_name); to_entity) { if (auto relative_pose = relativePose(from_entity->getMapPose(), to_entity->getMapPose()); relative_pose && relative_pose->position.x <= 0) { - const double time_headway = (relative_pose->position.x * -1) / - getEntityOrThrow(to_entity_name)->getCurrentTwist().linear.x; + const double time_headway = + (relative_pose->position.x * -1) / getEntity(to_entity_name)->getCurrentTwist().linear.x; return std::isnan(time_headway) ? std::numeric_limits::infinity() : time_headway; } } @@ -158,14 +151,11 @@ auto API::setEntityStatus( const std::string & name, const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - if (const auto entity = getEntity(name)) { - EntityStatus status = static_cast(entity->getStatus()); - status.pose = map_pose; - status.action_status = action_status; - setEntityStatus(name, status); - } else { - THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); - } + const auto entity = getEntity(name); + EntityStatus status = static_cast(entity->getStatus()); + status.pose = map_pose; + status.action_status = action_status; + setEntityStatus(name, status); } auto API::setEntityStatus( @@ -173,14 +163,10 @@ auto API::setEntityStatus( const geometry_msgs::msg::Pose & relative_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - if (const auto reference_entity = getEntity(reference_entity_name)) { - setEntityStatus( - name, transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), - action_status); - } else { - THROW_SIMULATION_ERROR( - "Cannot get entity \"", reference_entity_name, "\" - such entity does not exist."); - } + const auto reference_entity = getEntity(reference_entity_name); + setEntityStatus( + name, transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), + action_status); } auto API::setEntityStatus( @@ -285,7 +271,7 @@ bool API::updateEntitiesStatusInSim() req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { const auto entity_status = - static_cast(entity_manager_ptr_->getEntityOrThrow(entity_name)->getStatus()); + static_cast(entity_manager_ptr_->getEntity(entity_name)->getStatus()); simulation_interface::toProto(entity_status, *req.add_status()); if (entity_manager_ptr_->is(entity_name)) { req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); @@ -297,7 +283,7 @@ bool API::updateEntitiesStatusInSim() for (const auto & res_status : res.status()) { auto entity_name = res_status.name(); auto entity_status = - static_cast(entity_manager_ptr_->getEntityOrThrow(entity_name)->getStatus()); + static_cast(entity_manager_ptr_->getEntity(entity_name)->getStatus()); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 98df2b25a0c..adcf9c93e35 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -50,18 +50,16 @@ void EntityManager::broadcastEntityTransform() * so we publish the average of the coordinates of all entities. */ if (isEgoSpawned()) { - if (const auto ego = getEntity(getEgoName())) { - broadcastTransform( - geometry_msgs::build() - /** + broadcastTransform( + geometry_msgs::build() + /** * @note This is the intended implementation. * It is easier to create rviz config if the name "ego" is fixed, * so the frame_id “ego” is issued regardless of the name of the ego entity. */ - .header(std_msgs::build().stamp(clock_ptr_->now()).frame_id("ego")) - .pose(ego->getMapPose()), - true); - } + .header(std_msgs::build().stamp(clock_ptr_->now()).frame_id("ego")) + .pose(getEntity(getEgoName())->getMapPose()), + true); } if (const auto names = getEntityNames(); !names.empty()) { broadcastTransform( @@ -102,12 +100,13 @@ void EntityManager::broadcastTransform( } } +/// @todo it probably should be moved to SimulatorCore bool EntityManager::checkCollision( const std::string & first_entity_name, const std::string & second_entity_name) { if (first_entity_name != second_entity_name) { - if (const auto first_entity = getEntity(first_entity_name)) { - if (const auto second_entity = getEntity(second_entity_name)) { + if (const auto first_entity = getEntityOrNullptr(first_entity_name)) { + if (const auto second_entity = getEntityOrNullptr(second_entity_name)) { return math::geometry::checkCollision2D( first_entity->getMapPose(), first_entity->getBoundingBox(), second_entity->getMapPose(), second_entity->getBoundingBox()); @@ -145,7 +144,7 @@ auto EntityManager::getEntityNames() const -> const std::vector return names; } -auto EntityManager::getEntity(const std::string & name) const +auto EntityManager::getEntityOrNullptr(const std::string & name) const -> std::shared_ptr { if (auto it = entities_.find(name); it != entities_.end()) { @@ -155,17 +154,17 @@ auto EntityManager::getEntity(const std::string & name) const This method returns nullptr, due to the fact that the interpretation of the scenario operates in such a way that checking a condition, e.g. DistanceCondition, is called also for Entities that have not yet been spawned. For example, if for DistanceCondition any getEntity() returns - nullptr, the condition returns a distance equal to NaN. For this reason, throwing an exception - through getEntity() is not recommended. + nullptr, the condition returns a distance equal to NaN. For this reason, using getEntity() with + throwing an exception is not recommended. */ return nullptr; } }; -auto EntityManager::getEntityOrThrow(const std::string & name) const +auto EntityManager::getEntity(const std::string & name) const -> std::shared_ptr { - if (const auto entity = getEntity(name)) { + if (const auto entity = getEntityOrNullptr(name)) { return entity; } else { THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); @@ -249,45 +248,49 @@ bool EntityManager::isEgoSpawned() const return false; } +/// @todo it should be removed (use pose::isINLanelet instead) bool EntityManager::isInLanelet( const std::string & name, const lanelet::Id lanelet_id, const double tolerance) { - if (const auto entity = getEntity(name)) { - if (const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) { - return pose::isInLanelet( - canonicalized_lanelet_pose.value(), lanelet_id, tolerance, hdmap_utils_ptr_); - } + const auto entity = getEntity(name); + if (const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) { + return pose::isInLanelet( + canonicalized_lanelet_pose.value(), lanelet_id, tolerance, hdmap_utils_ptr_); + } else { + return false; } - return false; } bool EntityManager::isStopping(const std::string & name) const { - return std::fabs(getEntityOrThrow(name)->getCurrentTwist().linear.x) < + return std::fabs(getEntity(name)->getCurrentTwist().linear.x) < std::numeric_limits::epsilon(); } +/// @todo it probably should be moved to SimulatorCore bool EntityManager::reachPosition( const std::string & name, const std::string & target_entity_name, const double tolerance) const { - if (const auto target_entity = getEntity(target_entity_name)) { + if (const auto target_entity = getEntityOrNullptr(target_entity_name)) { return reachPosition(name, target_entity->getMapPose(), tolerance); } else { return false; } } +/// @todo it probably should be moved to SimulatorCore bool EntityManager::reachPosition( const std::string & name, const geometry_msgs::msg::Pose & target_pose, const double tolerance) const { - if (const auto entity = getEntity(name)) { + if (const auto entity = getEntityOrNullptr(name)) { return math::geometry::getDistance(entity->getMapPose(), target_pose) < tolerance; } else { return false; } } +/// @todo it probably should be moved to SimulatorCore bool EntityManager::reachPosition( const std::string & name, const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const @@ -298,7 +301,7 @@ bool EntityManager::reachPosition( void EntityManager::requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction) { - if (const auto entity = getEntity(name); entity && entity->laneMatchingSucceed()) { + if (const auto entity = getEntity(name); entity->laneMatchingSucceed()) { if ( const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( entity->getStatus().getLaneletId(), direction)) { @@ -310,8 +313,8 @@ void EntityManager::requestLaneChange( void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { - const auto & status = getEntityOrThrow(name)->getStatus(); - const auto behavior_parameter = getEntityOrThrow(name)->getBehaviorParameter(); + const auto & status = getEntity(name)->getStatus(); + const auto behavior_parameter = getEntity(name)->getBehaviorParameter(); if (is(name)) { THROW_SEMANTIC_ERROR( "Entity :", name, "is EgoEntity.", "You cannot reset behavior plugin of EgoEntity."); @@ -341,14 +344,10 @@ void EntityManager::resetBehaviorPlugin( auto EntityManager::getCurrentAction(const std::string & name) const -> std::string { - if (const auto entity = getEntity(name)) { - if (not npc_logic_started_ and not is(name)) { - return "waiting"; - } else { - return entity->getCurrentAction(); - } + if (not npc_logic_started_ and not is(name)) { + return "waiting"; } else { - THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); + return getEntity(name)->getCurrentAction(); } } @@ -373,17 +372,14 @@ auto EntityManager::updateNpcLogic( if (configuration.verbose) { std::cout << "update " << name << " behavior" << std::endl; } - if (const auto entity = getEntity(name)) { - // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego - if (npc_logic_started_) { - entity->onUpdate(current_time, step_time); - } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { - ego_entity->updateFieldOperatorApplication(); - } - return entity->getStatus(); - } else { - THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); + const auto entity = getEntity(name); + // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego + if (npc_logic_started_) { + entity->onUpdate(current_time, step_time); + } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { + ego_entity->updateFieldOperatorApplication(); } + return entity->getStatus(); } void EntityManager::update(const double current_time, const double step_time) diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 84009ec77ee..9fc84093c5a 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -151,13 +151,13 @@ class TestExecutor auto current_time = api_->getCurrentTime(); if (!std::isnan(current_time)) { - if (goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { + if (goal_reached_metric_.isGoalReached(api_->getEntity(ego_name_)->getStatus())) { scenario_completed_ = true; } bool timeout_reached = current_time >= test_timeout_; if (timeout_reached) { - if (!goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { + if (!goal_reached_metric_.isGoalReached(api_->getEntity(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Timeout reached"); error_reporter_.reportTimeout(); } @@ -178,9 +178,9 @@ class TestExecutor } if (almost_standstill_metric_.isAlmostStandingStill( - api_->getEntityOrThrow(ego_name_)->getStatus())) { + api_->getEntity(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Standstill duration exceeded"); - if (goal_reached_metric_.isGoalReached(api_->getEntityOrThrow(ego_name_)->getStatus())) { + if (goal_reached_metric_.isGoalReached(api_->getEntity(ego_name_)->getStatus())) { RCLCPP_INFO(logger_, "Goal reached, standstill expected"); } else { error_reporter_.reportStandStill(); diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 9004f27cf02..103b9745944 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -75,7 +75,7 @@ class MockTrafficSimulatorAPI MOCK_METHOD(bool, despawn, (const std::string), ()); MOCK_METHOD(std::string, getEgoName, (), ()); MOCK_METHOD(double, getCurrentTime, (), ()); - MOCK_METHOD(void, getEntityOrThrowMock, (const std::string &), ()); + MOCK_METHOD(void, getEntityMock, (const std::string &), ()); MOCK_METHOD(bool, entityExists, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); @@ -86,9 +86,9 @@ class MockTrafficSimulatorAPI return *field_operator_application_mock; } - std::shared_ptr getEntityOrThrow(const std::string & name) + std::shared_ptr getEntity(const std::string & name) { - getEntityOrThrowMock(name); + getEntityMock(name); /// @note set invalid LaneletPose so pass std::nullopt return std::make_shared( "", traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt), nullptr, @@ -165,7 +165,7 @@ TEST(TestExecutor, UpdateNoNPCs) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(1.0)); - EXPECT_CALL(*MockAPI, getEntityOrThrowMock).Times(2).InSequence(sequence); + EXPECT_CALL(*MockAPI, getEntityMock).Times(2).InSequence(sequence); EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); test_executor.update(); From e7091e52d3540defb2107832631ba4cec8619618 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 26 Jun 2024 15:40:34 +0200 Subject: [PATCH 16/49] feat(entity_base, traffic_simulator, cpp_mock): move isInLanelet to EntityBase, remove forwarding --- .../follow_lane/acquire_position_in_world_frame.cpp | 2 +- .../src/follow_lane/assign_route_in_world_frame.cpp | 2 +- .../src/follow_lane/cancel_request.cpp | 2 +- .../src/lane_change/lanechange_left.cpp | 2 +- .../src/lane_change/lanechange_left_with_id.cpp | 2 +- .../src/lane_change/lanechange_linear.cpp | 5 +++-- .../lanechange_longitudinal_distance.cpp | 2 +- .../src/lane_change/lanechange_right.cpp | 2 +- .../src/lane_change/lanechange_right_with_id.cpp | 2 +- mock/cpp_mock_scenarios/src/merge/merge_left.cpp | 2 +- .../src/random_scenario/random001.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic_simulator/entity/entity_base.hpp | 3 +++ .../traffic_simulator/entity/entity_manager.hpp | 2 -- .../traffic_simulator/src/entity/entity_base.cpp | 12 ++++++++++++ .../traffic_simulator/src/entity/entity_manager.cpp | 13 ------------- 16 files changed, 28 insertions(+), 28 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 450de807a29..0fa1c9407b6 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -40,7 +40,7 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34408, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34408, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 9794dc48934..54dcc159b25 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -40,7 +40,7 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34630, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34630, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 8db02b87da9..01a792f9071 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -47,7 +47,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode api_.cancelRequest("ego"); canceled = true; } - if (api_.isInLanelet("ego", 34507, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34507, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 9dfe60ac151..5f5438a7ce5 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -39,7 +39,7 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34513, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34513, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index c04184811e8..47fee1945d7 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -39,7 +39,7 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34513, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34513, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index 6cabbd683a8..e153fb0ae52 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -40,10 +40,11 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode bool lanechange_finished = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34513, 0.1)) { + const auto entity = api_.getEntity("ego"); + if (entity->isInLanelet(34513, 0.1)) { lanechange_finished = true; } - if (lanechange_finished && api_.isInLanelet("ego", 34510, 0.1)) { + if (lanechange_finished && entity->isInLanelet(34510, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index e04070e2380..0cc8343b02c 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -40,7 +40,7 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce bool lanechange_finished = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34513, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34513, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index 0210fc3c154..f98e7be572a 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -39,7 +39,7 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34462, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34462, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index eb3e2690d9e..a9e40b16c04 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -39,7 +39,7 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34462, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34462, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index 44978b272e3..27a9717985f 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -39,7 +39,7 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - if (api_.isInLanelet("ego", 34510, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34510, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 7dd60111e7d..8cb9b012dd2 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -147,7 +147,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } }; - if (api_.isInLanelet("ego", 34684, 0.1)) { + if (api_.getEntity("ego")->isInLanelet(34684, 0.1)) { spawn_and_change_lane("lane_following_0", 0.0); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 74c6445644b..53f2f34d406 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -359,7 +359,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); - FORWARD_TO_ENTITY_MANAGER(isInLanelet); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); FORWARD_TO_ENTITY_MANAGER(laneMatchingSucceed); FORWARD_TO_ENTITY_MANAGER(reachPosition); 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 e3cd2d6cceb..53505446a9f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -110,6 +110,9 @@ class EntityBase virtual auto getGoalPoses() -> std::vector = 0; + /* */ auto isInLanelet( + const lanelet::Id lanelet_id, std::optional tolerance = std::nullopt) const -> bool; + /* */ auto getCanonicalizedLaneletPose() const -> std::optional; /* */ auto getCanonicalizedLaneletPose(double matching_distance) const 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 bf32b08806c..887b90f12d2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -356,8 +356,6 @@ class EntityManager const std::string getEgoName() const; - bool isInLanelet(const std::string & name, const lanelet::Id lanelet_id, const double tolerance); - bool isStopping(const std::string & name) const; bool reachPosition( diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index b6a27e86327..87ec91a573a 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -82,6 +82,18 @@ auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const matching_distance, hdmap_utils_ptr_); } +auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional tolerance) const + -> bool +{ + if (const auto lanelet_pose = getCanonicalizedLaneletPose()) { + const auto tolerance_ = + tolerance ? tolerance.value() : getDefaultMatchingDistanceForLaneletPoseCalculation(); + return traffic_simulator::pose::isInLanelet( + lanelet_pose.value(), lanelet_id, tolerance_, hdmap_utils_ptr_); + } + return false; +} + auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double { return getBoundingBox().dimensions.y * 0.5 + 1.0; diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index adcf9c93e35..a34a7ca7cbe 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -248,19 +248,6 @@ bool EntityManager::isEgoSpawned() const return false; } -/// @todo it should be removed (use pose::isINLanelet instead) -bool EntityManager::isInLanelet( - const std::string & name, const lanelet::Id lanelet_id, const double tolerance) -{ - const auto entity = getEntity(name); - if (const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) { - return pose::isInLanelet( - canonicalized_lanelet_pose.value(), lanelet_id, tolerance, hdmap_utils_ptr_); - } else { - return false; - } -} - bool EntityManager::isStopping(const std::string & name) const { return std::fabs(getEntity(name)->getCurrentTwist().linear.x) < From ecb46de364e5141adcfe6609e2ee3783c355f4cf Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 26 Jun 2024 17:06:54 +0200 Subject: [PATCH 17/49] feat(entity_base, traffic_simulator): move reachPosition as isInPosition to EntityBase, remove forwarding --- docs/developer_guide/TrafficSimulator.md | 5 +-- .../src/follow_lane/cancel_request.cpp | 4 +-- ...line_trajectory_with_do_nothing_plugin.cpp | 11 ++++--- .../src/random_scenario/random001.cpp | 20 ++++++------ .../src/traffic_simulation_demo.cpp | 11 +++---- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic_simulator/entity/entity_base.hpp | 6 ++++ .../entity/entity_manager.hpp | 9 ------ .../src/entity/entity_base.cpp | 12 +++++++ .../src/entity/entity_manager.cpp | 31 ------------------- 10 files changed, 44 insertions(+), 66 deletions(-) diff --git a/docs/developer_guide/TrafficSimulator.md b/docs/developer_guide/TrafficSimulator.md index 223a9c0b3bd..ff119bf5090 100644 --- a/docs/developer_guide/TrafficSimulator.md +++ b/docs/developer_guide/TrafficSimulator.md @@ -79,14 +79,15 @@ public: private: void update() { - if (api_.reachPosition("ego", + const auto entity = getEntity("ego"); + if (entity->isInPosition( traffic_simulator::helper::constructLaneletPose(34615, 10.0), 5)) { api_.requestAcquirePosition("ego", traffic_simulator::helper::constructLaneletPose(35026, 0.0) ); api_.setTargetSpeed("npc2", 13, true); } - if (api_.reachPosition("ego", + if (entity->isInPosition( traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5)) { api_.setTargetSpeed("npc2", 3, true); diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 01a792f9071..5b2b49e86fa 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -39,8 +39,8 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode bool canceled = false; void onUpdate() override { - if (api_.reachPosition( - "ego", + const auto entity = api_.getEntity("ego"); + if (entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 30, 0, api_.getHdmapUtils()), 3.0)) { diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 001c24f9589..95357a9f09a 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -63,31 +63,32 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario void onUpdate() override { + const auto entity = api_.getEntity("ego"); // LCOV_EXCL_START if (api_.getCurrentTime() >= 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP - if (equals(api_.getCurrentTime(), 0.0, 0.01) && !api_.reachPosition("ego", spawn_pose, 0.1)) { + if (equals(api_.getCurrentTime(), 0.0, 0.01) && entity->isInPosition(spawn_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.0, 0.01) && - !api_.reachPosition("ego", trajectory_start_pose, 0.1)) { + !entity->isInPosition(trajectory_start_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.5, 0.01) && - !api_.reachPosition("ego", trajectory_waypoint_pose, 0.1)) { + !entity->isInPosition(trajectory_waypoint_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 2.0, 0.01) && - !api_.reachPosition("ego", trajectory_goal_pose, 0.1)) { + !entity->isInPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if (equals(api_.getCurrentTime(), 2.5, 0.01)) { - if (api_.reachPosition("ego", trajectory_goal_pose, 0.1)) { + if (entity->isInPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 8cb9b012dd2..7dda0ae590b 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -106,6 +106,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { + const auto ego_entity = api_.getEntity("ego"); { if (param_listener_->is_old(params_)) { /// When the parameter was updated, clear entity before re-spawning entity. @@ -137,17 +138,17 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode lane_change_requested = false; } /// Checking the ego entity overs the lane change position. - if (const auto entity = api_.getEntity("ego"); entity->laneMatchingSucceed()) { + if (ego_entity->laneMatchingSucceed()) { if ( - entity->getStatus().getLaneletId() == 34684 && - std::abs(entity->getStatus().getLaneletPose().s) >= lane_change_position) { + ego_entity->getStatus().getLaneletId() == 34684 && + std::abs(ego_entity->getStatus().getLaneletPose().s) >= lane_change_position) { api_.requestLaneChange(entity_name, traffic_simulator::lane_change::Direction::RIGHT); lane_change_requested = true; } } }; - if (api_.getEntity("ego")->isInLanelet(34684, 0.1)) { + if (ego_entity->isInLanelet(34684, 0.1)) { spawn_and_change_lane("lane_following_0", 0.0); } @@ -157,8 +158,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode constexpr lanelet::Id lanelet_id = 34392; if ( !api_.entityExists(entity_name) && - !api_.reachPosition( - "ego", + !ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 25.0, 0.0, api_.getHdmapUtils()), 5.0)) { @@ -191,7 +191,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode 34621, 10, 0.0, api_.getHdmapUtils()); const auto entity_name = "spawn_nearby_ego"; const auto ego = api_.getEntity("ego"); - if (api_.reachPosition("ego", trigger_position, 20.0) && !api_.entityExists(entity_name)) { + if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.entityExists(entity_name)) { api_.spawn( entity_name, traffic_simulator::pose::transformRelativePoseToGlobal( @@ -204,7 +204,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } else { stop(cpp_mock_scenarios::Result::FAILURE); } - if (!api_.reachPosition("ego", trigger_position, 20.0) && api_.entityExists(entity_name)) { + if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.entityExists(entity_name)) { api_.despawn(entity_name); } } @@ -223,11 +223,11 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode {traffic_simulator::helper::constructCanonicalizedLaneletPose( 34606, 0.0, 0.0, api_.getHdmapUtils())}, getVehicleParameters()); - const auto ego = api_.getEntity("ego"); + const auto ego_entity = api_.getEntity("ego"); api_.spawn( "parking_outside", traffic_simulator::pose::transformRelativePoseToGlobal( - ego->getMapPose(), + ego_entity->getMapPose(), geometry_msgs::build() .position(geometry_msgs::build().x(10).y(15).z(0)) .orientation(geometry_msgs::msg::Quaternion())), diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index b23bcc2a398..5912df197b0 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -55,8 +55,9 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode if (api_.getCurrentTime() >= 6 && api_.entityExists("obstacle")) { api_.despawn("obstacle"); } - if (api_.reachPosition( - "ego", + const auto ego_entity = api_.getEntity("ego"); + const auto npc_entity = api_.getEntity("npc2"); + if (ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34615, 10.0, 0.0, api_.getHdmapUtils()), 5)) { @@ -67,8 +68,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange("npc2", 13, true); } } - if (api_.reachPosition( - "ego", + if (ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 0.0, 0.0, api_.getHdmapUtils()), 5)) { @@ -79,8 +79,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange("npc2", 3, true); } } - if (api_.reachPosition( - "npc2", + if (npc_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), 5)) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 53f2f34d406..53824987f64 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -361,7 +361,6 @@ class API FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); FORWARD_TO_ENTITY_MANAGER(laneMatchingSucceed); - FORWARD_TO_ENTITY_MANAGER(reachPosition); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); 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 53505446a9f..532e8c5ed96 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -110,6 +110,12 @@ class EntityBase virtual auto getGoalPoses() -> std::vector = 0; + /* */ auto isInPosition( + const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool; + + /* */ auto isInPosition( + const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool; + /* */ auto isInLanelet( const lanelet::Id lanelet_id, std::optional tolerance = std::nullopt) const -> bool; 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 887b90f12d2..472603105d2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -358,15 +358,6 @@ class EntityManager bool isStopping(const std::string & name) const; - bool reachPosition( - const std::string & name, const geometry_msgs::msg::Pose & target_pose, - const double tolerance) const; - bool reachPosition( - const std::string & name, const CanonicalizedLaneletPose & lanelet_pose, - const double tolerance) const; - bool reachPosition( - const std::string & name, const std::string & target_entity_name, const double tolerance) const; - void requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 87ec91a573a..ccedf93c30a 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -82,6 +82,18 @@ auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const matching_distance, hdmap_utils_ptr_); } +auto EntityBase::isInPosition( + const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool +{ + return math::geometry::getDistance(getMapPose(), target_pose) < tolerance; +} + +auto EntityBase::isInPosition( + const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool +{ + return isInPosition(static_cast(lanelet_pose), tolerance); +} + auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional tolerance) const -> bool { diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index a34a7ca7cbe..f2cb98c034f 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -254,37 +254,6 @@ bool EntityManager::isStopping(const std::string & name) const std::numeric_limits::epsilon(); } -/// @todo it probably should be moved to SimulatorCore -bool EntityManager::reachPosition( - const std::string & name, const std::string & target_entity_name, const double tolerance) const -{ - if (const auto target_entity = getEntityOrNullptr(target_entity_name)) { - return reachPosition(name, target_entity->getMapPose(), tolerance); - } else { - return false; - } -} - -/// @todo it probably should be moved to SimulatorCore -bool EntityManager::reachPosition( - const std::string & name, const geometry_msgs::msg::Pose & target_pose, - const double tolerance) const -{ - if (const auto entity = getEntityOrNullptr(name)) { - return math::geometry::getDistance(entity->getMapPose(), target_pose) < tolerance; - } else { - return false; - } -} - -/// @todo it probably should be moved to SimulatorCore -bool EntityManager::reachPosition( - const std::string & name, const CanonicalizedLaneletPose & lanelet_pose, - const double tolerance) const -{ - return reachPosition(name, static_cast(lanelet_pose), tolerance); -} - void EntityManager::requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction) { From 0ed40a3e8f6c8dcb9b950d712bb0b371e9a5b66a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 26 Jun 2024 17:26:41 +0200 Subject: [PATCH 18/49] feat(entity_base, traffic_simulator): rename laneMatchingSucceed to isInLanelet, remove forwarding --- ...t_distance_in_lane_coordinate_distance.cpp | 4 +-- .../src/random_scenario/random001.cpp | 2 +- .../define_traffic_source_large.cpp | 2 +- .../define_traffic_source_outside_lane.cpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 36 +++++++++---------- .../src/pedestrian/follow_lane_action.cpp | 2 +- .../follow_front_entity_action.cpp | 2 +- .../follow_lane_action.cpp | 4 +-- .../move_backward_action.cpp | 4 +-- .../stop_at_crossing_entity_action.cpp | 4 +-- .../stop_at_stop_line_action.cpp | 2 +- .../stop_at_traffic_light_action.cpp | 4 +-- .../follow_lane_sequence/yield_action.cpp | 4 +-- .../src/vehicle/lane_change_action.cpp | 2 +- .../ego_entity_simulation.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 1 - .../data_type/entity_status.hpp | 2 +- .../traffic_simulator/entity/entity_base.hpp | 13 ++----- .../entity/entity_manager.hpp | 9 +++-- .../src/data_type/entity_status.cpp | 4 +-- .../src/entity/entity_base.cpp | 4 +-- .../src/entity/entity_manager.cpp | 2 +- .../src/entity/pedestrian_entity.cpp | 4 +-- .../src/entity/vehicle_entity.cpp | 6 ++-- 24 files changed, 54 insertions(+), 67 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index d1a26ede44b..bc190d19b79 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -46,7 +46,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar { const auto from_entity = api_.getEntity(from_entity_name); const auto to_entity = api_.getEntity(to_entity_name); - if (from_entity->laneMatchingSucceed() && to_entity->laneMatchingSucceed()) { + if (from_entity->isInLanelet() && to_entity->isInLanelet()) { return traffic_simulator::distance::lateralDistance( from_entity->getCanonicalizedLaneletPose().value(), to_entity->getCanonicalizedLaneletPose().value(), false, api_.getHdmapUtils()); @@ -76,7 +76,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar { const auto from_entity = api_.getEntity(from_entity_name); const auto to_entity = api_.getEntity(to_entity_name); - if (from_entity->laneMatchingSucceed() && to_entity->laneMatchingSucceed()) { + if (from_entity->isInLanelet() && to_entity->isInLanelet()) { return traffic_simulator::distance::longitudinalDistance( from_entity->getCanonicalizedLaneletPose().value(), to_entity->getCanonicalizedLaneletPose().value(), false, true, false, api_.getHdmapUtils()); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 7dda0ae590b..54339008a69 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -138,7 +138,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode lane_change_requested = false; } /// Checking the ego entity overs the lane change position. - if (ego_entity->laneMatchingSucceed()) { + if (ego_entity->isInLanelet()) { if ( ego_entity->getStatus().getLaneletId() == 34684 && std::abs(ego_entity->getStatus().getLaneletPose().s) >= lane_change_position) { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 3ce79c3b545..8956ac9d0e4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -56,7 +56,7 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode const bool is_vehicle = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::VEHICLE; - if (!entity->laneMatchingSucceed() || !is_vehicle) { + if (!entity->isInLanelet() || !is_vehicle) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 7b9e2c07c23..239212b457b 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -56,7 +56,7 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod const bool is_pedestrian = entity->getEntityType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - if (entity->laneMatchingSucceed() || !is_pedestrian) { + if (entity->isInLanelet() || !is_pedestrian) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index b686714ffc2..0812813f1af 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -100,7 +100,7 @@ auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const std::vector ret; for (const auto & status : other_entity_status) { if ( - status.second.laneMatchingSucceed() && + status.second.isInLanelet() && traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { ret.emplace_back(status.second); } @@ -116,7 +116,7 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c const auto right_of_way_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet); for (const auto right_of_way_id : right_of_way_ids) { const auto other_status = getOtherEntityStatus(right_of_way_id); - if (!other_status.empty() && entity_status->laneMatchingSucceed()) { + if (!other_status.empty() && entity_status->isInLanelet()) { const auto lanelet_pose = entity_status->getLaneletPose(); const auto distance_forward = hdmap_utils->getLongitudinalDistance( lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0)); @@ -153,7 +153,7 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) for (const auto & following_lanelet : following_lanelets) { for (const lanelet::Id & lanelet_id : lanelet_ids_list.at(following_lanelet)) { if ( - status.second.laneMatchingSucceed() && + status.second.isInLanelet() && traffic_simulator::isSameLaneletId(status.second, lanelet_id) && not is_the_same_right_of_way(lanelet_id, following_lanelet)) { ret.emplace_back(status.second); @@ -167,7 +167,7 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) auto ActionNode::getRightOfWayEntities() const -> std::vector { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { return {}; } std::vector ret; @@ -178,7 +178,7 @@ auto ActionNode::getRightOfWayEntities() const for (const auto & status : other_entity_status) { for (const lanelet::Id & lanelet_id : lanelet_ids) { if ( - status.second.laneMatchingSucceed() && + status.second.isInLanelet() && traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { ret.emplace_back(status.second); } @@ -269,7 +269,7 @@ auto ActionNode::getDistanceToTargetEntityOnCrosswalk( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { - if (status.laneMatchingSucceed()) { + if (status.isInLanelet()) { return spline.getCollisionPointIn2D( hdmap_utils->getLaneletPolygon(status.getLaneletId()), false); } @@ -292,7 +292,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double length_extension_rear) const -> std::optional { const auto & status = getEntityStatus(target_name); - if (status.laneMatchingSucceed()) { + if (status.isInLanelet()) { return getDistanceToTargetEntityPolygon( spline, status, width_extension_right, width_extension_left, length_extension_front, length_extension_rear); @@ -306,7 +306,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed()) { + if (status.isInLanelet()) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -348,10 +348,9 @@ auto ActionNode::getConflictingEntityStatusOnCrossWalk(const lanelet::Ids & rout auto conflicting_crosswalks = hdmap_utils->getConflictingCrosswalkIds(route_lanelets); for (const auto & status : other_entity_status) { if ( - status.second.laneMatchingSucceed() && - std::count( - conflicting_crosswalks.begin(), conflicting_crosswalks.end(), - status.second.getLaneletId()) >= 1) { + status.second.isInLanelet() && std::count( + conflicting_crosswalks.begin(), conflicting_crosswalks.end(), + status.second.getLaneletId()) >= 1) { conflicting_entity_status.emplace_back(status.second); } } @@ -365,7 +364,7 @@ auto ActionNode::getConflictingEntityStatusOnLane(const lanelet::Ids & route_lan auto conflicting_lanes = hdmap_utils->getConflictingLaneIds(route_lanelets); for (const auto & status : other_entity_status) { if ( - status.second.laneMatchingSucceed() && + status.second.isInLanelet() && std::count( conflicting_lanes.begin(), conflicting_lanes.end(), status.second.getLaneletId()) >= 1) { conflicting_entity_status.emplace_back(status.second); @@ -380,14 +379,13 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) auto conflicting_lanes = hdmap_utils->getConflictingLaneIds(following_lanelets); for (const auto & status : other_entity_status) { if ( - status.second.laneMatchingSucceed() && - std::count( - conflicting_crosswalks.begin(), conflicting_crosswalks.end(), - status.second.getLaneletId()) >= 1) { + status.second.isInLanelet() && std::count( + conflicting_crosswalks.begin(), conflicting_crosswalks.end(), + status.second.getLaneletId()) >= 1) { return true; } if ( - status.second.laneMatchingSucceed() && + status.second.isInLanelet() && std::count( conflicting_lanes.begin(), conflicting_lanes.end(), status.second.getLaneletId()) >= 1) { return true; @@ -410,7 +408,7 @@ auto ActionNode::calculateUpdatedEntityStatus( const geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); const geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("Entity ", entity_status->getName(), " is not matched to the lanelet."); } else { auto lanelet_pose = entity_status->getLaneletPose(); diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp index d1d8790ac60..034a6209feb 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp @@ -38,7 +38,7 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { stopEntity(); setOutput( "non_canonicalized_updated_status", diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index ed88dab181e..ef15f31dd72 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -52,7 +52,7 @@ FollowFrontEntityAction::calculateObstacle(const traffic_simulator_msgs::msg::Wa const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 014973cc74b..76517e78a37 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -39,7 +39,7 @@ const std::optional FollowLaneAction::cal const traffic_simulator_msgs::msg::WaypointsArray FollowLaneAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { @@ -74,7 +74,7 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { stopEntity(); setOutput( "non_canonicalized_updated_status", diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp index 41ab85ca134..8eb366bdb34 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp @@ -35,7 +35,7 @@ const std::optional MoveBackwardAction::c const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { @@ -70,7 +70,7 @@ BT::NodeStatus MoveBackwardAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } const auto waypoints = calculateWaypoints(); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index e5810a1f86d..54377e9b086 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -54,7 +54,7 @@ StopAtCrossingEntityAction::calculateObstacle(const traffic_simulator_msgs::msg: const traffic_simulator_msgs::msg::WaypointsArray StopAtCrossingEntityAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { @@ -92,7 +92,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index c5d131be2f7..949a6ff1e0c 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -53,7 +53,7 @@ const std::optional StopAtStopLineAction: const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp index 2e26a8f722a..96e4b0b3347 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp @@ -52,7 +52,7 @@ StopAtTrafficLightAction::calculateObstacle(const traffic_simulator_msgs::msg::W const traffic_simulator_msgs::msg::WaypointsArray StopAtTrafficLightAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { @@ -92,7 +92,7 @@ BT::NodeStatus StopAtTrafficLightAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } if (!behavior_parameter.see_around) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp index 456622d8daa..5cc359cd7d2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp @@ -52,7 +52,7 @@ const std::optional YieldAction::calculat const traffic_simulator_msgs::msg::WaypointsArray YieldAction::calculateWaypoints() { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } if (entity_status->getTwist().linear.x >= 0) { @@ -96,7 +96,7 @@ BT::NodeStatus YieldAction::tick() if (!behavior_parameter.see_around) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } const auto right_of_way_entities = getRightOfWayEntities(route_lanelets); diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index d387bc604d4..c5f1f154732 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -102,7 +102,7 @@ BT::NodeStatus LaneChangeAction::tick() } if (!curve_) { if (request == traffic_simulator::behavior::Request::LANE_CHANGE) { - if (!entity_status->laneMatchingSucceed()) { + if (!entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } const auto lanelet_pose = entity_status->getLaneletPose(); diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 01df3e82dc4..4b64ae6bdc2 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -352,7 +352,7 @@ void EgoEntitySimulation::update( auto EgoEntitySimulation::calculateEgoPitch() const -> double { // calculate prev/next point of lanelet centerline nearest to ego pose. - if (!status_.laneMatchingSucceed()) { + if (!status_.isInLanelet()) { return 0.0; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 53824987f64..eb062f616ae 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -360,7 +360,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); - FORWARD_TO_ENTITY_MANAGER(laneMatchingSucceed); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 3389a0d5f0c..7ad96ef1ee9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -60,7 +60,7 @@ class CanonicalizedEntityStatus auto getLinearJerk() const noexcept -> double; auto setLinearJerk(double) -> void; - auto laneMatchingSucceed() const noexcept -> bool; + auto isInLanelet() const noexcept -> bool; auto getLaneletId() const noexcept -> lanelet::Id; auto getLaneletIds() const noexcept -> lanelet::Ids; auto getLaneletPose() const noexcept -> LaneletPose; 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 532e8c5ed96..a3781678a73 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -84,17 +84,6 @@ class EntityBase // clang-format on #undef DEFINE_GETTER - // clang-format off -#define DEFINE_CHECK_FUNCTION(FUNCTION_NAME, BOOL_VARIABLE) \ - /** \ - @note This function was defined by DEFINE_CHECK_FUNCTION function. \ - */ \ - /* */ auto FUNCTION_NAME() const->bool { return BOOL_VARIABLE; } - - DEFINE_CHECK_FUNCTION(laneMatchingSucceed, status_.laneMatchingSucceed()) - // clang-format on -#undef DEFINE_CHECK_FUNCTION - /* */ auto get2DPolygon() const -> std::vector; virtual auto getCurrentAction() const -> std::string = 0; @@ -116,6 +105,8 @@ class EntityBase /* */ auto isInPosition( const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool; + /* */ auto isInLanelet() const -> bool { return status_.isInLanelet(); }; + /* */ auto isInLanelet( const lanelet::Id lanelet_id, std::optional tolerance = std::nullopt) const -> bool; 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 472603105d2..4b769d0d952 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -249,21 +249,21 @@ class EntityManager static_assert(true, "") // clang-format on + FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); + FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getTraveledDistance, const); - FORWARD_TO_ENTITY(laneMatchingSucceed, const); - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); - FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(isControlledBySimulator, ); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); + FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); + FORWARD_TO_ENTITY(requestSpeedChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); @@ -276,7 +276,6 @@ class EntityManager FORWARD_TO_ENTITY(setMapPose, ); FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); - FORWARD_TO_ENTITY(requestSpeedChange, ); #undef FORWARD_TO_ENTITY diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 66a88b7c1a3..94347647b64 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -83,7 +83,7 @@ auto CanonicalizedEntityStatus::set( set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } -auto CanonicalizedEntityStatus::laneMatchingSucceed() const noexcept -> bool +auto CanonicalizedEntityStatus::isInLanelet() const noexcept -> bool { return canonicalized_lanelet_pose_.has_value(); } @@ -120,7 +120,7 @@ auto CanonicalizedEntityStatus::getLaneletId() const noexcept -> lanelet::Id auto CanonicalizedEntityStatus::getLaneletIds() const noexcept -> lanelet::Ids { - return laneMatchingSucceed() ? lanelet::Ids{getLaneletId()} : lanelet::Ids{}; + return isInLanelet() ? lanelet::Ids{getLaneletId()} : lanelet::Ids{}; } auto CanonicalizedEntityStatus::getCanonicalizedLaneletPose() const noexcept diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index ccedf93c30a..a64f9d156ec 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -156,7 +156,7 @@ void EntityBase::requestLaneChange( { lanelet::Id reference_lanelet_id = 0; if (target.entity_name == name) { - if (not laneMatchingSucceed()) { + if (not isInLanelet()) { THROW_SEMANTIC_ERROR( "Source entity does not assigned to lanelet. Please check source entity name : ", name, " exists on lane."); @@ -167,7 +167,7 @@ void EntityBase::requestLaneChange( THROW_SEMANTIC_ERROR( "Target entity : ", target.entity_name, " does not exist. Please check ", target.entity_name, " exists."); - } else if (!other_status_.at(target.entity_name).laneMatchingSucceed()) { + } else if (!other_status_.at(target.entity_name).isInLanelet()) { THROW_SEMANTIC_ERROR( "Target entity does not assigned to lanelet. Please check Target entity name : ", target.entity_name, " exists on lane."); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index f2cb98c034f..e96009734b6 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -257,7 +257,7 @@ bool EntityManager::isStopping(const std::string & name) const void EntityManager::requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction) { - if (const auto entity = getEntity(name); entity->laneMatchingSucceed()) { + if (const auto entity = getEntity(name); entity->isInLanelet()) { if ( const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( entity->getStatus().getLaneletId(), direction)) { diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 317b5fd533c..03d0ba79b19 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -53,7 +53,7 @@ void PedestrianEntity::appendDebugMarker(visualization_msgs::msg::MarkerArray & void PedestrianEntity::requestAssignRoute(const std::vector & waypoints) { - if (!laneMatchingSucceed()) { + if (!isInLanelet()) { return; } behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); @@ -136,7 +136,7 @@ void PedestrianEntity::requestWalkStraight() void PedestrianEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_.isInLanelet()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index c34e39fdcd6..c8f8eda38c0 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -120,7 +120,7 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin try { return behavior_plugin_ptr_->getWaypoints(); } catch (const std::runtime_error & e) { - if (not status_.laneMatchingSucceed()) { + if (not status_.isInLanelet()) { THROW_SIMULATION_ERROR( "Failed to calculate waypoints in NPC logics, please check Entity : ", name, " is in a lane coordinate."); @@ -172,7 +172,7 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_.isInLanelet()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -193,7 +193,7 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ void VehicleEntity::requestAssignRoute(const std::vector & waypoints) { - if (!laneMatchingSucceed()) { + if (!isInLanelet()) { return; } behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); From 249ec7d3078bc79f3156311d75f4f0ec2d200fde Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 11:52:56 +0200 Subject: [PATCH 19/49] feat(entity_base, traffic_simulator): remove forwarding setters to EntityBase, also some getters, left setVelocityLimit and setBehaviorParameter --- .../src/collision/crashing_npc.cpp | 4 +-- .../src/collision/spawn_with_offset.cpp | 4 +-- .../src/crosswalk/stop_at_crosswalk.cpp | 4 +-- .../accelerate_and_follow.cpp | 4 +-- .../decelerate_and_follow.cpp | 4 +-- .../assign_route_in_world_frame.cpp | 2 +- .../src/follow_lane/cancel_request.cpp | 2 +- .../src/follow_lane/follow_with_offset.cpp | 2 +- ...line_trajectory_with_do_nothing_plugin.cpp | 2 +- .../src/lane_change/lanechange_left.cpp | 2 +- .../lane_change/lanechange_left_with_id.cpp | 2 +- .../src/lane_change/lanechange_linear.cpp | 2 +- .../lanechange_linear_lateral_velocity.cpp | 2 +- .../lane_change/lanechange_linear_time.cpp | 2 +- .../lanechange_longitudinal_distance.cpp | 2 +- .../src/lane_change/lanechange_right.cpp | 2 +- .../lane_change/lanechange_right_with_id.cpp | 2 +- .../src/lane_change/lanechange_time.cpp | 2 +- ...t_distance_in_lane_coordinate_distance.cpp | 6 ++-- .../get_distance_to_lane_bound.cpp | 2 +- .../src/merge/merge_left.cpp | 4 +-- .../src/metrics/traveled_distance.cpp | 21 +++++------- .../src/move_backward/move_backward.cpp | 2 +- .../src/pedestrian/walk_straight.cpp | 4 +-- .../src/random_scenario/random001.cpp | 4 +-- .../speed_planning/request_speed_change.cpp | 4 +-- .../request_speed_change_continuous_false.cpp | 2 +- .../request_speed_change_relative.cpp | 4 +-- .../request_speed_change_step.cpp | 4 +-- .../request_speed_change_with_limit.cpp | 2 +- ...uest_speed_change_with_time_constraint.cpp | 4 +-- ...eed_change_with_time_constraint_linear.cpp | 2 +- ...d_change_with_time_constraint_relative.cpp | 4 +-- .../src/traffic_simulation_demo.cpp | 10 +++--- .../define_traffic_source_delay.cpp | 2 +- .../define_traffic_source_high_rate.cpp | 2 +- .../define_traffic_source_large.cpp | 2 +- .../define_traffic_source_mixed.cpp | 2 +- .../define_traffic_source_multiple.cpp | 2 +- .../define_traffic_source_outside_lane.cpp | 2 +- .../define_traffic_source_pedestrian.cpp | 2 +- .../define_traffic_source_vehicle.cpp | 2 +- .../include/traffic_simulator/api/api.hpp | 16 ++------- .../entity/entity_manager.hpp | 15 -------- simulation/traffic_simulator/src/api/api.cpp | 34 +++++++++---------- .../src/entity/entity_manager.cpp | 13 +++---- 46 files changed, 94 insertions(+), 125 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index 3ebe51a9925..45720c82a16 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -56,7 +56,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 15, true); traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; behavior_parameter.see_around = false; @@ -67,7 +67,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc", 0.0); + api_.getEntity("npc")->setLinearVelocity(0.0); api_.requestSpeedChange("npc", 5, true); } }; diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp index 75f19793f70..35c8ed43977 100644 --- a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp @@ -54,7 +54,7 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.2, 1.3, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange("ego", 0, true); api_.spawn( @@ -62,7 +62,7 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, -0.874, api_.getHdmapUtils()), getPedestrianParameters()); - api_.setLinearVelocity("bob", 0); + api_.getEntity("bob")->setLinearVelocity(0); api_.requestSpeedChange("bob", 0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index a98b05b10d4..8dc11e5dabe 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -78,7 +78,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ @@ -92,7 +92,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.setLinearVelocity("bob", 0); + api_.getEntity("bob")->setLinearVelocity(0); api_.requestSpeedChange( "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index c0a17be1255..e89aa5e7678 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -63,14 +63,14 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 3); + api_.getEntity("ego")->setLinearVelocity(3); api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc", 10); + api_.getEntity("npc")->setLinearVelocity(10); api_.requestSpeedChange("npc", 10, true); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index b7fe47c3b7a..e649fb01369 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -63,14 +63,14 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 15); + api_.getEntity("ego")->setLinearVelocity(15); api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 15.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc", 10); + api_.getEntity("npc")->setLinearVelocity(10); api_.requestSpeedChange("npc", 10, true); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 54dcc159b25..739315c01c1 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -51,7 +51,7 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::pose::toMapPose( diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 5b2b49e86fa..c03122090c9 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -58,7 +58,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 7); + api_.getEntity("ego")->setLinearVelocity(7); api_.requestSpeedChange("ego", 7, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils()); diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index 2d3765134ef..f7bcac6a313 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -58,7 +58,7 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 3.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 95357a9f09a..2a655661019 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -100,7 +100,7 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario api_.spawn( "ego", spawn_pose, getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestFollowTrajectory( "ego", diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 5f5438a7ce5..28e3a714a23 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -55,7 +55,7 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange("ego", traffic_simulator::lane_change::Direction::LEFT); } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index 47fee1945d7..fcb95413f60 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -55,7 +55,7 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange("ego", 34513); } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index e153fb0ae52..c9df7dabf70 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -60,7 +60,7 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange( "ego", diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index f6bf51b7c80..1546738b99e 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -63,7 +63,7 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange( "ego", diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index bc00c1b7640..dde46363f19 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -64,7 +64,7 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange( "ego", diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index 0cc8343b02c..4a1722252a1 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -51,7 +51,7 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 1); + api_.getEntity("ego")->setLinearVelocity(1); api_.requestSpeedChange("ego", 1, true); api_.requestLaneChange( "ego", diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index f98e7be572a..b4e689c9523 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -55,7 +55,7 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange("ego", traffic_simulator::lane_change::Direction::RIGHT); // api_.requestLaneChange("ego", 34462); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index a9e40b16c04..6f23e69b9c8 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -55,7 +55,7 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange("ego", 34462); } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index 88a2c446d61..f330e995db1 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -64,7 +64,7 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 10, true); api_.requestLaneChange( "ego", diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index bc190d19b79..44dd73d5347 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -140,7 +140,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 5.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 3, true); api_.spawn( @@ -148,7 +148,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 1.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 10); + api_.getEntity("front")->setLinearVelocity(10); api_.requestSpeedChange("front", 3, true); api_.spawn( @@ -156,7 +156,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, -1.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("behind", 10); + api_.getEntity("behind")->setLinearVelocity(10); api_.requestSpeedChange("behind", 3, true); } }; diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index b9cc90045c7..281cfe4c8b3 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -61,7 +61,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 5.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 3, true); } }; diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index 27a9717985f..bdd6af8ba8f 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -58,7 +58,7 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 15.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 5); + api_.getEntity("ego")->setLinearVelocity(5); api_.requestSpeedChange("ego", 5, true); api_.requestLaneChange("ego", 34513); @@ -67,7 +67,7 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc", 10); + api_.getEntity("npc")->setLinearVelocity(10); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index ee4786cc94d..395aeee668e 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -47,20 +47,15 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode const auto entity = api_.getEntity("ego"); if (!entity) { stop(cpp_mock_scenarios::Result::FAILURE); - } - const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); - const auto traveled_distance = api_.getTraveledDistance("ego"); - if (!lanelet_pose) { + } else if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { stop(cpp_mock_scenarios::Result::FAILURE); - } - const auto difference = std::abs( - static_cast(lanelet_pose.value()).s - traveled_distance); - if (difference > std::numeric_limits::epsilon()) { + } else if (const auto difference = std::abs( + static_cast(lanelet_pose.value()).s - + entity->getTraveledDistance()); + difference > std::numeric_limits::epsilon()) { stop(cpp_mock_scenarios::Result::FAILURE); - } - // LCOV_EXCL_STOP - - if (api_.getCurrentTime() >= 12) { + } // LCOV_EXCL_STOP + else if (api_.getCurrentTime() >= 12) { stop(cpp_mock_scenarios::Result::SUCCESS); } } @@ -72,7 +67,7 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 3); + api_.getEntity("ego")->setLinearVelocity(3); api_.requestSpeedChange("ego", 3, true); } }; diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 44834cbdb5a..3d6eaa2c8bf 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -56,7 +56,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", -3); + api_.getEntity("ego")->setLinearVelocity(-3); api_.requestSpeedChange("ego", -3, true); } }; diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 95f238d31d7..e9f5c0799f0 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -74,7 +74,7 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ @@ -87,7 +87,7 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.setLinearVelocity("bob", 0); + api_.getEntity("bob")->setLinearVelocity(0); api_.requestWalkStraight("bob"); api_.requestSpeedChange( "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 54339008a69..5e7e3175b93 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -131,7 +131,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode params_.random_parameters.lane_following_vehicle.max_speed); const auto speed = speed_distribution(engine_); api_.requestSpeedChange(entity_name, speed, true); - api_.setLinearVelocity(entity_name, speed); + api_.getEntity(entity_name)->setLinearVelocity(speed); std::uniform_real_distribution<> lane_change_position_distribution( 0.0, traffic_simulator::pose::laneletLength(34684, api_.getHdmapUtils())); lane_change_position = lane_change_position_distribution(engine_); @@ -174,7 +174,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode getPedestrianParameters()); const auto speed = speed_distribution(engine_); api_.requestSpeedChange(entity_name, speed, true); - api_.setLinearVelocity(entity_name, speed); + api_.getEntity(entity_name)->setLinearVelocity(speed); } if ( api_.entityExists(entity_name) && diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 11813ab4ace..c6a35c7a544 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -59,7 +59,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( @@ -71,7 +71,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 0); + api_.getEntity("front")->setLinearVelocity(0); api_.requestSpeedChange( "front", 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 1f98bbc1ef5..28073f16f0c 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -68,7 +68,7 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index 8491e9cfbdb..3073bb8de4d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -60,7 +60,7 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 3); + api_.getEntity("ego")->setLinearVelocity(3); api_.requestSpeedChange("ego", 3.0, true); api_.spawn( @@ -68,7 +68,7 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 3); + api_.getEntity("front")->setLinearVelocity(3); api_.requestSpeedChange( "front", traffic_simulator::speed_change::RelativeTargetSpeed( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index 2533b29030b..79551ea14bb 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -57,7 +57,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( @@ -69,7 +69,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 0); + api_.getEntity("front")->setLinearVelocity(0); api_.requestSpeedChange( "front", 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 99a60495f26..ab2a36b0036 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -56,7 +56,7 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.setVelocityLimit("ego", 5.0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index c68a8470290..66aeadc7b26 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -60,7 +60,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::AUTO, traffic_simulator::speed_change::Constraint( @@ -72,7 +72,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 10); + api_.getEntity("front")->setLinearVelocity(10); api_.requestSpeedChange( "front", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index 8a0fd52fdc8..88c4f1a5762 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -57,7 +57,7 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0); + api_.getEntity("ego")->setLinearVelocity(0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index 6bfee132189..f745844ee47 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -61,7 +61,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 1.0); + api_.getEntity("ego")->setLinearVelocity(1.0); api_.requestSpeedChange( "ego", traffic_simulator::speed_change::RelativeTargetSpeed( @@ -76,7 +76,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("front", 10); + api_.getEntity("front")->setLinearVelocity(10); api_.requestSpeedChange( "front", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 5912df197b0..b59af77e1af 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -102,7 +102,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 10); + api_.getEntity("ego")->setLinearVelocity(10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ @@ -125,7 +125,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.setLinearVelocity("bob", 1.0); + api_.getEntity("bob")->setLinearVelocity(1.0); api_.requestSpeedChange("bob", 1, true); api_.spawn( @@ -133,7 +133,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc1", 5.0); + api_.getEntity("npc1")->setLinearVelocity(5.0); api_.requestSpeedChange("npc1", 5, true); api_.requestAcquirePosition( "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -144,7 +144,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34606, 20.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc2", 5); + api_.getEntity("npc2")->setLinearVelocity(5); api_.requestSpeedChange("npc2", 0, true); api_.spawn( @@ -152,7 +152,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34468, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("npc3", 10); + api_.getEntity("npc3")->setLinearVelocity(10); api_.spawn( "obstacle", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index e55155ee36f..beb4fa354ac 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -102,7 +102,7 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index ba71d1f2bcc..14914ad98bc 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -94,7 +94,7 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 8956ac9d0e4..dd4692f2ea9 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -84,7 +84,7 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 7c973a9efb9..57aaf5db32c 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -106,7 +106,7 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 027969c431f..f445bf0c8e4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -124,7 +124,7 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 239212b457b..67a459dd84c 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -85,7 +85,7 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index f26808e95d0..f2e498625c1 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -92,7 +92,7 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 141cdca23dd..8f92a3bb56b 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -94,7 +94,7 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 0.0); + api_.getEntity("ego")->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 0.0, true); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index eb062f616ae..7059ed5eff9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -352,38 +352,26 @@ class API FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); - FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr); FORWARD_TO_ENTITY_MANAGER(getEntityNames); + FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); - FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); + FORWARD_TO_ENTITY_MANAGER(requestClearRoute); FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); - FORWARD_TO_ENTITY_MANAGER(requestClearRoute); FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); - FORWARD_TO_ENTITY_MANAGER(setAcceleration); - FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit); - FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence); - FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit); - FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); - FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); - FORWARD_TO_ENTITY_MANAGER(setMapPose); - FORWARD_TO_ENTITY_MANAGER(setTwist); FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); -private: - FORWARD_TO_ENTITY_MANAGER(getDefaultMatchingDistanceForLaneletPoseCalculation); - public: #undef FORWARD_TO_ENTITY_MANAGER 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 4b769d0d952..37fbf151241 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -252,11 +252,6 @@ class EntityManager FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); FORWARD_TO_ENTITY(cancelRequest, ); - FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); - FORWARD_TO_ENTITY(getEntityTypename, const); - FORWARD_TO_ENTITY(getRouteLanelets, const); - FORWARD_TO_ENTITY(getTraveledDistance, const); - FORWARD_TO_ENTITY(isControlledBySimulator, ); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); FORWARD_TO_ENTITY(requestClearRoute, ); @@ -264,17 +259,7 @@ class EntityManager FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestSpeedChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(setAcceleration, ); - FORWARD_TO_ENTITY(setAccelerationLimit, ); - FORWARD_TO_ENTITY(setAccelerationRateLimit, ); FORWARD_TO_ENTITY(setBehaviorParameter, ); - FORWARD_TO_ENTITY(setControlledBySimulator, ); - FORWARD_TO_ENTITY(setDecelerationLimit, ); - FORWARD_TO_ENTITY(setDecelerationRateLimit, ); - FORWARD_TO_ENTITY(setLinearJerk, ); - FORWARD_TO_ENTITY(setLinearVelocity, ); - FORWARD_TO_ENTITY(setMapPose, ); - FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); #undef FORWARD_TO_ENTITY diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index a7ff86fade0..d262582897d 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -60,8 +60,9 @@ auto API::respawn( } else if (new_pose.header.frame_id != "map") { throw std::runtime_error("Respawn request with frame id other than map not supported."); } else { + auto entity = entity_manager_ptr_->getEntity(name); // set new pose and default action status in EntityManager - entity_manager_ptr_->setControlledBySimulator(name, true); + entity->setControlledBySimulator(true); setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); // read status from EntityManager, then send it to SimpleSensorSimulator @@ -70,8 +71,8 @@ auto API::respawn( static_cast(entity_manager_ptr_->getEntity(name)->getStatus()), *req.add_status()); req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); - req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(name)); - entity_manager_ptr_->setControlledBySimulator(name, false); + req.set_overwrite_ego_status(entity->isControlledBySimulator()); + entity->setControlledBySimulator(false); // check response if (const auto res = zeromq_client_.call(req); not res.result().success()) { @@ -86,13 +87,12 @@ auto API::respawn( res_name + "\"."); } else { // if valid, set response in EntityManager, then plan path and engage - auto entity_status = - static_cast(entity_manager_ptr_->getEntity(name)->getStatus()); + auto entity_status = static_cast(entity->getStatus()); simulation_interface::toMsg(res_status->pose(), entity_status.pose); simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); - setMapPose(name, entity_status.pose); - setTwist(name, entity_status.action_status.twist); - setAcceleration(name, entity_status.action_status.accel); + entity->setMapPose(entity_status.pose); + entity->setTwist(entity_status.action_status.twist); + entity->setAcceleration(entity_status.action_status.accel); entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose}); @@ -270,11 +270,11 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusRequest req; req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { - const auto entity_status = - static_cast(entity_manager_ptr_->getEntity(entity_name)->getStatus()); + const auto entity = entity_manager_ptr_->getEntity(entity_name); + const auto entity_status = static_cast(entity->getStatus()); simulation_interface::toProto(entity_status, *req.add_status()); if (entity_manager_ptr_->is(entity_name)) { - req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); + req.set_overwrite_ego_status(entity->isControlledBySimulator()); } } @@ -282,15 +282,15 @@ bool API::updateEntitiesStatusInSim() if (auto res = zeromq_client_.call(req); res.result().success()) { for (const auto & res_status : res.status()) { auto entity_name = res_status.name(); - auto entity_status = - static_cast(entity_manager_ptr_->getEntity(entity_name)->getStatus()); + auto entity = entity_manager_ptr_->getEntity(entity_name); + auto entity_status = static_cast(entity->getStatus()); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); if (entity_manager_ptr_->is(entity_name)) { - setMapPose(entity_name, entity_status.pose); - setTwist(entity_name, entity_status.action_status.twist); - setAcceleration(entity_name, entity_status.action_status.accel); + entity->setMapPose(entity_status.pose); + entity->setTwist(entity_status.action_status.twist); + entity->setAcceleration(entity_status.action_status.accel); } else { setEntityStatus(entity_name, entity_status); } @@ -384,7 +384,7 @@ auto API::addTrafficSource( radius, rate, pose, distribution, seed, getCurrentTime(), configuration, entity_manager_ptr_->getHdmapUtils(), [this, speed](const auto & name, auto &&... xs) { this->spawn(name, std::forward(xs)...); - setLinearVelocity(name, speed); + getEntity(name)->setLinearVelocity(speed); }); } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index e96009734b6..51e9f83287a 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -213,7 +213,7 @@ auto EntityManager::getPedestrianParameters(const std::string & name) const return entity->pedestrian_parameters; } THROW_SIMULATION_ERROR( - "EntityType: ", getEntityTypename(name), ", does not have pedestrian parameter.", + "EntityType: ", getEntity(name)->getEntityTypename(), ", does not have pedestrian parameter.", "Please check description of the scenario and entity type of the Entity: " + name); } @@ -224,7 +224,7 @@ auto EntityManager::getVehicleParameters(const std::string & name) const return vehicle->vehicle_parameters; } THROW_SIMULATION_ERROR( - "EntityType: ", getEntityTypename(name), ", does not have pedestrian parameter.", + "EntityType: ", getEntity(name)->getEntityTypename(), ", does not have pedestrian parameter.", "Please check description of the scenario and entity type of the Entity: " + name); } @@ -292,10 +292,11 @@ void EntityManager::resetBehaviorPlugin( THROW_SIMULATION_ERROR( "Entity :", name, "is unkown entity type.", "Please contact to developer."); } - setLinearJerk(name, status.getLinearJerk()); - setAcceleration(name, status.getAccel()); - setTwist(name, status.getTwist()); - setBehaviorParameter(name, behavior_parameter); + auto entity = getEntity(name); + entity->setLinearJerk(status.getLinearJerk()); + entity->setAcceleration(status.getAccel()); + entity->setTwist(status.getTwist()); + entity->setBehaviorParameter(behavior_parameter); } auto EntityManager::getCurrentAction(const std::string & name) const -> std::string From 9233cf80840f8e5f8d12cc8f915f80adaee31d43 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 15:07:19 +0200 Subject: [PATCH 20/49] feat(entity_base, traffic_simulator): move isStopping to EntityBase, develop templated is() in EntityBase and use it, rename isEgoSpawned to isAnyEgoSpawned, refactor --- .../include/traffic_simulator/api/api.hpp | 2 +- .../data_type/entity_status.hpp | 3 +- .../traffic_simulator/entity/entity_base.hpp | 10 ++++ .../entity/entity_manager.hpp | 17 +----- simulation/traffic_simulator/src/api/api.cpp | 14 ++--- .../src/data_type/entity_status.cpp | 7 ++- .../src/entity/entity_base.cpp | 5 ++ .../src/entity/entity_manager.cpp | 58 ++++++++----------- .../metrics/almost_standstill_metric.hpp | 4 +- .../random_test_runner/test_executor.hpp | 4 +- .../test/test_test_executor.cpp | 6 +- 11 files changed, 65 insertions(+), 65 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 7059ed5eff9..37fc5e11ee4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -357,7 +357,7 @@ class API FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); - FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); + FORWARD_TO_ENTITY_MANAGER(isAnyEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 7ad96ef1ee9..ed82419a0b8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -34,7 +34,8 @@ class CanonicalizedEntityStatus explicit CanonicalizedEntityStatus( const EntityStatus & may_non_canonicalized_entity_status, const std::optional & canonicalized_lanelet_pose); - explicit CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj); + CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj); + CanonicalizedEntityStatus(CanonicalizedEntityStatus && obj) noexcept; explicit operator EntityStatus() const noexcept { return entity_status_; } auto set( 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 a3781678a73..c6992c66e43 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -55,6 +55,12 @@ class EntityBase virtual ~EntityBase() = default; + template + /* */ auto is() const -> bool + { + return dynamic_cast(this) != nullptr; + } + virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray &); virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &; @@ -81,6 +87,8 @@ class EntityBase DEFINE_GETTER(StandStillDuration, double, stand_still_duration_) DEFINE_GETTER(Status, const CanonicalizedEntityStatus &, status_) DEFINE_GETTER(TraveledDistance, double, traveled_distance_) + DEFINE_GETTER(Name, const std::string &, getStatus().getName()) + // clang-format on #undef DEFINE_GETTER @@ -99,6 +107,8 @@ class EntityBase virtual auto getGoalPoses() -> std::vector = 0; + /* */ auto isStopping() const -> bool; + /* */ auto isInPosition( const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool; 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 37fbf151241..a5ecbfa6c4c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -330,17 +330,9 @@ class EntityManager } } - template - bool is(const std::string & name) const - { - return dynamic_cast(entities_.at(name).get()) != nullptr; - } - - bool isEgoSpawned() const; - - const std::string getEgoName() const; + auto isAnyEgoSpawned() const -> bool; - bool isStopping(const std::string & name) const; + auto getEgoName() const -> const std::string &; void requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction); @@ -366,10 +358,7 @@ class EntityManager EntityStatus entity_status; if constexpr (std::is_same_v, EgoEntity>) { - if (auto iter = std::find_if( - std::begin(entities_), std::end(entities_), - [this](auto && each) { return is(each.first); }); - iter != std::end(entities_)) { + if (isAnyEgoSpawned()) { THROW_SEMANTIC_ERROR("multi ego simulation does not support yet"); } else { entity_status.type.type = traffic_simulator_msgs::msg::EntityType::EGO; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index d262582897d..f65fbe1f399 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -55,12 +55,11 @@ auto API::respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) -> void { - if (not entity_manager_ptr_->is(name)) { + if (auto entity = entity_manager_ptr_->getEntity(name); not entity->is()) { throw std::runtime_error("Respawn of any entities other than EGO is not supported."); } else if (new_pose.header.frame_id != "map") { throw std::runtime_error("Respawn request with frame id other than map not supported."); } else { - auto entity = entity_manager_ptr_->getEntity(name); // set new pose and default action status in EntityManager entity->setControlledBySimulator(true); setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); @@ -273,7 +272,7 @@ bool API::updateEntitiesStatusInSim() const auto entity = entity_manager_ptr_->getEntity(entity_name); const auto entity_status = static_cast(entity->getStatus()); simulation_interface::toProto(entity_status, *req.add_status()); - if (entity_manager_ptr_->is(entity_name)) { + if (entity->is()) { req.set_overwrite_ego_status(entity->isControlledBySimulator()); } } @@ -281,18 +280,17 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusResponse res; if (auto res = zeromq_client_.call(req); res.result().success()) { for (const auto & res_status : res.status()) { - auto entity_name = res_status.name(); - auto entity = entity_manager_ptr_->getEntity(entity_name); + auto entity = entity_manager_ptr_->getEntity(res_status.name()); auto entity_status = static_cast(entity->getStatus()); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); - if (entity_manager_ptr_->is(entity_name)) { + if (entity->is()) { entity->setMapPose(entity_status.pose); entity->setTwist(entity_status.action_status.twist); entity->setAcceleration(entity_status.action_status.accel); } else { - setEntityStatus(entity_name, entity_status); + setEntityStatus(entity->getName(), entity_status); } } return true; @@ -302,7 +300,7 @@ bool API::updateEntitiesStatusInSim() bool API::updateFrame() { - if (configuration.standalone_mode && entity_manager_ptr_->isEgoSpawned()) { + if (configuration.standalone_mode && entity_manager_ptr_->isAnyEgoSpawned()) { THROW_SEMANTIC_ERROR("Ego simulation is no longer supported in standalone mode"); } diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 94347647b64..a8faf57c39b 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -19,7 +19,6 @@ namespace traffic_simulator { namespace entity_status { - CanonicalizedEntityStatus::CanonicalizedEntityStatus( const EntityStatus & may_non_canonicalized_entity_status, const std::optional & canonicalized_lanelet_pose) @@ -53,6 +52,12 @@ CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntitySt { } +CanonicalizedEntityStatus::CanonicalizedEntityStatus(CanonicalizedEntityStatus && obj) noexcept +: canonicalized_lanelet_pose_(std::move(obj.canonicalized_lanelet_pose_)), + entity_status_(std::move(obj.entity_status_)) +{ +} + auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> void { assert(getType() == status.getType()); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index a64f9d156ec..80b80e73eba 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -111,6 +111,11 @@ auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> return getBoundingBox().dimensions.y * 0.5 + 1.0; } +auto EntityBase::isStopping() const -> bool +{ + return std::fabs(getCurrentTwist().linear.x) < std::numeric_limits::epsilon(); +} + auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool { return speed_planner_->isTargetSpeedReached(target_speed, getCurrentTwist()); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 51e9f83287a..f11b485e41c 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -49,7 +49,7 @@ void EntityManager::broadcastEntityTransform() * In the past, we used to publish the frames of all entities, but that would be too heavy processing, * so we publish the average of the coordinates of all entities. */ - if (isEgoSpawned()) { + if (isAnyEgoSpawned()) { broadcastTransform( geometry_msgs::build() /** @@ -179,16 +179,15 @@ auto EntityManager::getHdmapUtils() -> const std::shared_ptr std::size_t { return std::count_if(std::begin(entities_), std::end(entities_), [this](const auto & each) { - return is(each.first); + return each.second->template is(); }); } -const std::string EntityManager::getEgoName() const +auto EntityManager::getEgoName() const -> const std::string & { - const auto names = getEntityNames(); - for (const auto & name : names) { - if (is(name)) { - return name; + for (const auto & each : entities_) { + if (each.second->template is()) { + return each.second->getName(); } } THROW_SEMANTIC_ERROR( @@ -238,20 +237,11 @@ auto EntityManager::getWaypoints(const std::string & name) } } -bool EntityManager::isEgoSpawned() const +auto EntityManager::isAnyEgoSpawned() const -> bool { - for (const auto & name : getEntityNames()) { - if (is(name)) { - return true; - } - } - return false; -} - -bool EntityManager::isStopping(const std::string & name) const -{ - return std::fabs(getEntity(name)->getCurrentTwist().linear.x) < - std::numeric_limits::epsilon(); + return std::any_of(std::begin(entities_), std::end(entities_), [this](const auto & each) { + return each.second->template is(); + }); } void EntityManager::requestLaneChange( @@ -269,21 +259,22 @@ void EntityManager::requestLaneChange( void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { - const auto & status = getEntity(name)->getStatus(); - const auto behavior_parameter = getEntity(name)->getBehaviorParameter(); - if (is(name)) { + const auto reference_entity = getEntity(name); + const CanonicalizedEntityStatus status = reference_entity->getStatus(); + const auto behavior_parameter = reference_entity->getBehaviorParameter(); + if (reference_entity->is()) { THROW_SEMANTIC_ERROR( "Entity :", name, "is EgoEntity.", "You cannot reset behavior plugin of EgoEntity."); - } else if (is(name)) { + } else if (reference_entity->is()) { THROW_SEMANTIC_ERROR( "Entity :", name, "is MiscObjectEntity.", "You cannot reset behavior plugin of MiscObjectEntity."); - } else if (is(name)) { + } else if (reference_entity->is()) { const auto parameters = getVehicleParameters(name); despawnEntity(name); spawnEntity( name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name); - } else if (is(name)) { + } else if (reference_entity->is()) { const auto parameters = getPedestrianParameters(name); despawnEntity(name); spawnEntity( @@ -292,19 +283,20 @@ void EntityManager::resetBehaviorPlugin( THROW_SIMULATION_ERROR( "Entity :", name, "is unkown entity type.", "Please contact to developer."); } - auto entity = getEntity(name); - entity->setLinearJerk(status.getLinearJerk()); - entity->setAcceleration(status.getAccel()); - entity->setTwist(status.getTwist()); - entity->setBehaviorParameter(behavior_parameter); + auto spawned_entity = getEntity(name); + spawned_entity->setLinearJerk(status.getLinearJerk()); + spawned_entity->setAcceleration(status.getAccel()); + spawned_entity->setTwist(status.getTwist()); + spawned_entity->setBehaviorParameter(behavior_parameter); } auto EntityManager::getCurrentAction(const std::string & name) const -> std::string { - if (not npc_logic_started_ and not is(name)) { + const auto entity = getEntity(name); + if (not npc_logic_started_ and not entity->is()) { return "waiting"; } else { - return getEntity(name)->getCurrentAction(); + return entity->getCurrentAction(); } } diff --git a/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp b/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp index 5235ff0f1c0..fe0609a7a93 100644 --- a/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp @@ -28,7 +28,7 @@ class AlmostStandstillMetric bool isAlmostStandingStill(const traffic_simulator::CanonicalizedEntityStatus & status) { if (!last_status_) { - last_status_ = status; + last_status_->set(status); return false; } @@ -43,7 +43,7 @@ class AlmostStandstillMetric almost_standstill_time_ = 0.0; } - last_status_ = status; + last_status_->set(status); if (almost_standstill_time_ > almost_standstill_timeout_) { return true; diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 9fc84093c5a..e8c4023be85 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -139,11 +139,11 @@ class TestExecutor auto update() -> void { executeWithErrorHandling([this]() { - if (not api_->isEgoSpawned() and not api_->isNpcLogicStarted()) { + if (not api_->isAnyEgoSpawned() and not api_->isNpcLogicStarted()) { api_->startNpcLogic(); } if ( - api_->isEgoSpawned() and not api_->isNpcLogicStarted() and + api_->isAnyEgoSpawned() and not api_->isNpcLogicStarted() and api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { api_->startNpcLogic(); } diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 103b9745944..15bdcdbab7f 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -69,7 +69,7 @@ class MockTrafficSimulatorAPI const traffic_simulator_msgs::msg::VehicleParameters &), ()); MOCK_METHOD(void, requestSpeedChange, (const std::string &, double, bool), ()); - MOCK_METHOD(bool, isEgoSpawned, (), ()); + MOCK_METHOD(bool, isAnyEgoSpawned, (), ()); MOCK_METHOD(bool, isNpcLogicStarted, (), ()); MOCK_METHOD(void, startNpcLogic, (), ()); MOCK_METHOD(bool, despawn, (const std::string), ()); @@ -148,7 +148,7 @@ TEST(TestExecutor, UpdateNoNPCs) MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), 20.0, ArchitectureType::AWF_UNIVERSE, rclcpp::get_logger("test_executor_test")); - EXPECT_CALL(*MockAPI, isEgoSpawned) + EXPECT_CALL(*MockAPI, isAnyEgoSpawned) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(false)); @@ -157,7 +157,7 @@ TEST(TestExecutor, UpdateNoNPCs) .InSequence(sequence) .WillOnce(::testing::Return(false)); EXPECT_CALL(*MockAPI, startNpcLogic).Times(1).InSequence(sequence); - EXPECT_CALL(*MockAPI, isEgoSpawned) + EXPECT_CALL(*MockAPI, isAnyEgoSpawned) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(false)); From e3bc5f97625bdfb388f51797ece8e36cd1ac99dd Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 15:25:40 +0200 Subject: [PATCH 21/49] feat(entity_base, traffic_simulator, simulator_core): remove forwarding setBehaviorParameter and setVelocityLimit --- .../src/collision/crashing_npc.cpp | 8 +++++--- .../request_speed_change_with_limit.cpp | 5 +++-- .../openscenario_interpreter/simulator_core.hpp | 15 ++++++++------- .../include/traffic_simulator/api/api.hpp | 2 -- .../traffic_simulator/entity/entity_manager.hpp | 2 -- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index 45720c82a16..f93993daa35 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -56,18 +56,20 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); api_.requestSpeedChange("ego", 15, true); traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; behavior_parameter.see_around = false; - api_.setBehaviorParameter("ego", behavior_parameter); + ego_entity->setBehaviorParameter(behavior_parameter); api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc")->setLinearVelocity(0.0); + auto npc_entity = api_.getEntity("npc"); + npc_entity->setLinearVelocity(0.0); api_.requestSpeedChange("npc", 5, true); } }; diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index ab2a36b0036..fd0f783358d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -56,8 +56,9 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.setVelocityLimit("ego", 5.0); + auto entity = api_.getEntity("ego"); + entity->setLinearVelocity(0); + entity->setVelocityLimit(5.0); api_.requestSpeedChange( "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index f8ecc4ddca2..9786019fa08 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -277,8 +277,9 @@ class SimulatorCore static auto applyProfileAction( const EntityRef & entity_ref, const DynamicConstraints & dynamic_constraints) -> void { - return core->setBehaviorParameter(entity_ref, [&]() { - auto behavior_parameter = core->getEntity(entity_ref)->getBehaviorParameter(); + auto entity = core->getEntity(entity_ref); + return entity->setBehaviorParameter([&]() { + auto behavior_parameter = entity->getBehaviorParameter(); if (not std::isinf(dynamic_constraints.max_speed)) { behavior_parameter.dynamic_constraints.max_speed = dynamic_constraints.max_speed; @@ -312,12 +313,12 @@ class SimulatorCore static auto applyAssignControllerAction( const std::string & entity_ref, Controller && controller) -> void { - core->setVelocityLimit( - entity_ref, controller.properties.template get( - "maxSpeed", std::numeric_limits::max())); + auto entity = core->getEntity(entity_ref); + entity->setVelocityLimit(controller.properties.template get( + "maxSpeed", std::numeric_limits::max())); - core->setBehaviorParameter(entity_ref, [&]() { - auto message = core->getEntity(entity_ref)->getBehaviorParameter(); + entity->setBehaviorParameter([&]() { + auto message = entity->getBehaviorParameter(); message.see_around = not controller.properties.template get("isBlind"); /// The default values written in https://github.com/tier4/scenario_simulator_v2/blob/master/simulation/traffic_simulator_msgs/msg/DynamicConstraints.msg message.dynamic_constraints.max_acceleration = diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 37fc5e11ee4..d9f6d46d691 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -368,9 +368,7 @@ class API FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); - FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence); - FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); public: #undef FORWARD_TO_ENTITY_MANAGER 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 a5ecbfa6c4c..47c24df9a38 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -259,8 +259,6 @@ class EntityManager FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestSpeedChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(setBehaviorParameter, ); - FORWARD_TO_ENTITY(setVelocityLimit, ); #undef FORWARD_TO_ENTITY From 6dbf3aca1c4db90b951b0c8993f48945c4cb1073 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 17:16:51 +0200 Subject: [PATCH 22/49] feat(entity_base, traffic_simulator, simulator_core): remove forwarding request*, move requestLaneChange to EntityBase --- .../src/collision/crashing_npc.cpp | 4 +- .../src/collision/spawn_with_offset.cpp | 10 ++- .../src/cpp_scenario_node.cpp | 3 +- .../src/crosswalk/stop_at_crosswalk.cpp | 23 +++--- .../accelerate_and_follow.cpp | 5 +- .../decelerate_and_follow.cpp | 5 +- .../acquire_position_in_world_frame.cpp | 5 +- .../assign_route_in_world_frame.cpp | 7 +- .../src/follow_lane/cancel_request.cpp | 15 ++-- .../src/follow_lane/follow_with_offset.cpp | 5 +- ...line_trajectory_with_do_nothing_plugin.cpp | 8 +- .../src/lane_change/lanechange_left.cpp | 7 +- .../lane_change/lanechange_left_with_id.cpp | 7 +- .../src/lane_change/lanechange_linear.cpp | 8 +- .../lanechange_linear_lateral_velocity.cpp | 8 +- .../lane_change/lanechange_linear_time.cpp | 8 +- .../lanechange_longitudinal_distance.cpp | 8 +- .../src/lane_change/lanechange_right.cpp | 9 ++- .../lane_change/lanechange_right_with_id.cpp | 7 +- .../src/lane_change/lanechange_time.cpp | 8 +- ...t_distance_in_lane_coordinate_distance.cpp | 15 ++-- .../get_distance_to_lane_bound.cpp | 5 +- .../src/merge/merge_left.cpp | 7 +- .../src/metrics/traveled_distance.cpp | 5 +- .../src/move_backward/move_backward.cpp | 5 +- .../src/pedestrian/walk_straight.cpp | 25 ++++--- .../src/random_scenario/random001.cpp | 15 ++-- .../speed_planning/request_speed_change.cpp | 14 ++-- .../request_speed_change_continuous_false.cpp | 7 +- .../request_speed_change_relative.cpp | 11 +-- .../request_speed_change_step.cpp | 14 ++-- .../request_speed_change_with_limit.cpp | 4 +- ...uest_speed_change_with_time_constraint.cpp | 14 ++-- ...eed_change_with_time_constraint_linear.cpp | 8 +- ...d_change_with_time_constraint_relative.cpp | 13 ++-- .../src/traffic_simulation_demo.cpp | 73 ++++++++++--------- .../define_traffic_source_delay.cpp | 5 +- .../define_traffic_source_high_rate.cpp | 5 +- .../define_traffic_source_large.cpp | 5 +- .../define_traffic_source_mixed.cpp | 5 +- .../define_traffic_source_multiple.cpp | 5 +- .../define_traffic_source_outside_lane.cpp | 5 +- .../define_traffic_source_pedestrian.cpp | 5 +- .../define_traffic_source_vehicle.cpp | 5 +- .../simulator_core.hpp | 30 +++++--- .../include/traffic_simulator/api/api.hpp | 23 ------ .../traffic_simulator/entity/entity_base.hpp | 21 +++--- .../entity/entity_manager.hpp | 11 --- simulation/traffic_simulator/src/api/api.cpp | 33 --------- .../src/entity/entity_base.cpp | 11 +++ .../src/entity/entity_manager.cpp | 12 --- .../random_test_runner/test_executor.hpp | 7 +- 52 files changed, 287 insertions(+), 296 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index f93993daa35..d7e7ea1e11c 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -58,7 +58,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode getVehicleParameters()); auto ego_entity = api_.getEntity("ego"); ego_entity->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 15, true); + ego_entity->requestSpeedChange(15, true); traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; behavior_parameter.see_around = false; ego_entity->setBehaviorParameter(behavior_parameter); @@ -70,7 +70,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode getVehicleParameters()); auto npc_entity = api_.getEntity("npc"); npc_entity->setLinearVelocity(0.0); - api_.requestSpeedChange("npc", 5, true); + npc_entity->requestSpeedChange(5, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp index 35c8ed43977..59cb53f753d 100644 --- a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp @@ -54,16 +54,18 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.2, 1.3, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange("ego", 0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange(0, true); api_.spawn( "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, -0.874, api_.getHdmapUtils()), getPedestrianParameters()); - api_.getEntity("bob")->setLinearVelocity(0); - api_.requestSpeedChange("bob", 0, true); + auto bob_entity = api_.getEntity("bob"); + bob_entity->setLinearVelocity(0); + bob_entity->requestSpeedChange(0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index dff52151963..a3f93334873 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -119,7 +119,8 @@ void CppScenarioNode::spawnEgoEntity( // clang-format on return configuration; }()); - api_.requestAssignRoute("ego", goal_lanelet_poses); + auto ego_entity = api_.getEntity("ego"); + ego_entity->requestAssignRoute(goal_lanelet_poses); using namespace std::chrono_literals; while (!api_.asFieldOperatorApplication("ego").engaged()) { diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 8dc11e5dabe..91e51d748c1 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -78,23 +78,24 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 8, true); - api_.requestAssignRoute( - "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(8, true); + ego_entity->requestAssignRoute(std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0.0, api_.getHdmapUtils())}); api_.spawn( "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.getEntity("bob")->setLinearVelocity(0); - api_.requestSpeedChange( - "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, + auto bob_entity = api_.getEntity("bob"); + bob_entity->setLinearVelocity(0); + bob_entity->requestSpeedChange( + 1.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), true); diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index e89aa5e7678..d6996179add 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -70,8 +70,9 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc")->setLinearVelocity(10); - api_.requestSpeedChange("npc", 10, true); + auto npc_entity = api_.getEntity("npc"); + npc_entity->setLinearVelocity(10); + npc_entity->requestSpeedChange(10, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index e649fb01369..fa749d5db7d 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -70,8 +70,9 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 15.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc")->setLinearVelocity(10); - api_.requestSpeedChange("npc", 10, true); + auto npc_entity = api_.getEntity("npc"); + npc_entity->setLinearVelocity(10); + npc_entity->requestSpeedChange(10, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 0fa1c9407b6..a14e52cbd31 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -56,11 +56,12 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), traffic_simulator::helper::constructActionStatus(10)); - api_.requestSpeedChange("ego", 10, true); + auto entity = api_.getEntity("ego"); + entity->requestSpeedChange(10, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34408, 1.0, 0.0, api_.getHdmapUtils())); - api_.requestAcquirePosition("ego", goal_pose); + entity->requestAcquirePosition(goal_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 739315c01c1..9a17485d32c 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -51,8 +51,9 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); + auto entity = api_.getEntity("ego"); + entity->setLinearVelocity(10); + entity->requestSpeedChange(10, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -60,7 +61,7 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN goal_poses.emplace_back(traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34408, 10, 0.0, api_.getHdmapUtils()))); - api_.requestAssignRoute("ego", goal_poses); + entity->requestAssignRoute(goal_poses); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index c03122090c9..23735bb1ed4 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -39,15 +39,15 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode bool canceled = false; void onUpdate() override { - const auto entity = api_.getEntity("ego"); - if (entity->isInPosition( + const auto ego_entity = api_.getEntity("ego"); + if (ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 30, 0, api_.getHdmapUtils()), 3.0)) { - api_.cancelRequest("ego"); + ego_entity->cancelRequest(); canceled = true; } - if (api_.getEntity("ego")->isInLanelet(34507, 0.1)) { + if (ego_entity->isInLanelet(34507, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } } @@ -58,11 +58,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(7); - api_.requestSpeedChange("ego", 7, true); + auto entity = api_.getEntity("ego"); + entity->setLinearVelocity(7); + entity->requestSpeedChange(7, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils()); - api_.requestAcquirePosition("ego", goal_pose); + entity->requestAcquirePosition(goal_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index f7bcac6a313..d811ed7749d 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -58,8 +58,9 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 3.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); + auto entity = api_.getEntity("ego"); + entity->setLinearVelocity(10); + entity->requestSpeedChange(10, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 2a655661019..375fe1ab65f 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -100,10 +100,10 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario api_.spawn( "ego", spawn_pose, getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestFollowTrajectory( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestFollowTrajectory( std::make_shared( traffic_simulator_msgs::build() .initial_distance_offset(0.0) diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 28e3a714a23..f20d484c2d8 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -55,9 +55,10 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange("ego", traffic_simulator::lane_change::Direction::LEFT); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange(traffic_simulator::lane_change::Direction::LEFT); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index fcb95413f60..3dc21831bd8 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -55,9 +55,10 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange("ego", 34513); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange(34513); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index c9df7dabf70..6197e18debd 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -60,10 +60,10 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( "ego", traffic_simulator::lane_change::Direction::LEFT, 1, 0), traffic_simulator::lane_change::TrajectoryShape::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index 1546738b99e..0865066149b 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -63,10 +63,10 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( "ego", traffic_simulator::lane_change::Direction::LEFT, 1, 0), traffic_simulator::lane_change::TrajectoryShape::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index dde46363f19..c7f323ab60f 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -64,10 +64,10 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( "ego", traffic_simulator::lane_change::Direction::LEFT, 1, 0), traffic_simulator::lane_change::TrajectoryShape::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index 4a1722252a1..cedf85c2273 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -51,10 +51,10 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(1); - api_.requestSpeedChange("ego", 1, true); - api_.requestLaneChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(1); + ego_entity->requestSpeedChange(1, true); + ego_entity->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( "ego", traffic_simulator::lane_change::Direction::LEFT, 1, 0), traffic_simulator::lane_change::TrajectoryShape::CUBIC, diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index b4e689c9523..1396b4ae61b 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -55,10 +55,11 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange("ego", traffic_simulator::lane_change::Direction::RIGHT); - // api_.requestLaneChange("ego", 34462); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange(traffic_simulator::lane_change::Direction::RIGHT); + // ego_entity->requestLaneChange(34462); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index 6f23e69b9c8..1d0d29db318 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -55,9 +55,10 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange("ego", 34462); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange(34462); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index f330e995db1..7a4a12cf0a7 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -64,10 +64,10 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 10, true); - api_.requestLaneChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); + ego_entity->requestLaneChange( traffic_simulator::lane_change::RelativeTarget( "ego", traffic_simulator::lane_change::Direction::LEFT, 1, 0), traffic_simulator::lane_change::TrajectoryShape::CUBIC, diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 44dd73d5347..2445e474915 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -140,24 +140,27 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 5.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 3, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(3, true); api_.spawn( "front", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 10.0, 1.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(10); - api_.requestSpeedChange("front", 3, true); + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(10); + front_entity->requestSpeedChange(3, true); api_.spawn( "behind", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, -1.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("behind")->setLinearVelocity(10); - api_.requestSpeedChange("behind", 3, true); + auto behind_entity = api_.getEntity("behind"); + behind_entity->setLinearVelocity(10); + behind_entity->requestSpeedChange(3, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index 281cfe4c8b3..a2384ba2e87 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -61,8 +61,9 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 5.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 3, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(3, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index bdd6af8ba8f..9e87b6bf79e 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -58,9 +58,10 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34462, 15.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(5); - api_.requestSpeedChange("ego", 5, true); - api_.requestLaneChange("ego", 34513); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(5); + ego_entity->requestSpeedChange(5, true); + ego_entity->requestLaneChange(34513); api_.spawn( "npc", diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index 395aeee668e..a3a33c62128 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -67,8 +67,9 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(3); - api_.requestSpeedChange("ego", 3, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(3); + ego_entity->requestSpeedChange(3, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 3d6eaa2c8bf..095b0df4087 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -56,8 +56,9 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(-3); - api_.requestSpeedChange("ego", -3, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(-3); + ego_entity->requestSpeedChange(-3, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index e9f5c0799f0..ae6b666ff43 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -74,23 +74,24 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 8, true); - api_.requestAssignRoute( - "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(8, true); + ego_entity->requestAssignRoute(std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0.0, api_.getHdmapUtils())}); api_.spawn( "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.getEntity("bob")->setLinearVelocity(0); - api_.requestWalkStraight("bob"); - api_.requestSpeedChange( - "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, + auto bob_entity = api_.getEntity("bob"); + bob_entity->setLinearVelocity(0); + bob_entity->requestWalkStraight(); + bob_entity->requestSpeedChange( + 1.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), true); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 5e7e3175b93..67f1d9a348c 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -75,7 +75,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode offset, api_.getHdmapUtils()), getVehicleParameters( get_entity_subtype(params_.random_parameters.road_parking_vehicle.entity_type))); - api_.requestSpeedChange(entity_name, 0, true); + api_.getEntity(entity_name)->requestSpeedChange(0, true); }; std::uniform_real_distribution<> dist( params_.random_parameters.road_parking_vehicle.min_offset, @@ -130,8 +130,9 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode params_.random_parameters.lane_following_vehicle.min_speed, params_.random_parameters.lane_following_vehicle.max_speed); const auto speed = speed_distribution(engine_); - api_.requestSpeedChange(entity_name, speed, true); - api_.getEntity(entity_name)->setLinearVelocity(speed); + auto entity = api_.getEntity(entity_name); + entity->requestSpeedChange(speed, true); + entity->setLinearVelocity(speed); std::uniform_real_distribution<> lane_change_position_distribution( 0.0, traffic_simulator::pose::laneletLength(34684, api_.getHdmapUtils())); lane_change_position = lane_change_position_distribution(engine_); @@ -142,7 +143,8 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode if ( ego_entity->getStatus().getLaneletId() == 34684 && std::abs(ego_entity->getStatus().getLaneletPose().s) >= lane_change_position) { - api_.requestLaneChange(entity_name, traffic_simulator::lane_change::Direction::RIGHT); + api_.getEntity(entity_name) + ->requestLaneChange(traffic_simulator::lane_change::Direction::RIGHT); lane_change_requested = true; } } @@ -173,8 +175,9 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode lanelet_id, 0.0, offset_distribution(engine_), api_.getHdmapUtils()), getPedestrianParameters()); const auto speed = speed_distribution(engine_); - api_.requestSpeedChange(entity_name, speed, true); - api_.getEntity(entity_name)->setLinearVelocity(speed); + auto entity = api_.getEntity(entity_name); + entity->requestSpeedChange(speed, true); + entity->setLinearVelocity(speed); } if ( api_.entityExists(entity_name) && diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index c6a35c7a544..942b0fd424d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -59,9 +59,10 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), true); @@ -71,9 +72,10 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(0); - api_.requestSpeedChange( - "front", 10.0, traffic_simulator::speed_change::Transition::STEP, + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(0); + front_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), true); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 28073f16f0c..126c8600ed1 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -68,9 +68,10 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), false); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index 3073bb8de4d..aa53b5bd704 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -60,17 +60,18 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(3); - api_.requestSpeedChange("ego", 3.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(3); + ego_entity->requestSpeedChange(3.0, true); api_.spawn( "front", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(3); - api_.requestSpeedChange( - "front", + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(3); + front_entity->requestSpeedChange( traffic_simulator::speed_change::RelativeTargetSpeed( "ego", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 2.0), traffic_simulator::speed_change::Transition::LINEAR, diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index 79551ea14bb..1b6f5e867b0 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -57,9 +57,10 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::STEP, + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), true); @@ -69,9 +70,10 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(0); - api_.requestSpeedChange( - "front", 10.0, traffic_simulator::speed_change::Transition::STEP, + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(0); + front_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::STEP, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::TIME, 0.0), true); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index fd0f783358d..b96533b9b41 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -59,8 +59,8 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar auto entity = api_.getEntity("ego"); entity->setLinearVelocity(0); entity->setVelocityLimit(5.0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), true); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 66aeadc7b26..275939799d9 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -60,9 +60,10 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::AUTO, + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::AUTO, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::TIME, 4.0), false); @@ -72,9 +73,10 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(10); - api_.requestSpeedChange( - "front", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(10); + front_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 4.0), true); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index 88c4f1a5762..8a1623eee5e 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -57,9 +57,11 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0); - api_.requestSpeedChange( - "ego", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + auto ego_entity = api_.getEntity("ego"); + + ego_entity->setLinearVelocity(0); + ego_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::TIME, 4.0), false); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index f745844ee47..26925f29ed0 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -61,9 +61,9 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(1.0); - api_.requestSpeedChange( - "ego", + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(1.0); + ego_entity->requestSpeedChange( traffic_simulator::speed_change::RelativeTargetSpeed( "ego", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, 2.0), traffic_simulator::speed_change::Transition::AUTO, @@ -76,9 +76,10 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("front")->setLinearVelocity(10); - api_.requestSpeedChange( - "front", 10.0, traffic_simulator::speed_change::Transition::LINEAR, + auto front_entity = api_.getEntity("front"); + front_entity->setLinearVelocity(10); + front_entity->requestSpeedChange( + 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 4.0), true); diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index b59af77e1af..0fdcba33ae4 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -56,37 +56,37 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.despawn("obstacle"); } const auto ego_entity = api_.getEntity("ego"); - const auto npc_entity = api_.getEntity("npc2"); + const auto npc2_entity = api_.getEntity("npc2"); if (ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34615, 10.0, 0.0, api_.getHdmapUtils()), 5)) { - api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 35026, 0.0, 0.0, api_.getHdmapUtils())); + ego_entity->requestAcquirePosition( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 35026, 0.0, 0.0, api_.getHdmapUtils())); if (api_.entityExists("npc2")) { - api_.requestSpeedChange("npc2", 13, true); + npc2_entity->requestSpeedChange(13, true); } } if (ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 0.0, 0.0, api_.getHdmapUtils()), 5)) { - api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + ego_entity->requestAcquirePosition( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils())); if (api_.entityExists("npc2")) { - api_.requestSpeedChange("npc2", 3, true); + npc2_entity->requestSpeedChange(3, true); } } - if (npc_entity->isInPosition( + if (npc2_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), 5)) { - api_.requestAcquirePosition( - "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34630, 0.0, 0.0, api_.getHdmapUtils())); - api_.requestSpeedChange("npc2", 13, true); + npc2_entity->requestAcquirePosition( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34630, 0.0, 0.0, api_.getHdmapUtils())); + npc2_entity->requestSpeedChange(13, true); } if (api_.getCurrentTime() > 10.0 && api_.entityExists("bob")) { api_.despawn("bob"); @@ -102,14 +102,14 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(10); - api_.requestSpeedChange("ego", 8, true); - api_.requestAssignRoute( - "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(8, true); + ego_entity->requestAssignRoute(std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0.0, api_.getHdmapUtils())}); api_.spawn( "tom", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), @@ -117,42 +117,47 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.setEntityStatus( "tom", "ego", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), traffic_simulator::helper::constructActionStatus()); - api_.requestWalkStraight("tom"); - api_.requestSpeedChange("tom", 3, true); + auto tom_entity = api_.getEntity("tom"); + tom_entity->requestWalkStraight(); + tom_entity->requestSpeedChange(3, true); api_.spawn( "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34378, 0.0, 0.0, api_.getHdmapUtils()), getPedestrianParameters()); - api_.getEntity("bob")->setLinearVelocity(1.0); - api_.requestSpeedChange("bob", 1, true); + auto bob_entity = api_.getEntity("bob"); + bob_entity->setLinearVelocity(1.0); + bob_entity->requestSpeedChange(1, true); api_.spawn( "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc1")->setLinearVelocity(5.0); - api_.requestSpeedChange("npc1", 5, true); - api_.requestAcquirePosition( - "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + auto npc1_entity = api_.getEntity("npc1"); + npc1_entity->setLinearVelocity(5.0); + npc1_entity->requestSpeedChange(5, true); + npc1_entity->requestAcquirePosition( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils())); api_.spawn( "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34606, 20.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc2")->setLinearVelocity(5); - api_.requestSpeedChange("npc2", 0, true); + auto npc2_entity = api_.getEntity("npc1"); + npc2_entity->setLinearVelocity(5); + npc2_entity->requestSpeedChange(0, true); api_.spawn( "npc3", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34468, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("npc3")->setLinearVelocity(10); + auto npc3_entity = api_.getEntity("npc3"); + npc3_entity->setLinearVelocity(10); api_.spawn( "obstacle", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index beb4fa354ac..f9721239276 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -102,8 +102,9 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } bool traffic_source_added_ = false; diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 14914ad98bc..83bef35a10a 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -94,8 +94,9 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index dd4692f2ea9..409107bc0d6 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -84,8 +84,9 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 57aaf5db32c..5a0d2c35284 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -106,8 +106,9 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index f445bf0c8e4..8f0e99cf334 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -124,8 +124,9 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 67a459dd84c..f2ccc94911a 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -85,8 +85,9 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index f2e498625c1..ad1b4ed7f4e 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -92,8 +92,9 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 8f92a3bb56b..b85ed92608b 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -94,8 +94,9 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34570, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.getEntity("ego")->setLinearVelocity(0.0); - api_.requestSpeedChange("ego", 0.0, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0.0); + ego_entity->requestSpeedChange(0.0, true); } }; } // namespace cpp_mock_scenarios diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 9786019fa08..2f074734ca4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -262,9 +262,10 @@ class SimulatorCore { protected: template - static auto applyAcquirePositionAction(Ts &&... xs) + static auto applyAcquirePositionAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestAcquirePosition(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestAcquirePosition(std::forward(xs)...); } template @@ -413,9 +414,10 @@ class SimulatorCore } template - static auto applyAssignRouteAction(Ts &&... xs) + static auto applyAssignRouteAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestAssignRoute(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestAssignRoute(std::forward(xs)...); } template @@ -425,21 +427,24 @@ class SimulatorCore } template - static auto applyFollowTrajectoryAction(Ts &&... xs) + static auto applyFollowTrajectoryAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestFollowTrajectory(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestFollowTrajectory(std::forward(xs)...); } template - static auto applyLaneChangeAction(Ts &&... xs) + static auto applyLaneChangeAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestLaneChange(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestLaneChange(std::forward(xs)...); } template - static auto applySpeedAction(Ts &&... xs) + static auto applySpeedAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestSpeedChange(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestSpeedChange(std::forward(xs)...); } template @@ -449,9 +454,10 @@ class SimulatorCore } template - static auto applyWalkStraightAction(Ts &&... xs) + static auto applyWalkStraightAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestWalkStraight(std::forward(xs)...); + auto entity = core->getEntity(entity_ref); + return entity->requestWalkStraight(std::forward(xs)...); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d9f6d46d691..bdf37157fe1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -285,22 +285,6 @@ class API void startNpcLogic(); - void requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id); - - void requestLaneChange(const std::string & name, const lane_change::Direction & direction); - - void requestLaneChange(const std::string & name, const lane_change::Parameter &); - - void requestLaneChange( - const std::string & name, const lane_change::RelativeTarget & target, - const lane_change::TrajectoryShape trajectory_shape, - const lane_change::Constraint & constraint); - - void requestLaneChange( - const std::string & name, const lane_change::AbsoluteTarget & target, - const lane_change::TrajectoryShape trajectory_shape, - const lane_change::Constraint & constraint); - /** * @brief Add a traffic source to the simulation * @param radius The radius defining the area on which entities will be spawned @@ -344,7 +328,6 @@ class API FORWARD_TO_ENTITY_MANAGER(activateOutOfRangeJob); FORWARD_TO_ENTITY_MANAGER(asFieldOperatorApplication); - FORWARD_TO_ENTITY_MANAGER(cancelRequest); FORWARD_TO_ENTITY_MANAGER(checkCollision); FORWARD_TO_ENTITY_MANAGER(entityExists); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); @@ -359,12 +342,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isAnyEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); - FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); - FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); - FORWARD_TO_ENTITY_MANAGER(requestClearRoute); - FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); - FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); - FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); 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 c6992c66e43..b2331be03a5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -149,18 +149,21 @@ class EntityBase virtual void requestLaneChange(const lanelet::Id){}; - virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &){}; + virtual void requestLaneChange(const lane_change::Parameter &){}; - /* */ void requestLaneChange( - const lane_change::AbsoluteTarget &, const lane_change::TrajectoryShape, - const lane_change::Constraint &); + /* */ auto requestLaneChange(const lane_change::Direction & direction) -> void; - /* */ void requestLaneChange( - const lane_change::RelativeTarget &, const lane_change::TrajectoryShape, - const lane_change::Constraint &); + /* */ auto requestLaneChange( + const lane_change::AbsoluteTarget & target, const lane_change::TrajectoryShape trajectory_shape, + const lane_change::Constraint & constraint) -> void; - virtual void requestSpeedChange( - const double, const speed_change::Transition, const speed_change::Constraint, const bool); + /* */ auto requestLaneChange( + const lane_change::RelativeTarget & target, const lane_change::TrajectoryShape trajectory_shape, + const lane_change::Constraint & constraint) -> void; + + virtual auto requestSpeedChange( + const double, const speed_change::Transition, const speed_change::Constraint, const bool) + -> void; virtual void requestSpeedChange( const speed_change::RelativeTargetSpeed &, const speed_change::Transition, 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 47c24df9a38..e234920413a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -251,14 +251,6 @@ class EntityManager FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); - FORWARD_TO_ENTITY(cancelRequest, ); - FORWARD_TO_ENTITY(requestAcquirePosition, ); - FORWARD_TO_ENTITY(requestAssignRoute, ); - FORWARD_TO_ENTITY(requestClearRoute, ); - FORWARD_TO_ENTITY(requestFollowTrajectory, ); - FORWARD_TO_ENTITY(requestLaneChange, ); - FORWARD_TO_ENTITY(requestSpeedChange, ); - FORWARD_TO_ENTITY(requestWalkStraight, ); #undef FORWARD_TO_ENTITY @@ -332,9 +324,6 @@ class EntityManager auto getEgoName() const -> const std::string &; - void requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::Direction & direction); - /** * @brief Reset behavior plugin of the target entity. * The internal behavior is to take over the various parameters and save them, then respawn the Entity and set the parameters. diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index f65fbe1f399..fca26390703 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -334,39 +334,6 @@ void API::startNpcLogic() } } -void API::requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id) -{ - entity_manager_ptr_->requestLaneChange(name, lanelet_id); -} - -void API::requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::Direction & direction) -{ - entity_manager_ptr_->requestLaneChange(name, direction); -} - -void API::requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::Parameter & parameter) -{ - entity_manager_ptr_->requestLaneChange(name, parameter); -} - -void API::requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::RelativeTarget & target, - const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, - const lane_change::Constraint & constraint) -{ - entity_manager_ptr_->requestLaneChange(name, target, trajectory_shape, constraint); -} - -void API::requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::AbsoluteTarget & target, - const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, - const lane_change::Constraint & constraint) -{ - entity_manager_ptr_->requestLaneChange(name, target, trajectory_shape, constraint); -} - auto API::addTrafficSource( const double radius, const double rate, const double speed, const geometry_msgs::msg::Pose & pose, const traffic::TrafficSource::Distribution & distribution, const bool allow_spawn_outside_lane, diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 80b80e73eba..0221a5c393b 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -146,6 +146,17 @@ void EntityBase::resetDynamicConstraints() setDynamicConstraints(getDefaultDynamicConstraints()); } +auto EntityBase::requestLaneChange(const lane_change::Direction & direction) -> void +{ + if (isInLanelet()) { + if ( + const auto target = + hdmap_utils_ptr_->getLaneChangeableLaneletId(getStatus().getLaneletId(), direction)) { + requestLaneChange(target.value()); + } + } +} + void EntityBase::requestLaneChange( const traffic_simulator::lane_change::AbsoluteTarget & target, const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index f11b485e41c..2fd3a87c592 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -244,18 +244,6 @@ auto EntityManager::isAnyEgoSpawned() const -> bool }); } -void EntityManager::requestLaneChange( - const std::string & name, const traffic_simulator::lane_change::Direction & direction) -{ - if (const auto entity = getEntity(name); entity->isInLanelet()) { - if ( - const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( - entity->getStatus().getLaneletId(), direction)) { - requestLaneChange(name, target.value()); - } - } -} - void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index e8c4023be85..bda2e217ba9 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -117,8 +117,8 @@ class TestExecutor // XXX dirty hack: wait for autoware system to launch // ugly but helps for now std::this_thread::sleep_for(std::chrono::milliseconds{5000}); - - api_->requestAssignRoute(ego_name_, std::vector({test_description_.ego_goal_pose})); + api_->getEntity(ego_name_)->requestAssignRoute( + std::vector({test_description_.ego_goal_pose})); api_->asFieldOperatorApplication(ego_name_).engage(); goal_reached_metric_.setGoal(test_description_.ego_goal_pose); @@ -128,10 +128,11 @@ class TestExecutor api_->spawn( npc_descr.name, npc_descr.start_position, getVehicleParameters(), traffic_simulator::VehicleBehavior::defaultBehavior(), "taxi"); + auto entity = api_->getEntity(npc_descr.name); api_->setEntityStatus( npc_descr.name, npc_descr.start_position, traffic_simulator::helper::constructActionStatus(npc_descr.speed)); - api_->requestSpeedChange(npc_descr.name, npc_descr.speed, true); + entity->requestSpeedChange(npc_descr.speed, true); } }); } From f952a1121ce701f12b9b36d79e24bf981380c362 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 17:54:38 +0200 Subject: [PATCH 23/49] feat(entity_manager, behavior_tree, cpp_mock): remove getCurrentAction forwarding, set "waiting" as init action state in behavior_tree --- .../load_do_nothing_plugin.cpp | 19 +++++++++++++------ .../lanechange_linear_lateral_velocity.cpp | 5 +++-- .../lane_change/lanechange_linear_time.cpp | 5 +++-- .../src/lane_change/lanechange_time.cpp | 5 +++-- .../simulator_core.hpp | 4 ++-- .../transition_events/transition_event.hpp | 2 +- .../include/traffic_simulator/api/api.hpp | 1 - .../entity/entity_manager.hpp | 2 -- .../src/entity/entity_manager.cpp | 10 ---------- 9 files changed, 25 insertions(+), 28 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index d961cde5e32..11513fe587d 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -40,14 +40,21 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { /// @note When using the do_nothing plugin, the return value of the `getCurrentAction` function is always do_nothing. + auto const ego_entity = api_.getEntity("ego"); + auto const pedestrian_entity = api_.getEntity("pedestrian"); + auto const vehicle_spawn_with_behavior_tree_entity = + api_.getEntity("vehicle_spawn_with_behavior_tree"); + auto const pedestrian_spawn_with_behavior_tree_entity = + api_.getEntity("pedestrian_spawn_with_behavior_tree"); + if ( - api_.getCurrentAction("ego") != "do_nothing" || - api_.getCurrentAction("pedestrian") != "do_nothing") { + ego_entity->getCurrentAction() != "do_nothing" || + pedestrian_entity->getCurrentAction() != "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - api_.getCurrentAction("vehicle_spawn_with_behavior_tree") == "do_nothing" || - api_.getCurrentAction("pedestrian_spawn_with_behavior_tree") == "do_nothing") { + vehicle_spawn_with_behavior_tree_entity->getCurrentAction() == "do_nothing" || + pedestrian_spawn_with_behavior_tree_entity->getCurrentAction() == "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } api_.resetBehaviorPlugin( @@ -57,8 +64,8 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode "pedestrian_spawn_with_behavior_tree", traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing()); if ( - api_.getCurrentAction("vehicle_spawn_with_behavior_tree") != "do_nothing" || - api_.getCurrentAction("pedestrian_spawn_with_behavior_tree") != "do_nothing") { + vehicle_spawn_with_behavior_tree_entity->getCurrentAction() != "do_nothing" || + pedestrian_spawn_with_behavior_tree_entity->getCurrentAction() != "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index 0865066149b..b268fae2ac0 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -39,10 +39,11 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc int lanechange_frames = 0; void onUpdate() override { - if (api_.getCurrentAction("ego") == "lane_change") { + const auto ego_entity = api_.getEntity("ego"); + if (ego_entity->getCurrentAction() == "lane_change") { lanechange_frames++; } - if (api_.getCurrentAction("ego") != "lane_change" && api_.getCurrentTime() >= 2.0) { + if (ego_entity->getCurrentAction() != "lane_change" && api_.getCurrentTime() >= 2.0) { double duration = static_cast(lanechange_frames) * 0.05; if (duration >= 3.05 && 3.1 >= duration) { stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index c7f323ab60f..910781c7ae9 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -39,10 +39,11 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode int lanechange_frames = 0; void onUpdate() override { - if (api_.getCurrentAction("ego") == "lane_change") { + const auto ego_entity = api_.getEntity("ego"); + if (ego_entity->getCurrentAction() == "lane_change") { lanechange_frames++; } - if (api_.getCurrentAction("ego") != "lane_change" && api_.getCurrentTime() >= 10.0) { + if (ego_entity->getCurrentAction() != "lane_change" && api_.getCurrentTime() >= 10.0) { if (const double step_time = 50e-3, expected_time = 20.0, real_time = static_cast(lanechange_frames) * step_time; expected_time - step_time <= real_time && real_time <= expected_time + step_time) { diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index 7a4a12cf0a7..b49516c316c 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -39,10 +39,11 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode int lanechange_frames = 0; void onUpdate() override { - if (api_.getCurrentAction("ego") == "lane_change") { + const auto ego_entity = api_.getEntity("ego"); + if (ego_entity->getCurrentAction() == "lane_change") { lanechange_frames++; } - if (api_.getCurrentAction("ego") != "lane_change" && api_.getCurrentTime() >= 10.0) { + if (ego_entity->getCurrentAction() != "lane_change" && api_.getCurrentTime() >= 10.0) { if (const double step_time = 50e-3, expected_time = 20.0, real_time = static_cast(lanechange_frames) * step_time; expected_time - step_time <= real_time && real_time <= expected_time + step_time) { diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 2f074734ca4..defddac56f5 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -551,9 +551,9 @@ class SimulatorCore } template - static auto evaluateCurrentState(Ts &&... xs) -> decltype(auto) + static auto evaluateCurrentState(const std::string & entity_ref, Ts &&... xs) -> decltype(auto) { - return core->getCurrentAction(std::forward(xs)...); + return core->getEntity(entity_ref)->getCurrentAction(std::forward(xs)...); } template diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp index 04a38a1e3c9..a12bc880398 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp @@ -36,7 +36,7 @@ class TransitionEvent BT::TimePoint first_timestamp_; std::vector subscribers_; BT::TimestampType type_; - std::string current_action_; + std::string current_action_{"waiting"}; }; } // namespace behavior_tree_plugin diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index bdf37157fe1..7cf729b7a12 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -332,7 +332,6 @@ class API FORWARD_TO_ENTITY_MANAGER(entityExists); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); - FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityNames); 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 e234920413a..0e8e365a511 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -254,8 +254,6 @@ class EntityManager #undef FORWARD_TO_ENTITY - auto getCurrentAction(const std::string & name) const -> std::string; - visualization_msgs::msg::MarkerArray makeDebugMarker() const; bool trafficLightsChanged(); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 2fd3a87c592..473395d2c52 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -278,16 +278,6 @@ void EntityManager::resetBehaviorPlugin( spawned_entity->setBehaviorParameter(behavior_parameter); } -auto EntityManager::getCurrentAction(const std::string & name) const -> std::string -{ - const auto entity = getEntity(name); - if (not npc_logic_started_ and not entity->is()) { - return "waiting"; - } else { - return entity->getCurrentAction(); - } -} - bool EntityManager::trafficLightsChanged() { return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or From f142c10db691bc9ca9414127e297a94fcedc5b28 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 18:04:41 +0200 Subject: [PATCH 24/49] feat(entity_manager, simulator_core): remove activateOutOfRangeJob forwarding --- .../openscenario_interpreter/simulator_core.hpp | 10 ++++++---- .../include/traffic_simulator/api/api.hpp | 1 - .../traffic_simulator/entity/entity_manager.hpp | 1 - 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index defddac56f5..a9ae33a01f0 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -533,10 +533,12 @@ class SimulatorCore const std::string & entity_ref, const Performance & performance, const Properties & properties) { - core->activateOutOfRangeJob( - entity_ref, -performance.max_speed, +performance.max_speed, -performance.max_deceleration, - +performance.max_acceleration, properties.template get("minJerk", Double::lowest()), - properties.template get("maxJerk", Double::max())); + core->getEntity(entity_ref) + ->activateOutOfRangeJob( + -performance.max_speed, +performance.max_speed, -performance.max_deceleration, + +performance.max_acceleration, + properties.template get("minJerk", Double::lowest()), + properties.template get("maxJerk", Double::max())); } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 7cf729b7a12..f4ab010d4e4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -326,7 +326,6 @@ class API static_assert(true, "") // clang-format on - FORWARD_TO_ENTITY_MANAGER(activateOutOfRangeJob); FORWARD_TO_ENTITY_MANAGER(asFieldOperatorApplication); FORWARD_TO_ENTITY_MANAGER(checkCollision); FORWARD_TO_ENTITY_MANAGER(entityExists); 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 0e8e365a511..64ad4fda34f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -249,7 +249,6 @@ class EntityManager static_assert(true, "") // clang-format on - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); #undef FORWARD_TO_ENTITY From f057e688869747bbca9148e298ecb32ae244b754 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 27 Jun 2024 18:36:38 +0200 Subject: [PATCH 25/49] feat(api, entity_manager, cpp_mock): rename entityExist to isEntitySpawned, move checkCollision directly to API --- .../src/crosswalk/stop_at_crosswalk.cpp | 2 +- .../src/pedestrian/walk_straight.cpp | 2 +- .../src/random_scenario/random001.cpp | 12 +++++------ .../src/traffic_simulation_demo.cpp | 12 +++++------ .../include/traffic_simulator/api/api.hpp | 11 ++++++---- .../entity/entity_manager.hpp | 5 +---- simulation/traffic_simulator/src/api/api.cpp | 18 +++++++++++++++- .../src/entity/entity_manager.cpp | 21 ++----------------- .../random_test_runner/test_executor.hpp | 2 +- .../test/test_test_executor.cpp | 2 +- 10 files changed, 43 insertions(+), 44 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 91e51d748c1..f43c6880418 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -40,7 +40,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode { const auto t = api_.getCurrentTime(); // LCOV_EXCL_START - if (api_.entityExists("bob") && api_.checkCollision("ego", "bob")) { + if (api_.isEntitySpawned("bob") && api_.checkCollision("ego", "bob")) { stop(cpp_mock_scenarios::Result::FAILURE); } /** diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index ae6b666ff43..3ec1dc88d53 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -40,7 +40,7 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode { const auto t = api_.getCurrentTime(); // LCOV_EXCL_START - if (api_.entityExists("bob") && api_.checkCollision("ego", "bob")) { + if (api_.isEntitySpawned("bob") && api_.checkCollision("ego", "bob")) { stop(cpp_mock_scenarios::Result::FAILURE); } /** diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 67f1d9a348c..1ab2521157f 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -98,7 +98,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode { for (int i = 0; i < params_.random_parameters.crossing_pedestrian.number_of_pedestrian; i++) { std::string entity_name = "pedestrian" + std::to_string(i); - if (api_.entityExists(entity_name)) { + if (api_.isEntitySpawned(entity_name)) { api_.despawn(entity_name); } } @@ -120,7 +120,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode }; const auto spawn_and_change_lane = [&](const auto & entity_name, const auto spawn_s_value) { - if (!api_.entityExists(entity_name)) { + if (!api_.isEntitySpawned(entity_name)) { api_.spawn( entity_name, traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -159,7 +159,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode std::string entity_name = "pedestrian" + std::to_string(entity_index); constexpr lanelet::Id lanelet_id = 34392; if ( - !api_.entityExists(entity_name) && + !api_.isEntitySpawned(entity_name) && !ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 25.0, 0.0, api_.getHdmapUtils()), @@ -180,7 +180,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode entity->setLinearVelocity(speed); } if ( - api_.entityExists(entity_name) && + api_.isEntitySpawned(entity_name) && api_.getEntity(entity_name)->getStandStillDuration() >= 0.5) { api_.despawn(entity_name); } @@ -194,7 +194,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode 34621, 10, 0.0, api_.getHdmapUtils()); const auto entity_name = "spawn_nearby_ego"; const auto ego = api_.getEntity("ego"); - if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.entityExists(entity_name)) { + if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { api_.spawn( entity_name, traffic_simulator::pose::transformRelativePoseToGlobal( @@ -207,7 +207,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } else { stop(cpp_mock_scenarios::Result::FAILURE); } - if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.entityExists(entity_name)) { + if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { api_.despawn(entity_name); } } diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 0fdcba33ae4..6a977abc138 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -42,17 +42,17 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode if (api_.getCurrentTime() >= 60) { stop(cpp_mock_scenarios::Result::SUCCESS); } - if (api_.getCurrentTime() >= 4 && api_.entityExists("tom")) { + if (api_.getCurrentTime() >= 4 && api_.isEntitySpawned("tom")) { api_.despawn("tom"); } - if (api_.getCurrentTime() >= 4 && api_.entityExists("obstacle")) { + if (api_.getCurrentTime() >= 4 && api_.isEntitySpawned("obstacle")) { api_.setEntityStatus( "obstacle", traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), traffic_simulator::helper::constructActionStatus(10)); } - if (api_.getCurrentTime() >= 6 && api_.entityExists("obstacle")) { + if (api_.getCurrentTime() >= 6 && api_.isEntitySpawned("obstacle")) { api_.despawn("obstacle"); } const auto ego_entity = api_.getEntity("ego"); @@ -64,7 +64,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode ego_entity->requestAcquirePosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 35026, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.entityExists("npc2")) { + if (api_.isEntitySpawned("npc2")) { npc2_entity->requestSpeedChange(13, true); } } @@ -75,7 +75,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode ego_entity->requestAcquirePosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34675, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.entityExists("npc2")) { + if (api_.isEntitySpawned("npc2")) { npc2_entity->requestSpeedChange(3, true); } } @@ -88,7 +88,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode 34630, 0.0, 0.0, api_.getHdmapUtils())); npc2_entity->requestSpeedChange(13, true); } - if (api_.getCurrentTime() > 10.0 && api_.entityExists("bob")) { + if (api_.getCurrentTime() > 10.0 && api_.isEntitySpawned("bob")) { api_.despawn("bob"); } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index f4ab010d4e4..3b3c72f7535 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -70,7 +70,8 @@ class API entity_manager_ptr_( std::make_shared(node, configuration, node_parameters_)), traffic_controller_ptr_(std::make_shared( - entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, + entity_manager_ptr_->getHdmapUtils(), + [this]() { return entity_manager_ptr_->getEntityNames(); }, [this](const auto & entity_name) { return getEntity(entity_name)->getMapPose(); }, [this](const auto & name) { return API::despawn(name); }, configuration.auto_sink)), clock_pub_(rclcpp::create_publisher( @@ -138,7 +139,7 @@ class API { auto register_to_entity_manager = [&]() { if (behavior == VehicleBehavior::autoware()) { - return entity_manager_ptr_->entityExists(name) or + return entity_manager_ptr_->isEntitySpawned(name) or entity_manager_ptr_->spawnEntity( name, pose, parameters, getCurrentTime(), configuration, node_parameters_); } else { @@ -233,6 +234,9 @@ class API bool despawn(const std::string & name); bool despawnEntities(); + bool checkCollision( + const std::string & first_entity_name, const std::string & second_entity_name); + auto setEntityStatus(const std::string & name, const EntityStatus & status) -> void; auto respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, @@ -327,8 +331,7 @@ class API // clang-format on FORWARD_TO_ENTITY_MANAGER(asFieldOperatorApplication); - FORWARD_TO_ENTITY_MANAGER(checkCollision); - FORWARD_TO_ENTITY_MANAGER(entityExists); + FORWARD_TO_ENTITY_MANAGER(isEntitySpawned); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); FORWARD_TO_ENTITY_MANAGER(getEgoName); 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 64ad4fda34f..c2a156ca5d6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -265,12 +265,9 @@ class EntityManager void broadcastTransform( const geometry_msgs::msg::PoseStamped & pose, const bool static_transform = true); - bool checkCollision( - const std::string & first_entity_name, const std::string & second_entity_name); - bool despawnEntity(const std::string & name); - bool entityExists(const std::string & name); + bool isEntitySpawned(const std::string & name); auto getEntityNames() const -> const std::vector; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index fca26390703..530d21a3380 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -46,7 +47,7 @@ bool API::despawn(const std::string & name) bool API::despawnEntities() { - auto entities = getEntityNames(); + auto entities = entity_manager_ptr_->getEntityNames(); return std::all_of( entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); }); } @@ -100,6 +101,21 @@ auto API::respawn( } } +bool API::checkCollision( + const std::string & first_entity_name, const std::string & second_entity_name) +{ + if (first_entity_name != second_entity_name) { + if (const auto first_entity = getEntityOrNullptr(first_entity_name)) { + if (const auto second_entity = getEntityOrNullptr(second_entity_name)) { + return math::geometry::checkCollision2D( + first_entity->getMapPose(), first_entity->getBoundingBox(), second_entity->getMapPose(), + second_entity->getBoundingBox()); + } + } + } + return false; +} + auto API::setEntityStatus( const std::string & name, const std::optional & canonicalized_lanelet_pose, diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 473395d2c52..b50ef31f58a 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -100,22 +99,6 @@ void EntityManager::broadcastTransform( } } -/// @todo it probably should be moved to SimulatorCore -bool EntityManager::checkCollision( - const std::string & first_entity_name, const std::string & second_entity_name) -{ - if (first_entity_name != second_entity_name) { - if (const auto first_entity = getEntityOrNullptr(first_entity_name)) { - if (const auto second_entity = getEntityOrNullptr(second_entity_name)) { - return math::geometry::checkCollision2D( - first_entity->getMapPose(), first_entity->getBoundingBox(), second_entity->getMapPose(), - second_entity->getBoundingBox()); - } - } - } - return false; -} - visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const { visualization_msgs::msg::MarkerArray marker; @@ -127,10 +110,10 @@ visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const bool EntityManager::despawnEntity(const std::string & name) { - return entityExists(name) && entities_.erase(name); + return isEntitySpawned(name) && entities_.erase(name); } -bool EntityManager::entityExists(const std::string & name) +bool EntityManager::isEntitySpawned(const std::string & name) { return entities_.find(name) != std::end(entities_); } diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index bda2e217ba9..8f923751b87 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -168,7 +168,7 @@ class TestExecutor } for (const auto & npc : test_description_.npcs_descriptions) { - if (api_->entityExists(npc.name) && api_->checkCollision(ego_name_, npc.name)) { + if (api_->isEntitySpawned(npc.name) && api_->checkCollision(ego_name_, npc.name)) { if (ego_collision_metric_.isThereEgosCollisionWith(npc.name, current_time)) { std::string message = fmt::format("New collision occurred between ego and {}", npc.name); diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 15bdcdbab7f..9654416aa32 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -76,7 +76,7 @@ class MockTrafficSimulatorAPI MOCK_METHOD(std::string, getEgoName, (), ()); MOCK_METHOD(double, getCurrentTime, (), ()); MOCK_METHOD(void, getEntityMock, (const std::string &), ()); - MOCK_METHOD(bool, entityExists, (const std::string &), ()); + MOCK_METHOD(bool, isEntitySpawned, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); ::testing::StrictMock & asFieldOperatorApplication( From d287360375c0d185713fd69dda92b6eef80a1133 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 28 Jun 2024 13:17:43 +0200 Subject: [PATCH 26/49] feat(ego_entity, sumulator_core): remove asFieldOperatorApplication, develop getEgoEntity and dedicated methods in EgoEntity --- .../concealer/field_operator_application.hpp | 8 +-- ...ator_application_for_autoware_universe.hpp | 8 +-- ...ator_application_for_autoware_universe.cpp | 10 ++-- .../src/cpp_scenario_node.cpp | 10 ++-- .../simulator_core.hpp | 56 ++++++++++++++----- .../src/openscenario_interpreter.cpp | 6 +- .../syntax/user_defined_value_condition.cpp | 20 ++----- .../include/traffic_simulator/api/api.hpp | 2 +- .../traffic_simulator/entity/ego_entity.hpp | 21 ++++++- .../traffic_simulator/entity/entity_base.hpp | 2 - .../entity/entity_manager.hpp | 44 ++++++++------- simulation/traffic_simulator/src/api/api.cpp | 24 ++++---- .../src/entity/ego_entity.cpp | 41 +++++++++++++- .../src/entity/entity_base.cpp | 8 --- .../src/entity/test_misc_object_entity.cpp | 8 --- .../random_test_runner/test_executor.hpp | 26 +++++---- .../test/test_test_executor.cpp | 24 ++++---- 17 files changed, 187 insertions(+), 131 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 75b1cd0a62b..e56d59a8bc2 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -129,13 +129,13 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto clearRoute() -> void = 0; - virtual auto getAutowareStateName() const -> std::string = 0; + virtual auto getAutowareStateName() const -> const std::string & = 0; - virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0; + virtual auto getMinimumRiskManeuverBehaviorName() const -> const std::string & = 0; - virtual auto getMinimumRiskManeuverStateName() const -> std::string = 0; + virtual auto getMinimumRiskManeuverStateName() const -> const std::string & = 0; - virtual auto getEmergencyStateName() const -> std::string = 0; + virtual auto getEmergencyStateName() const -> const std::string & = 0; virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index e9d14665928..cffa663f1a7 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -145,18 +145,18 @@ class FieldOperatorApplicationFor auto engaged() const -> bool override; - auto getAutowareStateName() const -> std::string override; + auto getAutowareStateName() const -> const std::string & override; auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; auto getTurnIndicatorsCommand() const -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand override; - auto getEmergencyStateName() const -> std::string override; + auto getEmergencyStateName() const -> const std::string & override; - auto getMinimumRiskManeuverBehaviorName() const -> std::string override; + auto getMinimumRiskManeuverBehaviorName() const -> const std::string & override; - auto getMinimumRiskManeuverStateName() const -> std::string override; + auto getMinimumRiskManeuverStateName() const -> const std::string & override; auto initialize(const geometry_msgs::msg::Pose &) -> void override; diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index b86d4b3a657..c66d9e1aab1 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -407,7 +407,8 @@ auto FieldOperatorApplicationFor::restrictTargetSpeed(double v return value; } -auto FieldOperatorApplicationFor::getAutowareStateName() const -> std::string +auto FieldOperatorApplicationFor::getAutowareStateName() const + -> const std::string & { #define IF(IDENTIFIER, RETURN) \ if (getAutowareState().state == tier4_system_msgs::msg::AutowareState::IDENTIFIER) { \ @@ -427,19 +428,20 @@ auto FieldOperatorApplicationFor::getAutowareStateName() const #undef IF } -auto FieldOperatorApplicationFor::getEmergencyStateName() const -> std::string +auto FieldOperatorApplicationFor::getEmergencyStateName() const + -> const std::string & { return minimum_risk_maneuver_state; } auto FieldOperatorApplicationFor::getMinimumRiskManeuverBehaviorName() const - -> std::string + -> const std::string & { return minimum_risk_maneuver_behavior; } auto FieldOperatorApplicationFor::getMinimumRiskManeuverStateName() const - -> std::string + -> const std::string & { return minimum_risk_maneuver_state; } diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index a3f93334873..1add79a3cb6 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -100,7 +100,8 @@ void CppScenarioNode::spawnEgoEntity( api_.updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); api_.spawn("ego", spawn_lanelet_pose, parameters, traffic_simulator::VehicleBehavior::autoware()); - api_.asFieldOperatorApplication("ego").declare_parameter("allow_goal_modification", true); + auto ego_entity = api_.getEgoEntity("ego"); + ego_entity->setParameter("allow_goal_modification", true); api_.attachLidarSensor("ego", 0.0); api_.attachDetectionSensor("ego", 200.0, true, 0.0, 0, 0.0, 0.0); @@ -119,13 +120,12 @@ void CppScenarioNode::spawnEgoEntity( // clang-format on return configuration; }()); - auto ego_entity = api_.getEntity("ego"); ego_entity->requestAssignRoute(goal_lanelet_poses); using namespace std::chrono_literals; - while (!api_.asFieldOperatorApplication("ego").engaged()) { - if (api_.asFieldOperatorApplication("ego").engageable()) { - api_.asFieldOperatorApplication("ego").engage(); + while (!ego_entity->isEngaged()) { + if (ego_entity->isEngageable()) { + ego_entity->engage(); } api_.updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index a9ae33a01f0..a2ccf94e25b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -336,6 +336,7 @@ class SimulatorCore }()); if (controller.isAutoware()) { + auto ego_entity = core->getEgoEntity(entity_ref); core->attachLidarSensor( entity_ref, controller.properties.template get("pointcloudPublishingDelay")); @@ -378,10 +379,9 @@ class SimulatorCore return configuration; }()); - core->asFieldOperatorApplication(entity_ref) - .declare_parameter( - "allow_goal_modification", - controller.properties.template get("allowGoalModification")); + ego_entity->setParameter( + "allow_goal_modification", + controller.properties.template get("allowGoalModification")); for (const auto & module : [](std::string manual_modules_string) { @@ -401,8 +401,7 @@ class SimulatorCore }(controller.properties.template get( "featureIdentifiersRequiringExternalPermissionForAutonomousDecisions"))) { try { - core->asFieldOperatorApplication(entity_ref) - .requestAutoModeForCooperation(module, false); + ego_entity->requestAutoModeForCooperation(module, false); } catch (const Error & error) { throw Error( "featureIdentifiersRequiringExternalPermissionForAutonomousDecisions is not " @@ -541,12 +540,6 @@ class SimulatorCore properties.template get("maxJerk", Double::max())); } - template - static auto asFieldOperatorApplication(Ts &&... xs) -> decltype(auto) - { - return core->asFieldOperatorApplication(std::forward(xs)...); - } - static auto activateNonUserDefinedControllers() -> decltype(auto) { return core->startNpcLogic(); @@ -611,11 +604,46 @@ class SimulatorCore return core->resetV2ITrafficLightPublishRate(std::forward(xs)...); } + static auto engage(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->engage(); + } + + static auto isEngageable(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->isEngageable(); + } + + static auto isEngaged(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->isEngaged(); + } + template static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto) { - return asFieldOperatorApplication(core->getEgoName()) - .sendCooperateCommand(std::forward(xs)...); + /// @note here ego name is not passed from OpenScenarioInterpreter, it uses first found + return core->getEgoEntity()->sendCooperateCommand(std::forward(xs)...); + } + + static auto getMinimumRiskManeuverBehaviorName(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->getMinimumRiskManeuverBehaviorName(); + } + + static auto getMinimumRiskManeuverStateName(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->getMinimumRiskManeuverStateName(); + } + + static auto getEmergencyStateName(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->getEmergencyStateName(); + } + + static auto getTurnIndicatorsCommandName(const std::string & ego_ref) -> decltype(auto) + { + return core->getEgoEntity(ego_ref)->getTurnIndicatorsCommandName(); } template diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index dadfb59233c..350720bcac4 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -138,7 +138,7 @@ auto Interpreter::engage() const -> void if ( scenario_object.template as().is_added and scenario_object.template as().object_controller.isAutoware()) { - asFieldOperatorApplication(name).engage(); + NonStandardOperation::engage(name); } } } @@ -151,7 +151,7 @@ auto Interpreter::engageable() const -> bool const auto & [name, scenario_object] = each; return not scenario_object.template as().is_added or not scenario_object.template as().object_controller.isAutoware() or - asFieldOperatorApplication(name).engageable(); + NonStandardOperation::isEngageable(name); }); } @@ -163,7 +163,7 @@ auto Interpreter::engaged() const -> bool const auto & [name, scenario_object] = each; return not scenario_object.template as().is_added or not scenario_object.template as().object_controller.isAutoware() or - asFieldOperatorApplication(name).engaged(); + NonStandardOperation::isEngaged(name); }); } diff --git a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp index fd99947d906..480d04a2784 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -86,27 +85,16 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node "currentState", [result]() { return make(evaluateCurrentState(result.str(1))); }), std::make_pair( "currentMinimumRiskManeuverState.behavior", - [result]() { - return make( - asFieldOperatorApplication(result.str(1)).getMinimumRiskManeuverBehaviorName()); - }), + [result]() { return make(getMinimumRiskManeuverBehaviorName(result.str(1))); }), std::make_pair( "currentMinimumRiskManeuverState.state", - [result]() { - return make( - asFieldOperatorApplication(result.str(1)).getMinimumRiskManeuverStateName()); - }), + [result]() { return make(getMinimumRiskManeuverStateName(result.str(1))); }), std::make_pair( "currentEmergencyState", - [result]() { - return make(asFieldOperatorApplication(result.str(1)).getEmergencyStateName()); - }), + [result]() { return make(getEmergencyStateName(result.str(1))); }), std::make_pair( "currentTurnIndicatorsState", - [result]() { - return make(boost::lexical_cast( - asFieldOperatorApplication(result.str(1)).getTurnIndicatorsCommand())); - }), + [result]() { return make(getTurnIndicatorsCommandName(result.str(1))); }), }; evaluate_value = dispatch.at(result.str(2)); // XXX catch } else if (std::regex_match(name, result, FunctionCallExpression::pattern())) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 3b3c72f7535..76b1ede4208 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -330,10 +330,10 @@ class API static_assert(true, "") // clang-format on - FORWARD_TO_ENTITY_MANAGER(asFieldOperatorApplication); FORWARD_TO_ENTITY_MANAGER(isEntitySpawned); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); + FORWARD_TO_ENTITY_MANAGER(getEgoEntity); FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityNames); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index afa347dd605..02db9b3bad2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -65,8 +65,6 @@ class EgoEntity : public VehicleEntity auto operator=(const EgoEntity &) -> EgoEntity & = delete; - auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & override; - auto getCurrentAction() const -> std::string override; auto getCurrentPose() const -> geometry_msgs::msg::Pose; @@ -139,6 +137,25 @@ class EgoEntity : public VehicleEntity auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void; auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; + + template + auto setParameter(const std::string & name, const ParameterType & default_value = {}) const + -> ParameterType + { + return field_operator_application->template declare_parameter( + name, default_value); + } + + auto engage() -> void; + auto isEngaged() const -> bool; + auto isEngageable() const -> bool; + auto replanRoute(const std::vector & route) -> void; + auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void; + auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void; + auto getMinimumRiskManeuverBehaviorName() const -> const std::string &; + auto getMinimumRiskManeuverStateName() const -> const std::string &; + auto getEmergencyStateName() const -> const std::string &; + auto getTurnIndicatorsCommandName() const -> const std::string; }; } // namespace entity } // namespace traffic_simulator 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 b2331be03a5..fee71963c78 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -63,8 +63,6 @@ class EntityBase virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray &); - virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &; - virtual void cancelRequest(); // clang-format off 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 c2a156ca5d6..5be7121c04f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -232,27 +232,6 @@ class EntityManager } } - // clang-format off -#define FORWARD_TO_ENTITY(IDENTIFIER, ...) \ - /*! \ - @brief Forward to arguments to the EntityBase::IDENTIFIER function. \ - @return return value of the EntityBase::IDENTIFIER function. \ - @note This function was defined by FORWARD_TO_ENTITY macro. 　 　　　　　 \ - */ \ - template \ - decltype(auto) IDENTIFIER(const std::string & name, Ts &&... xs) __VA_ARGS__ \ - try { \ - return entities_.at(name)->IDENTIFIER(std::forward(xs)...); \ - } catch (const std::out_of_range &) { \ - THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); \ - } \ - static_assert(true, "") - // clang-format on - - FORWARD_TO_ENTITY(asFieldOperatorApplication, const); - -#undef FORWARD_TO_ENTITY - visualization_msgs::msg::MarkerArray makeDebugMarker() const; bool trafficLightsChanged(); @@ -277,6 +256,29 @@ class EntityManager auto getEntity(const std::string & name) const -> std::shared_ptr; + auto getEgoEntity() const -> std::shared_ptr + { + for (const auto & each : entities_) { + if (each.second->template is()) { + return std::dynamic_pointer_cast(each.second); + } + } + THROW_SEMANTIC_ERROR("getEgoEntity function was called, but ego vehicle does not exist"); + } + + auto getEgoEntity(const std::string & name) const + -> std::shared_ptr + { + if (auto it = entities_.find(name); it == entities_.end()) { + THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); + } else { + if (auto ego_entity = std::dynamic_pointer_cast(it->second); !ego_entity) { + THROW_SEMANTIC_ERROR("entity : ", name, " exists, but it is not ego"); + } else + return ego_entity; + } + } + auto getHdmapUtils() -> const std::shared_ptr &; auto getNumberOfEgo() const -> std::size_t; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 530d21a3380..117d1153a7c 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -56,13 +56,12 @@ auto API::respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) -> void { - if (auto entity = entity_manager_ptr_->getEntity(name); not entity->is()) { - throw std::runtime_error("Respawn of any entities other than EGO is not supported."); - } else if (new_pose.header.frame_id != "map") { + if (new_pose.header.frame_id != "map") { throw std::runtime_error("Respawn request with frame id other than map not supported."); } else { + auto ego_entity = entity_manager_ptr_->getEgoEntity(name); // set new pose and default action status in EntityManager - entity->setControlledBySimulator(true); + ego_entity->setControlledBySimulator(true); setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); // read status from EntityManager, then send it to SimpleSensorSimulator @@ -71,8 +70,8 @@ auto API::respawn( static_cast(entity_manager_ptr_->getEntity(name)->getStatus()), *req.add_status()); req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); - req.set_overwrite_ego_status(entity->isControlledBySimulator()); - entity->setControlledBySimulator(false); + req.set_overwrite_ego_status(ego_entity->isControlledBySimulator()); + ego_entity->setControlledBySimulator(false); // check response if (const auto res = zeromq_client_.call(req); not res.result().success()) { @@ -87,16 +86,13 @@ auto API::respawn( res_name + "\"."); } else { // if valid, set response in EntityManager, then plan path and engage - auto entity_status = static_cast(entity->getStatus()); + auto entity_status = static_cast(ego_entity->getStatus()); simulation_interface::toMsg(res_status->pose(), entity_status.pose); simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); - entity->setMapPose(entity_status.pose); - entity->setTwist(entity_status.action_status.twist); - entity->setAcceleration(entity_status.action_status.accel); - - entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); - entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose}); - entity_manager_ptr_->asFieldOperatorApplication(name).engage(); + ego_entity->setMapPose(entity_status.pose); + ego_entity->setTwist(entity_status.action_status.twist); + ego_entity->setAcceleration(entity_status.action_status.accel); + ego_entity->replanRoute({goal_pose}); } } } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 00ae5de4ee7..1170fae5217 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -77,10 +77,45 @@ EgoEntity::EgoEntity( { } -auto EgoEntity::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & +auto EgoEntity::engage() -> void { field_operator_application->engage(); } + +auto EgoEntity::isEngaged() const -> bool { return field_operator_application->engaged(); } + +auto EgoEntity::isEngageable() const -> bool { return field_operator_application->engageable(); } + +auto EgoEntity::replanRoute(const std::vector & route) -> void +{ + field_operator_application->clearRoute(); + field_operator_application->plan(route); + field_operator_application->engage(); +} + +auto EgoEntity::sendCooperateCommand(const std::string & module_name, const std::string & command) + -> void +{ + field_operator_application->sendCooperateCommand(module_name, command); +} + +auto EgoEntity::requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void +{ + field_operator_application->requestAutoModeForCooperation(module_name, enable); +} + +auto EgoEntity::getMinimumRiskManeuverBehaviorName() const -> const std::string & +{ + return field_operator_application->getMinimumRiskManeuverBehaviorName(); +} +auto EgoEntity::getMinimumRiskManeuverStateName() const -> const std::string & +{ + return field_operator_application->getMinimumRiskManeuverStateName(); +} +auto EgoEntity::getEmergencyStateName() const -> const std::string & +{ + return field_operator_application->getEmergencyStateName(); +} +auto EgoEntity::getTurnIndicatorsCommandName() const -> const std::string { - assert(field_operator_application); - return *field_operator_application; + return boost::lexical_cast(field_operator_application->getTurnIndicatorsCommand()); } auto EgoEntity::getCurrentAction() const -> std::string diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 0221a5c393b..2e7e48553c4 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -48,14 +48,6 @@ EntityBase::EntityBase( void EntityBase::appendDebugMarker(visualization_msgs::msg::MarkerArray &) {} -auto EntityBase::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication & -{ - throw common::Error( - "An operation was requested for Entity ", std::quoted(name), - " that is valid only for the entity controlled by Autoware, but ", std::quoted(name), - " is not the entity controlled by Autoware."); -} - void EntityBase::cancelRequest() {} auto EntityBase::get2DPolygon() const -> std::vector diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 4c0fc3d2b5f..ce82dc4d5f1 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -266,14 +266,6 @@ TEST_F(MiscObjectEntityTest_FullObject, appendDebugMarker) } } -/** - * @note Test basic functionality; test whether the function throws an error. - */ -TEST_F(MiscObjectEntityTest_FullObject, asFieldOperatorApplication) -{ - EXPECT_THROW(misc_object.asFieldOperatorApplication(), common::Error); -} - /** * @note Test functionality used by other units; test correctness of 2d polygon calculations. */ diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 8f923751b87..accefe9bdd9 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -87,6 +87,8 @@ class TestExecutor ego_name_, test_description_.ego_start_position, traffic_simulator::helper::constructActionStatus()); + auto ego_entity = api_->getEgoEntity(ego_name_); + if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( traffic_simulator::helper::LidarType::VLP16, ego_name_, @@ -110,16 +112,14 @@ class TestExecutor return configuration; }()); - api_->asFieldOperatorApplication(ego_name_).template declare_parameter( - "allow_goal_modification", true); + ego_entity->template setParameter("allow_goal_modification", true); } // XXX dirty hack: wait for autoware system to launch // ugly but helps for now std::this_thread::sleep_for(std::chrono::milliseconds{5000}); - api_->getEntity(ego_name_)->requestAssignRoute( - std::vector({test_description_.ego_goal_pose})); - api_->asFieldOperatorApplication(ego_name_).engage(); + ego_entity->requestAssignRoute(std::vector({test_description_.ego_goal_pose})); + ego_entity->engage(); goal_reached_metric_.setGoal(test_description_.ego_goal_pose); @@ -140,13 +140,15 @@ class TestExecutor auto update() -> void { executeWithErrorHandling([this]() { - if (not api_->isAnyEgoSpawned() and not api_->isNpcLogicStarted()) { - api_->startNpcLogic(); - } - if ( - api_->isAnyEgoSpawned() and not api_->isNpcLogicStarted() and - api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { - api_->startNpcLogic(); + if (not api_->isNpcLogicStarted()) { + if (api_->isAnyEgoSpawned()) { + auto ego_entity = api_->getEgoEntity(api_->getEgoName()); + if (ego_entity->isEngageable()) { + api_->startNpcLogic(); + } + } else { + api_->startNpcLogic(); + } } auto current_time = api_->getCurrentTime(); diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 9654416aa32..d6979c79e15 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -17,6 +17,7 @@ #include #include +#include class MockFieldOperatorApplication { @@ -60,7 +61,6 @@ class MockTrafficSimulatorAPI void, attachDetectionSensor, (const simulation_api_schema::DetectionSensorConfiguration &), ()); MOCK_METHOD( bool, attachOccupancyGridSensor, (simulation_api_schema::OccupancyGridSensorConfiguration), ()); - MOCK_METHOD(void, asFieldOperatorApplicationMock, (const std::string &), ()); MOCK_METHOD( void, requestAssignRoute, (const std::string &, std::vector), ()); MOCK_METHOD( @@ -76,16 +76,10 @@ class MockTrafficSimulatorAPI MOCK_METHOD(std::string, getEgoName, (), ()); MOCK_METHOD(double, getCurrentTime, (), ()); MOCK_METHOD(void, getEntityMock, (const std::string &), ()); + MOCK_METHOD(void, getEgoEntityMock, (const std::string &), ()); MOCK_METHOD(bool, isEntitySpawned, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); - ::testing::StrictMock & asFieldOperatorApplication( - const std::string & name) - { - asFieldOperatorApplicationMock(name); - return *field_operator_application_mock; - } - std::shared_ptr getEntity(const std::string & name) { getEntityMock(name); @@ -95,6 +89,18 @@ class MockTrafficSimulatorAPI getVehicleParameters()); } + std::shared_ptr getEgoEntity(const std::string & name) + { + getEgoEntityMock(name); + /// @note set invalid LaneletPose so pass std::nullopt + return std::make_shared( + "", traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt), nullptr, + getVehicleParameters(), + traffic_simulator::Configuration( + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map"), + nullptr); + } + void setEntityStatusNecessaryValues( double time, geometry_msgs::msg::Pose map_pose, geometry_msgs::msg::Twist twist) { @@ -127,12 +133,10 @@ TEST(TestExecutor, InitializeWithNoNPCs) EXPECT_CALL(*MockAPI, attachLidarSensor).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, attachDetectionSensor).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, attachOccupancyGridSensor).Times(1).InSequence(sequence); - EXPECT_CALL(*MockAPI, asFieldOperatorApplicationMock).Times(1).InSequence(sequence); EXPECT_CALL(*(MockAPI->field_operator_application_mock), declare_parameter_mock) .Times(1) .InSequence(sequence); EXPECT_CALL(*MockAPI, requestAssignRoute).Times(1).InSequence(sequence); - EXPECT_CALL(*MockAPI, asFieldOperatorApplicationMock).Times(1).InSequence(sequence); EXPECT_CALL(*(MockAPI->field_operator_application_mock), engage).Times(1).InSequence(sequence); test_executor.initialize(); From 0fcc813d3eef215d1b4015034c4a052e9bf26a01 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 28 Jun 2024 15:42:01 +0200 Subject: [PATCH 27/49] feat(api, simulator_core): emove getTimeHeadway from API, use directly in calc it directly in evaluateTimeHeadway --- .../simulator_core.hpp | 21 ++++++++++++------- .../include/traffic_simulator/api/api.hpp | 2 -- simulation/traffic_simulator/src/api/api.cpp | 17 --------------- 3 files changed, 14 insertions(+), 26 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index bd4c6525cff..5ad7fe1907e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -512,15 +512,22 @@ class SimulatorCore return core->getEntity(name)->getStandStillDuration(); } - template - static auto evaluateTimeHeadway(Ts &&... xs) + static auto evaluateTimeHeadway( + const std::string & from_entity_name, const std::string & to_entity_name) { - if (const auto result = core->getTimeHeadway(std::forward(xs)...); result) { - return result.value(); - } else { - using value_type = typename std::decay::type::value_type; - return std::numeric_limits::quiet_NaN(); + if (auto from_entity = core->getEntityOrNullptr(from_entity_name); from_entity) { + if (auto to_entity = core->getEntityOrNullptr(to_entity_name); to_entity) { + if (auto relative_pose = traffic_simulator::pose::relativePose( + from_entity->getMapPose(), to_entity->getMapPose()); + relative_pose && relative_pose->position.x <= 0) { + const double time_headway = + (relative_pose->position.x * -1) / to_entity->getCurrentTwist().linear.x; + return std::isnan(time_headway) ? std::numeric_limits::infinity() + : time_headway; + } + } } + return std::numeric_limits::quiet_NaN(); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 671fa590bd9..1e11694d68c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -270,8 +270,6 @@ class API const traffic_simulator_msgs::msg::ActionStatus & action_status = helper::constructActionStatus()) -> void; - std::optional getTimeHeadway(const std::string & from, const std::string & to); - bool attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &); diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 7675b262ad2..54a9ff293d2 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -133,23 +133,6 @@ auto API::setEntityStatus(const std::string & name, const EntityStatus & status) getEntity(name)->setStatus(status); } -/// @todo it probably should be moved to SimulatorCore -std::optional API::getTimeHeadway( - const std::string & from_entity_name, const std::string & to_entity_name) -{ - if (auto from_entity = getEntityOrNullptr(from_entity_name); from_entity) { - if (auto to_entity = getEntityOrNullptr(to_entity_name); to_entity) { - if (auto relative_pose = relativePose(from_entity->getMapPose(), to_entity->getMapPose()); - relative_pose && relative_pose->position.x <= 0) { - const double time_headway = - (relative_pose->position.x * -1) / getEntity(to_entity_name)->getCurrentTwist().linear.x; - return std::isnan(time_headway) ? std::numeric_limits::infinity() : time_headway; - } - } - } - return std::nullopt; -} - auto API::setEntityStatus( const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void From 52b44cac21e903b40d9f50d04e5984669aabe544 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 28 Jun 2024 17:25:06 +0200 Subject: [PATCH 28/49] feat(simulator_core, api, entity_base, cpp_mock): move setEntityStatus to EntityBase, remove from api --- .../acquire_position_in_world_frame.cpp | 5 +- .../src/traffic_simulation_demo.cpp | 21 +++--- .../simulator_core.hpp | 16 ++++- .../include/traffic_simulator/api/api.hpp | 24 ------- .../traffic_simulator/entity/ego_entity.hpp | 16 +++-- .../traffic_simulator/entity/entity_base.hpp | 35 +++++++++- simulation/traffic_simulator/src/api/api.cpp | 68 +------------------ .../src/entity/ego_entity.cpp | 21 ------ .../src/entity/entity_base.cpp | 64 ++++++++++++++++- .../random_test_runner/test_executor.hpp | 9 ++- 10 files changed, 139 insertions(+), 140 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index a14e52cbd31..7901937a7a3 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -51,12 +51,11 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setEntityStatus( - "ego", + auto entity = api_.getEntity("ego"); + entity->setStatus( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), traffic_simulator::helper::constructActionStatus(10)); - auto entity = api_.getEntity("ego"); entity->requestSpeedChange(10, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 431d157dce0..d53b4cd426e 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -46,11 +46,11 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.despawn("tom"); } if (api_.getCurrentTime() >= 4 && api_.isEntitySpawned("obstacle")) { - api_.setEntityStatus( - "obstacle", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructActionStatus(10)); + api_.getEntity("obstacle") + ->setStatus( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); } if (api_.getCurrentTime() >= 6 && api_.isEntitySpawned("obstacle")) { api_.despawn("obstacle"); @@ -114,8 +114,8 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( "tom", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), getPedestrianParameters()); - api_.setEntityStatus( - "tom", "ego", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), + api_.getEntity("tom")->setStatus( + ego_entity->getMapPose(), traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), traffic_simulator::helper::constructActionStatus()); auto tom_entity = api_.getEntity("tom"); tom_entity->requestWalkStraight(); @@ -162,9 +162,10 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( "obstacle", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), getMiscObjectParameters()); - api_.setEntityStatus( - "obstacle", "ego", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), - traffic_simulator::helper::constructActionStatus()); + api_.getEntity("obstacle") + ->setStatus( + ego_entity->getMapPose(), traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), + traffic_simulator::helper::constructActionStatus()); api_.getConventionalTrafficLights()->setTrafficLightsColor( 34802, traffic_simulator::TrafficLight::Color::green); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 5ad7fe1907e..44cdfa52b85 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -447,9 +447,21 @@ class SimulatorCore } template - static auto applyTeleportAction(Ts &&... xs) + static auto applyTeleportAction(const std::string & name, Ts &&... xs) { - return core->setEntityStatus(std::forward(xs)...); + return core->getEntity(name)->setStatus(std::forward(xs)...); + } + + template + static auto applyTeleportAction( + const std::string & name, const std::string & reference_entity_name, Ts &&... xs) + { + if (const auto reference_entity = core->getEntityOrNullptr(reference_entity_name)) { + return core->getEntity(name)->setStatus( + reference_entity->getMapPose(), std::forward(xs)...); + } else { + throw Error("Reference entity \"", reference_entity_name, "\" does not exist"); + } } template diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1e11694d68c..0169f488f04 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -242,33 +242,9 @@ class API bool checkCollision( const std::string & first_entity_name, const std::string & second_entity_name); - auto setEntityStatus(const std::string & name, const EntityStatus & status) -> void; auto respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) -> void; - auto setEntityStatus( - const std::string & name, const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status = - helper::constructActionStatus()) -> void; - auto setEntityStatus( - const std::string & name, const LaneletPose & lanelet_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void; - auto setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status = - helper::constructActionStatus()) -> void; - auto setEntityStatus( - const std::string & name, const std::string & reference_entity_name, - const geometry_msgs::msg::Pose & relative_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status = - helper::constructActionStatus()) -> void; - auto setEntityStatus( - const std::string & name, const std::string & reference_entity_name, - const geometry_msgs::msg::Point & relative_position, - const geometry_msgs::msg::Vector3 & relative_rpy, - const traffic_simulator_msgs::msg::ActionStatus & action_status = - helper::constructActionStatus()) -> void; bool attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 02db9b3bad2..018e3da8cbb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -132,10 +132,6 @@ class EgoEntity : public VehicleEntity auto setVelocityLimit(double) -> void override; - void setStatus(const EntityStatus & status) override; - - auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void; - auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; template @@ -146,6 +142,18 @@ class EgoEntity : public VehicleEntity name, default_value); } + template + auto setStatus(Ts &&... xs) -> void + { + if (status_.getTime() > 0 && not isControlledBySimulator()) { + THROW_SEMANTIC_ERROR( + "You cannot set entity status to the ego vehicle named ", std::quoted(status_.getName()), + " after starting scenario."); + } else { + EntityBase::setStatus(std::forward(xs)...); + } + } + auto engage() -> void; auto isEngaged() const -> bool; auto isEngageable() const -> bool; 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 26255ecaca0..3d4615ca101 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -196,9 +196,38 @@ class EntityBase /* */ void setOtherStatus(const std::unordered_map &); - virtual void setStatus(const EntityStatus & status); - - virtual auto setCanonicalizedStatus(const CanonicalizedEntityStatus &) -> void; + /* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus &) -> void; + + virtual auto setStatus(const EntityStatus & status) -> void; + + virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void; + + virtual auto setStatus( + const geometry_msgs::msg::Pose & map_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status = + helper::constructActionStatus()) -> void; + + virtual auto setStatus( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & relative_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status = + helper::constructActionStatus()) -> void; + + virtual auto setStatus( + const geometry_msgs::msg::Pose & reference_pose, + const geometry_msgs::msg::Point & relative_position, + const geometry_msgs::msg::Vector3 & relative_rpy, + const traffic_simulator_msgs::msg::ActionStatus & action_status = + helper::constructActionStatus()) -> void; + + virtual auto setStatus( + const LaneletPose & lanelet_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status = + helper::constructActionStatus()) -> void; + + virtual auto setStatus( + const std::optional & canonicalized_lanelet_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status = + helper::constructActionStatus()) -> void; virtual auto setLinearAcceleration(const double linear_acceleration) -> void; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 54a9ff293d2..b39dc98a8b4 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -62,7 +62,7 @@ auto API::respawn( auto ego_entity = entity_manager_ptr_->getEgoEntity(name); // set new pose and default action status in EntityManager ego_entity->setControlledBySimulator(true); - setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); + ego_entity->setStatus(new_pose.pose.pose, helper::constructActionStatus()); // read status from EntityManager, then send it to SimpleSensorSimulator simulation_api_schema::UpdateEntityStatusRequest req; @@ -112,70 +112,6 @@ bool API::checkCollision( return false; } -auto API::setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void -{ - const auto entity = getEntity(name); - auto status = static_cast(entity->getStatus()); - status.action_status = action_status; - if (canonicalized_lanelet_pose) { - status.pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose_valid = true; - } - entity->setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); -} - -auto API::setEntityStatus(const std::string & name, const EntityStatus & status) -> void -{ - getEntity(name)->setStatus(status); -} - -auto API::setEntityStatus( - const std::string & name, const LaneletPose & lanelet_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void -{ - setEntityStatus( - name, canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils()), action_status); -} - -auto API::setEntityStatus( - const std::string & name, const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void -{ - const auto entity = getEntity(name); - EntityStatus status = static_cast(entity->getStatus()); - status.pose = map_pose; - status.action_status = action_status; - setEntityStatus(name, status); -} - -auto API::setEntityStatus( - const std::string & name, const std::string & reference_entity_name, - const geometry_msgs::msg::Pose & relative_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void -{ - const auto reference_entity = getEntity(reference_entity_name); - setEntityStatus( - name, transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), - action_status); -} - -auto API::setEntityStatus( - const std::string & name, const std::string & reference_entity_name, - const geometry_msgs::msg::Point & relative_position, - const geometry_msgs::msg::Vector3 & relative_rpy, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void -{ - const auto relative_pose = - geometry_msgs::build() - .position(relative_position) - .orientation(math::geometry::convertEulerAngleToQuaternion(relative_rpy)); - setEntityStatus(name, reference_entity_name, relative_pose, action_status); -} - bool API::attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration) { @@ -298,7 +234,7 @@ bool API::updateEntitiesStatusInSim() entity->setTwist(entity_status.action_status.twist); entity->setAcceleration(entity_status.action_status.accel); } else { - setEntityStatus(entity->getName(), entity_status); + entity->setStatus(entity_status); } } return true; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 1170fae5217..3575594dc7e 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -341,26 +341,5 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void setCanonicalizedStatus( CanonicalizedEntityStatus(static_cast(status_), canonicalized_lanelet_pose)); } - -void EgoEntity::setStatus(const EntityStatus & status) -{ - THROW_SEMANTIC_ERROR( - "You cannot set entity status to the ego vehicle named ", std::quoted(status.name), - " without specifing lanelets."); -} - -auto EgoEntity::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void -{ - if (status_.getTime() > 0 && not isControlledBySimulator()) { - THROW_SEMANTIC_ERROR( - "You cannot set entity status to the ego vehicle named ", std::quoted(status.name), - " after starting scenario."); - } else { - const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - status.pose, status.bounding_box, lanelet_ids, false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } -} } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 14a859ed5ac..7cbc9f1fedc 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -555,14 +556,73 @@ void EntityBase::setOtherStatus( other_status_.erase(name); } +auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void +{ + status_.set(status); +} + void EntityBase::setStatus(const EntityStatus & status) { status_.set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } -auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void +auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - status_.set(status); + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + status.pose, status.bounding_box, lanelet_ids, false, + getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); +} + +auto EntityBase::setStatus( + const geometry_msgs::msg::Pose & map_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void +{ + EntityStatus status = static_cast(getStatus()); + status.pose = map_pose; + status.action_status = action_status; + setStatus(status); +} + +auto EntityBase::setStatus( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & relative_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void +{ + setStatus(pose::transformRelativePoseToGlobal(reference_pose, relative_pose), action_status); +} + +auto EntityBase::setStatus( + const geometry_msgs::msg::Pose & reference_pose, + const geometry_msgs::msg::Point & relative_position, + const geometry_msgs::msg::Vector3 & relative_rpy, + const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void +{ + const auto relative_pose = + geometry_msgs::build() + .position(relative_position) + .orientation(math::geometry::convertEulerAngleToQuaternion(relative_rpy)); + setStatus(reference_pose, relative_pose, action_status); +} + +auto EntityBase::setStatus( + const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) + -> void +{ + setStatus(pose::canonicalize(lanelet_pose, hdmap_utils_ptr_), action_status); +} + +auto EntityBase::setStatus( + const std::optional & canonicalized_lanelet_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void +{ + auto status = static_cast(getStatus()); + status.action_status = action_status; + if (canonicalized_lanelet_pose) { + status.pose = static_cast(canonicalized_lanelet_pose.value()); + status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); + status.lanelet_pose_valid = true; + } + setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } auto EntityBase::setLinearVelocity(const double linear_velocity) -> void diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index accefe9bdd9..d93af7662cc 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -83,11 +83,10 @@ class TestExecutor api_->spawn( ego_name_, test_description_.ego_start_position, getVehicleParameters(), traffic_simulator::VehicleBehavior::autoware(), "lexus_rx450h"); - api_->setEntityStatus( - ego_name_, test_description_.ego_start_position, - traffic_simulator::helper::constructActionStatus()); auto ego_entity = api_->getEgoEntity(ego_name_); + ego_entity->setStatus( + test_description_.ego_start_position, traffic_simulator::helper::constructActionStatus()); if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( @@ -129,8 +128,8 @@ class TestExecutor npc_descr.name, npc_descr.start_position, getVehicleParameters(), traffic_simulator::VehicleBehavior::defaultBehavior(), "taxi"); auto entity = api_->getEntity(npc_descr.name); - api_->setEntityStatus( - npc_descr.name, npc_descr.start_position, + entity->setStatus( + npc_descr.start_position, traffic_simulator::helper::constructActionStatus(npc_descr.speed)); entity->requestSpeedChange(npc_descr.speed, true); } From 8b478db14d89b0559fea240d7abf613ea92494fe Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Fri, 28 Jun 2024 16:09:49 +0200 Subject: [PATCH 29/49] Fix mock scenario with invalid entity resetBehaviorPlugin respawns entity so the pointer stored from before the action is invalid and new one should be obtained Signed-off-by: Mateusz Palczuk --- .../behavior_plugin/load_do_nothing_plugin.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index 11513fe587d..2fb23334578 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -40,21 +40,15 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { /// @note When using the do_nothing plugin, the return value of the `getCurrentAction` function is always do_nothing. - auto const ego_entity = api_.getEntity("ego"); - auto const pedestrian_entity = api_.getEntity("pedestrian"); - auto const vehicle_spawn_with_behavior_tree_entity = - api_.getEntity("vehicle_spawn_with_behavior_tree"); - auto const pedestrian_spawn_with_behavior_tree_entity = - api_.getEntity("pedestrian_spawn_with_behavior_tree"); if ( - ego_entity->getCurrentAction() != "do_nothing" || - pedestrian_entity->getCurrentAction() != "do_nothing") { + api_.getEntity("ego")->getCurrentAction() != "do_nothing" || + api_.getEntity("pedestrian")->getCurrentAction() != "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } if ( - vehicle_spawn_with_behavior_tree_entity->getCurrentAction() == "do_nothing" || - pedestrian_spawn_with_behavior_tree_entity->getCurrentAction() == "do_nothing") { + api_.getEntity("vehicle_spawn_with_behavior_tree")->getCurrentAction() == "do_nothing" || + api_.getEntity("pedestrian_spawn_with_behavior_tree")->getCurrentAction() == "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } api_.resetBehaviorPlugin( @@ -64,8 +58,8 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode "pedestrian_spawn_with_behavior_tree", traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing()); if ( - vehicle_spawn_with_behavior_tree_entity->getCurrentAction() != "do_nothing" || - pedestrian_spawn_with_behavior_tree_entity->getCurrentAction() != "do_nothing") { + api_.getEntity("vehicle_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing" || + api_.getEntity("pedestrian_spawn_with_behavior_tree")->getCurrentAction() != "do_nothing") { stop(cpp_mock_scenarios::Result::FAILURE); } From 56ca20704ac959c0cf20f75d0ba3e1fbd9d6d4c3 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Fri, 28 Jun 2024 16:10:44 +0200 Subject: [PATCH 30/49] Restore previous scenario condition Signed-off-by: Mateusz Palczuk --- .../follow_polyline_trajectory_with_do_nothing_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 375fe1ab65f..4e5745df937 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -69,7 +69,7 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP - if (equals(api_.getCurrentTime(), 0.0, 0.01) && entity->isInPosition(spawn_pose, 0.1)) { + if (equals(api_.getCurrentTime(), 0.0, 0.01) && !entity->isInPosition(spawn_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( From 8cf8c3a7250e4652a7cf59645b1da23e170b47fa Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 1 Jul 2024 14:23:33 +0200 Subject: [PATCH 31/49] Fix concealer local address returning Signed-off-by: Mateusz Palczuk --- .../include/concealer/field_operator_application.hpp | 8 ++++---- ...ield_operator_application_for_autoware_universe.hpp | 8 ++++---- ...ield_operator_application_for_autoware_universe.cpp | 10 ++++------ .../include/traffic_simulator/entity/ego_entity.hpp | 6 +++--- simulation/traffic_simulator/src/entity/ego_entity.cpp | 6 +++--- 5 files changed, 18 insertions(+), 20 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index e56d59a8bc2..75b1cd0a62b 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -129,13 +129,13 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto clearRoute() -> void = 0; - virtual auto getAutowareStateName() const -> const std::string & = 0; + virtual auto getAutowareStateName() const -> std::string = 0; - virtual auto getMinimumRiskManeuverBehaviorName() const -> const std::string & = 0; + virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0; - virtual auto getMinimumRiskManeuverStateName() const -> const std::string & = 0; + virtual auto getMinimumRiskManeuverStateName() const -> std::string = 0; - virtual auto getEmergencyStateName() const -> const std::string & = 0; + virtual auto getEmergencyStateName() const -> std::string = 0; virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index cffa663f1a7..e9d14665928 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -145,18 +145,18 @@ class FieldOperatorApplicationFor auto engaged() const -> bool override; - auto getAutowareStateName() const -> const std::string & override; + auto getAutowareStateName() const -> std::string override; auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; auto getTurnIndicatorsCommand() const -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand override; - auto getEmergencyStateName() const -> const std::string & override; + auto getEmergencyStateName() const -> std::string override; - auto getMinimumRiskManeuverBehaviorName() const -> const std::string & override; + auto getMinimumRiskManeuverBehaviorName() const -> std::string override; - auto getMinimumRiskManeuverStateName() const -> const std::string & override; + auto getMinimumRiskManeuverStateName() const -> std::string override; auto initialize(const geometry_msgs::msg::Pose &) -> void override; diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index c66d9e1aab1..b86d4b3a657 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -407,8 +407,7 @@ auto FieldOperatorApplicationFor::restrictTargetSpeed(double v return value; } -auto FieldOperatorApplicationFor::getAutowareStateName() const - -> const std::string & +auto FieldOperatorApplicationFor::getAutowareStateName() const -> std::string { #define IF(IDENTIFIER, RETURN) \ if (getAutowareState().state == tier4_system_msgs::msg::AutowareState::IDENTIFIER) { \ @@ -428,20 +427,19 @@ auto FieldOperatorApplicationFor::getAutowareStateName() const #undef IF } -auto FieldOperatorApplicationFor::getEmergencyStateName() const - -> const std::string & +auto FieldOperatorApplicationFor::getEmergencyStateName() const -> std::string { return minimum_risk_maneuver_state; } auto FieldOperatorApplicationFor::getMinimumRiskManeuverBehaviorName() const - -> const std::string & + -> std::string { return minimum_risk_maneuver_behavior; } auto FieldOperatorApplicationFor::getMinimumRiskManeuverStateName() const - -> const std::string & + -> std::string { return minimum_risk_maneuver_state; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 018e3da8cbb..34bde0bc90b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -160,9 +160,9 @@ class EgoEntity : public VehicleEntity auto replanRoute(const std::vector & route) -> void; auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void; auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void; - auto getMinimumRiskManeuverBehaviorName() const -> const std::string &; - auto getMinimumRiskManeuverStateName() const -> const std::string &; - auto getEmergencyStateName() const -> const std::string &; + auto getMinimumRiskManeuverBehaviorName() const -> std::string; + auto getMinimumRiskManeuverStateName() const -> std::string; + auto getEmergencyStateName() const -> std::string; auto getTurnIndicatorsCommandName() const -> const std::string; }; } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 3575594dc7e..8a3ea22b3a7 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -101,15 +101,15 @@ auto EgoEntity::requestAutoModeForCooperation(const std::string & module_name, b field_operator_application->requestAutoModeForCooperation(module_name, enable); } -auto EgoEntity::getMinimumRiskManeuverBehaviorName() const -> const std::string & +auto EgoEntity::getMinimumRiskManeuverBehaviorName() const -> std::string { return field_operator_application->getMinimumRiskManeuverBehaviorName(); } -auto EgoEntity::getMinimumRiskManeuverStateName() const -> const std::string & +auto EgoEntity::getMinimumRiskManeuverStateName() const -> std::string { return field_operator_application->getMinimumRiskManeuverStateName(); } -auto EgoEntity::getEmergencyStateName() const -> const std::string & +auto EgoEntity::getEmergencyStateName() const -> std::string { return field_operator_application->getEmergencyStateName(); } From 07aae1531877519156a740c90fa80b429ca1c225 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 1 Jul 2024 15:55:24 +0200 Subject: [PATCH 32/49] feat(entity_base, entity_status): improve ::setStatus --- .../data_type/entity_status.hpp | 5 ++- .../traffic_simulator/entity/entity_base.hpp | 4 +- .../src/data_type/entity_status.cpp | 11 ++++- .../src/entity/entity_base.cpp | 40 ++++++++++--------- 4 files changed, 36 insertions(+), 24 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index ed82419a0b8..80b83d1c755 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -38,10 +38,13 @@ class CanonicalizedEntityStatus CanonicalizedEntityStatus(CanonicalizedEntityStatus && obj) noexcept; explicit operator EntityStatus() const noexcept { return entity_status_; } + auto set(const CanonicalizedEntityStatus & status) -> void; + auto set( + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void; auto set( const EntityStatus & status, const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) -> void; - auto set(const CanonicalizedEntityStatus & status) -> void; auto setAction(const std::string & action) -> void; auto getTime() const noexcept -> double; 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 3d4615ca101..ca0c8948679 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -220,12 +220,12 @@ class EntityBase helper::constructActionStatus()) -> void; virtual auto setStatus( - const LaneletPose & lanelet_pose, + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status = helper::constructActionStatus()) -> void; virtual auto setStatus( - const std::optional & canonicalized_lanelet_pose, + const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status = helper::constructActionStatus()) -> void; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index a8faf57c39b..0a5150ce544 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -69,7 +69,7 @@ auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> } auto CanonicalizedEntityStatus::set( - const EntityStatus & status, const double matching_distance, + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) -> void { const auto include_crosswalk = @@ -82,12 +82,19 @@ auto CanonicalizedEntityStatus::set( } else { // prefer the current lanelet canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - status.pose, getBoundingBox(), getLaneletIds(), include_crosswalk, matching_distance, + status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance, hdmap_utils_ptr); } set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } +auto CanonicalizedEntityStatus::set( + const EntityStatus & status, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void +{ + set(status, getLaneletIds(), matching_distance, hdmap_utils_ptr); +} + auto CanonicalizedEntityStatus::isInLanelet() const noexcept -> bool { return canonicalized_lanelet_pose_.has_value(); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 7cbc9f1fedc..51c791df570 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -561,17 +561,15 @@ auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status status_.set(status); } -void EntityBase::setStatus(const EntityStatus & status) +auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - status_.set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_.set( + status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } -auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void +void EntityBase::setStatus(const EntityStatus & status) { - const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - status.pose, status.bounding_box, lanelet_ids, false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); + status_.set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } auto EntityBase::setStatus( @@ -605,24 +603,28 @@ auto EntityBase::setStatus( } auto EntityBase::setStatus( - const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) - -> void + const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - setStatus(pose::canonicalize(lanelet_pose, hdmap_utils_ptr_), action_status); + auto status = static_cast(getStatus()); + status.action_status = action_status; + status.pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose_valid = true; + setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } auto EntityBase::setStatus( - const std::optional & canonicalized_lanelet_pose, - const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void + const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) + -> void { - auto status = static_cast(getStatus()); - status.action_status = action_status; - if (canonicalized_lanelet_pose) { - status.pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose_valid = true; + if (const auto canonicalized_lanelet_pose = pose::canonicalize(lanelet_pose, hdmap_utils_ptr_)) { + setStatus(canonicalized_lanelet_pose.value(), action_status); + } else { + std::stringstream ss; + ss << "Status can not be set. lanelet pose: " << lanelet_pose << " is not canonicalizable for "; + THROW_SEMANTIC_ERROR(ss.str(), " entity named: ", std::quoted(name), "."); } - setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } auto EntityBase::setLinearVelocity(const double linear_velocity) -> void From 9ca9eed46ab6cfc333f49ecc520e2aa9d8d1615b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 30 Jul 2024 15:02:00 +0200 Subject: [PATCH 33/49] Fix random_test_runner Signed-off-by: Mateusz Palczuk --- .../metrics/almost_standstill_metric.hpp | 2 +- .../random_test_runner/test_executor.hpp | 132 ++++++++------ .../test/test_test_executor.cpp | 163 +++++++++++++----- 3 files changed, 200 insertions(+), 97 deletions(-) diff --git a/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp b/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp index fe0609a7a93..dfde759f34a 100644 --- a/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/metrics/almost_standstill_metric.hpp @@ -28,7 +28,7 @@ class AlmostStandstillMetric bool isAlmostStandingStill(const traffic_simulator::CanonicalizedEntityStatus & status) { if (!last_status_) { - last_status_->set(status); + last_status_.emplace(status); return false; } diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index d93af7662cc..d1c0db273a0 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -78,60 +78,86 @@ class TestExecutor RCLCPP_INFO_STREAM(logger_, message); scenario_completed_ = false; - api_->updateFrame(); - - api_->spawn( - ego_name_, test_description_.ego_start_position, getVehicleParameters(), - traffic_simulator::VehicleBehavior::autoware(), "lexus_rx450h"); - - auto ego_entity = api_->getEgoEntity(ego_name_); - ego_entity->setStatus( - test_description_.ego_start_position, traffic_simulator::helper::constructActionStatus()); - - if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { - api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( - traffic_simulator::helper::LidarType::VLP16, ego_name_, - stringFromArchitectureType(architecture_type_))); - - double constexpr detection_update_duration = 0.1; - api_->attachDetectionSensor( - traffic_simulator::helper::constructDetectionSensorConfiguration( - ego_name_, stringFromArchitectureType(architecture_type_), detection_update_duration)); - - api_->attachOccupancyGridSensor([&]() { - simulation_api_schema::OccupancyGridSensorConfiguration configuration; - configuration.set_architecture_type(stringFromArchitectureType(architecture_type_)); - configuration.set_entity(ego_name_); - configuration.set_filter_by_range(true); - configuration.set_height(200); - configuration.set_range(300); - configuration.set_resolution(0.5); - configuration.set_update_duration(0.1); - configuration.set_width(200); - return configuration; - }()); - - ego_entity->template setParameter("allow_goal_modification", true); - } - - // XXX dirty hack: wait for autoware system to launch - // ugly but helps for now - std::this_thread::sleep_for(std::chrono::milliseconds{5000}); - ego_entity->requestAssignRoute(std::vector({test_description_.ego_goal_pose})); - ego_entity->engage(); - - goal_reached_metric_.setGoal(test_description_.ego_goal_pose); - - for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) { - const auto & npc_descr = test_description_.npcs_descriptions[i]; + if (const auto ego_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( + test_description_.ego_start_position, api_->getHdmapUtils()); + !ego_start_canonicalized_lanelet_pose) { + throw std::runtime_error( + "Can not canonicalize ego start lanelet pose: id: " + + std::to_string(test_description_.ego_start_position.lanelet_id) + + " s: " + std::to_string(test_description_.ego_start_position.s) + + " offset: " + std::to_string(test_description_.ego_start_position.offset)); + } else { + api_->updateFrame(); api_->spawn( - npc_descr.name, npc_descr.start_position, getVehicleParameters(), - traffic_simulator::VehicleBehavior::defaultBehavior(), "taxi"); - auto entity = api_->getEntity(npc_descr.name); - entity->setStatus( - npc_descr.start_position, - traffic_simulator::helper::constructActionStatus(npc_descr.speed)); - entity->requestSpeedChange(npc_descr.speed, true); + ego_name_, ego_start_canonicalized_lanelet_pose.value(), getVehicleParameters(), + traffic_simulator::VehicleBehavior::autoware(), "lexus_rx450h"); + + auto ego_entity = api_->getEgoEntity(ego_name_); + + ego_entity->setStatus( + ego_start_canonicalized_lanelet_pose.value(), + traffic_simulator::helper::constructActionStatus()); + + if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { + api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( + traffic_simulator::helper::LidarType::VLP16, ego_name_, + stringFromArchitectureType(architecture_type_))); + + double constexpr detection_update_duration = 0.1; + api_->attachDetectionSensor( + traffic_simulator::helper::constructDetectionSensorConfiguration( + ego_name_, stringFromArchitectureType(architecture_type_), + detection_update_duration)); + + api_->attachOccupancyGridSensor([&]() { + simulation_api_schema::OccupancyGridSensorConfiguration configuration; + configuration.set_architecture_type(stringFromArchitectureType(architecture_type_)); + configuration.set_entity(ego_name_); + configuration.set_filter_by_range(true); + configuration.set_height(200); + configuration.set_range(300); + configuration.set_resolution(0.5); + configuration.set_update_duration(0.1); + configuration.set_width(200); + return configuration; + }()); + + ego_entity->template setParameter("allow_goal_modification", true); + } + + // XXX dirty hack: wait for autoware system to launch + // ugly but helps for now + std::this_thread::sleep_for(std::chrono::milliseconds{5000}); + + ego_entity->requestAssignRoute(std::vector({test_description_.ego_goal_pose})); + ego_entity->engage(); + + goal_reached_metric_.setGoal(test_description_.ego_goal_pose); + + for (const auto & npc_descr : test_description_.npcs_descriptions) { + if (const auto npc_start_canonicalized_lanelet_pose = + traffic_simulator::pose::canonicalize( + npc_descr.start_position, api_->getHdmapUtils()); + not npc_start_canonicalized_lanelet_pose.has_value()) { + throw std::runtime_error( + "Can not canonicalize npc start lanelet pose: id: " + + std::to_string(npc_descr.start_position.lanelet_id) + + " s: " + std::to_string(npc_descr.start_position.s) + + " offset: " + std::to_string(npc_descr.start_position.offset)); + } else { + api_->spawn( + npc_descr.name, npc_start_canonicalized_lanelet_pose.value(), getVehicleParameters(), + traffic_simulator::VehicleBehavior::defaultBehavior(), "taxi"); + + auto entity = api_->getEntity(npc_descr.name); + + entity->setStatus( + npc_start_canonicalized_lanelet_pose.value(), + traffic_simulator::helper::constructActionStatus(npc_descr.speed)); + + entity->requestSpeedChange(npc_descr.speed, true); + } + } } }); } diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index d6979c79e15..ff1cb5ca30f 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -19,6 +19,36 @@ #include #include +/// This class is for all entities to keep it simple +class MockEntity +{ +public: + MOCK_METHOD( + void, setStatus, + (const std::optional &, + const traffic_simulator_msgs::msg::ActionStatus &), + ()); + MOCK_METHOD(void, requestSpeedChange, (double, bool), ()); + MOCK_METHOD(const traffic_simulator::CanonicalizedEntityStatus &, getStatus, (), (const)); + + // Ego member functions + MOCK_METHOD(void, engage, (), ()); + MOCK_METHOD(bool, isEngageable, (), (const)); + MOCK_METHOD(bool, setParameterMock, (const std::string &, const bool &), (const)); + MOCK_METHOD(void, requestAssignRoute, (const std::vector &), ()); + + template + auto setParameter(const std::string & name, const ParameterType & default_value = {}) const + -> ParameterType + { + if constexpr (std::is_same_v) { + setParameterMock(name, default_value); + return default_value; + } + throw std::runtime_error("Unexpected typename in MockEntity::setParameter function"); + } +}; + class MockFieldOperatorApplication { public: @@ -37,6 +67,33 @@ class MockFieldOperatorApplication } }; +/// Simplest possible valid lanelet pose that can be converted to CanonicalizedLaneletPose +auto getTestDescription() -> TestDescription +{ + TestDescription td; + td.ego_goal_position = traffic_simulator_msgs::build() + .lanelet_id(34513) + .s(10.0) + .offset(0.0) + .rpy(geometry_msgs::msg::Vector3()); + + td.ego_goal_pose = + geometry_msgs::build() + .position( + geometry_msgs::build().x(3704.31).y(73766.2).z(-0.875988)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.23587).w( + 0.971785)); + + td.ego_start_position = traffic_simulator_msgs::build() + .lanelet_id(34513) + .s(0.0) + .offset(0.0) + .rpy(geometry_msgs::msg::Vector3()); + + return td; +} + class MockTrafficSimulatorAPI { public: @@ -48,14 +105,9 @@ class MockTrafficSimulatorAPI MOCK_METHOD(bool, updateFrame, (), ()); MOCK_METHOD( bool, spawn, - (const std::string &, const traffic_simulator::LaneletPose &, + (const std::string &, const traffic_simulator::CanonicalizedLaneletPose &, const traffic_simulator_msgs::msg::VehicleParameters &, const std::string &, std::string), ()); - MOCK_METHOD( - void, setEntityStatus, - (const std::string &, const traffic_simulator::LaneletPose &, - const traffic_simulator_msgs::msg::ActionStatus), - ()); MOCK_METHOD(void, attachLidarSensor, (const simulation_api_schema::LidarConfiguration &), ()); MOCK_METHOD( void, attachDetectionSensor, (const simulation_api_schema::DetectionSensorConfiguration &), ()); @@ -65,79 +117,96 @@ class MockTrafficSimulatorAPI void, requestAssignRoute, (const std::string &, std::vector), ()); MOCK_METHOD( void, spawn, - (const std::string &, const traffic_simulator::LaneletPose &, + (const std::string &, const traffic_simulator::CanonicalizedLaneletPose &, const traffic_simulator_msgs::msg::VehicleParameters &), ()); - MOCK_METHOD(void, requestSpeedChange, (const std::string &, double, bool), ()); MOCK_METHOD(bool, isAnyEgoSpawned, (), ()); MOCK_METHOD(bool, isNpcLogicStarted, (), ()); MOCK_METHOD(void, startNpcLogic, (), ()); MOCK_METHOD(bool, despawn, (const std::string), ()); MOCK_METHOD(std::string, getEgoName, (), ()); MOCK_METHOD(double, getCurrentTime, (), ()); - MOCK_METHOD(void, getEntityMock, (const std::string &), ()); - MOCK_METHOD(void, getEgoEntityMock, (const std::string &), ()); + MOCK_METHOD(void, getEntityMock, (const std::string &), (const)); + MOCK_METHOD(void, getEgoEntityMock, (const std::string &), (const)); + MOCK_METHOD(void, getHdmapUtilsMock, (), ()); MOCK_METHOD(bool, isEntitySpawned, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); - std::shared_ptr getEntity(const std::string & name) + auto getEntity(const std::string & name) const + -> std::shared_ptr<::testing::StrictMock> { getEntityMock(name); - /// @note set invalid LaneletPose so pass std::nullopt - return std::make_shared( - "", traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt), nullptr, - getVehicleParameters()); + if (name == ego_name_) { + return ego_entity_; + } + return std::make_shared<::testing::StrictMock>(); } - std::shared_ptr getEgoEntity(const std::string & name) + auto getEgoEntity(const std::string & name) const + -> std::shared_ptr<::testing::StrictMock> { getEgoEntityMock(name); - /// @note set invalid LaneletPose so pass std::nullopt - return std::make_shared( - "", traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt), nullptr, - getVehicleParameters(), - traffic_simulator::Configuration( - ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map"), - nullptr); + return ego_entity_; + } + + /// Real member function required for the canonicalization of the lanelet pose in TestExecutor.InitializeWithNoNPCs test + auto getHdmapUtils() -> const std::shared_ptr & + { + getHdmapUtilsMock(); + + return hdmap_utils_ptr_; } - void setEntityStatusNecessaryValues( - double time, geometry_msgs::msg::Pose map_pose, geometry_msgs::msg::Twist twist) + auto setEntityStatusNecessaryValues( + double time, geometry_msgs::msg::Pose map_pose, geometry_msgs::msg::Twist twist) -> void { entity_status_.time = time; entity_status_.pose = map_pose; entity_status_.action_status.twist = twist; } + + const std::shared_ptr hdmap_utils_ptr_ = + std::make_shared( + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + const std::string ego_name_ = "ego"; + const std::shared_ptr<::testing::StrictMock> ego_entity_ = + std::make_shared<::testing::StrictMock>(); }; TEST(TestExecutor, InitializeWithNoNPCs) { ::testing::Sequence sequence; auto MockAPI = std::make_shared<::testing::StrictMock>(); + auto & mock_ego = MockAPI->ego_entity_; auto test_case = common::junit::SimpleTestCase("test_case"); auto test_executor = TestExecutor( - MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), 20.0, + MockAPI, getTestDescription(), JunitXmlReporterTestCase(test_case), 20.0, ArchitectureType::AWF_UNIVERSE, rclcpp::get_logger("test_executor_test")); + EXPECT_CALL(*MockAPI, getHdmapUtilsMock).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); EXPECT_CALL( - *MockAPI, - spawn( - ::testing::A(), ::testing::A(), - ::testing::A(), - ::testing::A(), ::testing::A())) + *MockAPI, spawn( + ::testing::A(), + ::testing::A(), + ::testing::A(), + ::testing::A(), ::testing::A())) .Times(1) .InSequence(sequence); - EXPECT_CALL(*MockAPI, setEntityStatus).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, getEgoEntityMock).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, setStatus).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, attachLidarSensor).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, attachDetectionSensor).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, attachOccupancyGridSensor).Times(1).InSequence(sequence); - EXPECT_CALL(*(MockAPI->field_operator_application_mock), declare_parameter_mock) - .Times(1) - .InSequence(sequence); - EXPECT_CALL(*MockAPI, requestAssignRoute).Times(1).InSequence(sequence); - EXPECT_CALL(*(MockAPI->field_operator_application_mock), engage).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, setParameterMock).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, requestAssignRoute).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, engage).Times(1).InSequence(sequence); test_executor.initialize(); } @@ -146,30 +215,38 @@ TEST(TestExecutor, UpdateNoNPCs) { ::testing::Sequence sequence; auto MockAPI = std::make_shared<::testing::StrictMock>(); + auto & mock_ego = MockAPI->ego_entity_; auto test_case = common::junit::SimpleTestCase("test_case"); auto test_executor = TestExecutor( MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), 20.0, ArchitectureType::AWF_UNIVERSE, rclcpp::get_logger("test_executor_test")); + traffic_simulator::CanonicalizedEntityStatus status( + traffic_simulator_msgs::msg::EntityStatus(), std::nullopt); - EXPECT_CALL(*MockAPI, isAnyEgoSpawned) - .Times(1) - .InSequence(sequence) - .WillOnce(::testing::Return(false)); EXPECT_CALL(*MockAPI, isNpcLogicStarted) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(false)); - EXPECT_CALL(*MockAPI, startNpcLogic).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, isAnyEgoSpawned) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(false)); + EXPECT_CALL(*MockAPI, startNpcLogic).Times(1).InSequence(sequence); EXPECT_CALL(*MockAPI, getCurrentTime) .Times(1) .InSequence(sequence) .WillOnce(::testing::Return(1.0)); - EXPECT_CALL(*MockAPI, getEntityMock).Times(2).InSequence(sequence); + EXPECT_CALL(*MockAPI, getEntityMock).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, getStatus) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::ReturnRef(status)); + EXPECT_CALL(*MockAPI, getEntityMock).Times(1).InSequence(sequence); + EXPECT_CALL(*mock_ego, getStatus) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::ReturnRef(status)); EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); test_executor.update(); From 8940b3e2f127bbca92295a91d580887030e45be6 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 31 Jul 2024 12:21:19 +0200 Subject: [PATCH 34/49] Adjust mock scenarios to new API Signed-off-by: Mateusz Palczuk --- .../src/spawn/spawn_in_map_frame.cpp | 2 +- .../synchronized_action.cpp | 24 ++++++++++++------- .../synchronized_action_with_speed.cpp | 24 ++++++++++++------- .../src/traffic_simulation_demo.cpp | 2 +- 4 files changed, 34 insertions(+), 18 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp index 56d48813a89..e4c308a6854 100644 --- a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp +++ b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp @@ -41,7 +41,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode const auto map_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.reachPosition("ego", map_pose, 0.1)) { + if (api_.getEntity("ego")->reachPosition(map_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); 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 42140bbee4c..a9c3a99e6c6 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -44,11 +44,14 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { + auto npc = api_.getEntity("npc"); + auto ego = api_.getEntity("ego"); + // SUCCESS if ( - api_.requestSynchronize("npc", "ego", ego_target, npc_target, 0, 0.5) && - api_.reachPosition("ego", ego_target, 1.0) && api_.reachPosition("npc", npc_target, 1.0) && - api_.getCurrentTwist("npc").linear.x < 0.5) { + npc->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) && + ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && + npc->getCurrentTwist().linear.x < 0.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } @@ -67,24 +70,29 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34976, 20, 0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 3); - api_.requestSpeedChange("ego", 3, true); + + auto ego = api_.getEntity("ego"); + ego->setLinearVelocity(3); + ego->requestSpeedChange(3, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20, 0, api_.getHdmapUtils())); - api_.requestAssignRoute("ego", goal_poses); + ego->requestAssignRoute(goal_poses); api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 0, 0, api_.getHdmapUtils()), getVehicleParameters()); + + auto npc = api_.getEntity("npc"); + std::vector npc_goal_poses; npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34564, 20, 0, api_.getHdmapUtils())); - api_.requestAssignRoute("npc", npc_goal_poses); - api_.setLinearVelocity("npc", 6); + npc->requestAssignRoute(npc_goal_poses); + npc->setLinearVelocity(6); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index 2aa571173fa..5395b90f857 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -44,11 +44,14 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { + auto npc = api_.getEntity("npc"); + auto ego = api_.getEntity("ego"); + // SUCCESS if ( - api_.requestSynchronize("npc", "ego", ego_target, npc_target, 2, 0.5) && - api_.reachPosition("ego", ego_target, 1.0) && api_.reachPosition("npc", npc_target, 1.0) && - api_.getCurrentTwist("npc").linear.x < 2.5) { + npc->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) && + ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && + npc->getCurrentTwist().linear.x < 2.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } @@ -67,24 +70,29 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34976, 20, 0, api_.getHdmapUtils()), getVehicleParameters()); - api_.setLinearVelocity("ego", 3); - api_.requestSpeedChange("ego", 3, true); + + auto ego = api_.getEntity("ego"); + ego->setLinearVelocity(3); + ego->requestSpeedChange(3, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 20, 0, api_.getHdmapUtils())); - api_.requestAssignRoute("ego", goal_poses); + ego->requestAssignRoute(goal_poses); api_.spawn( "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 0, 0, api_.getHdmapUtils()), getVehicleParameters()); + + auto npc = api_.getEntity("npc"); + std::vector npc_goal_poses; npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( 34564, 20, 0, api_.getHdmapUtils())); - api_.requestAssignRoute("npc", npc_goal_poses); - api_.setLinearVelocity("npc", 6); + npc->requestAssignRoute(npc_goal_poses); + npc->setLinearVelocity(6); } auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 251a2806cf3..d53b4cd426e 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -163,7 +163,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode "obstacle", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), getMiscObjectParameters()); api_.getEntity("obstacle") - ->setCanonicalizedStatus( + ->setStatus( ego_entity->getMapPose(), traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), traffic_simulator::helper::constructActionStatus()); From fec882182984e05631c1a5455ea8b09b273ea544 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 14 Oct 2024 14:31:31 +0200 Subject: [PATCH 35/49] fix(cpp_mock_scenarios): merge changes in random001 --- .../src/random_scenario/random001.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 405eb6344d2..4b97671343d 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -195,12 +195,12 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode 34606, 0.0, 0.0, api_.getHdmapUtils()); const auto entity_name = "spawn_nearby_ego"; - if (const auto ego = api_.getEntity("ego")) { - if (api_.isInPosition("ego", trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { + if (ego_entity) { + if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { api_.spawn( entity_name, traffic_simulator::pose::transformRelativePoseToGlobal( - ego->getMapPose(), + ego_entity->getMapPose(), geometry_msgs::build() .position(geometry_msgs::build().x(10).y(-5).z(0)) .orientation(geometry_msgs::msg::Quaternion())), @@ -208,12 +208,12 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); } - if (!api_.isInPosition("ego", trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { + if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { api_.despawn(entity_name); } } - if (api_.isInPosition("ego", ego_goal_position, 1.0)) { + if (ego_entity->isInPosition(ego_goal_position, 1.0)) { api_.despawn("ego"); stop(cpp_mock_scenarios::Result::SUCCESS); } From f559200d38f15b58ba9013d7b7733b69eac7aa47 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 14 Oct 2024 17:25:13 +0200 Subject: [PATCH 36/49] ref(cpp_mock_scenarios): ref random001 --- .../src/random_scenario/random001.cpp | 50 +++++++++---------- 1 file changed, 23 insertions(+), 27 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 4b97671343d..8023ebc75a6 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -188,37 +188,33 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode spawn_and_cross_pedestrian(i); } - { - const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10, 0.0, api_.getHdmapUtils()); - const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0.0, 0.0, api_.getHdmapUtils()); - const auto entity_name = "spawn_nearby_ego"; - - if (ego_entity) { - if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { - api_.spawn( - entity_name, - traffic_simulator::pose::transformRelativePoseToGlobal( - ego_entity->getMapPose(), - geometry_msgs::build() - .position(geometry_msgs::build().x(10).y(-5).z(0)) - .orientation(geometry_msgs::msg::Quaternion())), - getVehicleParameters(), - traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); - } + const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34621, 10, 0.0, api_.getHdmapUtils()); + constexpr auto entity_name = "spawn_nearby_ego"; + if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { + api_.spawn( + entity_name, + traffic_simulator::pose::transformRelativePoseToGlobal( + ego_entity->getMapPose(), + geometry_msgs::build() + .position(geometry_msgs::build().x(10).y(-5).z(0)) + .orientation(geometry_msgs::msg::Quaternion())), + getVehicleParameters(), + traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); + } - if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { - api_.despawn(entity_name); - } - } + if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { + api_.despawn(entity_name); + } - if (ego_entity->isInPosition(ego_goal_position, 1.0)) { - api_.despawn("ego"); - stop(cpp_mock_scenarios::Result::SUCCESS); - } + const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34606, 0.0, 0.0, api_.getHdmapUtils()); + if (ego_entity->isInPosition(ego_goal_position, 1.0)) { + api_.despawn("ego"); + stop(cpp_mock_scenarios::Result::SUCCESS); } } + void onInitialize() override { // api_.setVerbose(true); From 18daa8b55856cbe0d1735f4a8efd84d9cc1dfb49 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 15 Oct 2024 11:21:11 +0200 Subject: [PATCH 37/49] ref(behavior_tree,traffic_simulator): improve code by basing on sonarcloud --- .../behavior_tree_plugin/src/action_node.cpp | 3 +-- .../traffic_simulator/entity/ego_entity.hpp | 2 +- .../traffic_simulator/entity/entity_base.hpp | 2 +- .../entity/entity_manager.hpp | 6 +++--- .../src/entity/ego_entity.cpp | 2 +- .../src/entity/entity_base.cpp | 4 ++-- .../src/entity/entity_manager.cpp | 18 +++++++++--------- .../random_test_runner/test_executor.hpp | 4 ++-- 8 files changed, 20 insertions(+), 21 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index dcb2cd9ae51..b6e413ac281 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -294,8 +294,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_right, double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - const auto & status = getEntityStatus(target_name); - if (status.isInLanelet()) { + if (const auto & status = getEntityStatus(target_name); status.isInLanelet()) { return getDistanceToTargetEntityPolygon( spline, status, width_extension_right, width_extension_left, length_extension_front, length_extension_rear); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 832dd365a63..50496ed7bda 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -161,7 +161,7 @@ class EgoEntity : public VehicleEntity auto getMinimumRiskManeuverBehaviorName() const -> std::string; auto getMinimumRiskManeuverStateName() const -> std::string; auto getEmergencyStateName() const -> std::string; - auto getTurnIndicatorsCommandName() const -> const std::string; + auto getTurnIndicatorsCommandName() const -> std::string; }; } // namespace entity } // namespace traffic_simulator 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 24331225ff9..ea5a98f6f21 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -150,7 +150,7 @@ class EntityBase virtual void requestLaneChange(const lanelet::Id) {} - virtual void requestLaneChange(const lane_change::Parameter &){}; + virtual void requestLaneChange(const lane_change::Parameter &) {} /* */ auto requestLaneChange(const lane_change::Direction & direction) -> void; 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 3545f8e7b78..0680c3fe4d5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -182,9 +182,9 @@ class EntityManager auto getEgoEntity() const -> std::shared_ptr { - for (const auto & each : entities_) { - if (each.second->template is()) { - return std::dynamic_pointer_cast(each.second); + for (const auto & [name, entity] : entities_) { + if (entity->template is()) { + return std::dynamic_pointer_cast(entity); } } THROW_SEMANTIC_ERROR("getEgoEntity function was called, but ego vehicle does not exist"); diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 47c6e1e736b..948174fb52a 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -116,7 +116,7 @@ auto EgoEntity::getEmergencyStateName() const -> std::string { return field_operator_application->getEmergencyStateName(); } -auto EgoEntity::getTurnIndicatorsCommandName() const -> const std::string +auto EgoEntity::getTurnIndicatorsCommandName() const -> std::string { return boost::lexical_cast(field_operator_application->getTurnIndicatorsCommand()); } diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 936ab7fd358..b3e0e93cb3e 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -186,7 +186,7 @@ void EntityBase::requestLaneChange( { lanelet::Id reference_lanelet_id = 0; if (target.entity_name == name) { - if (not isInLanelet()) { + if (!isInLanelet()) { THROW_SEMANTIC_ERROR( "Source entity does not assigned to lanelet. Please check source entity name : ", name, " exists on lane."); @@ -592,7 +592,7 @@ auto EntityBase::setStatus( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - EntityStatus status = static_cast(getCanonicalizedStatus()); + auto status = static_cast(getCanonicalizedStatus()); status.pose = map_pose; status.action_status = action_status; setStatus(status); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 9198aa4af61..fcf92485906 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -110,8 +110,8 @@ void EntityManager::broadcastTransform( visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const { visualization_msgs::msg::MarkerArray marker; - for (const auto & entity : entities_) { - entity.second->appendDebugMarker(marker); + for (const auto & [name, entity] : entities_) { + entity->appendDebugMarker(marker); } return marker; } @@ -129,8 +129,8 @@ bool EntityManager::isEntitySpawned(const std::string & name) auto EntityManager::getEntityNames() const -> const std::vector { std::vector names{}; - for (const auto & each : entities_) { - names.push_back(each.first); + for (const auto & [name, entity] : entities_) { + names.push_back(name); } return names; } @@ -176,9 +176,9 @@ auto EntityManager::getNumberOfEgo() const -> std::size_t auto EntityManager::getEgoName() const -> const std::string & { - for (const auto & each : entities_) { - if (each.second->template is()) { - return each.second->getName(); + for (const auto & [name, entity] : entities_) { + if (entity->template is()) { + return entity->getName(); } } THROW_SEMANTIC_ERROR( @@ -272,8 +272,8 @@ void EntityManager::resetBehaviorPlugin( void EntityManager::setVerbose(const bool verbose) { configuration.verbose = verbose; - for (auto & entity : entities_) { - entity.second->verbose = verbose; + for (const auto & [name, entity] : entities_) { + entity->verbose = verbose; } } diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 707dbc236b0..85b02aa648f 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -138,7 +138,7 @@ class TestExecutor if (const auto npc_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( npc_descr.start_position, api_->getHdmapUtils()); - not npc_start_canonicalized_lanelet_pose.has_value()) { + !npc_start_canonicalized_lanelet_pose.has_value()) { throw std::runtime_error( "Can not canonicalize npc start lanelet pose: id: " + std::to_string(npc_descr.start_position.lanelet_id) + @@ -165,7 +165,7 @@ class TestExecutor auto update() -> void { executeWithErrorHandling([this]() { - if (not api_->isNpcLogicStarted()) { + if (!api_->isNpcLogicStarted()) { if (api_->isAnyEgoSpawned()) { auto ego_entity = api_->getEgoEntity(api_->getEgoName()); if (ego_entity->isEngageable()) { From 4eef0ecd09d2a29fde82dc1f78f8dad78455560c Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 15 Oct 2024 14:38:23 +0200 Subject: [PATCH 38/49] ref(cpp_mock_scenarios): improve overall --- .../src/crosswalk/stop_at_crosswalk.cpp | 5 +++-- .../accelerate_and_follow.cpp | 12 ++++++------ .../decelerate_and_follow.cpp | 12 ++++++------ .../acquire_position_in_world_frame.cpp | 8 ++++---- .../assign_route_in_world_frame.cpp | 8 ++++---- .../src/follow_lane/cancel_request.cpp | 8 ++++---- .../src/follow_lane/follow_with_offset.cpp | 15 ++++++--------- ...line_trajectory_with_do_nothing_plugin.cpp | 12 ++++++------ .../src/lane_change/lanechange_linear.cpp | 6 +++--- ...t_distance_in_lane_coordinate_distance.cpp | 4 ++-- .../src/metrics/traveled_distance.cpp | 9 +++------ .../src/spawn/spawn_in_map_frame.cpp | 2 +- .../speed_planning/request_speed_change.cpp | 7 +++---- .../request_speed_change_continuous_false.cpp | 11 ++++------- .../request_speed_change_relative.cpp | 9 ++++----- .../request_speed_change_with_limit.cpp | 15 +++++++-------- ...uest_speed_change_with_time_constraint.cpp | 7 +++---- ...d_change_with_time_constraint_relative.cpp | 7 +++---- .../synchronized_action.cpp | 2 +- .../synchronized_action_with_speed.cpp | 2 +- .../define_traffic_source_delay.cpp | 7 ++----- .../define_traffic_source_high_rate.cpp | 8 ++------ .../define_traffic_source_mixed.cpp | 8 ++------ .../define_traffic_source_multiple.cpp | 11 +++-------- .../define_traffic_source_pedestrian.cpp | 6 ++---- .../define_traffic_source_vehicle.cpp | 8 ++------ .../traffic_simulator/entity/entity_base.hpp | 8 -------- .../src/entity/entity_base.cpp | 19 +------------------ 28 files changed, 88 insertions(+), 148 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index f43c6880418..3c645ea754d 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -54,13 +54,14 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); } } + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; if (t >= 6.6) { if (7.5 >= t) { - if (std::fabs(0.1) <= api_.getEntity("ego")->getCurrentTwist().linear.x) { + if (std::fabs(0.1) <= ego_linear_velocity) { stop(cpp_mock_scenarios::Result::FAILURE); } } else { - if (0.1 >= api_.getEntity("ego")->getCurrentTwist().linear.x) { + if (0.1 >= ego_linear_velocity) { stop(cpp_mock_scenarios::Result::FAILURE); } } diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index d6996179add..e3ef445909a 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -39,13 +39,13 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x; - double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; - // double npc_accel = static_cast(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x; - ; + double ego_linear_acceleration = api_.getEntity("ego")->getCurrentAccel().linear.x; + double ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; + // double npc_linear_acceleration = api_.getEntity("npc")->getCurrentAccel().linear.x; + double npc_linear_velocity = api_.getEntity("npc")->getCurrentTwist().linear.x; + // LCOV_EXCL_START - if (npc_twist > (ego_twist + 1) && ego_accel < 0) { + if (npc_linear_velocity > (ego_linear_velocity + 1) && ego_linear_acceleration < 0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.checkCollision("ego", "npc")) { diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index fa749d5db7d..25d23f49941 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -39,13 +39,13 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - double ego_accel = api_.getEntity("ego")->getCurrentAccel().linear.x; - double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; - // double npc_accel = static_cast(api_.getEntity("npc")->getStatus()).action_status.accel.linear.x; - double npc_twist = api_.getEntity("npc")->getCurrentTwist().linear.x; - ; + double ego_linear_acceleration = api_.getEntity("ego")->getCurrentAccel().linear.x; + double ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; + // double npc_linear_acceleration = api_.getEntity("npc")->getCurrentAccel().linear.x; + double npc_linear_velocity = api_.getEntity("npc")->getCurrentTwist().linear.x; + // LCOV_EXCL_START - if (ego_twist > (npc_twist + 1) && ego_accel > 0) { + if (ego_linear_velocity > (npc_linear_velocity + 1) && ego_linear_acceleration > 0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.checkCollision("ego", "npc")) { diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 7901937a7a3..4912db96ea2 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -51,16 +51,16 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 10.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - auto entity = api_.getEntity("ego"); - entity->setStatus( + auto ego_entity = api_.getEntity("ego"); + ego_entity->setStatus( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), traffic_simulator::helper::constructActionStatus(10)); - entity->requestSpeedChange(10, true); + ego_entity->requestSpeedChange(10, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34408, 1.0, 0.0, api_.getHdmapUtils())); - entity->requestAcquirePosition(goal_pose); + ego_entity->requestAcquirePosition(goal_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 9a17485d32c..41360172631 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -51,9 +51,9 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - auto entity = api_.getEntity("ego"); - entity->setLinearVelocity(10); - entity->requestSpeedChange(10, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -61,7 +61,7 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN goal_poses.emplace_back(traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34408, 10, 0.0, api_.getHdmapUtils()))); - entity->requestAssignRoute(goal_poses); + ego_entity->requestAssignRoute(goal_poses); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 23735bb1ed4..54c59f313fd 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -58,12 +58,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - auto entity = api_.getEntity("ego"); - entity->setLinearVelocity(7); - entity->requestSpeedChange(7, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(7); + ego_entity->requestSpeedChange(7, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils()); - entity->requestAcquirePosition(goal_pose); + ego_entity->requestAcquirePosition(goal_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index d811ed7749d..da8c60d40e6 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -40,14 +40,11 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode bool requested = false; void onUpdate() override { - const auto entity = api_.getEntity("ego"); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (const auto ego_entity = api_.getEntity("ego"); !ego_entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); - } else if (traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34507, 0.1, api_.getHdmapUtils())) { + } else if (ego_entity->isInLanelet(34507, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); - } else if ( - std::abs(static_cast(lanelet_pose.value()).offset) <= 2.8) { + } else if (std::abs(ego_entity->getCanonicalizedStatus().getLaneletPose().offset) <= 2.8) { stop(cpp_mock_scenarios::Result::FAILURE); } } @@ -58,9 +55,9 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 3.0, api_.getHdmapUtils()), getVehicleParameters()); - auto entity = api_.getEntity("ego"); - entity->setLinearVelocity(10); - entity->requestSpeedChange(10, true); + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(10); + ego_entity->requestSpeedChange(10, true); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 4e5745df937..18a062c731a 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -63,32 +63,32 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario void onUpdate() override { - const auto entity = api_.getEntity("ego"); + const auto ego_entity = api_.getEntity("ego"); // LCOV_EXCL_START if (api_.getCurrentTime() >= 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP - if (equals(api_.getCurrentTime(), 0.0, 0.01) && !entity->isInPosition(spawn_pose, 0.1)) { + if (equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isInPosition(spawn_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.0, 0.01) && - !entity->isInPosition(trajectory_start_pose, 0.1)) { + !ego_entity->isInPosition(trajectory_start_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.5, 0.01) && - !entity->isInPosition(trajectory_waypoint_pose, 0.1)) { + !ego_entity->isInPosition(trajectory_waypoint_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 2.0, 0.01) && - !entity->isInPosition(trajectory_goal_pose, 0.1)) { + !ego_entity->isInPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if (equals(api_.getCurrentTime(), 2.5, 0.01)) { - if (entity->isInPosition(trajectory_goal_pose, 0.1)) { + if (ego_entity->isInPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index 6197e18debd..466bac3ea24 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -40,11 +40,11 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode bool lanechange_finished = false; void onUpdate() override { - const auto entity = api_.getEntity("ego"); - if (entity->isInLanelet(34513, 0.1)) { + const auto ego_entity = api_.getEntity("ego"); + if (ego_entity->isInLanelet(34513, 0.1)) { lanechange_finished = true; } - if (lanechange_finished && entity->isInLanelet(34510, 0.1)) { + if (lanechange_finished && ego_entity->isInLanelet(34510, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 2445e474915..dbb5ef0172f 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -58,9 +58,9 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar const std::string & from_entity_name, const std::string & to_entity_name, const double matching_distance) -> std::optional { - auto from_entity_lanelet_pose = + const auto from_entity_lanelet_pose = api_.getEntity(from_entity_name)->getCanonicalizedLaneletPose(matching_distance); - auto to_entity_lanelet_pose = + const auto to_entity_lanelet_pose = api_.getEntity(to_entity_name)->getCanonicalizedLaneletPose(matching_distance); if (from_entity_lanelet_pose && to_entity_lanelet_pose) { return traffic_simulator::distance::lateralDistance( diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index a3a33c62128..223ea80ac8d 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -44,14 +44,11 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { // LCOV_EXCL_START - const auto entity = api_.getEntity("ego"); - if (!entity) { - stop(cpp_mock_scenarios::Result::FAILURE); - } else if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (const auto ego_entity = api_.getEntity("ego"); !ego_entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); } else if (const auto difference = std::abs( - static_cast(lanelet_pose.value()).s - - entity->getTraveledDistance()); + ego_entity->getCanonicalizedStatus().getLaneletPose().s - + ego_entity->getTraveledDistance()); difference > std::numeric_limits::epsilon()) { stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp index e4c308a6854..c34e8236e62 100644 --- a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp +++ b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp @@ -41,7 +41,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode const auto map_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.getEntity("ego")->reachPosition(map_pose, 0.1)) { + if (api_.getEntity("ego")->isInPosition(map_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 942b0fd424d..ce683734067 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -39,15 +39,14 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; if (api_.getEntity("front")->getCurrentTwist().linear.x < 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } - if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { + if (api_.getCurrentTime() <= 0.9 && ego_linear_velocity > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } - if ( - api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 && - api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) { + if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 10.0 && ego_linear_velocity >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 126c8600ed1..917241b6ed8 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -43,19 +43,16 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp /** * @brief checking linear speed */ + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; if (api_.getCurrentTime() <= 0.95) { - if (!equals( - api_.getCurrentTime() * 10.0, api_.getEntity("ego")->getCurrentTwist().linear.x, - 0.01)) { + if (!equals(api_.getCurrentTime() * 10.0, ego_linear_velocity, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } - if (api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0) { + if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 10.0) { speed_reached = true; } - if ( - speed_reached && api_.getCurrentTime() >= 1.5 && - api_.getEntity("ego")->getCurrentTwist().linear.x >= 13.88) { + if (speed_reached && api_.getCurrentTime() >= 1.5 && ego_linear_velocity >= 13.88) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index aa53b5bd704..e893641813d 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -39,16 +39,15 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari private: void onUpdate() override { + const auto entity_linear_velocity = api_.getEntity("front")->getCurrentTwist().linear.x; if (api_.getCurrentTime() <= 1.9) { - if (!equals( - api_.getCurrentTime() + 3.0, api_.getEntity("front")->getCurrentTwist().linear.x, - 0.01)) { + if (!equals(api_.getCurrentTime() + 3.0, entity_linear_velocity, 0.01)) { stop(cpp_mock_scenarios::Result::FAILURE); } } if ( - api_.getCurrentTime() >= 2.0 && api_.getEntity("front")->getCurrentTwist().linear.x <= 5.05 && - api_.getEntity("front")->getCurrentTwist().linear.x >= 4.95) { + api_.getCurrentTime() >= 2.0 && entity_linear_velocity <= 5.05 && + entity_linear_velocity >= 4.95) { stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index b96533b9b41..03a1b8f8df1 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -39,12 +39,11 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar private: void onUpdate() override { - if (api_.getCurrentTime() <= 0.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; + if (api_.getCurrentTime() <= 0.9 && ego_linear_velocity > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } - if ( - api_.getCurrentTime() >= 1.0 && api_.getEntity("ego")->getCurrentTwist().linear.x <= 5.1 && - api_.getEntity("ego")->getCurrentTwist().linear.x >= 4.9) { + if (api_.getCurrentTime() >= 1.0 && ego_linear_velocity <= 5.1 && ego_linear_velocity >= 4.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } } @@ -56,10 +55,10 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar traffic_simulator::helper::constructCanonicalizedLaneletPose( 34741, 0.0, 0.0, api_.getHdmapUtils()), getVehicleParameters()); - auto entity = api_.getEntity("ego"); - entity->setLinearVelocity(0); - entity->setVelocityLimit(5.0); - entity->requestSpeedChange( + auto ego_entity = api_.getEntity("ego"); + ego_entity->setLinearVelocity(0); + ego_entity->setVelocityLimit(5.0); + ego_entity->requestSpeedChange( 10.0, traffic_simulator::speed_change::Transition::LINEAR, traffic_simulator::speed_change::Constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 10.0), diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 275939799d9..6fc3217c9cd 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -39,13 +39,12 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: private: void onUpdate() override { - if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x > 10.0) { + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; + if (api_.getCurrentTime() <= 3.9 && ego_linear_velocity > 10.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.999) { - if ( - api_.getEntity("ego")->getCurrentTwist().linear.x <= 10.0 && - api_.getEntity("ego")->getCurrentTwist().linear.x >= 9.9) { + if (ego_linear_velocity <= 10.0 && ego_linear_velocity >= 9.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index 26925f29ed0..5192be24fa8 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -40,13 +40,12 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario private: void onUpdate() override { - if (api_.getCurrentTime() <= 3.9 && api_.getEntity("ego")->getCurrentTwist().linear.x >= 3.0) { + const auto ego_linear_velocity = api_.getEntity("ego")->getCurrentTwist().linear.x; + if (api_.getCurrentTime() <= 3.9 && ego_linear_velocity >= 3.0) { stop(cpp_mock_scenarios::Result::FAILURE); } if (api_.getCurrentTime() >= 3.9999) { - if ( - api_.getEntity("ego")->getCurrentTwist().linear.x <= 3.1 && - api_.getEntity("ego")->getCurrentTwist().linear.x >= 2.9) { + if (ego_linear_velocity <= 3.1 && ego_linear_velocity >= 2.9) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); 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 a9c3a99e6c6..9db459e8b53 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -50,7 +50,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode // SUCCESS if ( npc->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) && - ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && + ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) && npc->getCurrentTwist().linear.x < 0.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index 5395b90f857..a03ebadbbb2 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -50,7 +50,7 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode // SUCCESS if ( npc->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) && - ego->reachPosition(ego_target, 1.0) && npc->reachPosition(npc_target, 1.0) && + ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) && npc->getCurrentTwist().linear.x < 2.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 078c47c3f98..a6e7f503031 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -76,14 +76,11 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode } for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { const bool valid_vehicle_lanelet = - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); + entity->isInLanelet(34705, 50.0) || entity->isInLanelet(34696, 50.0); if (!valid_vehicle_lanelet || !isVehicle(name)) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 0838ac14e1a..81b12eee1bd 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -53,15 +53,11 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode } for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { const bool valid_vehicle_lanelet = - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - + entity->isInLanelet(34705, 50.0) || entity->isInLanelet(34696, 50.0); if (!valid_vehicle_lanelet || !isVehicle(name)) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index bc7e36b41ff..d6e1d7a5558 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -54,15 +54,11 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { const bool valid_vehicle_lanelet = - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - + entity->isInLanelet(34705, 50.0) || entity->isInLanelet(34696, 50.0); if (isVehicle(name)) { ++vehicle_count; } else if (isPedestrian(name)) { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index a1de5c70926..783f524c4c7 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -54,17 +54,12 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { const bool valid_vehicle_lanelet = - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - const bool valid_pedestrian_lanelet = traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34385, 10.0, api_.getHdmapUtils()); - + entity->isInLanelet(34705, 50.0) || entity->isInLanelet(34696, 50.0); + const bool valid_pedestrian_lanelet = entity->isInLanelet(34385, 10.0); if (isVehicle(name)) { ++vehicle_count; } else if (isPedestrian(name)) { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 39235504b1b..0469791afe9 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -53,12 +53,10 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode } for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { - const bool valid_pedestrian_lanelet = traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34385, 10.0, api_.getHdmapUtils()); - + const bool valid_pedestrian_lanelet = entity->isInLanelet(34385, 10.0); if (!valid_pedestrian_lanelet || !isPedestrian(name)) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 15886d450e4..d470a0c3197 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -53,15 +53,11 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode } for (const auto & name : names) { const auto entity = api_.getEntity(name); - if (const auto lanelet_pose = entity->getCanonicalizedLaneletPose(); not lanelet_pose) { + if (!entity->isInLanelet()) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } else { const bool valid_vehicle_lanelet = - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34705, 50.0, api_.getHdmapUtils()) || - traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), 34696, 50.0, api_.getHdmapUtils()); - + entity->isInLanelet(34705, 50.0) || entity->isInLanelet(34696, 50.0); if (!valid_vehicle_lanelet || !isVehicle(name)) { stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } 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 ea5a98f6f21..164f6e5065d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -258,14 +258,6 @@ class EntityBase /* */ void updateEntityStatusTimestamp(const double current_time); - /* */ bool reachPosition( - const geometry_msgs::msg::Pose & target_pose, const double tolerance) const; - - /* */ bool reachPosition( - const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const; - - /* */ bool reachPosition(const std::string & target_name, const double tolerance) const; - /* */ auto requestSynchronize( const std::string & target_name, const CanonicalizedLaneletPose & target_sync_pose, const CanonicalizedLaneletPose & entity_target, const double target_speed, diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index b3e0e93cb3e..8a57c905014 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -736,23 +736,6 @@ void EntityBase::updateEntityStatusTimestamp(const double current_time) status_->setTime(current_time); } -bool EntityBase::reachPosition(const std::string & target_name, const double tolerance) const -{ - return reachPosition(other_status_.find(target_name)->second.getMapPose(), tolerance); -} - -bool EntityBase::reachPosition( - const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -{ - return math::geometry::getDistance(getMapPose(), target_pose) < tolerance; -} - -bool EntityBase::reachPosition( - const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -{ - return reachPosition(static_cast(lanelet_pose), tolerance); -} - /*** * @brief Request synchronize the entity with the target entity. * @param target_name The name of the target entity. @@ -778,7 +761,7 @@ auto EntityBase::requestSynchronize( } ///@brief Check if the entity has already arrived to the target lanelet. - if (reachPosition(entity_target, tolerance)) { + if (isInPosition(entity_target, tolerance)) { if (getCurrentTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { } else { RCLCPP_WARN_ONCE( From 1d771b2e748731ea5a6d1927e86f5918f660aac3 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 15 Oct 2024 14:40:06 +0200 Subject: [PATCH 39/49] ref(cpp_mock_scenarios): improve move_backward --- mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 095b0df4087..b3d1fe71e97 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -42,8 +42,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::SUCCESS); } // LCOV_EXCL_START - double ego_twist = api_.getEntity("ego")->getCurrentTwist().linear.x; - if (ego_twist >= -2.9) { + if (api_.getEntity("ego")->getCurrentTwist().linear.x >= -2.9) { stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP From ace3bc8134eb8eeaa4b1a5e24804ada782fc09d1 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 15 Oct 2024 16:31:02 +0200 Subject: [PATCH 40/49] ref(traffic_simulator, simulator_core): improve overall --- .../simulator_core.hpp | 42 +++++-------- .../behavior_tree_plugin/src/action_node.cpp | 63 +++++++++---------- .../include/traffic_simulator/api/api.hpp | 3 +- .../data_type/entity_status.hpp | 1 - .../traffic_simulator/entity/ego_entity.hpp | 8 ++- .../traffic_simulator/entity/entity_base.hpp | 4 +- .../entity/entity_manager.hpp | 22 +------ simulation/traffic_simulator/src/api/api.cpp | 16 ++--- .../src/data_type/entity_status.cpp | 6 -- .../src/entity/ego_entity.cpp | 19 +++--- .../src/entity/entity_base.cpp | 11 ++-- .../src/entity/entity_manager.cpp | 34 +++++++++- 12 files changed, 109 insertions(+), 120 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 0bfae4f7fb7..8968b678386 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -286,9 +286,7 @@ class SimulatorCore template static auto applyAcquirePositionAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - entity->requestClearRoute(); - return entity->requestAcquirePosition(std::forward(xs)...); + return core->getEntity(entity_ref)->requestAcquirePosition(std::forward(xs)...); } template @@ -302,7 +300,7 @@ class SimulatorCore const EntityRef & entity_ref, const DynamicConstraints & dynamic_constraints) -> void { auto entity = core->getEntity(entity_ref); - return entity->setBehaviorParameter([&]() { + entity->setBehaviorParameter([&]() { auto behavior_parameter = entity->getBehaviorParameter(); if (not std::isinf(dynamic_constraints.max_speed)) { @@ -359,8 +357,6 @@ class SimulatorCore }()); if (controller.isAutoware()) { - auto ego_entity = core->getEgoEntity(entity_ref); - core->attachImuSensor(entity_ref, [&]() { simulation_api_schema::ImuSensorConfiguration configuration; configuration.set_entity(entity_ref); @@ -442,6 +438,8 @@ class SimulatorCore return configuration; }()); + auto ego_entity = core->getEgoEntity(entity_ref); + ego_entity->setParameter( "allow_goal_modification", controller.properties.template get("allowGoalModification")); @@ -478,8 +476,7 @@ class SimulatorCore template static auto applyAssignRouteAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - return entity->requestAssignRoute(std::forward(xs)...); + return core->getEntity(entity_ref)->requestAssignRoute(std::forward(xs)...); } template @@ -491,22 +488,20 @@ class SimulatorCore template static auto applyFollowTrajectoryAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - return entity->requestFollowTrajectory(std::forward(xs)...); + return core->getEntity(entity_ref) + ->requestFollowTrajectory(std::forward(xs)...); } template static auto applyLaneChangeAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - return entity->requestLaneChange(std::forward(xs)...); + return core->getEntity(entity_ref)->requestLaneChange(std::forward(xs)...); } template static auto applySpeedAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - return entity->requestSpeedChange(std::forward(xs)...); + return core->getEntity(entity_ref)->requestSpeedChange(std::forward(xs)...); } template < @@ -524,19 +519,15 @@ class SimulatorCore static auto applyTeleportAction( const std::string & name, const std::string & reference_entity_name, Ts &&... xs) { - if (const auto reference_entity = core->getEntityOrNullptr(reference_entity_name)) { - return core->getEntity(name)->setStatus( - reference_entity->getMapPose(), std::forward(xs)...); - } else { - throw Error("Reference entity \"", reference_entity_name, "\" does not exist"); - } + const auto reference_entity = core->getEntity(reference_entity_name); + return core->getEntity(name)->setStatus( + reference_entity->getMapPose(), std::forward(xs)...); } template static auto applyWalkStraightAction(const std::string & entity_ref, Ts &&... xs) { - auto entity = core->getEntity(entity_ref); - return entity->requestWalkStraight(std::forward(xs)...); + return core->getEntity(entity_ref)->requestWalkStraight(std::forward(xs)...); } }; @@ -595,9 +586,9 @@ class SimulatorCore static auto evaluateTimeHeadway( const std::string & from_entity_name, const std::string & to_entity_name) { - if (auto from_entity = core->getEntityOrNullptr(from_entity_name); from_entity) { - if (auto to_entity = core->getEntityOrNullptr(to_entity_name); to_entity) { - if (auto relative_pose = traffic_simulator::pose::relativePose( + if (const auto from_entity = core->getEntityOrNullptr(from_entity_name)) { + if (const auto to_entity = core->getEntityOrNullptr(to_entity_name)) { + if (const auto relative_pose = traffic_simulator::pose::relativePose( from_entity->getMapPose(), to_entity->getMapPose()); relative_pose && relative_pose->position.x <= 0) { const double time_headway = @@ -667,7 +658,6 @@ class SimulatorCore return Double::nan(); } - template static auto engage(const std::string & ego_ref) -> decltype(auto) { return core->getEgoEntity(ego_ref)->engage(); diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index b6e413ac281..a1484f494b1 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -105,11 +105,9 @@ auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const -> std::vector { std::vector ret; - for (const auto & status : other_entity_status) { - if ( - status.second.isInLanelet() && - traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { - ret.emplace_back(status.second); + for (const auto & [name, status] : other_entity_status) { + if (status.isInLanelet() && traffic_simulator::isSameLaneletId(status, lanelet_id)) { + ret.emplace_back(status); } } return ret; @@ -156,14 +154,13 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) std::vector ret; const auto lanelet_ids_list = hdmap_utils->getRightOfWayLaneletIds(following_lanelets); - for (const auto & status : other_entity_status) { + for (const auto & [name, status] : other_entity_status) { for (const auto & following_lanelet : following_lanelets) { for (const lanelet::Id & lanelet_id : lanelet_ids_list.at(following_lanelet)) { if ( - status.second.isInLanelet() && - traffic_simulator::isSameLaneletId(status.second, lanelet_id) && + status.isInLanelet() && traffic_simulator::isSameLaneletId(status, lanelet_id) && not is_the_same_right_of_way(lanelet_id, following_lanelet)) { - ret.emplace_back(status.second); + ret.emplace_back(status); } } } @@ -183,12 +180,10 @@ auto ActionNode::getRightOfWayEntities() const if (lanelet_ids.empty()) { return ret; } - for (const auto & status : other_entity_status) { + for (const auto & [name, status] : other_entity_status) { for (const lanelet::Id & lanelet_id : lanelet_ids) { - if ( - status.second.isInLanelet() && - traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { - ret.emplace_back(status.second); + if (status.isInLanelet() && traffic_simulator::isSameLaneletId(status, lanelet_id)) { + ret.emplace_back(status); } } } @@ -240,11 +235,11 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf { std::vector distances; std::vector entities; - for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(spline, each.first); + for (const auto & [name, status] : other_entity_status) { + const auto distance = getDistanceToTargetEntityPolygon(spline, name); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, - other_entity_status.at(each.first).getMapPose().orientation); + other_entity_status.at(name).getMapPose().orientation); /** * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity. */ @@ -252,7 +247,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <= boost::math::constants::half_pi()) { if (distance && distance.value() < 40) { - entities.emplace_back(each.first); + entities.emplace_back(name); distances.emplace_back(distance.value()); } } @@ -348,12 +343,12 @@ auto ActionNode::getConflictingEntityStatusOnCrossWalk(const lanelet::Ids & rout { std::vector conflicting_entity_status; auto conflicting_crosswalks = hdmap_utils->getConflictingCrosswalkIds(route_lanelets); - for (const auto & status : other_entity_status) { + for (const auto & [name, status] : other_entity_status) { if ( - status.second.isInLanelet() && std::count( - conflicting_crosswalks.begin(), conflicting_crosswalks.end(), - status.second.getLaneletId()) >= 1) { - conflicting_entity_status.emplace_back(status.second); + status.isInLanelet() && + std::count( + conflicting_crosswalks.begin(), conflicting_crosswalks.end(), status.getLaneletId()) >= 1) { + conflicting_entity_status.emplace_back(status); } } return conflicting_entity_status; @@ -364,12 +359,11 @@ auto ActionNode::getConflictingEntityStatusOnLane(const lanelet::Ids & route_lan { std::vector conflicting_entity_status; auto conflicting_lanes = hdmap_utils->getConflictingLaneIds(route_lanelets); - for (const auto & status : other_entity_status) { + for (const auto & [name, status] : other_entity_status) { if ( - status.second.isInLanelet() && - std::count( - conflicting_lanes.begin(), conflicting_lanes.end(), status.second.getLaneletId()) >= 1) { - conflicting_entity_status.emplace_back(status.second); + status.isInLanelet() && + std::count(conflicting_lanes.begin(), conflicting_lanes.end(), status.getLaneletId()) >= 1) { + conflicting_entity_status.emplace_back(status); } } return conflicting_entity_status; @@ -379,17 +373,16 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) { auto conflicting_crosswalks = hdmap_utils->getConflictingCrosswalkIds(following_lanelets); auto conflicting_lanes = hdmap_utils->getConflictingLaneIds(following_lanelets); - for (const auto & status : other_entity_status) { + for (const auto & [name, status] : other_entity_status) { if ( - status.second.isInLanelet() && std::count( - conflicting_crosswalks.begin(), conflicting_crosswalks.end(), - status.second.getLaneletId()) >= 1) { + status.isInLanelet() && + std::count( + conflicting_crosswalks.begin(), conflicting_crosswalks.end(), status.getLaneletId()) >= 1) { return true; } if ( - status.second.isInLanelet() && - std::count( - conflicting_lanes.begin(), conflicting_lanes.end(), status.second.getLaneletId()) >= 1) { + status.isInLanelet() && + std::count(conflicting_lanes.begin(), conflicting_lanes.end(), status.getLaneletId()) >= 1) { return true; } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1c7879cfe5f..55b2268d603 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -77,7 +77,8 @@ class API entity_manager_ptr_->getHdmapUtils(), [this]() { return entity_manager_ptr_->getEntityNames(); }, [this](const auto & entity_name) { return getEntity(entity_name)->getMapPose(); }, - [this](const auto & name) { return API::despawn(name); }, configuration.auto_sink)), + [this](const auto & entity_name) { return API::despawn(entity_name); }, + configuration.auto_sink)), clock_pub_(rclcpp::create_publisher( node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), rclcpp::PublisherOptionsWithAllocator())), diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index c9144204dba..2c1b32e96f2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -35,7 +35,6 @@ class CanonicalizedEntityStatus const EntityStatus & may_non_canonicalized_entity_status, const std::optional & canonicalized_lanelet_pose); CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj); - CanonicalizedEntityStatus(CanonicalizedEntityStatus && obj) noexcept; explicit operator EntityStatus() const noexcept { return entity_status_; } auto set(const CanonicalizedEntityStatus & status) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 50496ed7bda..bda5fbbb528 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -111,7 +111,11 @@ class EgoEntity : public VehicleEntity const speed_change::RelativeTargetSpeed &, const speed_change::Transition, const speed_change::Constraint, const bool continuous) -> void override; - void requestClearRoute() override; + auto requestClearRoute() -> void; + + auto requestReplanRoute(const std::vector & route) -> void; + + auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void; auto isControlledBySimulator() const -> bool override; @@ -155,9 +159,7 @@ class EgoEntity : public VehicleEntity auto engage() -> void; auto isEngaged() const -> bool; auto isEngageable() const -> bool; - auto replanRoute(const std::vector & route) -> void; auto sendCooperateCommand(const std::string & module_name, const std::string & command) -> void; - auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void; auto getMinimumRiskManeuverBehaviorName() const -> std::string; auto getMinimumRiskManeuverStateName() const -> std::string; auto getEmergencyStateName() const -> std::string; 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 164f6e5065d..524dadbda37 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -104,7 +104,7 @@ class EntityBase virtual auto getGoalPoses() -> std::vector = 0; - /* */ auto isStopping() const -> bool; + /* */ auto isStopped() const -> bool; /* */ auto isInPosition( const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool; @@ -174,8 +174,6 @@ class EntityBase virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool); - virtual void requestClearRoute() {} - virtual auto isControlledBySimulator() const -> bool; virtual auto setControlledBySimulator(bool) -> void; 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 0680c3fe4d5..f3ac581c079 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -180,28 +180,10 @@ class EntityManager auto getEntity(const std::string & name) const -> std::shared_ptr; - auto getEgoEntity() const -> std::shared_ptr - { - for (const auto & [name, entity] : entities_) { - if (entity->template is()) { - return std::dynamic_pointer_cast(entity); - } - } - THROW_SEMANTIC_ERROR("getEgoEntity function was called, but ego vehicle does not exist"); - } + auto getEgoEntity() const -> std::shared_ptr; auto getEgoEntity(const std::string & name) const - -> std::shared_ptr - { - if (auto it = entities_.find(name); it == entities_.end()) { - THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); - } else { - if (auto ego_entity = std::dynamic_pointer_cast(it->second); !ego_entity) { - THROW_SEMANTIC_ERROR("entity : ", name, " exists, but it is not ego"); - } else - return ego_entity; - } - } + -> std::shared_ptr; auto getHdmapUtils() -> const std::shared_ptr &; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 41b6088c5ff..0eefafb5603 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -47,7 +47,7 @@ bool API::despawn(const std::string & name) bool API::despawnEntities() { - auto entities = entity_manager_ptr_->getEntityNames(); + const auto entities = entity_manager_ptr_->getEntityNames(); return std::all_of( entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); }); } @@ -92,7 +92,7 @@ auto API::respawn( ego_entity->setMapPose(entity_status.pose); ego_entity->setTwist(entity_status.action_status.twist); ego_entity->setAcceleration(entity_status.action_status.accel); - ego_entity->replanRoute({goal_pose}); + ego_entity->requestReplanRoute({goal_pose}); } } } @@ -101,12 +101,12 @@ bool API::checkCollision( const std::string & first_entity_name, const std::string & second_entity_name) { if (first_entity_name != second_entity_name) { - if (const auto first_entity = getEntityOrNullptr(first_entity_name)) { - if (const auto second_entity = getEntityOrNullptr(second_entity_name)) { - return math::geometry::checkCollision2D( - first_entity->getMapPose(), first_entity->getBoundingBox(), second_entity->getMapPose(), - second_entity->getBoundingBox()); - } + const auto first_entity = getEntityOrNullptr(first_entity_name); + const auto second_entity = getEntityOrNullptr(second_entity_name); + if (first_entity && second_entity) { + return math::geometry::checkCollision2D( + first_entity->getMapPose(), first_entity->getBoundingBox(), second_entity->getMapPose(), + second_entity->getBoundingBox()); } } return false; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 338de514b19..61655d31b7a 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -52,12 +52,6 @@ CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntitySt { } -CanonicalizedEntityStatus::CanonicalizedEntityStatus(CanonicalizedEntityStatus && obj) noexcept -: canonicalized_lanelet_pose_(std::move(obj.canonicalized_lanelet_pose_)), - entity_status_(std::move(obj.entity_status_)) -{ -} - auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> void { assert(getType() == status.getType()); diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 948174fb52a..c63997e73da 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -86,13 +86,6 @@ auto EgoEntity::isEngaged() const -> bool { return field_operator_application->e auto EgoEntity::isEngageable() const -> bool { return field_operator_application->engageable(); } -auto EgoEntity::replanRoute(const std::vector & route) -> void -{ - field_operator_application->clearRoute(); - field_operator_application->plan(route); - field_operator_application->engage(); -} - auto EgoEntity::sendCooperateCommand(const std::string & module_name, const std::string & command) -> void { @@ -206,11 +199,13 @@ void EgoEntity::onUpdate(double current_time, double step_time) void EgoEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { + requestClearRoute(); requestAssignRoute({lanelet_pose}); } void EgoEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) { + requestClearRoute(); requestAssignRoute({map_pose}); } @@ -291,7 +286,15 @@ auto EgoEntity::requestSpeedChange( "purposes only."); } -void EgoEntity::requestClearRoute() { field_operator_application->clearRoute(); } +auto EgoEntity::requestClearRoute() -> void { field_operator_application->clearRoute(); } + +auto EgoEntity::requestReplanRoute(const std::vector & route) + -> void +{ + field_operator_application->clearRoute(); + field_operator_application->plan(route); + field_operator_application->engage(); +} auto EgoEntity::getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 8a57c905014..ac9e7266f5a 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -110,10 +110,9 @@ auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional -> bool { if (const auto lanelet_pose = getCanonicalizedLaneletPose()) { - const auto tolerance_ = + const auto tolerance_value = tolerance ? tolerance.value() : getDefaultMatchingDistanceForLaneletPoseCalculation(); - return traffic_simulator::pose::isInLanelet( - lanelet_pose.value(), lanelet_id, tolerance_, hdmap_utils_ptr_); + return pose::isInLanelet(lanelet_pose.value(), lanelet_id, tolerance_value, hdmap_utils_ptr_); } return false; } @@ -123,7 +122,7 @@ auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> return getBoundingBox().dimensions.y * 0.5 + 1.0; } -auto EntityBase::isStopping() const -> bool +auto EntityBase::isStopped() const -> bool { return std::fabs(getCurrentTwist().linear.x) < std::numeric_limits::epsilon(); } @@ -164,9 +163,9 @@ auto EntityBase::requestLaneChange(const lane_change::Direction & direction) -> { if (isInLanelet()) { if ( - const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( + const auto target_lanelet_id = hdmap_utils_ptr_->getLaneChangeableLaneletId( getCanonicalizedStatus().getLaneletId(), direction)) { - requestLaneChange(target.value()); + requestLaneChange(target_lanelet_id.value()); } } } diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index fcf92485906..df0e0203a98 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -53,6 +53,10 @@ auto EntityManager::broadcastEntityTransform() -> void * so we publish the average of the coordinates of all entities. */ if (isAnyEgoSpawned()) { + if (!is_send) { + pose = getEntity(getEgoName())->getMapPose(); + is_send = true; + } broadcastTransform( geometry_msgs::build() /** @@ -61,9 +65,10 @@ auto EntityManager::broadcastEntityTransform() -> void * so the frame_id "ego" is issued regardless of the name of the ego entity. */ .header(std_msgs::build().stamp(clock_ptr_->now()).frame_id("ego")) - .pose(getEntity(getEgoName())->getMapPose()), + .pose(pose), true); } + if (!names.empty()) { if (!is_send) { pose = geometry_msgs::build() @@ -158,7 +163,30 @@ auto EntityManager::getEntity(const std::string & name) const if (const auto entity = getEntityOrNullptr(name)) { return entity; } else { - THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); + THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " does not exist."); + } +} + +auto EntityManager::getEgoEntity() const -> std::shared_ptr +{ + for (const auto & [name, entity] : entities_) { + if (entity->template is()) { + return std::dynamic_pointer_cast(entity); + } + } + THROW_SEMANTIC_ERROR("EgoEntity does not exist"); +} + +auto EntityManager::getEgoEntity(const std::string & name) const + -> std::shared_ptr +{ + if (auto it = entities_.find(name); it == entities_.end()) { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " does not exist."); + } else { + if (auto ego_entity = std::dynamic_pointer_cast(it->second); !ego_entity) { + THROW_SEMANTIC_ERROR("Entity : ", std::quoted(name), " exists, but it is not ego"); + } else + return ego_entity; } } @@ -239,7 +267,7 @@ void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { const auto reference_entity = getEntity(name); - const CanonicalizedEntityStatus status = reference_entity->getCanonicalizedStatus(); + const auto status = reference_entity->getCanonicalizedStatus(); const auto behavior_parameter = reference_entity->getBehaviorParameter(); if (reference_entity->is()) { THROW_SEMANTIC_ERROR( From a2d095f57fa566048a97f877b15db5d8d6a49afb Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 16 Oct 2024 12:19:29 +0200 Subject: [PATCH 41/49] ref(traffic_simulator): improve requestLaneChange, getEgoName --- .../include/traffic_simulator/api/api.hpp | 1 - .../include/traffic_simulator/entity/ego_entity.hpp | 4 ++-- .../include/traffic_simulator/entity/entity_base.hpp | 4 ++-- .../include/traffic_simulator/entity/vehicle_entity.hpp | 4 ++-- simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +- simulation/traffic_simulator/src/entity/entity_base.cpp | 8 ++++---- .../traffic_simulator/src/entity/entity_manager.cpp | 2 +- .../traffic_simulator/src/entity/vehicle_entity.cpp | 5 +++-- .../include/random_test_runner/test_executor.hpp | 2 +- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 55b2268d603..dc8f68045d3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -325,7 +325,6 @@ class API FORWARD_TO_ENTITY_MANAGER(isEntitySpawned); FORWARD_TO_ENTITY_MANAGER(getEgoEntity); - FORWARD_TO_ENTITY_MANAGER(getEgoName); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index bda5fbbb528..be45228aba2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -99,9 +99,9 @@ class EgoEntity : public VehicleEntity auto requestFollowTrajectory( const std::shared_ptr &) -> void override; - void requestLaneChange(const lanelet::Id) override; + auto requestLaneChange(const lanelet::Id) -> void override; - auto requestLaneChange(const traffic_simulator::lane_change::Parameter &) -> void override; + auto requestLaneChange(const lane_change::Parameter &) -> void override; auto requestSpeedChange( const double, const speed_change::Transition, const speed_change::Constraint, 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 524dadbda37..225ca7a917c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -148,9 +148,9 @@ class EntityBase virtual void requestAssignRoute(const std::vector &) = 0; - virtual void requestLaneChange(const lanelet::Id) {} + virtual auto requestLaneChange(const lanelet::Id) -> void {} - virtual void requestLaneChange(const lane_change::Parameter &) {} + virtual auto requestLaneChange(const lane_change::Parameter &) -> void {} /* */ auto requestLaneChange(const lane_change::Direction & direction) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index 7c4829d7224..27f4dedf111 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -107,9 +107,9 @@ class VehicleEntity : public EntityBase auto requestFollowTrajectory( const std::shared_ptr &) -> void override; - void requestLaneChange(const lanelet::Id to_lanelet_id) override; + auto requestLaneChange(const lanelet::Id to_lanelet_id) -> void override; - void requestLaneChange(const traffic_simulator::lane_change::Parameter &) override; + auto requestLaneChange(const traffic_simulator::lane_change::Parameter &) -> void override; void setVelocityLimit(double linear_velocity) override; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c63997e73da..96d4147e450 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -254,7 +254,7 @@ auto EgoEntity::requestFollowTrajectory( is_controlled_by_simulator_ = true; } -void EgoEntity::requestLaneChange(const lanelet::Id) +auto EgoEntity::requestLaneChange(const lanelet::Id) -> void { THROW_SEMANTIC_ERROR( "From scenario, a lane change was requested to Ego type entity ", std::quoted(name), diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index ac9e7266f5a..cce0c4853f9 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -170,18 +170,18 @@ auto EntityBase::requestLaneChange(const lane_change::Direction & direction) -> } } -void EntityBase::requestLaneChange( +auto EntityBase::requestLaneChange( const traffic_simulator::lane_change::AbsoluteTarget & target, const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, - const traffic_simulator::lane_change::Constraint & constraint) + const traffic_simulator::lane_change::Constraint & constraint) -> void { requestLaneChange(lane_change::Parameter(target, trajectory_shape, constraint)); } -void EntityBase::requestLaneChange( +auto EntityBase::requestLaneChange( const traffic_simulator::lane_change::RelativeTarget & target, const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, - const traffic_simulator::lane_change::Constraint & constraint) + const traffic_simulator::lane_change::Constraint & constraint) -> void { lanelet::Id reference_lanelet_id = 0; if (target.entity_name == name) { diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index df0e0203a98..88c885242e1 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -54,7 +54,7 @@ auto EntityManager::broadcastEntityTransform() -> void */ if (isAnyEgoSpawned()) { if (!is_send) { - pose = getEntity(getEgoName())->getMapPose(); + pose = getEgoEntity()->getMapPose(); is_send = true; } broadcastTransform( diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 57ff4db1d05..37b6f40a3c7 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -229,7 +229,7 @@ auto VehicleEntity::requestFollowTrajectory( route_planner_.setWaypoints(waypoints); } -void VehicleEntity::requestLaneChange(const lanelet::Id to_lanelet_id) +auto VehicleEntity::requestLaneChange(const lanelet::Id to_lanelet_id) -> void { behavior_plugin_ptr_->setRequest(behavior::Request::LANE_CHANGE); const auto parameter = lane_change::Parameter( @@ -238,7 +238,8 @@ void VehicleEntity::requestLaneChange(const lanelet::Id to_lanelet_id) behavior_plugin_ptr_->setLaneChangeParameters(parameter); } -void VehicleEntity::requestLaneChange(const traffic_simulator::lane_change::Parameter & parameter) +auto VehicleEntity::requestLaneChange(const traffic_simulator::lane_change::Parameter & parameter) + -> void { behavior_plugin_ptr_->setRequest(behavior::Request::LANE_CHANGE); behavior_plugin_ptr_->setLaneChangeParameters(parameter); diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 85b02aa648f..7d157689eac 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -167,7 +167,7 @@ class TestExecutor executeWithErrorHandling([this]() { if (!api_->isNpcLogicStarted()) { if (api_->isAnyEgoSpawned()) { - auto ego_entity = api_->getEgoEntity(api_->getEgoName()); + auto ego_entity = api_->getEgoEntity(ego_name_); if (ego_entity->isEngageable()) { api_->startNpcLogic(); } From 74b99dfaae57092830f943f631876ab3fbf7e114 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 8 Nov 2024 13:11:54 +0100 Subject: [PATCH 42/49] ref(traffic_simulator): add const to LongitudinalSpeedPlanner::isTargetSpeedReached arguments --- .../behavior/longitudinal_speed_planning.hpp | 4 ++-- .../src/behavior/longitudinal_speed_planning.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp index 95df82b7990..3c020e164c1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp @@ -50,8 +50,8 @@ class LongitudinalSpeedPlanner auto isDecelerating(double target_speed, const geometry_msgs::msg::Twist & current_twist) const -> bool; auto isTargetSpeedReached( - double target_speed, const geometry_msgs::msg::Twist & current_twist, - double tolerance = 0.01) const noexcept -> bool; + const double target_speed, const geometry_msgs::msg::Twist & current_twist, + const double tolerance = 0.01) const noexcept -> bool; const double step_time; const std::string entity; diff --git a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp index 9c3aee51c81..0a3f47fa73e 100644 --- a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp +++ b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp @@ -134,8 +134,8 @@ auto LongitudinalSpeedPlanner::getRunningDistance( } auto LongitudinalSpeedPlanner::isTargetSpeedReached( - double target_speed, const geometry_msgs::msg::Twist & current_twist, - double tolerance) const noexcept -> bool + const double target_speed, const geometry_msgs::msg::Twist & current_twist, + const double tolerance) const noexcept -> bool { return std::abs(target_speed - current_twist.linear.x) <= tolerance; } From 59e12f2fc0b24bfefd0c1114fb55854151dcc33a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 11:39:57 +0100 Subject: [PATCH 43/49] ref(traffic_simulator): improve requestClearRoute call for ego_entity --- simulation/traffic_simulator/src/entity/ego_entity.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e0ea0da8fb4..e5305a1bafa 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -200,41 +200,36 @@ void EgoEntity::onUpdate(double current_time, double step_time) void EgoEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { - requestClearRoute(); requestAssignRoute({lanelet_pose}); } void EgoEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) { - requestClearRoute(); requestAssignRoute({map_pose}); } void EgoEntity::requestAssignRoute(const std::vector & waypoints) { std::vector route; - for (const auto & waypoint : waypoints) { route.push_back(static_cast(waypoint)); } - requestAssignRoute(route); } void EgoEntity::requestAssignRoute(const std::vector & waypoints) { std::vector route; - for (const auto & waypoint : waypoints) { geometry_msgs::msg::PoseStamped pose_stamped; { pose_stamped.header.frame_id = "map"; pose_stamped.pose = waypoint; } - route.push_back(pose_stamped); } + requestClearRoute(); if (not field_operator_application->initialized()) { field_operator_application->initialize(getMapPose()); field_operator_application->plan(route); From 926c71201c9d8d7b8fafc692305a31d9b60c8137 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 14:29:05 +0100 Subject: [PATCH 44/49] ref(traffic_simulator): tiny formatting change --- .../include/traffic_simulator/entity/entity_base.hpp | 4 ++-- .../include/traffic_simulator/entity/entity_manager.hpp | 2 +- simulation/traffic_simulator/src/entity/entity_base.cpp | 9 +++++---- .../traffic_simulator/src/entity/entity_manager.cpp | 2 +- 4 files changed, 9 insertions(+), 8 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 aa4e72e636d..e47d6f743b6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -106,8 +106,8 @@ class EntityBase /* */ auto isStopped() const -> bool; - /* */ auto isInPosition( - const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool; + /* */ auto isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const + -> bool; /* */ auto isInPosition( const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool; 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 69b61c176ab..d8251820fba 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -168,7 +168,7 @@ class EntityManager bool despawnEntity(const std::string & name); - bool isEntitySpawned(const std::string & name); + auto isEntitySpawned(const std::string & name) const -> bool; auto getEntityNames() const -> const std::vector; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 7a9bceb14da..3ce8416daec 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -95,10 +95,10 @@ auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) con matching_distance, hdmap_utils_ptr_); } -auto EntityBase::isInPosition( - const geometry_msgs::msg::Pose & target_pose, const double tolerance) const -> bool +auto EntityBase::isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const + -> bool { - return math::geometry::getDistance(getMapPose(), target_pose) < tolerance; + return math::geometry::getDistance(getMapPose(), pose) < tolerance; } auto EntityBase::isInPosition( @@ -641,7 +641,8 @@ auto EntityBase::setStatus( setStatus(canonicalized_lanelet_pose.value(), action_status); } else { std::stringstream ss; - ss << "Status can not be set. lanelet pose: " << lanelet_pose << " is not canonicalizable for "; + ss << "Status can not be set. lanelet pose: " << lanelet_pose + << " is cannot be canonicalized for "; THROW_SEMANTIC_ERROR(ss.str(), " entity named: ", std::quoted(name), "."); } } diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 88c885242e1..86bfd7fd3a3 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -126,7 +126,7 @@ bool EntityManager::despawnEntity(const std::string & name) return isEntitySpawned(name) && entities_.erase(name); } -bool EntityManager::isEntitySpawned(const std::string & name) +auto EntityManager::isEntitySpawned(const std::string & name) const -> bool { return entities_.find(name) != std::end(entities_); } From 153e7b1fdefa9a2ec2be06e8d761c872cc59d5a7 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 12 Dec 2024 11:00:36 +0100 Subject: [PATCH 45/49] ref(traffic_simulator): rename isEntitySpawned to isEntityExist --- .../src/crosswalk/stop_at_crosswalk.cpp | 2 +- .../src/pedestrian/walk_straight.cpp | 2 +- .../src/random_scenario/random001.cpp | 12 ++++++------ .../src/traffic_simulation_demo.cpp | 12 ++++++------ .../include/traffic_simulator/api/api.hpp | 4 ++-- .../traffic_simulator/entity/entity_manager.hpp | 2 +- .../traffic_simulator/src/entity/entity_manager.cpp | 4 ++-- .../include/random_test_runner/test_executor.hpp | 2 +- .../random_test_runner/test/test_test_executor.cpp | 2 +- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 3c645ea754d..7e08349f415 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -40,7 +40,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode { const auto t = api_.getCurrentTime(); // LCOV_EXCL_START - if (api_.isEntitySpawned("bob") && api_.checkCollision("ego", "bob")) { + if (api_.isEntityExist("bob") && api_.checkCollision("ego", "bob")) { stop(cpp_mock_scenarios::Result::FAILURE); } /** diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 3ec1dc88d53..fb910e766c9 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -40,7 +40,7 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode { const auto t = api_.getCurrentTime(); // LCOV_EXCL_START - if (api_.isEntitySpawned("bob") && api_.checkCollision("ego", "bob")) { + if (api_.isEntityExist("bob") && api_.checkCollision("ego", "bob")) { stop(cpp_mock_scenarios::Result::FAILURE); } /** diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 8023ebc75a6..620f1583bed 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -98,7 +98,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode { for (int i = 0; i < params_.random_parameters.crossing_pedestrian.number_of_pedestrian; i++) { std::string entity_name = "pedestrian" + std::to_string(i); - if (api_.isEntitySpawned(entity_name)) { + if (api_.isEntityExist(entity_name)) { api_.despawn(entity_name); } } @@ -120,7 +120,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode }; const auto spawn_and_change_lane = [&](const auto & entity_name, const auto spawn_s_value) { - if (!api_.isEntitySpawned(entity_name)) { + if (!api_.isEntityExist(entity_name)) { api_.spawn( entity_name, traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -158,7 +158,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode std::string entity_name = "pedestrian" + std::to_string(entity_index); constexpr lanelet::Id lanelet_id = 34392; if ( - !api_.isEntitySpawned(entity_name) && + !api_.isEntityExist(entity_name) && !ego_entity->isInPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 25.0, 0.0, api_.getHdmapUtils()), @@ -179,7 +179,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode entity->setLinearVelocity(speed); } if ( - api_.isEntitySpawned(entity_name) && + api_.isEntityExist(entity_name) && api_.getEntity(entity_name)->getStandStillDuration() >= 0.5) { api_.despawn(entity_name); } @@ -191,7 +191,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( 34621, 10, 0.0, api_.getHdmapUtils()); constexpr auto entity_name = "spawn_nearby_ego"; - if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntitySpawned(entity_name)) { + if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntityExist(entity_name)) { api_.spawn( entity_name, traffic_simulator::pose::transformRelativePoseToGlobal( @@ -203,7 +203,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); } - if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntitySpawned(entity_name)) { + if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntityExist(entity_name)) { api_.despawn(entity_name); } diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index d53b4cd426e..6ca6d01363d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -42,17 +42,17 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode if (api_.getCurrentTime() >= 60) { stop(cpp_mock_scenarios::Result::SUCCESS); } - if (api_.getCurrentTime() >= 4 && api_.isEntitySpawned("tom")) { + if (api_.getCurrentTime() >= 4 && api_.isEntityExist("tom")) { api_.despawn("tom"); } - if (api_.getCurrentTime() >= 4 && api_.isEntitySpawned("obstacle")) { + if (api_.getCurrentTime() >= 4 && api_.isEntityExist("obstacle")) { api_.getEntity("obstacle") ->setStatus( traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils()), traffic_simulator::helper::constructActionStatus(10)); } - if (api_.getCurrentTime() >= 6 && api_.isEntitySpawned("obstacle")) { + if (api_.getCurrentTime() >= 6 && api_.isEntityExist("obstacle")) { api_.despawn("obstacle"); } const auto ego_entity = api_.getEntity("ego"); @@ -64,7 +64,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode ego_entity->requestAcquirePosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 35026, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.isEntitySpawned("npc2")) { + if (api_.isEntityExist("npc2")) { npc2_entity->requestSpeedChange(13, true); } } @@ -75,7 +75,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode ego_entity->requestAcquirePosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34675, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.isEntitySpawned("npc2")) { + if (api_.isEntityExist("npc2")) { npc2_entity->requestSpeedChange(3, true); } } @@ -88,7 +88,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode 34630, 0.0, 0.0, api_.getHdmapUtils())); npc2_entity->requestSpeedChange(13, true); } - if (api_.getCurrentTime() > 10.0 && api_.isEntitySpawned("bob")) { + if (api_.getCurrentTime() > 10.0 && api_.isEntityExist("bob")) { api_.despawn("bob"); } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index c319cd48e12..32778ca8ee3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -143,7 +143,7 @@ class API { auto register_to_entity_manager = [&]() { if (behavior == VehicleBehavior::autoware()) { - return entity_manager_ptr_->isEntitySpawned(name) or + return entity_manager_ptr_->isEntityExist(name) or entity_manager_ptr_->spawnEntity( name, pose, parameters, getCurrentTime(), configuration, node_parameters_); } else { @@ -321,7 +321,7 @@ class API static_assert(true, "") // clang-format on - FORWARD_TO_ENTITY_MANAGER(isEntitySpawned); + FORWARD_TO_ENTITY_MANAGER(isEntityExist); FORWARD_TO_ENTITY_MANAGER(getEgoEntity); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getEntityOrNullptr); 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 d8251820fba..3041fcf8595 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -168,7 +168,7 @@ class EntityManager bool despawnEntity(const std::string & name); - auto isEntitySpawned(const std::string & name) const -> bool; + auto isEntityExist(const std::string & name) const -> bool; auto getEntityNames() const -> const std::vector; diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 86bfd7fd3a3..bfe923a1001 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -123,10 +123,10 @@ visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const bool EntityManager::despawnEntity(const std::string & name) { - return isEntitySpawned(name) && entities_.erase(name); + return isEntityExist(name) && entities_.erase(name); } -auto EntityManager::isEntitySpawned(const std::string & name) const -> bool +auto EntityManager::isEntityExist(const std::string & name) const -> bool { return entities_.find(name) != std::end(entities_); } diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 7d157689eac..146dad86889 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -197,7 +197,7 @@ class TestExecutor } for (const auto & npc : test_description_.npcs_descriptions) { - if (api_->isEntitySpawned(npc.name) && api_->checkCollision(ego_name_, npc.name)) { + if (api_->isEntityExist(npc.name) && api_->checkCollision(ego_name_, npc.name)) { if (ego_collision_metric_.isThereEgosCollisionWith(npc.name, current_time)) { std::string message = fmt::format("New collision occurred between ego and {}", npc.name); diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 9cd7889a4f2..6d51bb3080e 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -130,7 +130,7 @@ class MockTrafficSimulatorAPI MOCK_METHOD(void, getEntityMock, (const std::string &), (const)); MOCK_METHOD(void, getEgoEntityMock, (const std::string &), (const)); MOCK_METHOD(void, getHdmapUtilsMock, (), ()); - MOCK_METHOD(bool, isEntitySpawned, (const std::string &), ()); + MOCK_METHOD(bool, isEntityExist, (const std::string &), ()); MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); auto getEntity(const std::string & name) const From 27efbd5b026300c80abfe74c3b632870b709e7c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Mon, 16 Dec 2024 12:50:55 +0100 Subject: [PATCH 46/49] ref(behavior_tree_plugin): rename default value of current_action_: "waiting" -> "none" Co-authored-by: Masaya Kataoka --- .../behavior_tree_plugin/transition_events/transition_event.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp index a12bc880398..e2c13e09a29 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/transition_events/transition_event.hpp @@ -36,7 +36,7 @@ class TransitionEvent BT::TimePoint first_timestamp_; std::vector subscribers_; BT::TimestampType type_; - std::string current_action_{"waiting"}; + std::string current_action_{"none"}; }; } // namespace behavior_tree_plugin From 40ff4227e06431d4e86f8294974f214dad1c00f8 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 16 Dec 2024 13:00:13 +0100 Subject: [PATCH 47/49] Merge branch 'master' into RJD-1057-remove-functions-forwarded-to-entity-base-middle --- .github/workflows/custom_spell.json | 4 +- common/math/arithmetic/CHANGELOG.rst | 78 ++++++ common/math/arithmetic/package.xml | 2 +- common/math/geometry/CHANGELOG.rst | 91 +++++++ .../math/geometry/include/geometry/plane.hpp | 45 ++++ .../quaternion/get_angle_difference.hpp | 39 +++ .../geometry/quaternion/get_normal_vector.hpp | 41 +++ common/math/geometry/package.xml | 2 +- common/math/geometry/src/plane.cpp | 39 +++ .../CHANGELOG.rst | 81 ++++++ .../concatenate.hpp | 6 +- .../scenario_simulator_exception/package.xml | 2 +- common/simple_junit/CHANGELOG.rst | 78 ++++++ common/simple_junit/package.xml | 2 +- common/status_monitor/CHANGELOG.rst | 78 ++++++ common/status_monitor/package.xml | 2 +- docs/developer_guide/Communication.md | 81 +++--- docs/developer_guide/OpenSCENARIOSupport.md | 11 +- external/concealer/CHANGELOG.rst | 84 +++++++ .../include/concealer/autoware_universe.hpp | 4 +- external/concealer/package.xml | 2 +- external/concealer/src/autoware_universe.cpp | 31 ++- ...ator_application_for_autoware_universe.cpp | 2 +- external/embree_vendor/CHANGELOG.rst | 78 ++++++ external/embree_vendor/package.xml | 2 +- map/kashiwanoha_map/CHANGELOG.rst | 78 ++++++ map/kashiwanoha_map/package.xml | 2 +- map/simple_cross_map/CHANGELOG.rst | 74 ++++++ map/simple_cross_map/package.xml | 2 +- mock/cpp_mock_scenarios/CHANGELOG.rst | 78 ++++++ mock/cpp_mock_scenarios/package.xml | 2 +- .../CHANGELOG.rst | 78 ++++++ .../package.xml | 2 +- .../openscenario_interpreter/CHANGELOG.rst | 109 ++++++++ .../openscenario_interpreter/CMakeLists.txt | 3 +- .../compatibility.hpp | 30 +++ .../simulator_core.hpp | 10 +- .../syntax/relative_speed_condition.hpp | 2 +- .../syntax/speed_condition.hpp | 6 +- .../syntax/time_to_collision_condition.hpp | 186 ++++++++++++++ .../time_to_collision_condition_target.hpp | 45 ++++ .../openscenario_interpreter/package.xml | 2 +- .../src/compatibility.cpp | 38 +++ .../src/openscenario_interpreter.cpp | 5 + .../src/syntax/entity_condition.cpp | 3 +- .../src/syntax/speed_condition.cpp | 13 +- .../time_to_collision_condition_target.cpp | 38 +++ .../CHANGELOG.rst | 78 ++++++ .../package.xml | 2 +- .../CHANGELOG.rst | 78 ++++++ .../openscenario_interpreter_msgs/package.xml | 2 +- .../openscenario_preprocessor/CHANGELOG.rst | 78 ++++++ .../openscenario_preprocessor/package.xml | 2 +- .../CHANGELOG.rst | 78 ++++++ .../package.xml | 2 +- .../openscenario_utility/CHANGELOG.rst | 78 ++++++ openscenario/openscenario_utility/package.xml | 2 +- .../openscenario_validator/CHANGELOG.rst | 78 ++++++ .../openscenario_validator/package.xml | 2 +- .../openscenario_visualization/CHANGELOG.rst | 78 ++++++ .../openscenario_visualization/package.xml | 2 +- .../CHANGELOG.rst | 78 ++++++ .../package.xml | 2 +- scenario_simulator_v2/CHANGELOG.rst | 78 ++++++ scenario_simulator_v2/package.xml | 2 +- simulation/behavior_tree_plugin/CHANGELOG.rst | 88 +++++++ .../behavior_tree_plugin/action_node.hpp | 2 + simulation/behavior_tree_plugin/package.xml | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 9 +- simulation/do_nothing_plugin/CHANGELOG.rst | 78 ++++++ simulation/do_nothing_plugin/package.xml | 2 +- .../simple_sensor_simulator/CHANGELOG.rst | 96 +++++++ .../detection_sensor/detection_sensor.hpp | 10 + .../simple_sensor_simulator/package.xml | 2 +- .../detection_sensor/detection_sensor.cpp | 48 ++++ .../ego_entity_simulation.cpp | 3 +- simulation/simulation_interface/CHANGELOG.rst | 81 ++++++ simulation/simulation_interface/package.xml | 2 +- simulation/traffic_simulator/CHANGELOG.rst | 95 +++++++ .../data_type/entity_status.hpp | 2 + .../hdmap_utils/hdmap_utils.hpp | 9 +- .../traffic/traffic_source.hpp | 3 +- simulation/traffic_simulator/package.xml | 2 +- .../src/data_type/entity_status.cpp | 5 + .../src/entity/ego_entity.cpp | 2 + .../src/hdmap_utils/hdmap_utils.cpp | 38 ++- .../src/traffic/traffic_source.cpp | 17 +- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 6 +- .../test/src/helper_functions.hpp | 5 +- .../test/src/utils/test_distance.cpp | 116 ++++----- .../traffic_simulator_msgs/CHANGELOG.rst | 78 ++++++ simulation/traffic_simulator_msgs/package.xml | 2 +- test_runner/random_test_runner/CHANGELOG.rst | 78 ++++++ test_runner/random_test_runner/package.xml | 2 +- .../scenario_test_runner/CHANGELOG.rst | 95 +++++++ .../scenario_test_runner/config/workflow.txt | 1 + .../launch/scenario_test_runner.launch.py | 10 +- test_runner/scenario_test_runner/package.xml | 2 +- ...ityCondition.TimeToCollisionCondition.yaml | 235 ++++++++++++++++++ .../scenario_test_runner/scenario/sample.yaml | 13 +- 100 files changed, 3464 insertions(+), 186 deletions(-) create mode 100644 common/math/geometry/include/geometry/plane.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp create mode 100644 common/math/geometry/src/plane.cpp create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp create mode 100644 openscenario/openscenario_interpreter/src/compatibility.cpp create mode 100644 openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp create mode 100644 test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index 676f628922b..50ca6f5fe94 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -46,6 +46,8 @@ "Tschirnhaus", "walltime", "xerces", - "xercesc" + "xercesc", + "Szymon", + "Parapura" ] } diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index b664724b675..c7b6a88b70f 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 908b76bdea3..f537c68e4ff 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.0.2 + 7.3.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 398d518f7aa..66ce967d505 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,97 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Remove comment +* Merge branch 'master' into feature/multi-level-lanelet-support +* Fix missing newline at end of file +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Cleanup +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Add new static member function `RelativeSpeedCondition::evaluate` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp new file mode 100644 index 00000000000..509e3c6e63f --- /dev/null +++ b/common/math/geometry/include/geometry/plane.hpp @@ -0,0 +1,45 @@ +// 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. + +#ifndef GEOMETRY__PLANE_HPP_ +#define GEOMETRY__PLANE_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ + +/// @class Plane +/// @brief Represents a plane in 3D space, defined by a normal vector and a point on the plane. +/// +/// The plane is described using the equation: +/// Ax + By + Cz + D = 0 +/// where: +/// - A, B, C are the components of the normal vector (normal_ attribute). +/// - D is the offset from the origin, calculated using the point and normal vector (d_ attribute). +struct Plane +{ + Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); + auto offset(const geometry_msgs::msg::Point & point) const -> double; + + const geometry_msgs::msg::Vector3 normal_; + const double d_; +}; +} // namespace geometry +} // namespace math +#endif // GEOMETRY__PLANE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp new file mode 100644 index 00000000000..4e9450b2b0a --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -0,0 +1,39 @@ +// 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. + +#ifndef GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ +#define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ + +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getAngleDifference(const T & quat1, const T & quat2) -> double +{ + const Eigen::Quaterniond q1(quat1.w, quat1.x, quat1.y, quat1.z); + const Eigen::Quaterniond q2(quat2.w, quat2.x, quat2.y, quat2.z); + + const Eigen::AngleAxisd delta(q1.inverse() * q2); + + return std::abs(delta.angle()); // [rad] +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp new file mode 100644 index 00000000000..318a239c16d --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp @@ -0,0 +1,41 @@ +// 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. + +#ifndef GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ +#define GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3 +{ + const Eigen::Matrix3d rotation_matrix = getRotationMatrix(orientation); + + return geometry_msgs::build() + .x(rotation_matrix(0, 2)) + .y(rotation_matrix(1, 2)) + .z(rotation_matrix(2, 2)); +} + +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 5116ae32538..8a7bc2b4d2c 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.0.2 + 7.3.0 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp new file mode 100644 index 00000000000..6439839443c --- /dev/null +++ b/common/math/geometry/src/plane.cpp @@ -0,0 +1,39 @@ +// 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 + +namespace math +{ +namespace geometry +{ +Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) +: normal_(normal), d_(-(normal.x * point.x + normal.y * point.y + normal.z * point.z)) +{ + if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { + THROW_SIMULATION_ERROR("Plane cannot be created using zero normal vector."); + } else if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { + THROW_SIMULATION_ERROR("Plane cannot be created using point with NaN value."); + } +} + +auto Plane::offset(const geometry_msgs::msg::Point & point) const -> double +{ + return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; +} +} // namespace geometry +} // namespace math diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index cf3bc3adf29..bafb69b3ca4 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,87 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp index 0e967d66821..efb9139b7e2 100644 --- a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp +++ b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp @@ -25,12 +25,8 @@ namespace common inline namespace scenario_simulator_exception { inline auto concatenate = [](auto &&... xs) { - auto write = [](auto && os, auto && x) { - os.get() << std::forward(x); - return std::forward(os); - }; std::stringstream result; - fold_left(write, std::ref(result), std::forward(xs)...); + (result << ... << std::forward(xs)); return result.str(); }; } // namespace scenario_simulator_exception diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index a005e5748d6..3559db66e79 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.0.2 + 7.3.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 4a6b3ee0902..d6f1cd208f2 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 25a635b3db9..7befc153190 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.0.2 + 7.3.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 8fe30da0cf0..13c51cc56d2 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 59d9d6502a2..4ce85918d8d 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.0.2 + 7.3.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 48d22522178..1da73ed82f0 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -5,48 +5,45 @@ | topic | type | note | |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition `currentEmergencyState` | +| `/api/external/get/rtc_status` | [`tier4_rtc_msgs/msg/CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition `currentMinimumRiskManeuverState` | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | +| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition `currentAutowareState` | | `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | | `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | -| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`tier4_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/PathWithLaneId.msg) | | -| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | -| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | -| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | -| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | -| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in `/simulation/openscenario_interpreter` | ### Publishers -| topic | type | note | -|------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | -| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | -| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | | -| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | -| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | -| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | -| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | -| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | -| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | -| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | - -[//]: # (| /rosout | rcl_interfaces/msg/Log | | |) -[//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) -[//]: # (| /parameter_events | rcl_interfaces/msg/ParameterEvent | | |) +| topic | type | note | +|------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | +| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | +| `/localization/acceleration` | [`geometry_msgs/msg/AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | Available if option `simulate_localization` is `true` (default is `true`). | +| `/localization/pose_estimator/pose_with_covariance` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Available if option `simulate_localization` is `false` (default is `true`). | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | +| `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by `simple_sensor_simulator`](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | +| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | +| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | +| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | ### Service Clients @@ -54,8 +51,8 @@ |-----------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|------| | `/api/autoware/set/velocity_limit` | [`tier4_external_api_msgs/srv/SetVelocityLimit`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/SetVelocityLimit.srv) | | | `/api/external/set/engage` | [`tier4_external_api_msgs/srv/Engage`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/Engage.srv) | | -| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | | `/api/external/set/rtc_auto_mode` | [`tier4_rtc_msgs/srv/AutoModeWithModule`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/AutoModeWithModule.srv) | | +| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | | `/api/operation_mode/enable_autoware_control` | [`autoware_adapi_v1_msgs/srv/ChangeOperationMode`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/operation_mode/srv/ChangeOperationMode.srv) | | ### Service Servers @@ -63,15 +60,3 @@ | service name | type | note | |---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------| | `/control/control_mode_request` | [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv) | Simulated by `simple_sensor_simulator` for a manual override | - -[//]: # (/simulation/openscenario_visualizer) - -[//]: # (Subscribers:) - -[//]: # (/simulation/entity/status: traffic_simulator_msgs/msg/EntityStatusWithTrajectoryArray) - -[//]: # (Publishers:) - -[//]: # (/simulation/entity/marker: visualization_msgs/msg/MarkerArray) - -[//]: # () diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index 2f64a14c51c..d8821dd97da 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -486,8 +486,8 @@ Currently, the only way to know the result of the simulation is by viewing the s | TimeOfDay | 1.3 | | | TimeOfDayCondition | unimplemented | | | TimeReference | 1.3 | | -| TimeToCollisionCondition | unimplemented | | -| TimeToCollisionConditionTarget | unimplemented | | +| TimeToCollisionCondition | 1.3.1 (partial) | [detail](#TimeToCollisionCondition) | +| TimeToCollisionConditionTarget | 1.3.1 | | | Timing | 1.3 | | | TrafficAction | unimplemented | | | TrafficArea | unimplemented | | @@ -848,6 +848,13 @@ Currently, the only way to know the result of the simulation is by viewing the s - Property `freespace` is ignored. - The simulator behaves as if `freespace` is `false`. +#### TimeToCollisionCondition + +- Since `TimeToCollisionCondition` is implemented using `DistanceCondition`, + `RelativeDistanceCondition`, `SpeedCondition`, and `RelativeSpeedCondition`, + if a combination of properties that is not supported by those Conditions is + given to `TimeToCollisionCondition`, an error will be thrown. + #### TransitionDynamics - Property `followingMode` created in version 1.2 is ignored. diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index c9ec6fde662..cc180c689ca 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,90 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add the missing semicolon +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* fix(concealer): increase max time to request enable autoware control +* Contributors: Kotaro Yoshimoto, satoshi-ota + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index d1b4a7bcc8d..0692c7717c0 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -45,6 +45,7 @@ class AutowareUniverse : public Autoware using GearCommand = autoware_vehicle_msgs::msg::GearCommand; using GearReport = autoware_vehicle_msgs::msg::GearReport; using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; @@ -57,6 +58,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setAcceleration; PublisherWrapper setOdometry; + PublisherWrapper setPose; PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; @@ -83,7 +85,7 @@ class AutowareUniverse : public Autoware auto stopAndJoin() -> void; public: - CONCEALER_PUBLIC explicit AutowareUniverse(); + CONCEALER_PUBLIC explicit AutowareUniverse(bool); ~AutowareUniverse(); diff --git a/external/concealer/package.xml b/external/concealer/package.xml index a5971def2e4..248f73c126d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.0.2 + 7.3.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 02aea619014..da3d240d571 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -16,15 +16,25 @@ namespace concealer { -AutowareUniverse::AutowareUniverse() +AutowareUniverse::AutowareUniverse(bool simulate_localization) : getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), - setAcceleration("/localization/acceleration", *this), - setOdometry("/localization/kinematic_state", *this), + setAcceleration( + simulate_localization ? "/localization/acceleration" + : "/simulation/debug/localization/acceleration", + *this), + setOdometry( + simulate_localization ? "/localization/kinematic_state" + : "/simulation/debug/localization/kinematic_state", + *this), + setPose( + simulate_localization ? "/simulation/debug/localization/pose_estimator/pose_with_covariance" + : "/localization/pose_estimator/pose_with_covariance", + *this), setSteeringReport("/vehicle/status/steering_status", *this), setGearReport("/vehicle/status/gear_status", *this), setControlModeReport("/vehicle/status/control_mode", *this), @@ -121,6 +131,21 @@ auto AutowareUniverse::updateLocalization() -> void return message; }()); + setPose([this]() { + // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 + geometry_msgs::msg::PoseWithCovarianceStamped message; + message.header.stamp = get_clock()->now(); + message.header.frame_id = "map"; + message.pose.pose = current_pose.load(); + message.pose.covariance.at(6 * 0 + 0) = 0.0225; // XYZRPY_COV_IDX::X_X + message.pose.covariance.at(6 * 1 + 1) = 0.0225; // XYZRPY_COV_IDX::Y_Y + message.pose.covariance.at(6 * 2 + 2) = 0.0225; // XYZRPY_COV_IDX::Z_Z + message.pose.covariance.at(6 * 3 + 3) = 0.000625; // XYZRPY_COV_IDX::ROLL_ROLL + message.pose.covariance.at(6 * 4 + 4) = 0.000625; // XYZRPY_COV_IDX::PITCH_PITCH + message.pose.covariance.at(6 * 5 + 5) = 0.000625; // XYZRPY_COV_IDX::YAW_YAW + return message; + }()); + setTransform(current_pose.load()); } diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index e75ff70ab01..b3438d35a89 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -475,7 +475,7 @@ auto FieldOperatorApplicationFor::enableAutowareControl() -> v { task_queue.delay([this]() { auto request = std::make_shared(); - requestEnableAutowareControl(request); + requestEnableAutowareControl(request, 30); }); } diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 6661d8bf53c..e605b06641b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,84 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 8acfa0e4a14..3defb0ef4dc 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.0.2 + 7.3.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index db775da252e..4448ce20d6b 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 540a6e44ea6..afd821ef354 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.0.2 + 7.3.0 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst index 9114c7740ce..88832ce9abb 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,80 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 0303b2c1d0e..4996688461e 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.0.2 + 7.3.0 map package for simple cross Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index f493b68bfa7..061df30c948 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 175546f8e77..958c0bc5792 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.0.2 + 7.3.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 4d0dcabb2da..fd15942339e 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 31f6f183bb3..573580351d8 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.0.2 + 7.3.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 813f25e053e..38ebf1dc490 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,115 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition + Feature/time to collision condition +* Update `TimeToCollisionCondition` to call `SpeedCondition` in standard compatible mode +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Remove static member function `evaluateTimeToCollisionCondition` +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Cleanup +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Fix `evaluateTimeToCollisionCondition` to not return meaningless value when collisions cannot occur +* Fix `TimeToCollisionCondition` to return inf if relative speed < zero +* Cleanup static member function `TimeToCollisionCondition::evaluate` +* Add new member function `evaluateCartesianTimeToCollisionCondition` +* Split `(Relative)?DistanceCondition::evaluate` into two overloads +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Add new static member function `TimeToCollisionCondition::evaluate` +* Update `unordered_map` of the `Entities` base class to private +* Move function `hypot` into new header `cmath/hypot.hpp` +* Add support for `DirectionalDimension` to `SpeedCondition` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Rename `(Relative)?DistanceCondition::distance` to `evaluate` +* Update member function `CoordinateSystem::distance` to be static member +* Add `const Position &` to the argument of `DistanceCondition::distance` +* Remove data member `DistanceCondition::consider_z` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Add new static member function `RelativeSpeedCondition::evaluate` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Update `RelativeDistanceCondition::distance` to static member function +* Move entity existence check into `distance` from speceialized `distance` +* Add static member function `ConditionEvaluation::evaluateRelativeSpeed` +* Add new structs `RelativeSpeedCondition` and `DirectionalDimension` +* Add new struct `TimeToCollisionConditionTarget` +* Add new struct `TimeToCollisionCondition` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Add new enumeration `Compatibility` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter/CMakeLists.txt b/openscenario/openscenario_interpreter/CMakeLists.txt index 72ac634e062..a1d95ea6350 100644 --- a/openscenario/openscenario_interpreter/CMakeLists.txt +++ b/openscenario/openscenario_interpreter/CMakeLists.txt @@ -59,8 +59,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_POSIX_SOURCES} ${${PROJECT_NAME}_SYNTAX_SOURCES} ${${PROJECT_NAME}_UTILITY_SOURCES} - src/object.cpp + src/compatibility.cpp src/evaluate.cpp + src/object.cpp src/openscenario_interpreter.cpp src/record.cpp src/scope.cpp) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp new file mode 100644 index 00000000000..683ba6f2377 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp @@ -0,0 +1,30 @@ +// 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. + +#ifndef OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ +#define OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ + +#include + +namespace openscenario_interpreter +{ +enum class Compatibility { + legacy, + standard, +}; + +auto operator>>(std::istream &, Compatibility &) -> std::istream &; +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index fb7aca4340a..087534e826b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -575,15 +575,9 @@ class SimulatorCore if (const auto observer = core->getEntity(from.name())) { if (const auto observed = core->getEntity(to.name())) { auto velocity = [](const auto & entity) -> Eigen::Vector3d { - auto direction = [](auto orientation) -> Eigen::Vector3d { - const auto euler_angle = math::geometry::convertQuaternionToEulerAngle(orientation); - const auto r = euler_angle.x; - const auto p = euler_angle.y; - const auto y = euler_angle.z; - return Eigen::Vector3d( - std::cos(y) * std::cos(p), std::sin(y) * std::cos(p), std::sin(p)); + auto direction = [](const auto & q) -> Eigen::Vector3d { + return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX(); }; - return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index e88e351fae1..c79bfdf34ff 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -26,7 +26,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - RelativeSpeedCondition 1.3.1 + RelativeSpeedCondition (OpenSCENARIO XML 1.3.1) The current relative speed of a triggering entity/entities to a reference entity is compared to a given value. The logical operator used for the diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 9c2bf1ffb92..f64c7499051 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_ +#include #include #include #include @@ -63,6 +64,8 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio std::vector> results; // for description + static inline auto compatibility = Compatibility::legacy; + explicit SpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; @@ -70,7 +73,8 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio static auto evaluate(const Entities *, const Entity &) -> Eigen::Vector3d; static auto evaluate( - const Entities *, const Entity &, const std::optional &) -> double; + const Entities *, const Entity &, const std::optional &, + const Compatibility = Compatibility::legacy) -> double; auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp new file mode 100644 index 00000000000..3dc68ba3cd9 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -0,0 +1,186 @@ +// 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. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionCondition (OpenSCENARIO XML 1.3.1) + + The currently predicted time to collision of a triggering entity/entities + and either a reference entity or an explicit position is compared to a given + time value. Time to collision is calculated as the distance divided by the + relative speed. Acceleration of entities is not taken into consideration. + For the relative speed calculation the same coordinate system and relative + distance type applies as for the distance calculation. If the relative speed + is negative, which means the entity is moving away from the position / the + entities are moving away from each other, then the time to collision cannot + be predicted and the condition evaluates to false. The logical operator for + comparison is defined by the rule attribute. The property "alongRoute" is + deprecated. If "coordinateSystem" or "relativeDistanceType" are set, + "alongRoute" is ignored. + + + + + + + + + deprecated + + + + + + + + + + +*/ +struct TimeToCollisionCondition : private Scope, private SimulatorCore::ConditionEvaluation +{ + const TimeToCollisionConditionTarget time_to_collision_condition_target; + + const Boolean freespace; + + const Rule rule; + + const Double value; + + const RelativeDistanceType relative_distance_type; + + const CoordinateSystem coordinate_system; + + const RoutingAlgorithm routing_algorithm; + + const TriggeringEntities triggering_entities; + + std::vector> evaluations; + + explicit TimeToCollisionCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : Scope(scope), + time_to_collision_condition_target( + readElement("TimeToCollisionConditionTarget", node, scope)), + freespace(readAttribute("freespace", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + relative_distance_type( + readAttribute("relativeDistanceType", node, scope)), + coordinate_system( + readAttribute("coordinateSystem", node, scope, CoordinateSystem::entity)), + routing_algorithm(readAttribute( + "routingAlgorithm", node, scope, RoutingAlgorithm::undefined)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), {Double::nan()}) + { + } + + auto description() const + { + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s time-to-collision to given "; + + if (time_to_collision_condition_target.is()) { + description << " entity " << time_to_collision_condition_target.as(); + } else { + description << " position"; + } + + description << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); + } + + static auto evaluate( + const Entities * entities, const Entity & triggering_entity, + const TimeToCollisionConditionTarget & time_to_collision_condition_target, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, Boolean freespace) + { + auto distance = [&]() { + if (time_to_collision_condition_target.is()) { + return RelativeDistanceCondition::evaluate( + entities, triggering_entity, time_to_collision_condition_target.as(), + coordinate_system, relative_distance_type, routing_algorithm, freespace); + } else { + return DistanceCondition::evaluate( + entities, triggering_entity, time_to_collision_condition_target.as(), + coordinate_system, relative_distance_type, routing_algorithm, freespace); + } + }; + + auto speed = [&]() { + auto directional_dimension = [&]() { + switch (relative_distance_type) { + case RelativeDistanceType::longitudinal: + return std::optional(DirectionalDimension::longitudinal); + case RelativeDistanceType::lateral: + return std::optional(DirectionalDimension::lateral); + default: + case RelativeDistanceType::euclidianDistance: + return std::optional(std::nullopt); + }; + }; + if (time_to_collision_condition_target.is()) { + return RelativeSpeedCondition::evaluate( + entities, triggering_entity, time_to_collision_condition_target.as(), + directional_dimension()); + } else { + return SpeedCondition::evaluate( + entities, triggering_entity, directional_dimension(), Compatibility::standard); + } + }; + + return distance() / std::max(speed(), 0.0); + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](const auto & triggering_entity) { + evaluations.push_back(triggering_entity.apply([this](const auto & triggering_entity) { + return evaluate( + global().entities, triggering_entity, time_to_collision_condition_target, + coordinate_system, relative_distance_type, routing_algorithm, freespace); + })); + return not evaluations.back().size() or std::invoke(rule, evaluations.back(), value).min(); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp new file mode 100644 index 00000000000..8e15f1f6f3c --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp @@ -0,0 +1,45 @@ +// 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. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ + +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionConditionTarget (OpenSCENARIO XML 1.3.1) + + Target position used in the TimeToCollisionCondition. Can be defined as + either an explicit position, or the position of a reference entity. + + + + + + + +*/ +struct TimeToCollisionConditionTarget : public ComplexType +{ + explicit TimeToCollisionConditionTarget(const pugi::xml_node &, Scope &); +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index c7d497d8741..ff4f7516cc7 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.0.2 + 7.3.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/src/compatibility.cpp b/openscenario/openscenario_interpreter/src/compatibility.cpp new file mode 100644 index 00000000000..617a5bcc08b --- /dev/null +++ b/openscenario/openscenario_interpreter/src/compatibility.cpp @@ -0,0 +1,38 @@ +// 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 + +namespace openscenario_interpreter +{ +auto operator>>(std::istream & input, Compatibility & compatiblity) -> std::istream & +{ + if (auto token = std::string(); input >> token) { + if (token == "legacy") { + compatiblity = Compatibility::legacy; + return input; + } else if (token == "standard") { + compatiblity = Compatibility::standard; + return input; + } else { + throw Error( + "Unknown compatiblity ", std::quoted(token), + " was specified. It must be \"legacy\" or \"standard\"."); + } + } else { + compatiblity = Compatibility::legacy; + return input; + } +} +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index cdcea9c17fb..7ac5ffd3a36 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,10 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options) DECLARE_PARAMETER(output_directory); DECLARE_PARAMETER(publish_empty_context); DECLARE_PARAMETER(record); + + declare_parameter("speed_condition", "legacy"); + SpeedCondition::compatibility = + boost::lexical_cast(get_parameter("speed_condition").as_string()); } Interpreter::~Interpreter() {} diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index 911b17c12de..554c5259963 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -38,7 +39,7 @@ EntityCondition::EntityCondition( std::make_pair( "CollisionCondition", [&](const auto & node) { return make< CollisionCondition>(node, scope, triggering_entities); }), std::make_pair( "OffroadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), std::make_pair( "TimeHeadwayCondition", [&](const auto & node) { return make< TimeHeadwayCondition>(node, scope, triggering_entities); }), - std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { return make< TimeToCollisionCondition>(node, scope, triggering_entities); }), std::make_pair( "AccelerationCondition", [&](const auto & node) { return make< AccelerationCondition>(node, scope, triggering_entities); }), std::make_pair( "StandStillCondition", [&](const auto & node) { return make< StandStillCondition>(node, scope, triggering_entities); }), std::make_pair( "SpeedCondition", [&](const auto & node) { return make< SpeedCondition>(node, scope, triggering_entities); }), diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index cc8d64b0c41..5e2f8e34038 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -58,7 +58,8 @@ auto SpeedCondition::evaluate(const Entities * entities, const Entity & triggeri auto SpeedCondition::evaluate( const Entities * entities, const Entity & triggering_entity, - const std::optional & direction) -> double + const std::optional & direction, const Compatibility compatibility) + -> double { if (const Eigen::Vector3d v = evaluate(entities, triggering_entity); direction) { switch (*direction) { @@ -71,7 +72,13 @@ auto SpeedCondition::evaluate( return v.z(); } } else { - return v.norm(); + switch (compatibility) { + default: + case Compatibility::legacy: + return v.x(); + case Compatibility::standard: + return v.norm(); + } } } @@ -81,7 +88,7 @@ auto SpeedCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { - return evaluate(global().entities, triggering_entity, direction); + return evaluate(global().entities, triggering_entity, direction, compatibility); })); return not results.back().size() or std::invoke(rule, results.back(), value).min(); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp b/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp new file mode 100644 index 00000000000..829793c7a54 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp @@ -0,0 +1,38 @@ +// 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 + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +TimeToCollisionConditionTarget::TimeToCollisionConditionTarget( + const pugi::xml_node & node, Scope & scope) +// clang-format off +: ComplexType(choice(node, + std::make_pair( "Position", [&](auto && node) { return make(std::forward(node), scope); }), + std::make_pair("EntityRef", [&](auto && node) { return make< Entity>(std::forward(node), scope); }))) +// clang-format on +{ + if (is()) { + throw SyntaxError("EntitySelection is not allowed in TimeToCollisionConditionTarget.entityRef"); + } +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index a600ece6a67..dbaacb10980 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 68ff65fc757..eed406acb5e 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.0.2 + 7.3.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 47881a2a88a..4265a0ec76d 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 057014f0ee9..1d94189bad1 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.0.2 + 7.3.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index a1014e65fde..66a59a00197 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 834978f29f0..02e6c684ff9 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.0.2 + 7.3.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 1f31a52ab4f..1fa9123d832 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index d94c408de3e..abb4ba5485e 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.0.2 + 7.3.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index b7d93c751f1..744077acb99 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,84 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index b4f51b923e4..7d91f787b4d 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.0.2 + 7.3.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index c58b60badaa..4317f732786 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,84 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index a5dda80e4f9..8158dc4f1cd 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.0.2 + 7.3.0 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 608a87963bf..42ff8651cd7 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 2c596393415..f514929c106 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.0.2 + 7.3.0 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 7bc21f4ad87..b0a3243475b 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index 8f0243c3744..46c36eb6741 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 7.0.2 + 7.3.0 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 84fdea0f5e7..6a1ad058aac 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 35edc1a1a53..5048da71c6e 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.0.2 + 7.3.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 06d8350c0db..0a8d758a5bb 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,94 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'feature/multi-level-lanelet-support' of https://github.com/tier4/scenario_simulator_v2 into feature/multi-level-lanelet-support +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 4bcba6c41cc..dc5d2f1a8e1 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -139,6 +139,8 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; + auto isOtherEntityAtConsideredAltitude( + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 9be02b547d4..5ca8a0bda17 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.0.2 + 7.3.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 68d223a6448..f75ac46cb8b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -310,7 +310,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.isInLanelet()) { + if (status.isInLanelet() && isOtherEntityAtConsideredAltitude(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -320,6 +320,13 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } +auto ActionNode::isOtherEntityAtConsideredAltitude( + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool +{ + return hdmap_utils->isAltitudeMatching( + canonicalized_entity_status->getAltitude(), entity_status.getAltitude()); +} + auto ActionNode::getDistanceToConflictingEntity( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 57a85dbd380..d2c96703654 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 9fc2c66d90f..76c4550ce48 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.0.2 + 7.3.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index d30b0cc1d6f..c3130844d7f 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,102 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(simple_sensor_simulator): Fix if condition by adding negation to ensure proper logic +* Remove unused function and update comment +* fix(simple_senor_simulator): fix after Szymon discussion +* fix(simple_sensor_simulator): fix after detection_sensor refactor +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Refactor code to improve readability based on SonarQube findings +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Removed unrecognized words because spell-check flagged them as invalid +* [RJD-1369] Improve Collision Solving for Multi-Level Support + - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, + allowing to ignore objects located at different altitudes. + - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, + preventing unnecessary slowing or stopping caused by incorrect detections. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index ec4dcf6ef18..0aff66be710 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -17,7 +17,10 @@ #include +#include +#include #include +#include #include #include #include @@ -48,6 +51,9 @@ class DetectionSensorBase const std::vector &) const -> std::vector::const_iterator; + auto isOnOrAboveEgoPlane( + const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool; + public: virtual ~DetectionSensorBase() = default; @@ -55,6 +61,10 @@ class DetectionSensorBase const double current_simulation_time, const std::vector &, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) = 0; + +private: + std::optional ego_plane_opt_{std::nullopt}; + std::optional ego_plane_pose_opt_{std::nullopt}; }; template diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index bb558e2d07a..55060b3d3b9 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.0.2 + 7.3.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index dc6c1837f0d..cf5925688ff 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -62,6 +64,51 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } } +auto DetectionSensorBase::isOnOrAboveEgoPlane( + const geometry_msgs::Pose & entity_pose, const geometry_msgs::Pose & ego_pose) -> bool +{ + /* + The threshold for detecting significant changes in ego vehicle's orientation (unit: radian). + The value determines the minimum angular difference required to consider the ego orientation + as "changed". + + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double rotation_threshold_ = 0.04; + /* + Maximum downward offset in Z-axis relative to the ego position (unit: meter). + If the NPC is lower than this offset relative to the ego position, + the NPC will be excluded from detection + + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; + + const auto hasEgoOrientationChanged = [this](const geometry_msgs::msg::Pose & ego_pose_ros) { + return math::geometry::getAngleDifference( + ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_; + }; + + // if other entity is at the same altitude as Ego or within max_downward_z_offset_ below Ego + if (entity_pose.position().z() >= (ego_pose.position().z() - max_downward_z_offset_)) { + return true; + // otherwise check if other entity is above ego plane + } else { + // update ego plane if needed + geometry_msgs::msg::Pose ego_pose_ros; + simulation_interface::toMsg(ego_pose, ego_pose_ros); + if (!ego_plane_opt_ || !ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) { + ego_plane_opt_.emplace( + ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation)); + ego_plane_pose_opt_ = ego_pose_ros; + } + + geometry_msgs::msg::Pose entity_pose_ros; + simulation_interface::toMsg(entity_pose, entity_pose_ros); + return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0; + } +} + template auto make(From &&...) -> To; @@ -321,6 +368,7 @@ auto DetectionSensor::update( auto is_in_range = [&](const auto & status) { return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and + isOnOrAboveEgoPlane(status.pose(), ego_entity_status->pose()) and (configuration_.detect_all_objects_in_range() or std::find( lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) != diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 2452d66f732..99fb0380396 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -41,7 +41,8 @@ EgoEntitySimulation::EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time, const std::shared_ptr & hdmap_utils, const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope) -: autoware(std::make_unique()), +: autoware( + std::make_unique(getParameter("simulate_localization"))), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), status_(initial_status, std::nullopt), diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 695f680fc6e..12177cf663a 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,87 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(traffic_simulator): revert clang changes +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 21bee384cc4..929c22f9a7f 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.0.2 + 7.3.0 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 525db7453f5..b6a05f7c227 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,101 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge pull request `#1481 `_ from tier4/feature/multi-level-lanelet-support + Feature/multi level lanelet support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* fix(traffic_simulator): revert clang changes +* ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks +* Merge branch 'master' into feature/multi-level-lanelet-support +* [RJD-1370] Fix 3D Lanelet Matching Issue in cpp_mock_scenario + - Updated the makeRandomPose method to correctly support 3D lanelet matching. +* Fix an issue with unit tests - distanceTest +* Merge branch 'master' into feature/multi-level-lanelet-support +* Removed unrecognized words because spell-check flagged them as invalid +* [RJD-1369] Improve lanelet matching - 3D support + - Enhanced lanelet matching algorithm (`toLaneletPose` method) by incorporating lanelet altitude. + - Defined the `altitude_threshold` parameter that sets the maximum altitude difference to determine when an entity can be matched with a specific lanelet. +* Contributors: Dawid Moszynski, Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge pull request `#1465 `_ from tier4/fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* fix(api): request enable autoware control +* Contributors: Kotaro Yoshimoto, satoshi-ota + 7.0.2 (2024-12-12) ------------------ * Merge pull request `#1470 `_ from tier4/fix/snor-cloud-issue-8-1 diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 2c1b32e96f2..9404fff41eb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -54,6 +54,8 @@ class CanonicalizedEntityStatus auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &; auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void; + auto getAltitude() const -> double; + auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &; auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; auto setLinearVelocity(double linear_velocity) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 0f34b2c7c97..9be623a2cab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -160,7 +160,7 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; + auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, @@ -380,6 +380,13 @@ class HdMapUtils auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true) const -> geometry_msgs::msg::PoseStamped; + auto isAltitudeMatching(const double current_altitude, const double target_altitude) const + -> bool; + + auto getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance = 1.0) const -> std::optional; + private: /** @defgroup cache * Declared mutable for caching diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp index cb2b70636cd..c9daa49273e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp @@ -117,7 +117,8 @@ class TrafficSource : public TrafficModuleBase const std::size_t id; private: - auto makeRandomPose(const bool random_orientation = false) -> geometry_msgs::msg::Pose; + auto makeRandomPose(const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose; auto isPoseValid(const VehicleOrPedestrianParameter &, const geometry_msgs::msg::Pose &) -> std::pair>; diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 19e650a4e2d..9578bdb927f 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.0.2 + 7.3.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 37fb5e6e0a9..c586e14f2cf 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -121,6 +121,11 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms return entity_status_.pose; } +auto CanonicalizedEntityStatus::getAltitude() const -> double +{ + return entity_status_.pose.position.z; +} + auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & { if (canonicalized_lanelet_pose_) { diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index d9f67535928..3f04d0c9c19 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -54,6 +54,7 @@ auto EgoEntity::makeFieldOperatorApplication( parameters.push_back("use_foa:=false"); parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false")); parameters.push_back("use_sim_time:=" + std::string(getParameter(node_parameters, "use_sim_time", false) ? "true" : "false")); + parameters.push_back("localization_sim_mode:=" + std::string(getParameter(node_parameters, "simulate_localization") ? "api" : "pose_twist_estimator")); // clang-format on return getParameter(node_parameters, "launch_autoware", true) @@ -289,6 +290,7 @@ auto EgoEntity::requestReplanRoute(const std::vectorclearRoute(); field_operator_application->plan(route); + field_operator_application->enableAutowareControl(); field_operator_application->engage(); } diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 650fb0fd802..eab04aaf2f2 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -333,7 +333,7 @@ auto HdMapUtils::getNearbyLaneletIds( return lanelet_ids; } -auto HdMapUtils::getHeight(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const +auto HdMapUtils::getAltitude(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const -> double { return toMapPose(lanelet_pose).pose.position.z; @@ -594,6 +594,11 @@ auto HdMapUtils::toLaneletPose( return std::nullopt; } auto pose_on_centerline = spline->getPose(s.value()); + + if (!isAltitudeMatching(pose.position.z, pose_on_centerline.position.z)) { + return std::nullopt; + } + auto rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); @@ -617,6 +622,25 @@ auto HdMapUtils::toLaneletPose( return lanelet_pose; } +auto HdMapUtils::isAltitudeMatching( + const double current_altitude, const double target_altitude) const -> bool +{ + /* + Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + entity's Z-position is always relative to its base. This eliminates the + need to dynamically adjust the threshold based on the entity's dimensions, + ensuring consistent altitude matching regardless of the entity type. + + The position of the entity is defined relative to its base, typically + the center of the rear axle projected onto the ground in the case of vehicles. + + There is no technical basis for this value; it was determined based on + experiments. + */ + static constexpr double altitude_threshold = 1.0; + return std::abs(current_altitude - target_altitude) <= altitude_threshold; +} + auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, const double matching_distance) const -> std::optional @@ -2166,6 +2190,18 @@ auto HdMapUtils::toPolygon(const lanelet::ConstLineString3d & line_string) const return ret; } +auto HdMapUtils::getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) const -> std::optional +{ + if (const auto spline = getCenterPointsSpline(lanelet_id)) { + if (const auto s = spline->getSValue(pose, matching_distance)) { + return spline->getPoint(s.value()).z; + } + } + return std::nullopt; +} + HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_map) { vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create( diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 350920a42d1..68f82fdce61 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -72,7 +72,9 @@ auto TrafficSource::Validator::operator()( }); } -auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_msgs::msg::Pose +auto TrafficSource::makeRandomPose( + const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose { const double angle = angle_distribution_(engine_); @@ -83,6 +85,17 @@ auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_ms random_pose.position.x += radius * std::cos(angle); random_pose.position.y += radius * std::sin(angle); + if (const auto nearby_lanelets = hdmap_utils_->getNearbyLaneletIds( + random_pose.position, radius, std::holds_alternative(parameter)); + !nearby_lanelets.empty()) { + // Get the altitude of the first nearby lanelet + if ( + const auto altitude = + hdmap_utils_->getLaneletAltitude(nearby_lanelets.front(), random_pose, radius)) { + random_pose.position.z = altitude.value(); + } + } + if (random_orientation) { random_pose.orientation = math::geometry::convertEulerAngleToQuaternion( traffic_simulator::helper::constructRPY(0.0, 0.0, angle_distribution_(engine_))); @@ -106,7 +119,7 @@ void TrafficSource::execute( static constexpr auto max_randomization_attempts = 10000; for (auto tries = 0; tries < max_randomization_attempts; ++tries) { - auto candidate_pose = makeRandomPose(configuration_.use_random_orientation); + auto candidate_pose = makeRandomPose(configuration_.use_random_orientation, parameter); if (auto [valid, lanelet_pose] = isPoseValid(parameter, candidate_pose); valid) { return std::make_pair(candidate_pose, lanelet_pose); } diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index eaa36c849a4..dd0dda7330f 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -2098,9 +2098,11 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLaneletNoRoute) { const auto pose_to = hdmap_utils.toLaneletPose( - makePose(makePoint(81590.79, 50067.66), makeQuaternionFromYaw(90.0)), lanelet::Id{3002185}); + makePose(makePoint(81590.79, 50067.66, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002185}); const auto pose_from = hdmap_utils.toLaneletPose( - makePose(makePoint(81596.20, 50068.04), makeQuaternionFromYaw(90.0)), lanelet::Id{3002166}); + makePose(makePoint(81596.20, 50068.04, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002166}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index a12b5e33328..c10cc1d4931 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -63,13 +63,14 @@ auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } -auto makePose(const double x, const double y, const double yaw_deg) -> geometry_msgs::msg::Pose +auto makePose(const double x, const double y, const double z, const double yaw_deg) + -> geometry_msgs::msg::Pose { /** * @note +x axis is 0 degrees; +y axis is 90 degrees */ return geometry_msgs::build() - .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .position(geometry_msgs::build().x(x).y(y).z(z)) .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z( convertDegToRad(yaw_deg)))); diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index d288117c96b..8dc5b5cdee8 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -238,10 +238,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + makePose(81595.44, 50006.09, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + makePose(81584.48, 50084.76, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -260,10 +260,10 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + makePose(3800.05, 73715.77, 0.5, 30.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + makePose(3841.26, 73748.80, 0.5, 110.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -283,10 +283,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + makePose(81585.79, 50042.62, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + makePose(81588.34, 50083.23, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -305,10 +305,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + makePose(81599.02, 50065.76, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + makePose(81599.61, 50045.16, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -328,10 +328,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + makePose(81595.47, 49982.80, 36.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + makePose(81599.34, 50022.34, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -343,10 +343,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + makePose(81612.35, 50015.63, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + makePose(81612.95, 49991.30, 35.5, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -367,10 +367,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + makePose(81592.96, 49997.94, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + makePose(81570.56, 50141.75, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -383,10 +383,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + makePose(81587.31, 50165.57, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + makePose(81610.25, 49988.59, 35.5, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -406,10 +406,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + makePose(86627.71, 44972.06, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -421,10 +421,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + makePose(86555.38, 45000.88, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -436,10 +436,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); + makePose(86553.48, 44990.56, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -451,10 +451,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); + makePose(86579.91, 44979.00, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -475,10 +475,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + makePose(86736.13, 44969.63, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + makePose(86642.95, 44958.78, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -490,10 +490,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + makePose(86732.06, 44976.58, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + makePose(86704.59, 44927.32, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -514,9 +514,9 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); + makePose(86637.19, 44967.35, 3.0, 340.0), false, hdmap_utils_ptr); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + makePose(86648.82, 44886.19, 3.0, 240.0), false, hdmap_utils_ptr); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -528,10 +528,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + makePose(86719.94, 44957.20, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + makePose(86599.32, 44975.01, 3.0, 180.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -550,8 +550,8 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch */ TEST(distance, boundingBoxDistance_intersection) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(30.0, 1.0); const auto bounding_box_to = makeCustom2DBoundingBox(1.0, 30.0); @@ -569,8 +569,8 @@ TEST(distance, boundingBoxDistance_intersection) */ TEST(distance, boundingBoxDistance_disjoint) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(1.0, 30.0); const auto bounding_box_to = makeCustom2DBoundingBox(30.0, 1.0); @@ -592,56 +592,56 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) constexpr lanelet::Id lanelet_id = 34741L; constexpr double tolerance = 0.1; { - const auto pose = makePose(3818.33, 73726.18, 30.0); + const auto pose = makePose(3818.33, 73726.18, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.5, tolerance); } { - const auto pose = makePose(3816.89, 73723.09, 30.0); + const auto pose = makePose(3816.89, 73723.09, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.6, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 30.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.7, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 120.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.3, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.4, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.4, tolerance); } { - const auto pose = makePose(3680.81, 73757.27, 30.0); + const auto pose = makePose(3680.81, 73757.27, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); EXPECT_NEAR(result, 5.1, tolerance); } { - const auto pose = makePose(3692.79, 73753.00, 30.0); + const auto pose = makePose(3692.79, 73753.00, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); @@ -656,7 +656,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{34603L, 34600L, 34621L, 34741L}; - const auto pose = makePose(3836.16, 73757.99, 120.0); + const auto pose = makePose(3836.16, 73757.99, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -678,7 +678,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 34426L; - const auto pose = makePose(3693.34, 73738.37, 300.0); + const auto pose = makePose(3693.34, 73738.37, 0.0, 300.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -693,7 +693,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) */ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToLeftLaneBound( @@ -709,56 +709,56 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) constexpr lanelet::Id lanelet_id = 660L; constexpr double tolerance = 0.1; { - const auto pose = makePose(86651.84, 44941.47, 135.0); + const auto pose = makePose(86651.84, 44941.47, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.1, tolerance); } { - const auto pose = makePose(86653.05, 44946.74, 135.0); + const auto pose = makePose(86653.05, 44946.74, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.6, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 120.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.3, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 210.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 210.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 3.1, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.0, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.1, tolerance); } { - const auto pose = makePose(86644.11, 44941.21, 0.0); + const auto pose = makePose(86644.11, 44941.21, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 11.2, tolerance); } { - const auto pose = makePose(86656.83, 44946.96, 0.0); + const auto pose = makePose(86656.83, 44946.96, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -773,7 +773,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; - const auto pose = makePose(86642.05, 44902.61, 60.0); + const auto pose = makePose(86642.05, 44902.61, 0.0, 60.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -795,7 +795,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 654L; - const auto pose = makePose(86702.79, 44929.05, 150.0); + const auto pose = makePose(86702.79, 44929.05, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -810,7 +810,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) */ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToRightLaneBound( diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 63e1f6fcc89..e9c52213db8 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index d2c17555369..b23095deb00 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.0.2 + 7.3.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 4340d7352b4..6289a66c67a 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,84 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 8f1a0332630..0c5024660a6 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.0.2 + 7.3.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 39c8efa3a9e..f0ca93e1ae8 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,101 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +7.3.0 (2024-12-16) +------------------ +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Merge branch 'master' into feature/multi-level-lanelet-support +* Contributors: Kotaro Yoshimoto, SzymonParapura + +7.2.0 (2024-12-16) +------------------ +* Merge pull request `#1484 `_ from tier4/RJD-736/autoware_msgs_support_and_localization_sim_mode_support + Rjd 736/autoware msgs support and localization sim mode support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge branch 'master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/master' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Merge remote-tracking branch 'origin/RJD-736/autoware_msgs_support' into RJD-736/autoware_msgs_support_and_localization_sim_mode_support +* Add launch argument `simulate_localization` to `scenario_test_runner` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.1.0 (2024-12-16) +------------------ +* Merge pull request `#1258 `_ from tier4/feature/time-to-collision-condition + Feature/time to collision condition +* Update `TimeToCollisionCondition` to call `SpeedCondition` in standard compatible mode +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Remove static member function `evaluateTimeToCollisionCondition` +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Fix `evaluateTimeToCollisionCondition` to not return meaningless value when collisions cannot occur +* Fix `TimeToCollisionCondition` to return inf if relative speed < zero +* Cleanup static member function `TimeToCollisionCondition::evaluate` +* Add new test scenario `...EntityCondition.TimeToCollisionCondition.yaml` +* Add new member function `evaluateCartesianTimeToCollisionCondition` +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Merge branch 'master' into feature/time-to-collision-condition +* Merge remote-tracking branch 'origin/master' into feature/time-to-collision-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.4 (2024-12-13) +------------------ +* Merge pull request `#1486 `_ from tier4/fix/speed-condition/backward-compatibility + Fix/speed condition/backward compatibility +* Merge branch 'master' into fix/speed-condition/backward-compatibility +* Merge remote-tracking branch 'origin/master' into fix/speed-condition/backward-compatibility +* Add new parameter `speed_condition` to switch compatibility +* Add new enumeration `Compatibility` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +7.0.3 (2024-12-13) +------------------ +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Merge branch 'master' into fix/request-enable-autoware-control +* Contributors: Kotaro Yoshimoto + 7.0.2 (2024-12-12) ------------------ * Merge branch 'master' into fix/snor-cloud-issue-8-1 diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index 46e762e2144..b5ae1aef8e6 100644 --- a/test_runner/scenario_test_runner/config/workflow.txt +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -7,6 +7,7 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeSpeedCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml $(find-pkg-share scenario_test_runner)/scenario/LateralAction.LaneChangeAction-RoadShoulder.yaml $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 98897b9badf..9ad03b6f027 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -87,6 +87,8 @@ def launch_setup(context, *args, **kwargs): scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + simulate_localization = LaunchConfiguration("simulate_localization", default=True) + speed_condition = LaunchConfiguration("speed_condition", default="legacy") use_sim_time = LaunchConfiguration("use_sim_time", default=False) vehicle_model = LaunchConfiguration("vehicle_model", default="") # fmt: on @@ -111,6 +113,8 @@ def launch_setup(context, *args, **kwargs): print(f"scenario := {scenario.perform(context)}") print(f"sensor_model := {sensor_model.perform(context)}") print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"simulate_localization := {simulate_localization.perform(context)}") + print(f"speed_condition := {speed_condition.perform(context)}") print(f"use_sim_time := {use_sim_time.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") @@ -135,6 +139,8 @@ def make_parameters(): {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout}, + {"simulate_localization": simulate_localization}, + {"speed_condition": speed_condition}, {"use_sim_time": use_sim_time}, {"vehicle_model": vehicle_model}, ] @@ -173,12 +179,14 @@ def collect_prefixed_parameters(): DeclareLaunchArgument("global_timeout", default_value=global_timeout ), DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), - DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), DeclareLaunchArgument("rviz_config", default_value=rviz_config ), DeclareLaunchArgument("scenario", default_value=scenario ), DeclareLaunchArgument("sensor_model", default_value=sensor_model ), DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("simulate_localization", default_value=simulate_localization ), + DeclareLaunchArgument("speed_condition", default_value=speed_condition ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), # fmt: on diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index f95ecbcf47a..bcb0fd35f27 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.0.2 + 7.3.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml new file mode 100644 index 00000000000..213ac429acd --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -0,0 +1,235 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: { revMajor: 1, revMinor: 1, date: '2024-11-25T04:18:18.703Z', description: '', author: Tatsuya Yamasaki } + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: { path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle } + RoadNetwork: + LogicFile: { filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map } + Entities: + ScenarioObject: + - name: ego + CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } + ObjectController: + Controller: + name: '' + Properties: + Property: [] + - name: vehicle_01 + CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34976' + s: 10 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - entityRef: vehicle_01 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34579' + s: 0 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - ControllerAction: + AssignControllerAction: + Controller: + name: '' + Properties: + Property: + - name: maxSpeed + value: '0.5' + Story: + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 30 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: lane + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: false + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: lane + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: false + rule: lessThan + value: 0 + relativeDistanceType: longitudinal + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: true + rule: lessThan + value: 0 + relativeDistanceType: cartesianDistance + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: ego ] + EntityCondition: + TimeToCollisionCondition: + freespace: false + rule: lessThan + value: 0 + relativeDistanceType: cartesianDistance + coordinateSystem: entity + routingAlgorithm: undefined + TimeToCollisionConditionTarget: + EntityRef: + entityRef: vehicle_01 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] diff --git a/test_runner/scenario_test_runner/scenario/sample.yaml b/test_runner/scenario_test_runner/scenario/sample.yaml index d465b19c270..37d6440f158 100644 --- a/test_runner/scenario_test_runner/scenario/sample.yaml +++ b/test_runner/scenario_test_runner/scenario/sample.yaml @@ -10,16 +10,7 @@ OpenSCENARIO: revMajor: 1 revMinor: 0 ParameterDeclarations: - ParameterDeclaration: - - name: random_offset - parameterType: double - value: $(ros2 run openscenario_interpreter_example uniform_distribution -1.0 1.0) - ConstraintGroup: - - ValueConstraint: - - rule: lessOrEqual - value: 1.0 - - rule: greaterOrEqual - value: -1.0 + ParameterDeclaration: [] CatalogLocations: VehicleCatalog: Directory: @@ -54,7 +45,7 @@ OpenSCENARIO: roadId: '' laneId: LANE_ID s: 1 - offset: $random_offset + offset: 0 Orientation: type: relative h: 0 From 9b252cb9126cc33c7139ddb98f2284b128293129 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 8 Jan 2025 13:33:39 +0100 Subject: [PATCH 48/49] ref(traffic_simulator, cpp_mock_scenarios): rename isInPosition to isNerbyPosition --- docs/developer_guide/TrafficSimulator.md | 4 ++-- .../src/follow_lane/cancel_request.cpp | 2 +- ...polyline_trajectory_with_do_nothing_plugin.cpp | 11 ++++++----- .../src/random_scenario/random001.cpp | 8 ++++---- .../src/spawn/spawn_in_map_frame.cpp | 2 +- .../synchronized_action/synchronized_action.cpp | 2 +- .../synchronized_action_with_speed.cpp | 2 +- .../src/traffic_simulation_demo.cpp | 6 +++--- .../include/simulation_interface/conversions.hpp | 15 ++++++--------- .../behavior/behavior_plugin_base.hpp | 14 +++++++------- .../traffic_simulator/entity/entity_base.hpp | 4 ++-- .../traffic_simulator/src/entity/entity_base.cpp | 10 +++++----- 12 files changed, 39 insertions(+), 41 deletions(-) diff --git a/docs/developer_guide/TrafficSimulator.md b/docs/developer_guide/TrafficSimulator.md index ff119bf5090..4fd980934d2 100644 --- a/docs/developer_guide/TrafficSimulator.md +++ b/docs/developer_guide/TrafficSimulator.md @@ -80,14 +80,14 @@ private: void update() { const auto entity = getEntity("ego"); - if (entity->isInPosition( + if (entity->isNearbyPosition( traffic_simulator::helper::constructLaneletPose(34615, 10.0), 5)) { api_.requestAcquirePosition("ego", traffic_simulator::helper::constructLaneletPose(35026, 0.0) ); api_.setTargetSpeed("npc2", 13, true); } - if (entity->isInPosition( + if (entity->isNearbyPosition( traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5)) { api_.setTargetSpeed("npc2", 3, true); diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 54c59f313fd..30fdb3cbbb6 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -40,7 +40,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { const auto ego_entity = api_.getEntity("ego"); - if (ego_entity->isInPosition( + if (ego_entity->isNearbyPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 30, 0, api_.getHdmapUtils()), 3.0)) { diff --git a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp index 18a062c731a..bdfc883a9e8 100644 --- a/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp @@ -69,26 +69,27 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario stop(cpp_mock_scenarios::Result::FAILURE); } // LCOV_EXCL_STOP - if (equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isInPosition(spawn_pose, 0.1)) { + if ( + equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isNearbyPosition(spawn_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.0, 0.01) && - !ego_entity->isInPosition(trajectory_start_pose, 0.1)) { + !ego_entity->isNearbyPosition(trajectory_start_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 1.5, 0.01) && - !ego_entity->isInPosition(trajectory_waypoint_pose, 0.1)) { + !ego_entity->isNearbyPosition(trajectory_waypoint_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if ( equals(api_.getCurrentTime(), 2.0, 0.01) && - !ego_entity->isInPosition(trajectory_goal_pose, 0.1)) { + !ego_entity->isNearbyPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::FAILURE); } if (equals(api_.getCurrentTime(), 2.5, 0.01)) { - if (ego_entity->isInPosition(trajectory_goal_pose, 0.1)) { + if (ego_entity->isNearbyPosition(trajectory_goal_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 620f1583bed..c41fa43391d 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -159,7 +159,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode constexpr lanelet::Id lanelet_id = 34392; if ( !api_.isEntityExist(entity_name) && - !ego_entity->isInPosition( + !ego_entity->isNearbyPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34576, 25.0, 0.0, api_.getHdmapUtils()), 5.0)) { @@ -191,7 +191,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( 34621, 10, 0.0, api_.getHdmapUtils()); constexpr auto entity_name = "spawn_nearby_ego"; - if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntityExist(entity_name)) { + if (ego_entity->isNearbyPosition(trigger_position, 20.0) && !api_.isEntityExist(entity_name)) { api_.spawn( entity_name, traffic_simulator::pose::transformRelativePoseToGlobal( @@ -203,13 +203,13 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); } - if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntityExist(entity_name)) { + if (!ego_entity->isNearbyPosition(trigger_position, 20.0) && api_.isEntityExist(entity_name)) { api_.despawn(entity_name); } const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( 34606, 0.0, 0.0, api_.getHdmapUtils()); - if (ego_entity->isInPosition(ego_goal_position, 1.0)) { + if (ego_entity->isNearbyPosition(ego_goal_position, 1.0)) { api_.despawn("ego"); stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp index c34e8236e62..e6db02901cc 100644 --- a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp +++ b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp @@ -41,7 +41,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode const auto map_pose = traffic_simulator::pose::toMapPose( traffic_simulator::helper::constructCanonicalizedLaneletPose( 120545, 0.0, 0.0, api_.getHdmapUtils())); - if (api_.getEntity("ego")->isInPosition(map_pose, 0.1)) { + if (api_.getEntity("ego")->isNearbyPosition(map_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); 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 9db459e8b53..391371e6965 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -50,7 +50,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode // SUCCESS if ( npc->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) && - ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) && + ego->isNearbyPosition(ego_target, 1.0) && npc->isNearbyPosition(npc_target, 1.0) && npc->getCurrentTwist().linear.x < 0.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index a03ebadbbb2..a035f81fec8 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -50,7 +50,7 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode // SUCCESS if ( npc->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) && - ego->isInPosition(ego_target, 1.0) && npc->isInPosition(npc_target, 1.0) && + ego->isNearbyPosition(ego_target, 1.0) && npc->isNearbyPosition(npc_target, 1.0) && npc->getCurrentTwist().linear.x < 2.5) { stop(cpp_mock_scenarios::Result::SUCCESS); } diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 6ca6d01363d..a66b3f912be 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -57,7 +57,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode } const auto ego_entity = api_.getEntity("ego"); const auto npc2_entity = api_.getEntity("npc2"); - if (ego_entity->isInPosition( + if (ego_entity->isNearbyPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34615, 10.0, 0.0, api_.getHdmapUtils()), 5)) { @@ -68,7 +68,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode npc2_entity->requestSpeedChange(13, true); } } - if (ego_entity->isInPosition( + if (ego_entity->isNearbyPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34579, 0.0, 0.0, api_.getHdmapUtils()), 5)) { @@ -79,7 +79,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode npc2_entity->requestSpeedChange(3, true); } } - if (npc2_entity->isInPosition( + if (npc2_entity->isNearbyPosition( traffic_simulator::helper::constructCanonicalizedLaneletPose( 34513, 0.0, 0.0, api_.getHdmapUtils()), 5)) { diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 0bd1f09e53f..6e8ee4b7ec8 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,8 +190,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -206,8 +205,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,8 +233,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index f4162f3e9b0..c4dac740c60 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off 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 19d160c73d1..15badb935c2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -106,10 +106,10 @@ class EntityBase /* */ auto isStopped() const -> bool; - /* */ auto isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const + /* */ auto isNearbyPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const -> bool; - /* */ auto isInPosition( + /* */ auto isNearbyPosition( const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool; /* */ auto isInLanelet() const -> bool { return status_->isInLanelet(); }; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 35d1700be20..ac6232e0ad9 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -95,16 +95,16 @@ auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) con matching_distance, hdmap_utils_ptr_); } -auto EntityBase::isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const - -> bool +auto EntityBase::isNearbyPosition( + const geometry_msgs::msg::Pose & pose, const double tolerance) const -> bool { return math::geometry::getDistance(getMapPose(), pose) < tolerance; } -auto EntityBase::isInPosition( +auto EntityBase::isNearbyPosition( const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool { - return isInPosition(static_cast(lanelet_pose), tolerance); + return isNearbyPosition(static_cast(lanelet_pose), tolerance); } auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional tolerance) const @@ -765,7 +765,7 @@ auto EntityBase::requestSynchronize( } ///@brief Check if the entity has already arrived to the target lanelet. - if (isInPosition(entity_target, tolerance)) { + if (isNearbyPosition(entity_target, tolerance)) { if (getCurrentTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { } else { RCLCPP_WARN_ONCE( From 5fa728194e352f10271743fdaa6c4055688227f4 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 8 Jan 2025 13:41:28 +0100 Subject: [PATCH 49/49] ref(traffic_simulator, simulator_interface): revert format unexpected changes --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 6e8ee4b7ec8..0bd1f09e53f 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,7 +190,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -205,7 +206,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -233,7 +235,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off