Merge branch 'RT1-8532-separate-target-lane-leading-based-on-obj-beha…

…vior' into RT1-8508-improve-delay-lane-change-logic

.github/CODEOWNERS

@@ -25,6 +25,7 @@ common/autoware_test_utils/** [email protected] [email protected] ta
 common/autoware_testing/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_time_utils/** [email protected] [email protected] [email protected]
 common/autoware_trajectory/** [email protected] [email protected] [email protected]
+common/autoware_traffic_light_utils/** [email protected] [email protected] [email protected]
 common/autoware_universe_utils/** [email protected] [email protected] [email protected]
 common/autoware_vehicle_info_utils/** [email protected] [email protected] [email protected] [email protected]
 common/bag_time_manager_rviz_plugin/** [email protected]
@@ -41,7 +42,6 @@ common/tier4_system_rviz_plugin/** [email protected]
 common/tier4_traffic_light_rviz_plugin/** [email protected]
 common/tier4_vehicle_rviz_plugin/** [email protected]
 common/traffic_light_recognition_marker_publisher/** [email protected] [email protected] [email protected]
-common/traffic_light_utils/** [email protected] [email protected] [email protected]
 control/autoware_autonomous_emergency_braking/** [email protected] [email protected] [email protected] [email protected] [email protected]
 control/autoware_collision_detector/** [email protected] [email protected] [email protected]
 control/autoware_control_validator/** [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -97,6 +97,7 @@ localization/yabloc/yabloc_image_processing/** [email protected] kento.yabuu
 localization/yabloc/yabloc_monitor/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_particle_filter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_pose_initializer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+map/autoware_lanelet2_map_visualizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_height_fitter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_projection_loader/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_tf_generator/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]

build_depends.repos

@@ -28,7 +28,7 @@ repositories:
   core/autoware_adapi_msgs:
     type: git
     url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
-    version: 1.3.0
+    version: beta/1.7.0
   core/autoware_internal_msgs:
     type: git
     url: https://github.com/autowarefoundation/autoware_internal_msgs.git

common/.pages

@@ -22,8 +22,8 @@ nav:
     - 'Signal Processing':
       - 'Introduction': common/autoware_signal_processing
       - 'Butterworth Filter': common/autoware_signal_processing/documentation/ButterworthFilter
+    - 'autoware_traffic_light_utils': common/autoware_traffic_light_utils
     - 'autoware_universe_utils': common/autoware_universe_utils
-    - 'traffic_light_utils': common/traffic_light_utils
   - 'RVIZ2 Plugins':
     - 'autoware_perception_rviz_plugin': common/autoware_perception_rviz_plugin
     - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin
@@ -40,7 +40,7 @@ nav:
     - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin
     - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin
     - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin
-    - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme
+    - 'autoware_traffic_light_recognition_marker_publisher': common/autoware_traffic_light_recognition_marker_publisher/Readme
   - 'Node':
     - 'Goal Distance Calculator': common/autoware_goal_distance_calculator/Readme
     - 'Path Distance Calculator': common/autoware_path_distance_calculator/Readme

common/autoware_test_utils/config/sample_topic_snapshot.yaml

@@ -24,3 +24,6 @@ fields:
   - name: dynamic_object
     type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects
     topic: /perception/object_recognition/objects
+#  - name: tracked_object
+#    type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects
+#    topic: /perception/object_recognition/tracking/objects

common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

@@ -20,6 +20,7 @@
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
@@ -47,6 +48,9 @@ using autoware_perception_msgs::msg::PredictedObjectKinematics;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
 using autoware_perception_msgs::msg::Shape;
+using autoware_perception_msgs::msg::TrackedObject;
+using autoware_perception_msgs::msg::TrackedObjectKinematics;
+using autoware_perception_msgs::msg::TrackedObjects;
 using autoware_perception_msgs::msg::TrafficLightElement;
 using autoware_perception_msgs::msg::TrafficLightGroup;
 using autoware_perception_msgs::msg::TrafficLightGroupArray;
@@ -131,6 +135,9 @@ std::vector<LaneletPrimitive> parse(const YAML::Node & node);
 template <>
 std::vector<LaneletSegment> parse(const YAML::Node & node);
 
+template <>
+LaneletRoute parse(const YAML::Node & node);
+
 template <>
 std::vector<PathPointWithLaneId> parse(const YAML::Node & node);
 
@@ -155,6 +162,15 @@ PredictedObject parse(const YAML::Node & node);
 template <>
 PredictedObjects parse(const YAML::Node & node);
 
+template <>
+TrackedObjectKinematics parse(const YAML::Node & node);
+
+template <>
+TrackedObject parse(const YAML::Node & node);
+
+template <>
+TrackedObjects parse(const YAML::Node & node);
+
 template <>
 TrafficLightGroupArray parse(const YAML::Node & node);
 

common/autoware_test_utils/launch/psim_intersection.launch.xml

@@ -2,7 +2,7 @@
 <launch>
   <!-- Essential parameters -->
   <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
-  <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
+  <arg name="sensor_model" default="awsim_sensor_kit" description="sensor model name"/>
   <arg name="rviz_config" default="$(find-pkg-share autoware_test_utils)/rviz/psim_test_map.rviz" description="rviz config"/>
   <arg name="use_sim_time" default="true"/>
 

common/autoware_test_utils/launch/psim_road_shoulder.launch.xml

@@ -2,7 +2,7 @@
 <launch>
   <!-- Essential parameters -->
   <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
-  <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
+  <arg name="sensor_model" default="awsim_sensor_kit" description="sensor model name"/>
   <arg name="rviz_config" default="$(find-pkg-share autoware_test_utils)/rviz/psim_test_map.rviz" description="rviz config"/>
   <arg name="use_sim_time" default="true"/>
 

common/autoware_test_utils/rviz/psim_test_map.rviz

@@ -24,6 +24,8 @@ Panels:
     Name: AutowareStatePanel
   - Class: rviz_plugins::SimulatedClockPanel
     Name: SimulatedClockPanel
+  - Class: rviz_plugins::TrafficLightPublishPanel
+    Name: TrafficLightPublishPanel
 Visualization Manager:
   Class: ""
   Displays:
@@ -4429,7 +4431,7 @@ Visualization Manager:
         Value: /rviz/routing/rough_goal
     - Acceleration: 0
       Class: rviz_plugins/PedestrianInitialPoseTool
-      Interactive: true
+      Interactive: false
       Max velocity: 33.29999923706055
       Min velocity: -33.29999923706055
       Pose Topic: /simulation/dummy_perception_publisher/object_info
@@ -4443,7 +4445,7 @@ Visualization Manager:
     - Acceleration: 0
       Class: rviz_plugins/CarInitialPoseTool
       H vehicle height: 2
-      Interactive: true
+      Interactive: false
       L vehicle length: 4
       Max velocity: 33.29999923706055
       Min velocity: -33.29999923706055
@@ -4459,7 +4461,7 @@ Visualization Manager:
     - Acceleration: 0
       Class: rviz_plugins/BusInitialPoseTool
       H vehicle height: 3.5
-      Interactive: true
+      Interactive: false
       L vehicle length: 10.5
       Max velocity: 33.29999923706055
       Min velocity: -33.29999923706055
@@ -4558,6 +4560,8 @@ Window Geometry:
     collapsed: false
   Tool Properties:
     collapsed: false
+  TrafficLightPublishPanel:
+    collapsed: false
   Views:
     collapsed: false
   Width: 1920

common/autoware_test_utils/src/mock_data_parser.cpp

@@ -204,6 +204,17 @@ std::vector<LaneletSegment> parse(const YAML::Node & node)
   return segments;
 }
 
+template <>
+LaneletRoute parse(const YAML::Node & node)
+{
+  LaneletRoute route;
+
+  route.start_pose = parse<Pose>(node["start_pose"]);
+  route.goal_pose = parse<Pose>(node["goal_pose"]);
+  route.segments = parse<std::vector<LaneletSegment>>(node["segments"]);
+  return route;
+}
+
 template <>
 std::vector<PathPointWithLaneId> parse<std::vector<PathPointWithLaneId>>(const YAML::Node & node)
 {
@@ -330,6 +341,44 @@ PredictedObjects parse(const YAML::Node & node)
   return msg;
 }
 
+template <>
+TrackedObjectKinematics parse(const YAML::Node & node)
+{
+  TrackedObjectKinematics msg;
+  msg.pose_with_covariance = parse<PoseWithCovariance>(node["pose_with_covariance"]);
+  msg.twist_with_covariance = parse<TwistWithCovariance>(node["twist_with_covariance"]);
+  msg.acceleration_with_covariance =
+    parse<AccelWithCovariance>(node["acceleration_with_covariance"]);
+  msg.orientation_availability = node["orientation_availability"].as<uint8_t>();
+  msg.is_stationary = node["is_stationary"].as<bool>();
+  return msg;
+}
+
+template <>
+TrackedObject parse(const YAML::Node & node)
+{
+  TrackedObject msg;
+  msg.object_id = parse<UUID>(node["object_id"]);
+  msg.existence_probability = node["existence_probability"].as<float>();
+  for (const auto & classification_node : node["classification"]) {
+    msg.classification.push_back(parse<ObjectClassification>(classification_node));
+  }
+  msg.kinematics = parse<TrackedObjectKinematics>(node["kinematics"]);
+  msg.shape = parse<Shape>(node["shape"]);
+  return msg;
+}
+
+template <>
+TrackedObjects parse(const YAML::Node & node)
+{
+  TrackedObjects msg;
+  msg.header = parse<Header>(node["header"]);
+  for (const auto & object_node : node["objects"]) {
+    msg.objects.push_back(parse<TrackedObject>(object_node));
+  }
+  return msg;
+}
+
 template <>
 TrafficLightElement parse(const YAML::Node & node)
 {

common/autoware_test_utils/src/topic_snapshot_saver.cpp

@@ -19,6 +19,7 @@
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
@@ -34,12 +35,13 @@
 #include <variant>
 
 using MessageType = std::variant<
-  nav_msgs::msg::Odometry,                               // 0
-  geometry_msgs::msg::AccelWithCovarianceStamped,        // 1
-  autoware_perception_msgs::msg::PredictedObjects,       // 2
-  autoware_adapi_v1_msgs::msg::OperationModeState,       // 3
-  autoware_planning_msgs::msg::LaneletRoute,             // 4
-  autoware_perception_msgs::msg::TrafficLightGroupArray  // 5
+  nav_msgs::msg::Odometry,                                // 0
+  geometry_msgs::msg::AccelWithCovarianceStamped,         // 1
+  autoware_perception_msgs::msg::PredictedObjects,        // 2
+  autoware_adapi_v1_msgs::msg::OperationModeState,        // 3
+  autoware_planning_msgs::msg::LaneletRoute,              // 4
+  autoware_perception_msgs::msg::TrafficLightGroupArray,  // 5
+  autoware_perception_msgs::msg::TrackedObjects           // 6
   >;
 
 std::optional<size_t> get_topic_index(const std::string & name)
@@ -62,6 +64,9 @@ std::optional<size_t> get_topic_index(const std::string & name)
   if (name == "TrafficLightGroupArray") {
     return 5;
   }
+  if (name == "TrackedObjects") {
+    return 6;
+  }
   return std::nullopt;
 }
 
@@ -185,6 +190,7 @@ class TopicSnapShotSaver
       REGISTER_CALLBACK(3);
       REGISTER_CALLBACK(4);
       REGISTER_CALLBACK(5);
+      REGISTER_CALLBACK(6);
     }
   }
 
@@ -231,6 +237,7 @@ class TopicSnapShotSaver
       REGISTER_WRITE_TYPE(3);
       REGISTER_WRITE_TYPE(4);
       REGISTER_WRITE_TYPE(5);
+      REGISTER_WRITE_TYPE(6);
     }
 
     const std::string desc = std::string(R"(#
@@ -241,7 +248,7 @@ class TopicSnapShotSaver
 # map_path_uri: package://<package-name>/<resource-path>
 # fields(this is array)
 #   - name: <field-name-for-your-yaml-of-this-topic, str>
-#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TBD}
+#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD}
 #     topic: <topic-name, str>
 #
 )");