RJD-1388/follow_trajectory

common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp

@@ -23,35 +23,27 @@ namespace math
 namespace arithmetic
 {
 template <typename T>
-auto isApproximatelyEqualTo(T a, T b)
+constexpr auto isApproximatelyEqualTo(T a, T b) -> bool
 {
   return std::abs(a - b) <=
          (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }
 
 template <typename T>
-auto isEssentiallyEqualTo(T a, T b)
+constexpr auto isEssentiallyEqualTo(T a, T b) -> bool
 {
   return std::abs(a - b) <=
          (std::numeric_limits<T>::epsilon() * std::min(std::abs(a), std::abs(b)));
 }
 
-template <typename T, typename... Ts>
-auto isDefinitelyLessThan(T a, T b, Ts... xs)
+template <typename T>
+constexpr auto isDefinitelyLessThan(T a, T b) -> bool
 {
-  auto compare = [](T a, T b) {
-    return (b - a) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
-  };
-
-  if constexpr (0 < sizeof...(Ts)) {
-    return compare(a, b) and compare(b, xs...);
-  } else {
-    return compare(a, b);
-  }
+  return (b - a) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }
 
 template <typename T>
-auto isDefinitelyGreaterThan(T a, T b)
+constexpr auto isDefinitelyGreaterThan(T a, T b) -> bool
 {
   return (a - b) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }

common/math/geometry/include/geometry/vector3/is_finite.hpp

@@ -0,0 +1,32 @@
+// 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.
+
+#ifndef GEOMETRY__VECTOR3__IS_FINITE_HPP_
+#define GEOMETRY__VECTOR3__IS_FINITE_HPP_
+
+#include <geometry/vector3/is_like_vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeVector3<T>::value, std::nullptr_t> = nullptr>
+auto isFinite(const T & v) -> bool
+{
+  return std::isfinite(v.x) and std::isfinite(v.y) and std::isfinite(v.z);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__IS_FINITE_HPP_

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp

@@ -22,7 +22,7 @@
 #include <openscenario_interpreter/syntax/trajectory_following_mode.hpp>
 #include <openscenario_interpreter/syntax/trajectory_ref.hpp>
 #include <pugixml.hpp>
-#include <traffic_simulator/behavior/follow_trajectory.hpp>
+#include <traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp>
 
 namespace openscenario_interpreter
 {

simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -56,7 +56,7 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
 
 auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
 {
-  auto getTargetSpeed = [&]() -> double {
+  const auto getTargetSpeed = [this]() -> double {
     if (target_speed.has_value()) {
       return target_speed.value();
     } else {
@@ -74,11 +74,15 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     THROW_SIMULATION_ERROR(
       "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such "
       "case.");
-  } else if (
-    const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
-      static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
-      *polyline_trajectory, behavior_parameter, hdmap_utils, step_time,
-      default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) {
+  } else if (const auto entity_status_updated =
+               traffic_simulator::follow_trajectory::makeUpdatedEntityStatus(
+                 traffic_simulator::follow_trajectory::ValidatedEntityStatus(
+                   static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
+                   behavior_parameter, step_time),
+                 hdmap_utils, *polyline_trajectory,
+                 default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(),
+                 step_time);
+             entity_status_updated.has_value()) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));

simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -56,7 +56,7 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
 
 auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
 {
-  auto getTargetSpeed = [&]() -> double {
+  const auto getTargetSpeed = [this]() -> double {
     if (target_speed.has_value()) {
       return target_speed.value();
     } else {
@@ -74,11 +74,15 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     THROW_SIMULATION_ERROR(
       "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such "
       "case.");
-  } else if (
-    const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
-      static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
-      *polyline_trajectory, behavior_parameter, hdmap_utils, step_time,
-      default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) {
+  } else if (const auto entity_status_updated =
+               traffic_simulator::follow_trajectory::makeUpdatedEntityStatus(
+                 traffic_simulator::follow_trajectory::ValidatedEntityStatus(
+                   static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
+                   behavior_parameter, step_time),
+                 hdmap_utils, *polyline_trajectory,
+                 default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(),
+                 step_time);
+             entity_status_updated.has_value()) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));

simulation/traffic_simulator/CMakeLists.txt

@@ -27,8 +27,10 @@ ament_auto_find_build_dependencies()
 
 ament_auto_add_library(traffic_simulator SHARED
   src/api/api.cpp
-  src/behavior/follow_trajectory.cpp
-  src/behavior/follow_waypoint_controller.cpp
+  src/behavior/follow_trajectory/follow_waypoint_controller.cpp
+  src/behavior/follow_trajectory/follow_trajectory.cpp
+  src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp
+  src/behavior/follow_trajectory/validated_entity_status.cpp
   src/behavior/longitudinal_speed_planning.cpp
   src/behavior/route_planner.cpp
   src/color_utils/color_utils.cpp

simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp

@@ -17,7 +17,7 @@
 
 #include <optional>
 #include <string>
-#include <traffic_simulator/behavior/follow_trajectory.hpp>
+#include <traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp>
 #include <traffic_simulator/data_type/behavior.hpp>
 #include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp → simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp

@@ -12,27 +12,31 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_
-#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_
+#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_
+#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_
 
 #include <optional>
+#include <traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp>
+#include <traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp>
+#include <traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp>
 #include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
-#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
 #include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
 
 namespace traffic_simulator
 {
 namespace follow_trajectory
 {
-auto makeUpdatedStatus(
-  const traffic_simulator_msgs::msg::EntityStatus &,
-  traffic_simulator_msgs::msg::PolylineTrajectory &,
-  const traffic_simulator_msgs::msg::BehaviorParameter &,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double, double,
-  std::optional<double> target_speed = std::nullopt) -> std::optional<EntityStatus>;
+/// @note side effects on polyline_trajectory
+auto makeUpdatedEntityStatus(
+  const ValidatedEntityStatus & validated_entity_status,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr,
+  traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+  const double matching_distance, const std::optional<double> target_speed, const double step_time)
+  -> std::optional<EntityStatus>;
+
 }  // namespace follow_trajectory
 }  // namespace traffic_simulator
 
-#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_
+#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp → simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc. All rights reserved.
+// 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.
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_
-#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_
+#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_
+#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_
 
 #include <algorithm>
 #include <cmath>
@@ -30,11 +30,11 @@ namespace traffic_simulator
 {
 namespace follow_trajectory
 {
-struct ControllerError : public common::Error
+struct ControllerError : public common::SimulationError
 {
   template <typename... Ts>
   explicit ControllerError(Ts &&... xs)
-  : common::Error(common::concatenate(
+  : common::SimulationError(common::concatenate(
       "An error occurred in the internal controller operation in the FollowTrajectoryAction. ",
       "Please report the following information to the developer: ",
       std::forward<decltype(xs)>(xs)...))
@@ -59,7 +59,7 @@ struct PredictedState
 
   auto isImmobile(const double tolerance) const
   {
-    return std::abs(speed) < tolerance && std::abs(acceleration) < tolerance;
+    return std::abs(speed) < tolerance and std::abs(acceleration) < tolerance;
   }
 
   template <typename StreamType>
@@ -129,7 +129,7 @@ class FollowWaypointController
      There is no technical basis for this value, it was determined based on
      Dawid Moszynski experiments.
   */
-  static constexpr std::size_t number_of_acceleration_candidates = 30;
+  static constexpr std::size_t number_of_acceleration_candidates = 30UL;
 
   /*
      This is a debugging method, it is not worth giving it much attention.
@@ -231,7 +231,7 @@ class FollowWaypointController
     max_acceleration_rate{behavior_parameter.dynamic_constraints.max_acceleration_rate},
     max_deceleration{behavior_parameter.dynamic_constraints.max_deceleration},
     max_deceleration_rate{behavior_parameter.dynamic_constraints.max_deceleration_rate},
-    target_speed{(target_speed) ? target_speed.value() : max_speed}
+    target_speed{target_speed.has_value() ? target_speed.value() : max_speed}
   {
   }
 
@@ -242,14 +242,13 @@ class FollowWaypointController
     const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const
     -> std::string
   {
-    if (const auto & vertices = polyline_trajectory.shape.vertices; !vertices.empty()) {
+    if (const auto & vertices = polyline_trajectory.shape.vertices; not vertices.empty()) {
       std::stringstream waypoint_details;
       waypoint_details << "Currently followed waypoint: ";
-      if (const auto first_waypoint_with_arrival_time_specified = std::find_if(
-            vertices.begin(), vertices.end(),
-            [](auto && vertex) { return not std::isnan(vertex.time); });
-          first_waypoint_with_arrival_time_specified !=
-          std::end(polyline_trajectory.shape.vertices)) {
+      const auto first_waypoint_with_arrival_time_specified = std::find_if(
+        vertices.cbegin(), vertices.cend(),
+        [](const auto & vertex) { return not std::isnan(vertex.time); });
+      if (first_waypoint_with_arrival_time_specified != vertices.cend()) {
         waypoint_details << "[" << first_waypoint_with_arrival_time_specified->position.position.x
                          << ", " << first_waypoint_with_arrival_time_specified->position.position.y
                          << "] with specified time equal to "
@@ -288,13 +287,13 @@ class FollowWaypointController
     const double speed) const -> double;
 
   auto areConditionsOfArrivalMet(
-    const double acceleration, const double speed, const double distance) const -> double
+    const double acceleration, const double speed, const double distance) const -> bool
   {
-    return (!with_breaking || std::abs(speed) < local_epsilon) &&
-           std::abs(acceleration) < local_epsilon && distance < finish_distance_tolerance;
+    return (not with_breaking or std::abs(speed) < local_epsilon) and
+           std::abs(acceleration) < local_epsilon and distance < finish_distance_tolerance;
   }
 };
 }  // namespace follow_trajectory
 }  // namespace traffic_simulator
 
-#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_
+#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp

@@ -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.
+
+#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_
+#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_
+
+#include <optional>
+#include <traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp>
+#include <traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp>
+#include <traffic_simulator/data_type/entity_status.hpp>
+#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
+#include <traffic_simulator_msgs/msg/entity_status.hpp>
+#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
+
+namespace traffic_simulator
+{
+namespace follow_trajectory
+{
+
+struct PolylineTrajectoryPositioner
+{
+public:
+  explicit PolylineTrajectoryPositioner(
+    const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr,
+    const ValidatedEntityStatus & validated_entity_status,
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+    const std::optional<double> target_speed, const double matching_distance,
+    const double step_time);
+
+  auto makeUpdatedEntityStatus() const -> std::optional<EntityStatus>;
+
+private:
+  const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
+  const ValidatedEntityStatus validated_entity_status_;
+  const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory_;
+  const double step_time_;
+  const double matching_distance_;
+
+  const std::vector<traffic_simulator_msgs::msg::Vertex>::const_iterator
+    nearest_waypoint_with_specified_time_it_;
+  const geometry_msgs::msg::Point nearest_waypoint_position_;
+  const double distance_to_nearest_waypoint_;
+  const double total_remaining_distance_;
+  const double time_to_nearest_waypoint_;
+  const double total_remaining_time_;
+
+  const FollowWaypointController follow_waypoint_controller_;
+
+  auto totalRemainingDistance() const -> double;
+  auto totalRemainingTime() const noexcept(false) -> double;
+  auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool;
+  auto nearestWaypointWithSpecifiedTimeIterator() const
+    -> std::vector<traffic_simulator_msgs::msg::Vertex>::const_iterator;
+  auto validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point;
+  auto validatedEntityDesiredAcceleration() const noexcept(false) -> double;
+  auto validatedEntityDesiredVelocity(const double desired_speed) const noexcept(false)
+    -> geometry_msgs::msg::Vector3;
+  auto validatedEntityDesiredSpeed(const double desired_acceleration) const noexcept(false)
+    -> double;
+  auto validatePredictedState(const double desired_acceleration) const noexcept(false) -> void;
+};
+}  // namespace follow_trajectory
+}  // namespace traffic_simulator
+
+#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_