From 895e9fc50a4619c9afd407889d22fffaf2b6188d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 14:15:10 +0100 Subject: [PATCH 01/26] RespawnEntity added MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../concealer/field_operator_application.hpp | 2 + ...ator_application_for_autoware_universe.hpp | 5 + ...ator_application_for_autoware_universe.cpp | 33 +++-- mock/cpp_mock_scenarios/CMakeLists.txt | 2 + .../src/cpp_scenario_node.cpp | 8 +- .../src/teleport_action/CMakeLists.txt | 14 ++ .../src/teleport_action/teleport_action.cpp | 139 ++++++++++++++++++ .../include/traffic_simulator/api/api.hpp | 50 +++++++ 8 files changed, 240 insertions(+), 13 deletions(-) create mode 100644 mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt create mode 100644 mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 38905be51de..75b1cd0a62b 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -127,6 +127,8 @@ class FieldOperatorApplication : public rclcpp::Node * -------------------------------------------------------------------------- */ virtual auto plan(const std::vector &) -> void = 0; + virtual auto clearRoute() -> void = 0; + virtual auto getAutowareStateName() const -> std::string = 0; virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 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 c8ac121fb51..825be6696a6 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 @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ class FieldOperatorApplicationFor ServiceWithValidation requestEngage; ServiceWithValidation requestInitialPose; ServiceWithValidation requestSetRoutePoints; + ServiceWithValidation requestClearRoute; ServiceWithValidation requestSetRtcAutoMode; ServiceWithValidation requestSetVelocityLimit; // clang-format on @@ -127,6 +129,7 @@ class FieldOperatorApplicationFor requestInitialPose("/api/localization/initialize", *this), // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons. requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), + requestClearRoute("/api/routing/clear_route", *this), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this) @@ -159,6 +162,8 @@ class FieldOperatorApplicationFor auto plan(const std::vector &) -> void override; + auto clearRoute() -> void override; + auto requestAutoModeForCooperation(const std::string &, bool) -> void override; auto restrictTargetSpeed(double) const -> double 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 a777cbda6a9..2020e7f6581 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -340,20 +340,31 @@ auto FieldOperatorApplicationFor::plan( }); } +auto FieldOperatorApplicationFor::clearRoute() -> void +{ + task_queue.delay([this] { + auto request = std::make_shared(); + + requestClearRoute(request); + }); +} + auto FieldOperatorApplicationFor::engage() -> void { - task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { - auto request = std::make_shared(); - request->engage = true; - try { - return requestEngage(request); - } catch (const decltype(requestEngage)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } + if (!engaged() && !isDriving()) { + task_queue.delay([this]() { + waitForAutowareStateToBeDriving([this]() { + auto request = std::make_shared(); + request->engage = true; + try { + return requestEngage(request); + } catch (const decltype(requestEngage)::TimeoutError &) { + // ignore timeout error because this service is validated by Autoware state transition. + return; + } + }); }); - }); + } } auto FieldOperatorApplicationFor::engageable() const -> bool diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 9d6e37a592a..581773866aa 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -53,6 +53,8 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/pedestrian) add_subdirectory(src/random_scenario) add_subdirectory(src/speed_planning) + add_subdirectory(src/teleport_action) + endif() install( diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index a949d245bbf..14e1c512291 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -93,6 +93,7 @@ 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); api_.attachLidarSensor("ego", 0.0); api_.attachDetectionSensor("ego", 200.0, true, 0.0, 0, 0.0, 0.0); @@ -124,8 +125,11 @@ void CppScenarioNode::spawnEgoEntity( std::atomic engaged(false); auto engage_thread = std::thread([&]() { while (!api_.asFieldOperatorApplication("ego").engaged()) { - api_.asFieldOperatorApplication("ego").engage(); - std::this_thread::sleep_for(1000ms); + if (!api_.asFieldOperatorApplication("ego").engageable()) { + api_.asFieldOperatorApplication("ego").engage(); + } else { + } + std::this_thread::sleep_for(3000ms); } engaged.store(true); }); diff --git a/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt b/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt new file mode 100644 index 00000000000..47ca7c60ae2 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(teleport_action + teleport_action.cpp +) +target_link_libraries(teleport_action cpp_scenario_node) + +install(TARGETS + teleport_action + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "teleport_action" "20.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp new file mode 100644 index 00000000000..a8aa1c63790 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp @@ -0,0 +1,139 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +// #include +// #include +// #include +// #include +// #include +// headers in STL +#include +#include +#include +// #include + + +namespace cpp_mock_scenarios +{ +class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit TeleportActionScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "teleport_action", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + // LCOV_EXCL_START + if (api_.entityExists("bob") && api_.checkCollision("ego", "bob")) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + /** + * @note The simulation time is internally managed as a fraction and must exactly equal to x.0 + * in the floating-point literal when the simulation time is an integer multiple of the frame rate frame, + * so in this case `std::abs(t - 1.0) <= std::numeric Decides that `t == 1.0` is more appropriate than `std::numeric_limits::epsilon();`. + * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P + */ + // if (t == 1.0) { + // if (t != api_.getCurrentTwist("bob").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) { + // stop(cpp_mock_scenarios::Result::FAILURE); + // } + // } else { + // if (0.1 >= api_.getCurrentTwist("ego").linear.x) { + // stop(cpp_mock_scenarios::Result::FAILURE); + // } + // } + // } + // LCOV_EXCL_STOP + if (t >= 30) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + } + + void onInitialize() override + { + new_position_subscriber = this->create_subscription("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { + api_.respawnEntity(message); + + geometry_msgs::msg::PoseStamped goal_msg; + // auto time = get_clock()->now(); + // goal_msg.header.stamp.sec = 0; + // goal_msg.header.stamp.nanosec = time.nanoseconds(); + goal_msg.header.frame_id = "map"; + goal_msg.pose = api_.toMapPose(api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))); + + api_.asFieldOperatorApplication("ego").clearRoute(); + api_.asFieldOperatorApplication("ego").plan(std::vector{goal_msg}); + + while (!api_.asFieldOperatorApplication("ego").engageable()) { + api_.updateFrame(); + std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + } + api_.asFieldOperatorApplication("ego").engage(); + }); + + spawnEgoEntity( + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)), + {api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, + getVehicleParameters()); + + api_.spawn( + "bob", api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34378, 0.0)), + getPedestrianParameters()); + api_.setLinearVelocity("bob", 0); + api_.requestSpeedChange( + "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + } + +private: + bool lanechange_executed_; + + rclcpp::Subscription::SharedPtr new_position_subscriber; +}; +} // namespace cpp_mock_scenarios + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 19f69e6e105..fe1b8598487 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -163,6 +163,56 @@ class API { return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose; } + + void respawnEntity([[maybe_unused]] const geometry_msgs::msg::PoseWithCovarianceStamped & message ) + { + // auto pose = message.pose; + std::cout<<"TeleportAction"<set_name("ego"); + spawn_req.set_asset_key(""); + + zeromq_client_.call(spawn_req).result().success(); + } + } template auto spawn( From 61214c1ac5e8f4d9901742114d264136f043bb91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 19:50:56 +0100 Subject: [PATCH 02/26] Interface to get vehicle parameters of entity MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../include/traffic_simulator/entity/entity_base.hpp | 3 +++ .../include/traffic_simulator/entity/entity_manager.hpp | 1 + .../include/traffic_simulator/entity/misc_object_entity.hpp | 5 +++++ .../include/traffic_simulator/entity/pedestrian_entity.hpp | 5 +++++ .../include/traffic_simulator/entity/vehicle_entity.hpp | 3 +++ simulation/traffic_simulator/src/entity/vehicle_entity.cpp | 6 ++++++ 6 files changed, 23 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 68c49c9b560..2ca5ea8b2e1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -120,6 +120,9 @@ class EntityBase virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0; + virtual auto getVehicleParameters() const + -> const traffic_simulator_msgs::msg::VehicleParameters = 0; + virtual auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0; 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 f9516e14c47..5c1adf59dc7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -319,6 +319,7 @@ class EntityManager FORWARD_TO_ENTITY(setDecelerationRateLimit, ); FORWARD_TO_ENTITY(setLinearVelocity, ); FORWARD_TO_ENTITY(setVelocityLimit, ); + FORWARD_TO_ENTITY(getVehicleParameters, ); #undef FORWARD_TO_ENTITY diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp index b5659d6756e..a3452ac6d6c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp @@ -102,6 +102,11 @@ class MiscObjectEntity : public EntityBase auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override; + auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override + { + THROW_SEMANTIC_ERROR("getVehicleParametrs function cannot be used in MiscObjectEntity"); + } + void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override; void setAccelerationLimit(double) override {} diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index bb6221cb03c..f33d40c699b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -87,6 +87,11 @@ class PedestrianEntity : public EntityBase void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &); + auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override + { + THROW_SEMANTIC_ERROR("getVehicleParametrs function cannot be used in PedestrianEntity"); + } + void setAccelerationLimit(double acceleration) override; void setAccelerationRateLimit(double acceleration_rate) override; 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 66d6e9dc71c..3126381f3a7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -90,6 +90,9 @@ class VehicleEntity : public EntityBase auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; + auto getVehicleParameters() const + -> const traffic_simulator_msgs::msg::VehicleParameters override; + void onUpdate(double current_time, double step_time) override; void requestAcquirePosition(const CanonicalizedLaneletPose &); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index d5b998230c6..f0192b0cb60 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -127,6 +127,12 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin } } +auto VehicleEntity::getVehicleParameters() const + -> const traffic_simulator_msgs::msg::VehicleParameters +{ + return behavior_plugin_ptr_->getVehicleParameters(); +} + void VehicleEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); From 9fa8a44642dfee261bcb31f6b7fd5f28fd06ec34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 20:28:03 +0100 Subject: [PATCH 03/26] Respawn logic moved to API MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/teleport_action/teleport_action.cpp | 10 +--- .../include/traffic_simulator/api/api.hpp | 47 +++++-------------- 2 files changed, 14 insertions(+), 43 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp index a8aa1c63790..7cfc0e4a004 100644 --- a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp +++ b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp @@ -86,7 +86,6 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode { new_position_subscriber = this->create_subscription("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { - api_.respawnEntity(message); geometry_msgs::msg::PoseStamped goal_msg; // auto time = get_clock()->now(); @@ -95,14 +94,7 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode goal_msg.header.frame_id = "map"; goal_msg.pose = api_.toMapPose(api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))); - api_.asFieldOperatorApplication("ego").clearRoute(); - api_.asFieldOperatorApplication("ego").plan(std::vector{goal_msg}); - - while (!api_.asFieldOperatorApplication("ego").engageable()) { - api_.updateFrame(); - std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - } - api_.asFieldOperatorApplication("ego").engage(); + api_.respawn("ego", message, goal_msg); }); spawnEgoEntity( diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index fe1b8598487..c9fa5e92e36 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -164,40 +164,10 @@ class API return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose; } - void respawnEntity([[maybe_unused]] const geometry_msgs::msg::PoseWithCovarianceStamped & message ) + void respawn(const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & message, const geometry_msgs::msg::PoseStamped goal_msg) { - // auto pose = message.pose; - std::cout<<"TeleportAction"<set_name("ego"); + simulation_interface::toProto(entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); + spawn_req.mutable_parameters()->set_name(name); spawn_req.set_asset_key(""); zeromq_client_.call(spawn_req).result().success(); } + + entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); + entity_manager_ptr_->asFieldOperatorApplication(name).plan(std::vector{goal_msg}); + + while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { + updateFrame(); + std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + } + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } template From f2b0518f3b5cd3be3c1883c138c2a7e203008b28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 20:36:14 +0100 Subject: [PATCH 04/26] Respawn scenario simplification MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/teleport_action/teleport_action.cpp | 78 ++++--------------- 1 file changed, 15 insertions(+), 63 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp index 7cfc0e4a004..1817c5b9ddc 100644 --- a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp +++ b/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp @@ -21,16 +21,11 @@ #include #include #include -// #include -// #include -// #include -// #include -// #include -// headers in STL + #include #include #include -// #include + namespace cpp_mock_scenarios @@ -41,7 +36,15 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode explicit TeleportActionScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "teleport_action", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option), + goal_pose{api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, + new_position_subscriber{create_subscription("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { + geometry_msgs::msg::PoseStamped goal_msg; + goal_msg.header.frame_id = "map"; + goal_msg.pose = api_.toMapPose(goal_pose); + api_.respawn("ego", message, goal_msg); + })} { start(); } @@ -49,72 +52,21 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - const auto t = api_.getCurrentTime(); - // LCOV_EXCL_START - if (api_.entityExists("bob") && api_.checkCollision("ego", "bob")) { - stop(cpp_mock_scenarios::Result::FAILURE); - } - /** - * @note The simulation time is internally managed as a fraction and must exactly equal to x.0 - * in the floating-point literal when the simulation time is an integer multiple of the frame rate frame, - * so in this case `std::abs(t - 1.0) <= std::numeric Decides that `t == 1.0` is more appropriate than `std::numeric_limits::epsilon();`. - * @sa https://wandbox.org/permlink/dSNRX7K2bQiqSI7P - */ - // if (t == 1.0) { - // if (t != api_.getCurrentTwist("bob").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) { - // stop(cpp_mock_scenarios::Result::FAILURE); - // } - // } else { - // if (0.1 >= api_.getCurrentTwist("ego").linear.x) { - // stop(cpp_mock_scenarios::Result::FAILURE); - // } - // } - // } - // LCOV_EXCL_STOP - if (t >= 30) { + if (api_.getCurrentTime() >= 30) { stop(cpp_mock_scenarios::Result::SUCCESS); } } void onInitialize() override - { - new_position_subscriber = this->create_subscription("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), - [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { - - geometry_msgs::msg::PoseStamped goal_msg; - // auto time = get_clock()->now(); - // goal_msg.header.stamp.sec = 0; - // goal_msg.header.stamp.nanosec = time.nanoseconds(); - goal_msg.header.frame_id = "map"; - goal_msg.pose = api_.toMapPose(api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))); - - api_.respawn("ego", message, goal_msg); - }); - + { spawnEgoEntity( api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)), - {api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, + {goal_pose}, getVehicleParameters()); - - api_.spawn( - "bob", api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34378, 0.0)), - getPedestrianParameters()); - api_.setLinearVelocity("bob", 0); - api_.requestSpeedChange( - "bob", 1.0, traffic_simulator::speed_change::Transition::LINEAR, - traffic_simulator::speed_change::Constraint( - traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), - true); } private: - bool lanechange_executed_; + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose; rclcpp::Subscription::SharedPtr new_position_subscriber; }; From b9ca1af6032338125dafee592c7ca8ac6363b579 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 20:59:18 +0100 Subject: [PATCH 05/26] Code cleaning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/CMakeLists.txt | 2 +- .../src/respawn_ego/CMakeLists.txt | 14 ++++++++ .../respawn_ego.cpp} | 35 ++++++++----------- .../src/teleport_action/CMakeLists.txt | 14 -------- .../include/traffic_simulator/api/api.hpp | 22 +++++++----- 5 files changed, 44 insertions(+), 43 deletions(-) create mode 100644 mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt rename mock/cpp_mock_scenarios/src/{teleport_action/teleport_action.cpp => respawn_ego/respawn_ego.cpp} (65%) delete mode 100644 mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 581773866aa..5ebb91bc626 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -53,7 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/pedestrian) add_subdirectory(src/random_scenario) add_subdirectory(src/speed_planning) - add_subdirectory(src/teleport_action) + add_subdirectory(src/respawn_ego) endif() diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt new file mode 100644 index 00000000000..8e656f86eec --- /dev/null +++ b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(respawn_ego + respawn_ego.cpp +) +target_link_libraries(respawn_ego cpp_scenario_node) + +install(TARGETS +respawn_ego + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "20.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp similarity index 65% rename from mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp rename to mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index 1817c5b9ddc..66a5d6ca9b8 100644 --- a/mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -12,33 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include -#include -#include -#include -#include - #include +#include #include +#include #include - - namespace cpp_mock_scenarios { -class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode +class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode { public: - explicit TeleportActionScenario(const rclcpp::NodeOptions & option) + explicit RespawnEgoScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( - "teleport_action", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", __FILE__, false, option), - goal_pose{api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, - new_position_subscriber{create_subscription("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + goal_pose{ + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, + new_position_subscriber{create_subscription( + "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { geometry_msgs::msg::PoseStamped goal_msg; goal_msg.header.frame_id = "map"; @@ -58,17 +53,17 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode } void onInitialize() override - { + { spawnEgoEntity( api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)), - {goal_pose}, - getVehicleParameters()); + {goal_pose}, getVehicleParameters()); } private: - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose; + const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose; - rclcpp::Subscription::SharedPtr new_position_subscriber; + const rclcpp::Subscription::SharedPtr + new_position_subscriber; }; } // namespace cpp_mock_scenarios @@ -76,7 +71,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::NodeOptions options; - auto component = std::make_shared(options); + auto component = std::make_shared(options); rclcpp::spin(component); rclcpp::shutdown(); return 0; diff --git a/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt b/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt deleted file mode 100644 index 47ca7c60ae2..00000000000 --- a/mock/cpp_mock_scenarios/src/teleport_action/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -ament_auto_add_executable(teleport_action - teleport_action.cpp -) -target_link_libraries(teleport_action cpp_scenario_node) - -install(TARGETS - teleport_action - DESTINATION lib/cpp_mock_scenarios -) - -if(BUILD_TESTING) - include(../../cmake/add_cpp_mock_scenario_test.cmake) - add_cpp_mock_scenario_test(${PROJECT_NAME} "teleport_action" "20.0") -endif() diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index c9fa5e92e36..61a31608a7f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -163,20 +163,25 @@ class API { return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose; } - - void respawn(const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & message, const geometry_msgs::msg::PoseStamped goal_msg) + + void respawn( + const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, + const geometry_msgs::msg::PoseStamped goal_pose) { + if (name != "ego") { + throw std::runtime_error("Respawning entities other than EGO is not supported"); + } + simulation_api_schema::DespawnEntityRequest req; req.set_name(name); - // auto res = zeromq_client_.call(req); - if (auto res = zeromq_client_.call(req); res.result().success()) { simulation_api_schema::SpawnVehicleEntityRequest spawn_req; spawn_req.set_is_ego(true); - simulation_interface::toProto(toMapPose(message.pose.pose), *spawn_req.mutable_pose()); + simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); spawn_req.set_initial_speed(0.0); - simulation_interface::toProto(entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); + simulation_interface::toProto( + entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); spawn_req.mutable_parameters()->set_name(name); spawn_req.set_asset_key(""); @@ -184,13 +189,14 @@ class API } entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); - entity_manager_ptr_->asFieldOperatorApplication(name).plan(std::vector{goal_msg}); + entity_manager_ptr_->asFieldOperatorApplication(name).plan( + std::vector{goal_pose}); while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); } - entity_manager_ptr_->asFieldOperatorApplication(name).engage(); + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } template From 22c608f9dec3b9ff943a8e2765d870f57f9de2c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 21 Feb 2024 22:27:15 +0100 Subject: [PATCH 06/26] Spellcheck fix MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 +- .../include/traffic_simulator/entity/misc_object_entity.hpp | 2 +- .../include/traffic_simulator/entity/pedestrian_entity.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 61a31608a7f..c4c72b76487 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -169,7 +169,7 @@ class API const geometry_msgs::msg::PoseStamped goal_pose) { if (name != "ego") { - throw std::runtime_error("Respawning entities other than EGO is not supported"); + throw std::runtime_error("Respawn of any entities other than EGO is not supported"); } simulation_api_schema::DespawnEntityRequest req; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp index a3452ac6d6c..7ecaf0bd538 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp @@ -104,7 +104,7 @@ class MiscObjectEntity : public EntityBase auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override { - THROW_SEMANTIC_ERROR("getVehicleParametrs function cannot be used in MiscObjectEntity"); + THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in MiscObjectEntity"); } void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index f33d40c699b..fc3a89821ab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -89,7 +89,7 @@ class PedestrianEntity : public EntityBase auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override { - THROW_SEMANTIC_ERROR("getVehicleParametrs function cannot be used in PedestrianEntity"); + THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in PedestrianEntity"); } void setAccelerationLimit(double acceleration) override; From f48075e67134970a9e7faeca2acff15215326467 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 22 Feb 2024 11:07:17 +0100 Subject: [PATCH 07/26] Adapting respawn_ego scenario time to tests timeout MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index 66a5d6ca9b8..8216255bdd9 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -47,7 +47,7 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 30) { + if (api_.getCurrentTime() >= 19) { stop(cpp_mock_scenarios::Result::SUCCESS); } } From 9f1c8a3b8e4380399ccc8e856a50f543acf706a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 22 Feb 2024 12:34:11 +0100 Subject: [PATCH 08/26] Removing unnecessary changes in field_operator_application_for_autoware_universe MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- ...ator_application_for_autoware_universe.cpp | 24 +++++----- .../src/cpp_scenario_node.cpp | 48 +++++++++---------- 2 files changed, 35 insertions(+), 37 deletions(-) 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 2020e7f6581..5f3f7e2967d 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -351,20 +351,18 @@ auto FieldOperatorApplicationFor::clearRoute() -> void auto FieldOperatorApplicationFor::engage() -> void { - if (!engaged() && !isDriving()) { - task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { - auto request = std::make_shared(); - request->engage = true; - try { - return requestEngage(request); - } catch (const decltype(requestEngage)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } - }); + task_queue.delay([this]() { + waitForAutowareStateToBeDriving([this]() { + auto request = std::make_shared(); + request->engage = true; + try { + return requestEngage(request); + } catch (const decltype(requestEngage)::TimeoutError &) { + // ignore timeout error because this service is validated by Autoware state transition. + return; + } }); - } + }); } auto FieldOperatorApplicationFor::engageable() const -> bool diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 14e1c512291..5376b050fa7 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -114,32 +114,32 @@ void CppScenarioNode::spawnEgoEntity( }()); api_.requestAssignRoute("ego", goal_lanelet_poses); using namespace std::chrono_literals; - std::atomic initialized(false); - auto initialize_thread = std::thread([&]() { - while (!api_.asFieldOperatorApplication("ego").engaged()) { - api_.updateFrame(); - std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - } - initialized.store(true); - }); - std::atomic engaged(false); - auto engage_thread = std::thread([&]() { - while (!api_.asFieldOperatorApplication("ego").engaged()) { - if (!api_.asFieldOperatorApplication("ego").engageable()) { - api_.asFieldOperatorApplication("ego").engage(); - } else { - } - std::this_thread::sleep_for(3000ms); - } - engaged.store(true); - }); - while (!api_.asFieldOperatorApplication("ego").engaged() && - !(initialized.load() && engaged.load())) { - // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization..."); + // std::atomic initialized(false); + // auto initialize_thread = std::thread([&]() { + // while (!api_.asFieldOperatorApplication("ego").engaged()) { + // api_.updateFrame(); + // std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + // } + // initialized.store(true); + // }); + // std::atomic engaged(false); + // auto engage_thread = std::thread([&]() { + while (!api_.asFieldOperatorApplication("ego").engaged()) { + if (api_.asFieldOperatorApplication("ego").engageable()) + api_.asFieldOperatorApplication("ego").engage(); + api_.updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + // std::this_thread::sleep_for(1000ms); } - initialize_thread.join(); - engage_thread.join(); + // engaged.store(true); + // }); + // while (!api_.asFieldOperatorApplication("ego").engaged() && + // !(initialized.load() && engaged.load())) { + // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization..."); + // std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + // } + // initialize_thread.join(); + // engage_thread.join(); } void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration) From 716726d8d80e26919a979a4495105477b1b19aea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 22 Feb 2024 12:35:23 +0100 Subject: [PATCH 09/26] Code cleaning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/cpp_scenario_node.cpp | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 5376b050fa7..b91c22772bb 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -113,33 +113,14 @@ void CppScenarioNode::spawnEgoEntity( return configuration; }()); api_.requestAssignRoute("ego", goal_lanelet_poses); + using namespace std::chrono_literals; - // std::atomic initialized(false); - // auto initialize_thread = std::thread([&]() { - // while (!api_.asFieldOperatorApplication("ego").engaged()) { - // api_.updateFrame(); - // std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - // } - // initialized.store(true); - // }); - // std::atomic engaged(false); - // auto engage_thread = std::thread([&]() { while (!api_.asFieldOperatorApplication("ego").engaged()) { if (api_.asFieldOperatorApplication("ego").engageable()) api_.asFieldOperatorApplication("ego").engage(); api_.updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - // std::this_thread::sleep_for(1000ms); } - // engaged.store(true); - // }); - // while (!api_.asFieldOperatorApplication("ego").engaged() && - // !(initialized.load() && engaged.load())) { - // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization..."); - // std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - // } - // initialize_thread.join(); - // engage_thread.join(); } void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration) From 474f841d19fcb8466585be3382079852d30e99cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 22 Feb 2024 13:08:10 +0100 Subject: [PATCH 10/26] CMakeList style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt | 4 ++-- mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt index 8e656f86eec..51778fbb8ca 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt @@ -4,11 +4,11 @@ ament_auto_add_executable(respawn_ego target_link_libraries(respawn_ego cpp_scenario_node) install(TARGETS -respawn_ego + respawn_ego DESTINATION lib/cpp_mock_scenarios ) if(BUILD_TESTING) include(../../cmake/add_cpp_mock_scenario_test.cmake) - add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "20.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "31.0") endif() diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index 8216255bdd9..66a5d6ca9b8 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -47,7 +47,7 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode private: void onUpdate() override { - if (api_.getCurrentTime() >= 19) { + if (api_.getCurrentTime() >= 30) { stop(cpp_mock_scenarios::Result::SUCCESS); } } From c94e858ffce3faab29699fc36fb7d2c99952a64f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 22 Feb 2024 14:40:51 +0100 Subject: [PATCH 11/26] Respawn ego scenario test time adjustment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt index 51778fbb8ca..9c3b34fc50f 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt @@ -10,5 +10,5 @@ install(TARGETS if(BUILD_TESTING) include(../../cmake/add_cpp_mock_scenario_test.cmake) - add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "31.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "40.0") endif() From d6e8506c45dd1118240792453edd91dc76ca3619 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 28 Feb 2024 09:19:37 +0100 Subject: [PATCH 12/26] Codestyle MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../field_operator_application_for_autoware_universe.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 825be6696a6..d2f1146b1dc 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 @@ -20,9 +20,9 @@ #endif #include +#include #include #include -#include #include #include #include From d3caba95aea6ee7cd0c20954fe859f85f1bc5359 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 28 Feb 2024 09:20:13 +0100 Subject: [PATCH 13/26] RespawnEgo scenario temporarly removed from build MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 5ebb91bc626..5898c793423 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -53,7 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/pedestrian) add_subdirectory(src/random_scenario) add_subdirectory(src/speed_planning) - add_subdirectory(src/respawn_ego) + # add_subdirectory(src/respawn_ego) endif() From a45715cb6f7e282ac9fd1aa6564b24f1c00b6c08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 28 Feb 2024 19:25:40 +0100 Subject: [PATCH 14/26] Reducing the execution time of EgoEntitySimulation::makeSimulationModel MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../ego_entity_simulation.cpp | 41 ++++++++++++------- 1 file changed, 26 insertions(+), 15 deletions(-) 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 ee6cdb45ebe..774dfed1d4f 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 @@ -32,6 +32,16 @@ static auto getParameter(const std::string & name, T value = {}) return value; } +template +static auto getParameterFromExistingNode( + rclcpp::Node & node, const std::string & name, T value = {}) +{ + node.declare_parameter(name, value); + node.get_parameter(name, value); + + return value; +} + EgoEntitySimulation::EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time, const std::shared_ptr & hdmap_utils, @@ -94,22 +104,23 @@ auto EgoEntitySimulation::makeSimulationModel( const traffic_simulator_msgs::msg::VehicleParameters & parameters) -> const std::shared_ptr { + rclcpp::Node node{"get_parameter", "simulation"}; // clang-format off - const auto acc_time_constant = getParameter("acc_time_constant", 0.1); - const auto acc_time_delay = getParameter("acc_time_delay", 0.1); - const auto acceleration_map_path = getParameter("acceleration_map_path", ""); - const auto debug_acc_scaling_factor = getParameter("debug_acc_scaling_factor", 1.0); - const auto debug_steer_scaling_factor = getParameter("debug_steer_scaling_factor", 1.0); - const auto steer_lim = getParameter("steer_lim", parameters.axles.front_axle.max_steering); // 1.0 - const auto steer_dead_band = getParameter("steer_dead_band", 0.0); - const auto steer_rate_lim = getParameter("steer_rate_lim", 5.0); - const auto steer_time_constant = getParameter("steer_time_constant", 0.27); - const auto steer_time_delay = getParameter("steer_time_delay", 0.24); - const auto vel_lim = getParameter("vel_lim", parameters.performance.max_speed); // 50.0 - const auto vel_rate_lim = getParameter("vel_rate_lim", parameters.performance.max_acceleration); // 7.0 - const auto vel_time_constant = getParameter("vel_time_constant", 0.1); - const auto vel_time_delay = getParameter("vel_time_delay", 0.1); - const auto wheel_base = getParameter("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); + const auto acc_time_constant = getParameterFromExistingNode(node, "acc_time_constant", 0.1); + const auto acc_time_delay = getParameterFromExistingNode(node, "acc_time_delay", 0.1); + const auto acceleration_map_path = getParameterFromExistingNode(node, "acceleration_map_path", ""); + const auto debug_acc_scaling_factor = getParameterFromExistingNode(node, "debug_acc_scaling_factor", 1.0); + const auto debug_steer_scaling_factor = getParameterFromExistingNode(node, "debug_steer_scaling_factor", 1.0); + const auto steer_lim = getParameterFromExistingNode(node, "steer_lim", parameters.axles.front_axle.max_steering); // 1.0 + const auto steer_dead_band = getParameterFromExistingNode(node, "steer_dead_band", 0.0); + const auto steer_rate_lim = getParameterFromExistingNode(node, "steer_rate_lim", 5.0); + const auto steer_time_constant = getParameterFromExistingNode(node, "steer_time_constant", 0.27); + const auto steer_time_delay = getParameterFromExistingNode(node, "steer_time_delay", 0.24); + const auto vel_lim = getParameterFromExistingNode(node, "vel_lim", parameters.performance.max_speed); // 50.0 + const auto vel_rate_lim = getParameterFromExistingNode(node, "vel_rate_lim", parameters.performance.max_acceleration); // 7.0 + const auto vel_time_constant = getParameterFromExistingNode(node, "vel_time_constant", 0.1); + const auto vel_time_delay = getParameterFromExistingNode(node, "vel_time_delay", 0.1); + const auto wheel_base = getParameterFromExistingNode(node, "wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); // clang-format on switch (vehicle_model_type) { From 06eee859d07a2baf1a5bdc6f455033437fcadc34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 29 Feb 2024 13:00:57 +0100 Subject: [PATCH 15/26] Exceptions inside API::respawn MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../include/traffic_simulator/api/api.hpp | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 953c076ac20..90de3543fe4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -169,23 +169,29 @@ class API const geometry_msgs::msg::PoseStamped goal_pose) { if (name != "ego") { - throw std::runtime_error("Respawn of any entities other than EGO is not supported"); + throw std::runtime_error("Respawn of any entities other than EGO is not supported."); } - simulation_api_schema::DespawnEntityRequest req; - req.set_name(name); + simulation_api_schema::DespawnEntityRequest despawn_req; + despawn_req.set_name(name); - if (auto res = zeromq_client_.call(req); res.result().success()) { - simulation_api_schema::SpawnVehicleEntityRequest spawn_req; - spawn_req.set_is_ego(true); - simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); - spawn_req.set_initial_speed(0.0); - simulation_interface::toProto( - entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); - spawn_req.mutable_parameters()->set_name(name); - spawn_req.set_asset_key(""); + if (not zeromq_client_.call(despawn_req).result().success()) { + throw common::SimulationError( + "Despawning failed while trying to respawn the " + name + " entity"); + } - zeromq_client_.call(spawn_req).result().success(); + simulation_api_schema::SpawnVehicleEntityRequest spawn_req; + spawn_req.set_is_ego(true); + simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); + spawn_req.set_initial_speed(0.0); + simulation_interface::toProto( + entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); + spawn_req.mutable_parameters()->set_name(name); + spawn_req.set_asset_key(""); + + if (not zeromq_client_.call(spawn_req).result().success()) { + throw common::SimulationError( + "Spawning failed while trying to respawn the " + name + " entity"); } entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); From ee64d9c5464b12c40f587b21d33fb82d817c6e41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Fri, 1 Mar 2024 19:07:26 +0100 Subject: [PATCH 16/26] Code cleaning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/cpp_scenario_node.cpp | 3 +- .../include/traffic_simulator/api/api.hpp | 45 ++----------------- simulation/traffic_simulator/src/api/api.cpp | 41 +++++++++++++++++ 3 files changed, 47 insertions(+), 42 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index b91c22772bb..a2b2980590c 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -116,8 +116,9 @@ void CppScenarioNode::spawnEgoEntity( using namespace std::chrono_literals; while (!api_.asFieldOperatorApplication("ego").engaged()) { - if (api_.asFieldOperatorApplication("ego").engageable()) + if (api_.asFieldOperatorApplication("ego").engageable()) { api_.asFieldOperatorApplication("ego").engage(); + } api_.updateFrame(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 90de3543fe4..0c14c30c2f0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -164,47 +164,6 @@ class API return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose; } - void respawn( - const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, - const geometry_msgs::msg::PoseStamped goal_pose) - { - if (name != "ego") { - throw std::runtime_error("Respawn of any entities other than EGO is not supported."); - } - - simulation_api_schema::DespawnEntityRequest despawn_req; - despawn_req.set_name(name); - - if (not zeromq_client_.call(despawn_req).result().success()) { - throw common::SimulationError( - "Despawning failed while trying to respawn the " + name + " entity"); - } - - simulation_api_schema::SpawnVehicleEntityRequest spawn_req; - spawn_req.set_is_ego(true); - simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); - spawn_req.set_initial_speed(0.0); - simulation_interface::toProto( - entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); - spawn_req.mutable_parameters()->set_name(name); - spawn_req.set_asset_key(""); - - if (not zeromq_client_.call(spawn_req).result().success()) { - throw common::SimulationError( - "Spawning failed while trying to respawn the " + name + " entity"); - } - - entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); - entity_manager_ptr_->asFieldOperatorApplication(name).plan( - std::vector{goal_pose}); - - while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { - updateFrame(); - std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); - } - entity_manager_ptr_->asFieldOperatorApplication(name).engage(); - } - template auto spawn( const std::string & name, const Pose & pose, @@ -262,6 +221,10 @@ class API bool despawn(const std::string & name); bool despawnEntities(); + void respawn( + const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, + const geometry_msgs::msg::PoseStamped & goal_pose); + auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; auto setEntityStatus( const std::string & name, const geometry_msgs::msg::Pose & map_pose, diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 31e87cd8354..51bf47a81f9 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -48,6 +48,47 @@ bool API::despawnEntities() entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); }); } +void API::respawn( + const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, + const geometry_msgs::msg::PoseStamped & goal_pose) +{ + if (name != "ego") { + throw std::runtime_error("Respawn of any entities other than EGO is not supported."); + } + + simulation_api_schema::DespawnEntityRequest despawn_req; + despawn_req.set_name(name); + + if (not zeromq_client_.call(despawn_req).result().success()) { + throw common::SimulationError( + "Despawning failed while trying to respawn the " + name + " entity"); + } + + simulation_api_schema::SpawnVehicleEntityRequest spawn_req; + spawn_req.set_is_ego(true); + simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); + spawn_req.set_initial_speed(0.0); + simulation_interface::toProto( + entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); + spawn_req.mutable_parameters()->set_name(name); + spawn_req.set_asset_key(""); + + if (not zeromq_client_.call(spawn_req).result().success()) { + throw common::SimulationError( + "Spawning failed while trying to respawn the " + name + " entity"); + } + + entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); + entity_manager_ptr_->asFieldOperatorApplication(name).plan( + std::vector{goal_pose}); + + while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { + updateFrame(); + std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + } + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); +} + auto API::setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status) -> void { From b3e63eb01192e80139e10152aa28f8ec5fd278ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Fri, 1 Mar 2024 19:43:46 +0100 Subject: [PATCH 17/26] QoS of the subscriber in respawn_ego scenario changed to match the one used in the initial_pose_adaptor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index 66a5d6ca9b8..878183024b9 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -33,7 +33,7 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode goal_pose{ api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))}, new_position_subscriber{create_subscription( - "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { geometry_msgs::msg::PoseStamped goal_msg; goal_msg.header.frame_id = "map"; From 15e6c1c940c754b9d225bf3bb8e86db5f429d090 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Mon, 4 Mar 2024 14:22:54 +0100 Subject: [PATCH 18/26] updateFrame removed from API::respawn MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- simulation/traffic_simulator/src/api/api.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 51bf47a81f9..3817a5cd7a3 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -83,7 +83,7 @@ void API::respawn( std::vector{goal_pose}); while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { - updateFrame(); + entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); } entity_manager_ptr_->asFieldOperatorApplication(name).engage(); From d8a005e2466dbc9aef73f088204e7f31c8e121f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 12 Mar 2024 12:40:15 +0100 Subject: [PATCH 19/26] Despawning and Spawning entity replaced with UpdateEntityStatusRequest MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- simulation/traffic_simulator/src/api/api.cpp | 55 ++++++++++++++----- .../src/entity/entity_manager.cpp | 2 +- 2 files changed, 41 insertions(+), 16 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 3817a5cd7a3..04b2a777bd1 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -52,30 +52,54 @@ void API::respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) { - if (name != "ego") { + if (not entity_manager_ptr_->isEgo(name)) { throw std::runtime_error("Respawn of any entities other than EGO is not supported."); } - simulation_api_schema::DespawnEntityRequest despawn_req; - despawn_req.set_name(name); + if (new_pose.header.frame_id != "map") { + throw std::runtime_error("Respawn request with frame id other than map not supported."); + } + + setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); - if (not zeromq_client_.call(despawn_req).result().success()) { + simulation_api_schema::UpdateEntityStatusRequest req; + req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); + simulation_interface::toProto( + static_cast(entity_manager_ptr_->getEntityStatus(name)), *req.add_status()); + req.set_overwrite_ego_status(entity_manager_ptr_->isEgo(name)); + + auto res = zeromq_client_.call(req); + + if (not res.result().success()) { throw common::SimulationError( - "Despawning failed while trying to respawn the " + name + " entity"); + "UpdateEntityStatus request failed for " + name + " entity during respawn."); } - simulation_api_schema::SpawnVehicleEntityRequest spawn_req; - spawn_req.set_is_ego(true); - simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose()); - spawn_req.set_initial_speed(0.0); - simulation_interface::toProto( - entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters()); - spawn_req.mutable_parameters()->set_name(name); - spawn_req.set_asset_key(""); + auto res_status = res.status().begin(); - if (not zeromq_client_.call(spawn_req).result().success()) { + if (res_status == res.status().end()) { throw common::SimulationError( - "Spawning failed while trying to respawn the " + name + " entity"); + "Failed to receive the new status of " + name + " entity after the update request."); + } + + auto entity_name = res_status->name(); + + if (entity_name != name) { + throw common::SimulationError( + "Wrong entity status received during respawn. Expected: " + name + + ". Received: " + entity_name + "."); + } + + auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); + simulation_interface::toMsg(res_status->pose(), entity_status.pose); + simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); + + if (entity_manager_ptr_->isEgo(entity_name)) { + setMapPose(entity_name, entity_status.pose); + setTwist(entity_name, entity_status.action_status.twist); + setAcceleration(entity_name, entity_status.action_status.accel); + } else { + setEntityStatus(entity_name, canonicalize(entity_status)); } entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); @@ -86,6 +110,7 @@ void API::respawn( entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); } + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 0fc5f2ecd73..c927bc025bc 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -702,7 +702,7 @@ void EntityManager::requestSpeedChange( auto EntityManager::setEntityStatus( const std::string & name, const CanonicalizedEntityStatus & status) -> void { - if (isEgo(name) && getCurrentTime() > 0) { + if (isEgo(name) && getCurrentTime() > 0 && isControlledBySimulator(name)) { THROW_SEMANTIC_ERROR( "You cannot set entity status to the ego vehicle name ", std::quoted(name), " after starting scenario."); From 351372f7795061fe5fa77eb5319b40110b1914c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Wed, 3 Apr 2024 14:13:44 +0200 Subject: [PATCH 20/26] fix(respawn): fix after merge - add updateFrame, adjust isEgo --- simulation/traffic_simulator/src/api/api.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index ace274786b7..2ff06f1aa41 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -52,7 +52,7 @@ void API::respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) { - if (not entity_manager_ptr_->isEgo(name)) { + if (not entity_manager_ptr_->is(name)) { throw std::runtime_error("Respawn of any entities other than EGO is not supported."); } @@ -66,7 +66,7 @@ void API::respawn( req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); simulation_interface::toProto( static_cast(entity_manager_ptr_->getEntityStatus(name)), *req.add_status()); - req.set_overwrite_ego_status(entity_manager_ptr_->isEgo(name)); + req.set_overwrite_ego_status(entity_manager_ptr_->is(name)); auto res = zeromq_client_.call(req); @@ -94,7 +94,7 @@ void API::respawn( simulation_interface::toMsg(res_status->pose(), entity_status.pose); simulation_interface::toMsg(res_status->action_status(), entity_status.action_status); - if (entity_manager_ptr_->isEgo(entity_name)) { + 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); @@ -107,10 +107,10 @@ void API::respawn( std::vector{goal_pose}); while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { + updateFrame(); entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); } - entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } From 7bee36d079cbb2ea5507599aa76473cd36dd73a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Wed, 3 Apr 2024 20:18:23 +0200 Subject: [PATCH 21/26] feat(EgoEntity): add setControlledBySimulator --- .../include/traffic_simulator/entity/ego_entity.hpp | 5 +++++ .../include/traffic_simulator/entity/entity_base.hpp | 2 ++ .../include/traffic_simulator/entity/entity_manager.hpp | 1 + simulation/traffic_simulator/src/entity/entity_base.cpp | 6 ++++++ 4 files changed, 14 insertions(+) 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 a72ec7dcadf..fdf0aa0d171 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -112,6 +112,11 @@ class EgoEntity : public VehicleEntity auto isControlledBySimulator() const -> bool override; + auto setControlledBySimulator(bool state) -> void override + { + is_controlled_by_simulator_ = state; + } + auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override; 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 56d624be9f1..bd6e9a2a2f7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -189,6 +189,8 @@ class EntityBase virtual auto isControlledBySimulator() const -> bool; + virtual auto setControlledBySimulator(bool) -> void; + virtual auto requestFollowTrajectory( const std::shared_ptr &) -> 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 fa42314ee5d..6083714875e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -319,6 +319,7 @@ class EntityManager 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, ); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index fb5549b321e..1c16c25b1ff 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -656,6 +656,12 @@ void EntityBase::requestSpeedChange( auto EntityBase::isControlledBySimulator() const -> bool { return true; } +auto EntityBase::setControlledBySimulator(bool /*unused*/) -> void +{ + THROW_SEMANTIC_ERROR( + getEntityTypename(), " type entities do not support setControlledBySimulator"); +} + auto EntityBase::requestFollowTrajectory( const std::shared_ptr &) -> void { From 1b9639fd8a0c5e2785a20bd16d3ae25e7aa125cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Wed, 3 Apr 2024 20:37:21 +0200 Subject: [PATCH 22/26] ref(respawn): improve current solution --- simulation/traffic_simulator/src/api/api.cpp | 101 ++++++++---------- .../src/entity/entity_manager.cpp | 2 +- 2 files changed, 47 insertions(+), 56 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 2ff06f1aa41..1b6d722bb4d 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -54,64 +54,55 @@ void API::respawn( { if (not entity_manager_ptr_->is(name)) { throw std::runtime_error("Respawn of any entities other than EGO is not supported."); - } - - if (new_pose.header.frame_id != "map") { + } else if (new_pose.header.frame_id != "map") { throw std::runtime_error("Respawn request with frame id other than map not supported."); - } - - setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); - - simulation_api_schema::UpdateEntityStatusRequest req; - req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); - simulation_interface::toProto( - static_cast(entity_manager_ptr_->getEntityStatus(name)), *req.add_status()); - req.set_overwrite_ego_status(entity_manager_ptr_->is(name)); - - auto res = zeromq_client_.call(req); - - if (not res.result().success()) { - throw common::SimulationError( - "UpdateEntityStatus request failed for " + name + " entity during respawn."); - } - - auto res_status = res.status().begin(); - - if (res_status == res.status().end()) { - throw common::SimulationError( - "Failed to receive the new status of " + name + " entity after the update request."); - } - - auto entity_name = res_status->name(); - - if (entity_name != name) { - throw common::SimulationError( - "Wrong entity status received during respawn. Expected: " + name + - ". Received: " + entity_name + "."); - } - - auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); - 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); } else { - setEntityStatus(entity_name, canonicalize(entity_status)); - } - - entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); - entity_manager_ptr_->asFieldOperatorApplication(name).plan( - std::vector{goal_pose}); - - while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { - updateFrame(); - entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); - std::this_thread::sleep_for(std::chrono::duration(1.0 / 20.0)); + // set new pose and default action status in EntityManager + entity_manager_ptr_->setControlledBySimulator(name, true); + setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus()); + + // 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()); + 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); + + // check response + if (const auto res = zeromq_client_.call(req); not res.result().success()) { + throw common::SimulationError( + "UpdateEntityStatus request failed for \"" + name + "\" entity during respawn."); + } else if (const auto res_status = res.status().begin(); res.status().size() != 1) { + throw common::SimulationError( + "Failed to receive the new status of \"" + name + "\" entity after the update request."); + } else if (const auto res_name = res_status->name(); res_name != name) { + throw common::SimulationError( + "Wrong entity status received during respawn. Expected: \"" + name + "\". Received: \"" + + res_name + "\"."); + } else { + // if valid, set response in EntityManager, then plan path and engage + auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(name)); + 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_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); + entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose}); + + // DIRTY HACK + // waiting for the possibility to engage, during this loop the update of the simulation is + // not performed, only the publication of the clock takes place - it is necessary + while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { + clock_pub_->publish(clock_.getCurrentRosTimeAsMsg()); + entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); + } } - entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } auto API::setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index df53cacbb02..14d6f222033 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -747,7 +747,7 @@ void EntityManager::requestSpeedChange( auto EntityManager::setEntityStatus( const std::string & name, const CanonicalizedEntityStatus & status) -> void { - if (is(name) && getCurrentTime() > 0 && isControlledBySimulator(name)) { + if (is(name) && getCurrentTime() > 0 && not isControlledBySimulator(name)) { THROW_SEMANTIC_ERROR( "You cannot set entity status to the ego vehicle name ", std::quoted(name), " after starting scenario."); From bedf9eca420711feedebd9cd9650a76a4b11462c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Thu, 4 Apr 2024 09:18:29 +0200 Subject: [PATCH 23/26] ref(respawn): remove engagable() check --- simulation/traffic_simulator/src/api/api.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 1b6d722bb4d..0de1b625cfb 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -91,15 +91,6 @@ void API::respawn( entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute(); entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose}); - - // DIRTY HACK - // waiting for the possibility to engage, during this loop the update of the simulation is - // not performed, only the publication of the clock takes place - it is necessary - while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) { - clock_pub_->publish(clock_.getCurrentRosTimeAsMsg()); - entity_manager_ptr_->asFieldOperatorApplication(name).spinSome(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } entity_manager_ptr_->asFieldOperatorApplication(name).engage(); } } From 1578d300af4d3b7b6b90cec4af94e8f21fc1657a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= Date: Mon, 22 Apr 2024 12:39:29 +0200 Subject: [PATCH 24/26] ref(traffic_simulator, respawn): apply requested PR changes --- ...ator_application_for_autoware_universe.hpp | 2 +- ...ator_application_for_autoware_universe.cpp | 4 +- .../ego_entity_simulation.cpp | 45 +++++++++---------- .../include/traffic_simulator/api/api.hpp | 4 +- .../traffic_simulator/entity/entity_base.hpp | 3 -- .../entity/entity_manager.hpp | 1 - .../entity/misc_object_entity.hpp | 5 --- .../entity/pedestrian_entity.hpp | 4 -- .../entity/vehicle_entity.hpp | 3 -- simulation/traffic_simulator/src/api/api.cpp | 4 +- .../src/entity/vehicle_entity.cpp | 6 --- 11 files changed, 26 insertions(+), 55 deletions(-) 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 d2f1146b1dc..02acfcc2fbe 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 @@ -65,11 +65,11 @@ class FieldOperatorApplicationFor SubscriberWrapper getTrajectory; SubscriberWrapper getTurnIndicatorsCommandImpl; + ServiceWithValidation requestClearRoute; ServiceWithValidation requestCooperateCommands; ServiceWithValidation requestEngage; ServiceWithValidation requestInitialPose; ServiceWithValidation requestSetRoutePoints; - ServiceWithValidation requestClearRoute; ServiceWithValidation requestSetRtcAutoMode; ServiceWithValidation requestSetVelocityLimit; // clang-format on 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 c7ed085a645..b86d4b3a657 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -350,9 +350,7 @@ auto FieldOperatorApplicationFor::plan( auto FieldOperatorApplicationFor::clearRoute() -> void { task_queue.delay([this] { - auto request = std::make_shared(); - - requestClearRoute(request); + requestClearRoute(std::make_shared()); }); } 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 1df05ff5eb1..b22345b752b 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 @@ -31,16 +31,6 @@ static auto getParameter(const std::string & name, T value = {}) return value; } -template -static auto getParameterFromExistingNode( - rclcpp::Node & node, const std::string & name, T value = {}) -{ - node.declare_parameter(name, value); - node.get_parameter(name, value); - - return value; -} - EgoEntitySimulation::EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time, const std::shared_ptr & hdmap_utils, @@ -108,22 +98,27 @@ auto EgoEntitySimulation::makeSimulationModel( -> const std::shared_ptr { rclcpp::Node node{"get_parameter", "simulation"}; + auto get_parameter = [](rclcpp::Node & node, const std::string & name, auto value = {}) { + node.declare_parameter(name, value); + node.get_parameter(name, value); + return value; + }; // clang-format off - const auto acc_time_constant = getParameterFromExistingNode(node, "acc_time_constant", 0.1); - const auto acc_time_delay = getParameterFromExistingNode(node, "acc_time_delay", 0.1); - const auto acceleration_map_path = getParameterFromExistingNode(node, "acceleration_map_path", ""); - const auto debug_acc_scaling_factor = getParameterFromExistingNode(node, "debug_acc_scaling_factor", 1.0); - const auto debug_steer_scaling_factor = getParameterFromExistingNode(node, "debug_steer_scaling_factor", 1.0); - const auto steer_lim = getParameterFromExistingNode(node, "steer_lim", parameters.axles.front_axle.max_steering); // 1.0 - const auto steer_dead_band = getParameterFromExistingNode(node, "steer_dead_band", 0.0); - const auto steer_rate_lim = getParameterFromExistingNode(node, "steer_rate_lim", 5.0); - const auto steer_time_constant = getParameterFromExistingNode(node, "steer_time_constant", 0.27); - const auto steer_time_delay = getParameterFromExistingNode(node, "steer_time_delay", 0.24); - const auto vel_lim = getParameterFromExistingNode(node, "vel_lim", parameters.performance.max_speed); // 50.0 - const auto vel_rate_lim = getParameterFromExistingNode(node, "vel_rate_lim", parameters.performance.max_acceleration); // 7.0 - const auto vel_time_constant = getParameterFromExistingNode(node, "vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator - const auto vel_time_delay = getParameterFromExistingNode(node, "vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator - const auto wheel_base = getParameterFromExistingNode(node, "wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); + const auto acc_time_constant = get_parameter(node, "acc_time_constant", 0.1); + const auto acc_time_delay = get_parameter(node, "acc_time_delay", 0.1); + const auto acceleration_map_path = get_parameter(node, "acceleration_map_path", std::string("")); + const auto debug_acc_scaling_factor = get_parameter(node, "debug_acc_scaling_factor", 1.0); + const auto debug_steer_scaling_factor = get_parameter(node, "debug_steer_scaling_factor", 1.0); + const auto steer_lim = get_parameter(node, "steer_lim", parameters.axles.front_axle.max_steering); // 1.0 + const auto steer_dead_band = get_parameter(node, "steer_dead_band", 0.0); + const auto steer_rate_lim = get_parameter(node, "steer_rate_lim", 5.0); + const auto steer_time_constant = get_parameter(node, "steer_time_constant", 0.27); + const auto steer_time_delay = get_parameter(node, "steer_time_delay", 0.24); + const auto vel_lim = get_parameter(node, "vel_lim", parameters.performance.max_speed); // 50.0 + const auto vel_rate_lim = get_parameter(node, "vel_rate_lim", parameters.performance.max_acceleration); // 7.0 + const auto vel_time_constant = get_parameter(node, "vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator + const auto vel_time_delay = get_parameter(node, "vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator + const auto wheel_base = get_parameter(node, "wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); // clang-format on switch (vehicle_model_type) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 64d210c21e0..089ce154de2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -221,9 +221,9 @@ class API bool despawn(const std::string & name); bool despawnEntities(); - void respawn( + auto respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, - const geometry_msgs::msg::PoseStamped & goal_pose); + const geometry_msgs::msg::PoseStamped & goal_pose) -> void; auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; auto setEntityStatus( 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 bd6e9a2a2f7..7338ed94868 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -120,9 +120,6 @@ class EntityBase virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0; - virtual auto getVehicleParameters() const - -> const traffic_simulator_msgs::msg::VehicleParameters = 0; - virtual auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0; 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 2b428376f46..09020f1cbe9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -327,7 +327,6 @@ class EntityManager FORWARD_TO_ENTITY(setMapPose, ); FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); - FORWARD_TO_ENTITY(getVehicleParameters, ); #undef FORWARD_TO_ENTITY diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp index a2ad5f729a6..4cafbb61963 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp @@ -102,11 +102,6 @@ class MiscObjectEntity : public EntityBase auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override; - auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override - { - THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in MiscObjectEntity"); - } - void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override; void setVelocityLimit(double) override{}; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index aac7e1f93a4..ae3beae7719 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -87,10 +87,6 @@ class PedestrianEntity : public EntityBase void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &); - auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override - { - THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in PedestrianEntity"); - } void setVelocityLimit(double linear_velocity) override; void setAccelerationLimit(double acceleration) override; 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 3acd9e4baa6..2ca6052bb02 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -92,9 +92,6 @@ class VehicleEntity : public EntityBase auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; - auto getVehicleParameters() const - -> const traffic_simulator_msgs::msg::VehicleParameters override; - void onUpdate(double current_time, double step_time) override; void requestAcquirePosition(const CanonicalizedLaneletPose &); diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 0de1b625cfb..71cdb1115bd 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -48,9 +48,9 @@ bool API::despawnEntities() entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); }); } -void API::respawn( +auto API::respawn( const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, - const geometry_msgs::msg::PoseStamped & goal_pose) + const geometry_msgs::msg::PoseStamped & goal_pose) -> void { if (not entity_manager_ptr_->is(name)) { throw std::runtime_error("Respawn of any entities other than EGO is not supported."); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index b06f0ad5c22..5e5989a1701 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -139,12 +139,6 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin } } -auto VehicleEntity::getVehicleParameters() const - -> const traffic_simulator_msgs::msg::VehicleParameters -{ - return behavior_plugin_ptr_->getVehicleParameters(); -} - void VehicleEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); From 16f7c6b5370fc3c25fcfe097d193ef648cd7108d Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 8 May 2024 09:32:13 +0200 Subject: [PATCH 25/26] ref(sss, concealer): apply requested PR changes - style --- .../field_operator_application_for_autoware_universe.hpp | 6 +++--- .../src/vehicle_simulation/ego_entity_simulation.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) 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 02acfcc2fbe..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 @@ -122,17 +122,17 @@ class FieldOperatorApplicationFor getLocalizationState("/api/localization/initialization_state", *this), #endif getMrmState("/api/fail_safe/mrm_state", *this, [this](const auto & v) { receiveMrmState(v); }), + getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this), getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", *this), getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", *this), + requestClearRoute("/api/routing/clear_route", *this), requestCooperateCommands("/api/external/set/rtc_commands", *this), requestEngage("/api/external/set/engage", *this), requestInitialPose("/api/localization/initialize", *this), // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons. requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), - requestClearRoute("/api/routing/clear_route", *this), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), - requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this) + requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this) // clang-format on { } 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 b22345b752b..ef90b2fba35 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 @@ -97,8 +97,9 @@ auto EgoEntitySimulation::makeSimulationModel( const traffic_simulator_msgs::msg::VehicleParameters & parameters) -> const std::shared_ptr { - rclcpp::Node node{"get_parameter", "simulation"}; - auto get_parameter = [](rclcpp::Node & node, const std::string & name, auto value = {}) { + auto node = rclcpp::Node("get_parameter", "simulation"); + + auto get_parameter = [&](const std::string & name, auto value = {}) { node.declare_parameter(name, value); node.get_parameter(name, value); return value; From 237d0f4b1717f38b2bb73b2bcfc3cdcac5556ae7 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 8 May 2024 09:48:49 +0200 Subject: [PATCH 26/26] fix(sss): missing adaptation change - get_parameter --- .../ego_entity_simulation.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) 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 ef90b2fba35..f1d7eb1bb4c 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 @@ -105,21 +105,21 @@ auto EgoEntitySimulation::makeSimulationModel( return value; }; // clang-format off - const auto acc_time_constant = get_parameter(node, "acc_time_constant", 0.1); - const auto acc_time_delay = get_parameter(node, "acc_time_delay", 0.1); - const auto acceleration_map_path = get_parameter(node, "acceleration_map_path", std::string("")); - const auto debug_acc_scaling_factor = get_parameter(node, "debug_acc_scaling_factor", 1.0); - const auto debug_steer_scaling_factor = get_parameter(node, "debug_steer_scaling_factor", 1.0); - const auto steer_lim = get_parameter(node, "steer_lim", parameters.axles.front_axle.max_steering); // 1.0 - const auto steer_dead_band = get_parameter(node, "steer_dead_band", 0.0); - const auto steer_rate_lim = get_parameter(node, "steer_rate_lim", 5.0); - const auto steer_time_constant = get_parameter(node, "steer_time_constant", 0.27); - const auto steer_time_delay = get_parameter(node, "steer_time_delay", 0.24); - const auto vel_lim = get_parameter(node, "vel_lim", parameters.performance.max_speed); // 50.0 - const auto vel_rate_lim = get_parameter(node, "vel_rate_lim", parameters.performance.max_acceleration); // 7.0 - const auto vel_time_constant = get_parameter(node, "vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator - const auto vel_time_delay = get_parameter(node, "vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator - const auto wheel_base = get_parameter(node, "wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); + const auto acc_time_constant = get_parameter("acc_time_constant", 0.1); + const auto acc_time_delay = get_parameter("acc_time_delay", 0.1); + const auto acceleration_map_path = get_parameter("acceleration_map_path", std::string("")); + const auto debug_acc_scaling_factor = get_parameter("debug_acc_scaling_factor", 1.0); + const auto debug_steer_scaling_factor = get_parameter("debug_steer_scaling_factor", 1.0); + const auto steer_lim = get_parameter("steer_lim", parameters.axles.front_axle.max_steering); // 1.0 + const auto steer_dead_band = get_parameter("steer_dead_band", 0.0); + const auto steer_rate_lim = get_parameter("steer_rate_lim", 5.0); + const auto steer_time_constant = get_parameter("steer_time_constant", 0.27); + const auto steer_time_delay = get_parameter("steer_time_delay", 0.24); + const auto vel_lim = get_parameter("vel_lim", parameters.performance.max_speed); // 50.0 + const auto vel_rate_lim = get_parameter("vel_rate_lim", parameters.performance.max_acceleration); // 7.0 + const auto vel_time_constant = get_parameter("vel_time_constant", 0.1); /// @note 0.5 is default value on simple_planning_simulator + const auto vel_time_delay = get_parameter("vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator + const auto wheel_base = get_parameter("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); // clang-format on switch (vehicle_model_type) {