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..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 @@ -20,6 +20,7 @@ #endif #include +#include #include #include #include @@ -64,6 +65,7 @@ class FieldOperatorApplicationFor SubscriberWrapper getTrajectory; SubscriberWrapper getTurnIndicatorsCommandImpl; + ServiceWithValidation requestClearRoute; ServiceWithValidation requestCooperateCommands; ServiceWithValidation requestEngage; ServiceWithValidation requestInitialPose; @@ -120,16 +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)), 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 { } @@ -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 ca7f7ffd423..b86d4b3a657 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -347,6 +347,13 @@ auto FieldOperatorApplicationFor::plan( }); } +auto FieldOperatorApplicationFor::clearRoute() -> void +{ + task_queue.delay([this] { + requestClearRoute(std::make_shared()); + }); +} + auto FieldOperatorApplicationFor::engage() -> void { task_queue.delay([this]() { diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 3088392fb53..508547a853c 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -54,6 +54,8 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/pedestrian) add_subdirectory(src/random_scenario) add_subdirectory(src/speed_planning) + # add_subdirectory(src/respawn_ego) + 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..a2b2980590c 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); @@ -112,30 +113,15 @@ 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()) { + while (!api_.asFieldOperatorApplication("ego").engaged()) { + if (api_.asFieldOperatorApplication("ego").engageable()) { api_.asFieldOperatorApplication("ego").engage(); - 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..."); + api_.updateFrame(); 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) 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..9c3b34fc50f --- /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" "40.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 new file mode 100644 index 00000000000..878183024b9 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -0,0 +1,78 @@ +// 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 + +namespace cpp_mock_scenarios +{ +class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit RespawnEgoScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "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)).reliable(), + [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(); + } + +private: + void onUpdate() override + { + if (api_.getCurrentTime() >= 30) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + } + + void onInitialize() override + { + spawnEgoEntity( + api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)), + {goal_pose}, getVehicleParameters()); + } + +private: + const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose; + + const 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/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index c06fe1f7a93..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 @@ -97,22 +97,29 @@ auto EgoEntitySimulation::makeSimulationModel( const traffic_simulator_msgs::msg::VehicleParameters & parameters) -> const std::shared_ptr { + 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; + }; // 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); /// @note 0.5 is default value on simple_planning_simulator - const auto vel_time_delay = getParameter("vel_time_delay", 0.1); /// @note 0.25 is default value on simple_planning_simulator - const auto wheel_base = getParameter("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) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index c86abe65d6e..089ce154de2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -221,6 +221,10 @@ class API bool despawn(const std::string & name); bool despawnEntities(); + auto respawn( + const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, + const geometry_msgs::msg::PoseStamped & goal_pose) -> void; + auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; auto setEntityStatus( const std::string & name, const geometry_msgs::msg::Pose & map_pose, 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 2233bbb6a8e..7338ed94868 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -186,6 +186,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 d043be4fc60..09020f1cbe9 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/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index a4aba66b910..71cdb1115bd 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -48,6 +48,54 @@ bool API::despawnEntities() entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); }); } +auto API::respawn( + const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, + const geometry_msgs::msg::PoseStamped & goal_pose) -> void +{ + if (not entity_manager_ptr_->is(name)) { + throw std::runtime_error("Respawn of any entities other than EGO is not supported."); + } else if (new_pose.header.frame_id != "map") { + throw std::runtime_error("Respawn request with frame id other than map not supported."); + } else { + // 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}); + entity_manager_ptr_->asFieldOperatorApplication(name).engage(); + } + } +} + auto API::setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status) -> void { diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index b4558881dba..8f5c0849091 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 { diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index a099e8e05b9..65f9f0345a7 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -789,7 +789,7 @@ void EntityManager::requestSpeedChange( auto EntityManager::setEntityStatus( const std::string & name, const CanonicalizedEntityStatus & status) -> void { - if (is(name) && getCurrentTime() > 0) { + 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.");