Add new message type traffic_simulator_msgs::msg::PolylineTrajectory

Signed-off-by: yamacir-kit <[email protected]>
tier4 · Jul 26, 2023 · 83b4015 · 83b4015
1 parent df7dd5d
commit 83b4015
Show file tree

Hide file tree

Showing 14 changed files with 30 additions and 71 deletions.
diff --git a/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp b/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp
@@ -100,17 +100,19 @@ auto FollowTrajectoryAction::start() -> void
       }
     };
 
-    applyFollowTrajectoryAction(
-      actor,
-      std::make_shared<
-        traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>(
-        initial_distance_offset,
-        trajectory_following_mode.following_mode == FollowingMode::position,
-        time_reference.as<Timing>().domain_absolute_relative == ReferenceContext::absolute
-          ? 0.0
-          : evaluateSimulationTime(),
-        trajectory_ref.trajectory.as<Trajectory>().closed,  //
-        repack_trajectory()));
+    auto parameter = std::make_shared<traffic_simulator_msgs::msg::PolylineTrajectory>();
+
+    parameter->initial_distance_offset = initial_distance_offset;
+    parameter->dynamic_constraints_ignorable =
+      trajectory_following_mode.following_mode == FollowingMode::position;
+    parameter->base_time =
+      time_reference.as<Timing>().domain_absolute_relative == ReferenceContext::absolute
+        ? std::numeric_limits<double>::quiet_NaN()
+        : evaluateSimulationTime();
+    parameter->closed = trajectory_ref.trajectory.as<Trajectory>().closed;
+    parameter->shape = repack_trajectory();
+
+    applyFollowTrajectoryAction(actor, parameter);
   }
 }
 }  // namespace syntax

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
@@ -54,7 +54,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(DebugMarker, std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(EntityStatus, traffic_simulator_msgs::msg::EntityStatus)
   DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict)
-  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>)
+  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
   DEFINE_GETTER_SETTER(GoalPoses, std::vector<geometry_msgs::msg::Pose>)
   DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter)

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
@@ -59,7 +59,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(DebugMarker, std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(EntityStatus, traffic_simulator_msgs::msg::EntityStatus)
   DEFINE_GETTER_SETTER(EntityTypeList, EntityTypeDict)
-  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>)
+  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
   DEFINE_GETTER_SETTER(GoalPoses, std::vector<geometry_msgs::msg::Pose>)
   DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter)

diff --git a/...vior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp b/...vior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp
@@ -23,9 +23,7 @@ namespace vehicle
 {
 struct FollowPolylineTrajectoryAction : public VehicleActionNode
 {
-  std::shared_ptr<
-    traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>
-    trajectory_parameter;
+  std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> trajectory_parameter;
 
   std::optional<double> target_speed;
 

diff --git a/..._tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/..._tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -64,8 +64,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
       not trajectory_parameter) {
     return BT::NodeStatus::FAILURE;
   } else if (
-    const auto updated_status =
-      makeUpdatedStatus(entity_status, trajectory_parameter, behavior_parameter, step_time)) {
+    const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
+      entity_status, trajectory_parameter, behavior_parameter, step_time)) {
     setOutput("updated_status", *updated_status);
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));

diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp
@@ -47,7 +47,7 @@ public:                                         \
   // clang-format off
   DEFINE_GETTER_SETTER(DebugMarker,                       std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(EntityTypeList,                    EntityTypeDict)
-  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>)
+  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
   DEFINE_GETTER_SETTER(GoalPoses,                         std::vector<geometry_msgs::msg::Pose>)
   DEFINE_GETTER_SETTER(HdMapUtils,                        std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters,              traffic_simulator::lane_change::Parameter)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
@@ -60,7 +60,7 @@ class BehaviorPluginBase
   DEFINE_GETTER_SETTER(BehaviorParameter, "behavior_parameter", traffic_simulator_msgs::msg::BehaviorParameter)
   DEFINE_GETTER_SETTER(EntityStatus, "entity_status", traffic_simulator_msgs::msg::EntityStatus)
   DEFINE_GETTER_SETTER(EntityTypeList, "entity_type_list", EntityTypeDict)
