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

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/polyline_trajectory_follower.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,14 @@ 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::PolylineTrajectoryFollower(
+                 static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
+                 behavior_parameter, hdmap_utils, step_time)
+                 .makeUpdatedEntityStatus(
+                   *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation,
+                   getTargetSpeed());
+             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,14 @@ 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::PolylineTrajectoryFollower(
+                 static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
+                 behavior_parameter, hdmap_utils, step_time)
+                 .makeUpdatedEntityStatus(
+                   *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation,
+                   getTargetSpeed());
+             entity_status_updated.has_value()) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));

simulation/traffic_simulator/CMakeLists.txt

@@ -27,7 +27,7 @@ ament_auto_find_build_dependencies()
 
 ament_auto_add_library(traffic_simulator SHARED
   src/api/api.cpp
-  src/behavior/follow_trajectory.cpp
+  src/behavior/polyline_trajectory_follower.cpp
   src/behavior/follow_waypoint_controller.cpp
   src/behavior/longitudinal_speed_planning.cpp
   src/behavior/route_planner.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/polyline_trajectory_follower.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_waypoint_controller.hpp

@@ -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)...))

simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp

@@ -0,0 +1,80 @@
+// 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__POLYLINE_TRAJECTORY_FOLLOWER_HPP_
+#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_
+
+#include <optional>
+#include <traffic_simulator/behavior/follow_waypoint_controller.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
+{
+struct PolylineTrajectoryFollower
+{
+public:
+  explicit PolylineTrajectoryFollower(
+    const traffic_simulator_msgs::msg::EntityStatus & entity_status,
+    const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter,
+    const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr, const double step_time);
+
+  auto makeUpdatedEntityStatus(
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+    const double matching_distance,
+    const std::optional<double> target_speed /*= std::nullopt*/) const
+    -> std::optional<EntityStatus>;
+
+private:
+  const traffic_simulator_msgs::msg::EntityStatus entity_status;
+  const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter;
+  const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr;
+  const double step_time;
+
+  auto discardTheFrontWaypointAndRecurse(
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+    const double matching_distance, const std::optional<double> target_speed) const
+    -> std::optional<EntityStatus>;
+  auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const
+    noexcept(true) -> EntityStatus;
+  auto getValidatedEntityAcceleration() const noexcept(false) -> double;
+  auto getValidatedEntitySpeed() const noexcept(false) -> double;
+  auto getValidatedEntityMaxAcceleration(const double acceleration) const noexcept(false) -> double;
+  auto getValidatedEntityMinAcceleration(const double acceleration) const noexcept(false) -> double;
+  auto getValidatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point;
+  auto getValidatedEntityTargetPosition(
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const
+    noexcept(false) -> geometry_msgs::msg::Point;
+  auto getValidatedEntityDesiredAcceleration(
+    const traffic_simulator::follow_trajectory::FollowWaypointController &
+      follow_waypoint_controller,
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+    const double remaining_time, const double distance, const double acceleration,
+    const double speed) const noexcept(false) -> double;
+  auto getValidatedEntityDesiredVelocity(
+    const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
+    const geometry_msgs::msg::Point & target_position, const geometry_msgs::msg::Point & position,
+    const double desired_speed) const noexcept(false) -> geometry_msgs::msg::Vector3;
+  auto getValidatedEntityDesiredSpeed(
+    const double entity_speed, const double desired_acceleration) const noexcept(false) -> double;
+};
+}  // namespace follow_trajectory
+}  // namespace traffic_simulator
+
+#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -22,8 +22,8 @@
 #include <optional>
 #include <queue>
 #include <string>
-#include <traffic_simulator/behavior/follow_trajectory.hpp>
 #include <traffic_simulator/behavior/longitudinal_speed_planning.hpp>
+#include <traffic_simulator/behavior/polyline_trajectory_follower.hpp>
 #include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/data_type/lane_change.hpp>
 #include <traffic_simulator/data_type/speed_change.hpp>