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