-  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, "polyline_trajectory_parameter", std::shared_ptr<traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>>)
+  DEFINE_GETTER_SETTER(FollowPolylineTrajectoryParameter, "polyline_trajectory_parameter", std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>)
   DEFINE_GETTER_SETTER(GoalPoses, "goal_poses", std::vector<geometry_msgs::msg::Pose>)
   DEFINE_GETTER_SETTER(HdMapUtils, "hdmap_utils", std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(Obstacle, "obstacle", std::optional<traffic_simulator_msgs::msg::Obstacle>)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/follow_trajectory.hpp
@@ -19,52 +19,15 @@
 #include <optional>
 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
-#include <traffic_simulator_msgs/msg/polyline.hpp>
+#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
 
 namespace traffic_simulator
 {
 namespace follow_trajectory
 {
-template <typename Shape>
-struct Parameter  // OpenSCENARIO 1.2 FollowTrajectoryAction
-{
-  double initial_distance_offset;
-
-  /*
-     true if trajectoryFollowingMode == FollowingMode.follow
-  */
-  bool dynamic_constraints_ignorable;
-
-  /*
-     This variable is set to NaN if the time information defined for the
-     waypoint is absolute simulation time. Otherwise, if the time information
-     defined for the waypoint is relative time, this variable will be filled
-     with the offset from zero simulation time.
-  */
-  double base_time;
-
-  bool closed;
-
-  Shape shape;
-
-  Parameter() = default;
-
-  explicit Parameter(
-    const double initial_distance_offset, const bool dynamic_constraints_ignorable,
-    const double base_time, const bool closed, const Shape & shape)
-  : initial_distance_offset(initial_distance_offset),
-    dynamic_constraints_ignorable(dynamic_constraints_ignorable),
-    base_time(base_time),
-    closed(closed),
-    shape(shape)
-  {
-  }
-};
-
 auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus &,
-  std::shared_ptr<
-    traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &,
+  std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &,
   const traffic_simulator_msgs::msg::BehaviorParameter &, double step_time)
   -> std::optional<traffic_simulator_msgs::msg::EntityStatus>;
 }  // namespace follow_trajectory

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -166,8 +166,7 @@ class EntityBase
   virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool);
 
   virtual auto requestFollowTrajectory(
-    const std::shared_ptr<follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &)
-    -> void;
+    const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void;
 
   virtual void requestWalkStraight();
 

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
@@ -98,8 +98,7 @@ class VehicleEntity : public EntityBase
   void requestAssignRoute(const std::vector<traffic_simulator_msgs::msg::LaneletPose> &) override;
 
   auto requestFollowTrajectory(
-    const std::shared_ptr<follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &)
-    -> void override;
+    const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;
 
   void requestLaneChange(const std::int64_t to_lanelet_id) override;
 

diff --git a/simulation/traffic_simulator/src/data_type/follow_trajectory.cpp b/simulation/traffic_simulator/src/data_type/follow_trajectory.cpp
@@ -43,9 +43,7 @@ auto any(F f, T && x, Ts &&... xs)
 
 auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
-  std::shared_ptr<
-    traffic_simulator::follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &
-    trajectory_parameter,
+  std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & trajectory_parameter,
   const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, double step_time)
   -> std::optional<traffic_simulator_msgs::msg::EntityStatus>
 {
@@ -170,7 +168,8 @@ auto makeUpdatedStatus(
           };
 
           if (const auto remaining_time =
-                (not std::isnan(trajectory_parameter->base_time) ? trajectory_parameter->base_time : 0.0) +
+                (not std::isnan(trajectory_parameter->base_time) ? trajectory_parameter->base_time
+                                                                 : 0.0) +
                 first_waypoint_with_arrival_time_specified->time - entity_status.time;
               /*
                  The condition below should ideally be remaining_time < 0.

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -666,8 +666,7 @@ void EntityBase::requestSpeedChange(
 }
 
 auto EntityBase::requestFollowTrajectory(
-  const std::shared_ptr<follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &)
-  -> void
+  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void
 {
   THROW_SEMANTIC_ERROR(
     getEntityTypename(), " type entities do not support follow trajectory action.");

diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -226,8 +226,7 @@ void VehicleEntity::requestAssignRoute(const std::vector<geometry_msgs::msg::Pos
 }
 
 auto VehicleEntity::requestFollowTrajectory(
-  const std::shared_ptr<follow_trajectory::Parameter<traffic_simulator_msgs::msg::Polyline>> &
-    parameter) -> void
+  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & parameter) -> void
 {
   behavior_plugin_ptr_->setFollowPolylineTrajectoryParameter(parameter);
   behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_POLYLINE_TRAJECTORY);

diff --git a/simulation/traffic_simulator_msgs/CMakeLists.txt b/simulation/traffic_simulator_msgs/CMakeLists.txt
@@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   msg/PedestrianParameters.msg
   msg/Performance.msg
   msg/Polyline.msg
+  msg/PolylineTrajectory.msg
   msg/VehicleParameters.msg
   msg/Vertex.msg
   msg/WaypointsArray.msg