RJD-1388/follow_trajectory #44

robomic · 2024-11-12T15:48:20Z

Abstract

This PR contains the refactor of follow_trajectory namespace, in particular makeUpdatedStatus function.

Details

As functionalities have been divided into several files, this namespace now has its own follow_trajectory directory.
ValidatedEntityStatus serves as a simple structure that holds repeatedly used variables. Its construction checks and validates numerous conditions that were previously handled in makeUpdatedStatus. An instance of ValidatedEntityStatus is passed as an argument to follow_trajectory::makeUpdatedEntityStatus.
follow_trajectory::makeUpdatedEntityStatus is now the new entry point of the algorithm. It replaces the recursion from the original makeUpdatedStatus with a while loop, iterating over the trajectory.
PolylineTrajectoryPositioner is responsible for a single step in the aforementioned while loop. In its constructor, it calculates several variables that were previously computed by lambdas in the makeUpdatedStatus function.
PolylineTrajectoryPositioner::makeUpdatedEntityStatus contains what remains of the original makeUpdatedStatus, now focusing solely on its core logic.

References

Regressions TODO

…urse

Signed-off-by: Mateusz Palczuk <[email protected]>

…e polyline_trajectory_follower file to follow_trajectory Signed-off-by: Mateusz Palczuk <[email protected]>

…e polyline_trajectory_follower file to follow_trajectory Change include paths and include guards Signed-off-by: Mateusz Palczuk <[email protected]>

Add "_" suffix to PolylineTrajectoryPositioner data fields Modify distanceAlongLanelet function to take 2 different bounding boxes as arguments Add comments on what data fields are implicitly required by functions used in PolylineTrajectoryPositioner constructor Signed-off-by: Mateusz Palczuk <[email protected]>

Signed-off-by: Mateusz Palczuk <[email protected]>

…at is wrong, but passes github CI test Signed-off-by: Mateusz Palczuk <[email protected]>

Rjd 1388/follow trajectory fix

…tory_update

Rjd 1388/follow trajectory update

…jectory Signed-off-by: Mateusz Palczuk <[email protected]>

Signed-off-by: Mateusz Palczuk <[email protected]>

dmoszynski · 2025-01-23T08:34:53Z

...traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp

