diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 669465e6482..883da343c49 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -55,8 +55,9 @@ if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/random_scenario) add_subdirectory(src/spawn) add_subdirectory(src/speed_planning) - add_subdirectory(src/traffic_source) add_subdirectory(src/synchronized_action) + add_subdirectory(src/traffic_sink) + add_subdirectory(src/traffic_source) # add_subdirectory(src/respawn_ego) endif() diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 4d1141c8fc7..680e4e128ef 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -33,7 +33,8 @@ class CppScenarioNode : public rclcpp::Node explicit CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, - const bool verbose, const rclcpp::NodeOptions & option); + const bool verbose, const rclcpp::NodeOptions & option, const bool auto_sink = false, + const std::set sinkable_entity_type = {}); void start(); void stop(Result result, const std::string & description = ""); void expectThrow() { exception_expect_ = true; } @@ -71,7 +72,8 @@ class CppScenarioNode : public rclcpp::Node int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, - const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration + const std::string & scenario_filename, const bool verbose, const bool auto_sink = false, + const std::set sinkable_entity_type = {}) -> traffic_simulator::Configuration { auto configuration = traffic_simulator::Configuration(map_path); { @@ -79,6 +81,8 @@ class CppScenarioNode : public rclcpp::Node // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; configuration.scenario_path = scenario_filename; configuration.verbose = verbose; + configuration.auto_sink = auto_sink; + configuration.sinkable_entity_type = sinkable_entity_type; } checkConfiguration(configuration); return configuration; diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 5dfb3e1043e..afa23b31524 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,7 +30,13 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction +from launch.actions import ( + EmitEvent, + RegisterEventHandler, + LogInfo, + TimerAction, + OpaqueFunction, +) from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -40,6 +46,7 @@ from pathlib import Path + class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -69,6 +76,7 @@ def on_stdout_output(event: launch.Event) -> None: if lines[0] == "cpp_scenario:success": print(Color.GREEN + "Scenario Succeed" + Color.END) + def architecture_types(): return ["awf/universe", "awf/universe/20230906", "awf/universe/20240605"] @@ -96,8 +104,13 @@ def default_autoware_launch_file_of(architecture_type): "awf/universe/20240605": "planning_simulator.launch.xml", }[architecture_type] + def default_rviz_config_file(): - return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + return ( + Path(get_package_share_directory("traffic_simulator")) + / "config/scenario_simulator_v2.rviz" + ) + def launch_setup(context, *args, **kwargs): # fmt: off @@ -127,20 +140,38 @@ def launch_setup(context, *args, **kwargs): junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") # fmt: on - print(f"architecture_type := {architecture_type.perform(context)}") - print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") - print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") - print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") - print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") - print(f"global_frame_rate := {global_frame_rate.perform(context)}") - print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") + print( + f"architecture_type := {architecture_type.perform(context)}" + ) + print( + f"autoware_launch_file := {autoware_launch_file.perform(context)}" + ) + print( + f"autoware_launch_package := {autoware_launch_package.perform(context)}" + ) + print( + f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}" + ) + print( + f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}" + ) + print( + f"global_frame_rate := {global_frame_rate.perform(context)}" + ) + print( + f"global_real_time_factor := {global_real_time_factor.perform(context)}" + ) print(f"global_timeout := {global_timeout.perform(context)}") - print(f"initialize_duration := {initialize_duration.perform(context)}") + print( + f"initialize_duration := {initialize_duration.perform(context)}" + ) print(f"launch_autoware := {launch_autoware.perform(context)}") print(f"launch_rviz := {launch_rviz.perform(context)}") print(f"output_directory := {output_directory.perform(context)}") print(f"port := {port.perform(context)}") - print(f"publish_empty_context := {publish_empty_context.perform(context)}") + print( + f"publish_empty_context := {publish_empty_context.perform(context)}" + ) print(f"record := {record.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") @@ -156,12 +187,14 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, - {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, + { + "consider_acceleration_by_road_slope": consider_acceleration_by_road_slope + }, {"consider_pose_by_road_slope": consider_pose_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, {"port": port}, - {"publish_empty_context" : publish_empty_context}, + {"publish_empty_context": publish_empty_context}, {"record": record}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, @@ -189,13 +222,13 @@ def description(): return parameters cpp_scenario_node = Node( - package=scenario_package, - executable=scenario, - name="scenario_node", - output="screen", - arguments=[("__log_level:=info")], - parameters=make_parameters() + [{"use_sim_time": use_sim_time}], - ) + package=scenario_package, + executable=scenario, + name="scenario_node", + output="screen", + arguments=[("__log_level:=info")], + parameters=make_parameters() + [{"use_sim_time": use_sim_time}], + ) io_handler = OnProcessIO( target_action=cpp_scenario_node, on_stderr=on_stderr_output, @@ -243,6 +276,7 @@ def description(): namespace="simulation", name="visualizer", output="screen", + remappings=[("/simulation/entity/status", "/entity/status")], ), Node( package="rviz2", @@ -251,10 +285,15 @@ def description(): output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), arguments=["-d", str(default_rviz_config_file())], + remappings=[ + ("/simulation/lanelet/marker", "/lanelet/marker"), + ("/simulation/debug_marker", "/debug_marker"), + ], ), RegisterEventHandler(event_handler=io_handler), RegisterEventHandler(event_handler=shutdown_handler), ] + def generate_launch_description(): return LaunchDescription([OpaqueFunction(function=launch_setup)]) diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index 7864980bdab..d49605402be 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -20,10 +20,13 @@ namespace cpp_mock_scenarios CppScenarioNode::CppScenarioNode( const std::string & node_name, const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, - const rclcpp::NodeOptions & option) + const rclcpp::NodeOptions & option, const bool auto_sink, + const std::set sinkable_entity_type) : Node(node_name, option), api_( - this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), + this, + configure( + map_path, lanelet2_map_file, scenario_filename, verbose, auto_sink, sinkable_entity_type), declare_parameter("global_real_time_factor", 1.0), declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..c0d70ef03e7 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -30,7 +30,7 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) : cpp_mock_scenarios::CppScenarioNode( "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", - "lanelet2_map.osm", __FILE__, false, option) + "lanelet2_map.osm", __FILE__, false, option, true) { start(); } diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt new file mode 100644 index 00000000000..e1966600cb5 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_auto_add_executable(auto_sink_vehicle + auto_sink_vehicle.cpp +) +target_link_libraries(auto_sink_vehicle cpp_scenario_node) + +install(TARGETS + auto_sink_vehicle + DESTINATION lib/cpp_mock_scenarios +) + +if(BUILD_TESTING) + include(../../cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "auto_sink_vehicle" "5.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp new file mode 100644 index 00000000000..fe1b2110b33 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -0,0 +1,76 @@ +// 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 + +namespace cpp_mock_scenarios +{ +class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit AutoSinkVehicleScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "auto_sink_vehicle", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option, true, {traffic_simulator::EntityType::VEHICLE}) + { + start(); + } + +private: + void onUpdate() override + { + if (api_.getCurrentTime() >= 0.1) { + if (api_.entityExists("bob") && !api_.entityExists("ego")) { + stop(cpp_mock_scenarios::Result::FAILURE); + } else { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils())), + getVehicleParameters()); + api_.spawn( + "bob", + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34774, 11.0, 0.0, api_.getHdmapUtils())), + getPedestrianParameters()); + } +}; +} // 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 1d5f44795fa..a30855dc2cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +76,14 @@ class API getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, + [this](const auto & entity_name) { + if (const auto entity = getEntity(entity_name)) { + return entity->getEntityType(); + } else { + THROW_SEMANTIC_ERROR("Entity ", std::quoted(entity_name), " does not exists."); + } + }, + configuration.sinkable_entity_type, [this](const auto & entity_name) { if (const auto entity = getEntity(entity_name)) { return entity->getMapPose(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 155fab255b9..c2b17c99d5e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -21,7 +21,9 @@ #include #include #include +#include #include +#include namespace traffic_simulator { @@ -33,6 +35,8 @@ struct Configuration bool auto_sink = true; + std::set sinkable_entity_type = {}; + bool verbose = false; bool standalone_mode = false; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index 75c2a53ea83..1dac416f036 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -27,6 +27,7 @@ #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_CONTROLLER_HPP_ #include +#include #include #include #include @@ -43,9 +44,11 @@ class TrafficController public: explicit TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink = false); + const std::function(void)> & get_entity_names, + const std::function & get_entity_type, + const std::set & sinkable_entity_type, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink = false); template void addModule(Ts &&... xs) @@ -60,9 +63,11 @@ class TrafficController void autoSink(); const std::shared_ptr hdmap_utils_; std::vector> modules_; - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_type; + const std::set sinkable_entity_type; + const std::function get_entity_pose; + const std::function despawn; public: const bool auto_sink; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index 0754ace3f92..81fbc8f5ad8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -41,11 +42,25 @@ namespace traffic class TrafficSink : public TrafficModuleBase { public: + /** + * @brief Construct a new Traffic Sink object + * @param lanelet_id Lanelet ID for visualization + * @todo lanelet_id value is only used for visualization and its very confusing. So it should be refactor. + * @param radius The entity despawn when the distance between the entity's coordinates in the Map coordinate system and the TrafficSink's coordinates is less than this value. + * @param position Position of the traffic sink. + * @param get_entity_names Function to get the name of entity + * @param get_entity_type Function to get the type of entity + * @param sinkable_entity_type If this type is applicable, the entity despawn only when it approaches radius [m] or less from the TrafficSink. If empty, all entity types are candidates for despawn. + * @param get_entity_pose Function to get the pose of entity. + * @param despawn Function to despawn entity. + */ explicit TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function); + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_type, + const std::set & sinkable_entity_type, + const std::function & get_entity_pose, + const std::function & despawn); const lanelet::Id lanelet_id; const double radius; const geometry_msgs::msg::Point position; @@ -54,9 +69,11 @@ class TrafficSink : public TrafficModuleBase -> void override; private: - const std::function(void)> get_entity_names_function; - const std::function get_entity_pose_function; - const std::function despawn_function; + const std::function(void)> get_entity_names; + const std::function get_entity_type; + const std::set sinkable_entity_type; + const std::function get_entity_pose; + const std::function despawn; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..6420e77aec3 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -17,6 +17,7 @@ namespace traffic_simulator { + inline namespace entity_status { diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cd8ab6d5a37..1510323a4f8 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -38,13 +39,17 @@ namespace traffic { TrafficController::TrafficController( std::shared_ptr hdmap_utils, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function, bool auto_sink) + const std::function(void)> & get_entity_names, + const std::function & get_entity_type, + const std::set & sinkable_entity_type, + const std::function & get_entity_pose, + const std::function & despawn, bool auto_sink) : hdmap_utils_(hdmap_utils), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function), + get_entity_names(get_entity_names), + get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), + get_entity_pose(get_entity_pose), + despawn(despawn), auto_sink(auto_sink) { if (auto_sink) { @@ -61,8 +66,8 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( - lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, - despawn_function); + lanelet_id, 1, pose.position, get_entity_names, get_entity_type, sinkable_entity_type, + get_entity_pose, despawn); } } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index b85663f17d6..82096d5bb02 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -38,28 +39,38 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, - const std::function(void)> & get_entity_names_function, - const std::function & get_entity_pose_function, - const std::function & despawn_function) + const lanelet::Id lanelet_id, const double radius, const geometry_msgs::msg::Point & position, + const std::function(void)> & get_entity_names, + const std::function & get_entity_type, + const std::set & sinkable_entity_type, + const std::function & get_entity_pose, + const std::function & despawn) : TrafficModuleBase(), lanelet_id(lanelet_id), radius(radius), position(position), - get_entity_names_function(get_entity_names_function), - get_entity_pose_function(get_entity_pose_function), - despawn_function(despawn_function) + get_entity_names(get_entity_names), + get_entity_type(get_entity_type), + sinkable_entity_type(sinkable_entity_type), + get_entity_pose(get_entity_pose), + despawn(despawn) { } void TrafficSink::execute( [[maybe_unused]] const double current_time, [[maybe_unused]] const double step_time) { - const auto names = get_entity_names_function(); + const auto names = get_entity_names(); for (const auto & name : names) { - const auto pose = get_entity_pose_function(name); - if (math::geometry::getDistance(position, pose) <= radius) { - despawn_function(name); + const auto is_sinkable_entity = [this](const auto & entity_name) { + return sinkable_entity_type.empty() + ? true + : sinkable_entity_type.find(get_entity_type(entity_name).type) != + sinkable_entity_type.end(); + }; + const auto pose = get_entity_pose(name); + if (is_sinkable_entity(name) and math::geometry::getDistance(position, pose) <= radius) { + despawn(name); } } }