Feature/enable specify entity type in autosink

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()

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<std::uint8_t> sinkable_entity_type = {});
   void start();
   void stop(Result result, const std::string & description = "");
   void expectThrow() { exception_expect_ = true; }
@@ -71,14 +72,17 @@ 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<std::uint8_t> sinkable_entity_type = {}) -> traffic_simulator::Configuration
   {
     auto configuration = traffic_simulator::Configuration(map_path);
     {
       configuration.lanelet2_map_file = lanelet2_map_file;
       // 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;

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)])

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<std::uint8_t> 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<double>("global_real_time_factor", 1.0),
     declare_parameter<double>("global_frame_rate", 20.0)),
   scenario_filename_(scenario_filename),

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();
   }

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()

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 <ament_index_cpp/get_package_share_directory.hpp>
+#include <cpp_mock_scenarios/catalogs.hpp>
+#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <traffic_simulator/api/api.hpp>
+#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
+#include <vector>
+
+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<cpp_mock_scenarios::AutoSinkVehicleScenario>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -25,6 +25,7 @@
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
+#include <set>
 #include <simulation_interface/conversions.hpp>
 #include <simulation_interface/zmq_multi_client.hpp>
 #include <std_msgs/msg/float64.hpp>
@@ -75,6 +76,14 @@ class API
       getROS2Parameter<std::string>("architecture_type", "awf/universe"))),
     traffic_controller_ptr_(std::make_shared<traffic::TrafficController>(
       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();

simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp

@@ -21,7 +21,9 @@
 #include <boost/range/iterator_range.hpp>
 #include <iomanip>
 #include <scenario_simulator_exception/exception.hpp>
+#include <set>
 #include <string>
+#include <traffic_simulator/data_type/entity_status.hpp>
 
 namespace traffic_simulator
 {
@@ -33,6 +35,8 @@ struct Configuration
 
   bool auto_sink = true;
 
+  std::set<std::uint8_t> sinkable_entity_type = {};
+
   bool verbose = false;
 
   bool standalone_mode = false;

simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp

@@ -27,6 +27,7 @@
 #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_CONTROLLER_HPP_
 
 #include <memory>
+#include <set>
 #include <string>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/traffic/traffic_module_base.hpp>
@@ -43,9 +44,11 @@ class TrafficController
 public:
   explicit TrafficController(
     std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils,
-    const std::function<std::vector<std::string>(void)> & get_entity_names_function,
-    const std::function<geometry_msgs::msg::Pose(const std::string &)> & get_entity_pose_function,
-    const std::function<void(std::string)> & despawn_function, bool auto_sink = false);
+    const std::function<std::vector<std::string>(void)> & get_entity_names,
+    const std::function<traffic_simulator::EntityType(const std::string &)> & get_entity_type,
+    const std::set<std::uint8_t> & sinkable_entity_type,
+    const std::function<geometry_msgs::msg::Pose(const std::string &)> & get_entity_pose,
+    const std::function<void(std::string)> & despawn, bool auto_sink = false);
 
   template <typename T, typename... Ts>
   void addModule(Ts &&... xs)
@@ -60,9 +63,11 @@ class TrafficController
   void autoSink();
   const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_;
   std::vector<std::shared_ptr<traffic_simulator::traffic::TrafficModuleBase>> modules_;
-  const std::function<std::vector<std::string>(void)> get_entity_names_function;
-  const std::function<geometry_msgs::msg::Pose(const std::string &)> get_entity_pose_function;
-  const std::function<void(const std::string &)> despawn_function;
+  const std::function<std::vector<std::string>(void)> get_entity_names;
+  const std::function<traffic_simulator::EntityType(const std::string &)> get_entity_type;
+  const std::set<std::uint8_t> sinkable_entity_type;
+  const std::function<geometry_msgs::msg::Pose(const std::string &)> get_entity_pose;
+  const std::function<void(const std::string &)> despawn;
 
 public:
   const bool auto_sink;