+/// @note side effects on polyline_trajectory
+auto makeUpdatedEntityStatus(
+  const ValidatedEntityStatus & validated_entity_status,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr,


please always pass std::shared_ptr<hdmap_utils::HdMapUtils> as the last argument - in the near future it will be removed, it is easier to make such changes for the last argument

dmoszynski · 2025-01-23T08:44:24Z

...imulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp

@@ -30,11 +30,11 @@ namespace traffic_simulator
 {
 namespace follow_trajectory
 {
-struct ControllerError : public common::Error
+struct ControllerError : public common::SimulationError


dmoszynski · 2025-01-23T08:44:31Z

...imulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp

@@ -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.value_or(max_speed)}


dmoszynski · 2025-01-23T08:51:18Z

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

@@ -144,15 +144,15 @@ class EntityBase
  virtual void requestLaneChange(const lanelet::Id)
  {
    /**
-     * @note There are Entities such as MiscObjectEntity for which lane change is not possible, 
+     * @note There are Entities such as MiscObjectEntity for which lane change is not possible,


reverse changes in comments (even such as whitespace characters)

dmoszynski · 2025-01-23T08:52:03Z

simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp

@@ -112,6 +112,14 @@ auto distanceToStopLine(
  const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array,
  const lanelet::Id target_stop_line_id,
  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
+
+auto distanceAlongLanelet(
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr,


please always pass std::shared_ptr<hdmap_utils::HdMapUtils> as the last argument - in the near future it will be removed, it is easier to make such changes for the last argument

dmoszynski · 2025-01-23T08:57:18Z

simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp

@@ -176,7 +178,7 @@ auto FollowWaypointController::clampAcceleration(
  const double candidate_acceleration, const double acceleration, const double speed) const
  -> double
 {
-  auto [local_min_acceleration, local_max_acceleration] =
+  const auto && [local_min_acceleration, local_max_acceleration] =


const auto [local_min_acceleration, local_max_acceleration]

dmoszynski · 2025-01-23T08:58:59Z

simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp

@@ -273,7 +275,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState(

      // Count the distance and time of movement with constant speed, use this to prediction.
      if (const double const_speed_distance = remaining_distance - state.traveled_distance;
-          const_speed_distance >= std::numeric_limits<double>::max() * const_speed_value) {
+          const_speed_distance == std::numeric_limits<double>::infinity()) {


const_speed_distance >= std::numeric_limits<double>::max() * const_speed_value)

dmoszynski · 2025-01-23T08:59:32Z

simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp

@@ -334,7 +337,7 @@ auto FollowWaypointController::getAcceleration(
  const double remaining_time_source, const double remaining_distance, const double acceleration,
  const double speed) const -> double
 {
-  const auto [local_min_acceleration, local_max_acceleration] =
+  const auto && [local_min_acceleration, local_max_acceleration] =


const auto && [local_min_acceleration, local_max_acceleration] =

dmoszynski · 2025-01-23T09:00:40Z

simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp


    if (const auto predicted_state_opt = getPredictedStopStateWithoutConsideringTime(
          candidate_acceleration, remaining_distance, acceleration, speed);
        predicted_state_opt) {
-      if (const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance;
-          (distance_diff >= 0 || min_distance_diff < 0) &&
+      if (const double distance_diff = remaining_distance - predicted_state_opt->traveled_distance;


why auto -> double?

dmoszynski · 2025-01-23T09:10:59Z

simulation/traffic_simulator/src/utils/distance.cpp

@@ -311,5 +312,29 @@ auto distanceToStopLine(
    return spline.getCollisionPointIn2D(polygon);
  }
 }
+
+auto distanceAlongLanelet(
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr,


please always pass std::shared_ptr<hdmap_utils::HdMapUtils> as the last argument - in the near future it will be removed, it is easier to make such changes for the last argument

dmoszynski · 2025-01-23T09:16:47Z

simulation/traffic_simulator/src/utils/distance.cpp

+  if (const auto from_lanelet_pose =
+        hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, false, matching_distance);
+      from_lanelet_pose.has_value()) {
+    if (const auto to_lanelet_pose =
+          hdmap_utils_ptr->toLaneletPose(to_position, to_bounding_box, false, matching_distance);
+        to_lanelet_pose.has_value()) {
+      if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance(
+            from_lanelet_pose.value(), to_lanelet_pose.value());
+          distance.has_value()) {
+        return distance.value();
+      }
+    }
+  }
+  return math::geometry::hypot(from_position, to_position);


Suggested change

if (const auto from_lanelet_pose =

hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, false, matching_distance);

from_lanelet_pose.has_value()) {

if (const auto to_lanelet_pose =

hdmap_utils_ptr->toLaneletPose(to_position, to_bounding_box, false, matching_distance);

to_lanelet_pose.has_value()) {

if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance(

from_lanelet_pose.value(), to_lanelet_pose.value());

distance.has_value()) {

return distance.value();

}

}

}

return math::geometry::hypot(from_position, to_position);

/// @note due to this hardcoded value, the method cannot be used for calculations along a crosswalk (for pedestrians)

constexpr auto include_crosswalk{false};

if (const auto from_lanelet_pose =

hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, include_crosswalk, matching_distance)) {

if (const auto to_lanelet_pose =

hdmap_utils_ptr->toLaneletPose(to_position, to_bounding_box, include_crosswalk, matching_distance)) {

if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance(

from_lanelet_pose.value(), to_lanelet_pose.value());

distance.has_value()) {

return distance.value();

}

}

}

return math::geometry::hypot(from_position, to_position);

in case of trouble I would not return the euclidean distance but std::optional,
only inside FollowTrajectoryAction I would implement if unable to calculate along the lane use euclidean - because it is a specific approach in FTA

robomic added 10 commits November 11, 2024 12:24

remove horrors of &-captures

02b9faf

extract distance_along_lanelet and discard_the_front_waypoint_and_rec…

6d626ac

…urse

remove a scary-looking lambda

642d726

further lambda extraction

e9baaea

const PolylineTrajectory, lambda extraction

b573bf0

flatten if-statements

ca13318

introduce a struct for follow_trajectory

ffedcb8

buildUpdatedEntityStatus

a442c8e

validators

d3213a7

PolylineTrajectoryFollower cleanup

9d895cb

robomic self-assigned this Nov 12, 2024

robomic and others added 19 commits November 12, 2024 17:38

add missing validator

eba2168

remove dead code

6bbba1e

use const iterator

3958568

inversion isfinite_vec3

ac535c3

typed and constexpr comparisons

a5ed84d

rename

834fb3a

typo

86471e7

Merge branch 'master' into RJD-1388/follow_trajectory

572ea56

Merge branch 'master' into RJD-1388/follow_trajectory

3c82ceb

Merge branch 'master' into RJD-1388/follow_trajectory

c8dc98b

inverse if-statement, specify captures

cc2908c

Merge tag '5.3.2' into RJD-1388/follow_trajectory

6b2ba8b

reorder algorithm steps

01ddaa9

debug info

abba124

debug info

edf1f6e

mutable polyline_trajectory

a850510

Merge branch 'master' into RJD-1388/follow_trajectory

147b584

acceleration validator

7d12a12

clean up FollowWaypointController

0ec90b2

TauTheLepton and others added 18 commits December 18, 2024 19:28

Refactor ValidatedEntityStatus: add behavior parameter getter

b4b7cf2

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor ValidatedEntityStatus: add current velocity getter

74e3591

Signed-off-by: Mateusz Palczuk <[email protected]>

Formatting of all getters

e12c4b0

Signed-off-by: Mateusz Palczuk <[email protected]>

Make ValidatedEntityStatus data members private

96318b4

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: remove duplicated data: behavior_parameter

08c2505

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: remove pointless class PolylineTrajectoryFollower

b68d9bb

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: rename whole polyline_trajectory_follower directory + renam…

0b7b59a

…e polyline_trajectory_follower file to follow_trajectory Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: rename whole polyline_trajectory_follower directory + renam…

251d94a

…e polyline_trajectory_follower file to follow_trajectory Change include paths and include guards Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor PolylineTrajectoryPositioner: Style changes

1ac46c4

Signed-off-by: Mateusz Palczuk <[email protected]>

Restore previous BehaviorPluginBase getter setter macro formatting th…

b3a28b5

…at is wrong, but passes github CI test Signed-off-by: Mateusz Palczuk <[email protected]>

Merge pull request #1494 from tier4/RJD-1388/follow_trajectory_fix

d88cdd1

Rjd 1388/follow trajectory fix

Merge branch 'RJD-1388/follow_trajectory' into RJD-1388/follow_trajec…

3b2a7c4

…tory_update

Merge pull request #1495 from tier4/RJD-1388/follow_trajectory_update

a8e758b

Rjd 1388/follow trajectory update

Merge remote-tracking branch 'origin/master' into RJD-1388/follow_tra…

faeace9

…jectory Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: Use value_or

ffcc0c0

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: Move initialization to if

3c07074

Signed-off-by: Mateusz Palczuk <[email protected]>

Refactor: Remove implicit conversion

f91f80a

Signed-off-by: Mateusz Palczuk <[email protected]>

robomic requested a review from dmoszynski January 7, 2025 15:39

dmoszynski reviewed Jan 23, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

RJD-1388/follow_trajectory #44

RJD-1388/follow_trajectory #44

robomic commented Nov 12, 2024 •

edited

Loading

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025

dmoszynski Jan 23, 2025 •

edited

Loading

RJD-1388/follow_trajectory #44

Are you sure you want to change the base?

RJD-1388/follow_trajectory #44

Conversation

robomic commented Nov 12, 2024 • edited Loading

Abstract

Details

References

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dmoszynski Jan 23, 2025 • edited Loading

Choose a reason for hiding this comment

robomic commented Nov 12, 2024 •

edited

Loading

dmoszynski Jan 23, 2025 •

edited

Loading