From 02b9faff617aa30f8ca3262b9352ee325306d13e Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 30 Oct 2024 10:39:08 +0100 Subject: [PATCH 01/82] remove horrors of &-captures --- .../src/behavior/follow_trajectory.cpp | 30 ++++++++++++------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index da2ced4710c..02db5d4fb6e 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -66,8 +66,9 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; - auto distance_along_lanelet = - [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { + auto distance_along_lanelet = [&hdmap_utils, &entity_status, matching_distance]( + const geometry_msgs::msg::Point & from, + const geometry_msgs::msg::Point & to) -> double { if (const auto from_lanelet_pose = hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); from_lanelet_pose) { @@ -84,7 +85,9 @@ auto makeUpdatedStatus( return hypot(from, to); }; - auto discard_the_front_waypoint_and_recurse = [&]() { + auto discard_the_front_waypoint_and_recurse = [&polyline_trajectory, &entity_status, + &behavior_parameter, &hdmap_utils, step_time, + matching_distance, target_speed]() { /* The OpenSCENARIO standard does not define the behavior when the value of Timing.domainAbsoluteRelative is "relative". The standard only states @@ -123,13 +126,14 @@ auto makeUpdatedStatus( auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; - auto first_waypoint_with_arrival_time_specified = [&]() { + auto first_waypoint_with_arrival_time_specified = [&polyline_trajectory]() { return std::find_if( polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), [](auto && vertex) { return not std::isnan(vertex.time); }); }; - auto is_breaking_waypoint = [&]() { + auto is_breaking_waypoint = [&first_waypoint_with_arrival_time_specified, + &polyline_trajectory]() { return first_waypoint_with_arrival_time_specified() >= std::prev(polyline_trajectory.shape.vertices.end()); }; @@ -181,14 +185,15 @@ auto makeUpdatedStatus( return discard_the_front_waypoint_and_recurse(); } else if ( const auto [distance, remaining_time] = - [&]() { + [&polyline_trajectory, &distance_along_lanelet, &first_waypoint_with_arrival_time_specified, + &entity_status, &distance_to_front_waypoint, step_time]() { /* Note for anyone working on adding support for followingMode follow to this function (FollowPolylineTrajectoryAction::tick) in the future: if followingMode is follow, this distance calculation may be inappropriate. */ - auto total_distance_to = [&](auto last) { + auto total_distance_to = [&polyline_trajectory, &distance_along_lanelet](auto last) { auto total_distance = 0.0; for (auto iter = std::begin(polyline_trajectory.shape.vertices); 0 < std::distance(iter, last); ++iter) { @@ -322,7 +327,9 @@ auto makeUpdatedStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const auto desired_acceleration = [&]() -> double { + const auto desired_acceleration = [&follow_waypoint_controller, &entity_status, + &polyline_trajectory, remaining_time, distance, acceleration, + speed]() -> double { try { return follow_waypoint_controller.getAcceleration( remaining_time, distance, acceleration, speed); @@ -348,7 +355,8 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); } else if (const auto desired_velocity = - [&]() { + [&polyline_trajectory, &entity_status, &target_position, &position, + desired_speed]() { /* Note: The followingMode in OpenSCENARIO is passed as variable dynamic_constraints_ignorable. the value of the @@ -390,7 +398,7 @@ auto makeUpdatedStatus( "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); } else if (const auto current_velocity = - [&]() { + [&entity_status, speed]() { const auto pitch = -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; const auto yaw = @@ -554,7 +562,7 @@ auto makeUpdatedStatus( updated_status.pose.position += desired_velocity * step_time; - updated_status.pose.orientation = [&]() { + updated_status.pose.orientation = [&entity_status, &desired_velocity]() { if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) { // do not change orientation if there is no designed_velocity vector return entity_status.pose.orientation; From 6d626acaaeeecbcae19362dc7c772594b542bf50 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 30 Oct 2024 11:17:40 +0100 Subject: [PATCH 02/82] extract distance_along_lanelet and discard_the_front_waypoint_and_recurse --- .../src/behavior/follow_trajectory.cpp | 162 ++++++++++-------- 1 file changed, 92 insertions(+), 70 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 02db5d4fb6e..97f08785022 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -43,6 +43,71 @@ auto any(F f, T && x, Ts &&... xs) } } +auto distance_along_lanelet( + const std::shared_ptr & hdmap_utils, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, + const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double +{ + if (const auto from_lanelet_pose = + hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); + from_lanelet_pose) { + if (const auto to_lanelet_pose = + hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); + to_lanelet_pose) { + if (const auto distance = hdmap_utils->getLongitudinalDistance( + from_lanelet_pose.value(), to_lanelet_pose.value()); + distance) { + return distance.value(); + } + } + } + return math::geometry::hypot(from, to); +}; + +auto discard_the_front_waypoint_and_recurse( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const std::shared_ptr & hdmap_utils, const double step_time, + const double matching_distance, const std::optional target_speed) + -> std::optional +{ + /* + The OpenSCENARIO standard does not define the behavior when the value of + Timing.domainAbsoluteRelative is "relative". The standard only states + "Definition of time value context as either absolute or relative", and + it is completely unclear when the relative time starts. + + This implementation has interpreted the specification as follows: + Relative time starts from the start of FollowTrajectoryAction or from + the time of reaching the previous "waypoint with arrival time". + + Note: not std::isnan(polyline_trajectory.base_time) means + "Timing.domainAbsoluteRelative is relative". + + Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) + means "The waypoint about to be popped is the waypoint with the + specified arrival time". + */ + if ( + not std::isnan(polyline_trajectory.base_time) and + not std::isnan(polyline_trajectory.shape.vertices.front().time)) { + polyline_trajectory.base_time = entity_status.time; + } + + if (std::rotate( + std::begin(polyline_trajectory.shape.vertices), + std::begin(polyline_trajectory.shape.vertices) + 1, + std::end(polyline_trajectory.shape.vertices)); + not polyline_trajectory.closed) { + polyline_trajectory.shape.vertices.pop_back(); + } + + return makeUpdatedStatus( + entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); +}; + auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, @@ -66,64 +131,6 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; - auto distance_along_lanelet = [&hdmap_utils, &entity_status, matching_distance]( - const geometry_msgs::msg::Point & from, - const geometry_msgs::msg::Point & to) -> double { - if (const auto from_lanelet_pose = - hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); - from_lanelet_pose) { - if (const auto to_lanelet_pose = - hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); - to_lanelet_pose) { - if (const auto distance = hdmap_utils->getLongitudinalDistance( - from_lanelet_pose.value(), to_lanelet_pose.value()); - distance) { - return distance.value(); - } - } - } - return hypot(from, to); - }; - - auto discard_the_front_waypoint_and_recurse = [&polyline_trajectory, &entity_status, - &behavior_parameter, &hdmap_utils, step_time, - matching_distance, target_speed]() { - /* - The OpenSCENARIO standard does not define the behavior when the value of - Timing.domainAbsoluteRelative is "relative". The standard only states - "Definition of time value context as either absolute or relative", and - it is completely unclear when the relative time starts. - - This implementation has interpreted the specification as follows: - Relative time starts from the start of FollowTrajectoryAction or from - the time of reaching the previous "waypoint with arrival time". - - Note: not std::isnan(polyline_trajectory.base_time) means - "Timing.domainAbsoluteRelative is relative". - - Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) - means "The waypoint about to be popped is the waypoint with the - specified arrival time". - */ - if ( - not std::isnan(polyline_trajectory.base_time) and - not std::isnan(polyline_trajectory.shape.vertices.front().time)) { - polyline_trajectory.base_time = entity_status.time; - } - - if (std::rotate( - std::begin(polyline_trajectory.shape.vertices), - std::begin(polyline_trajectory.shape.vertices) + 1, - std::end(polyline_trajectory.shape.vertices)); - not polyline_trajectory.closed) { - polyline_trajectory.shape.vertices.pop_back(); - } - - return makeUpdatedStatus( - entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); - }; - auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; auto first_waypoint_with_arrival_time_specified = [&polyline_trajectory]() { @@ -173,7 +180,8 @@ auto makeUpdatedStatus( problems. */ const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple( - distance_along_lanelet(position, target_position), + distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, position, target_position), (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + polyline_trajectory.shape.vertices.front().time - entity_status.time); /* @@ -182,23 +190,27 @@ auto makeUpdatedStatus( miraculously becomes zero. */ isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } else if ( const auto [distance, remaining_time] = - [&polyline_trajectory, &distance_along_lanelet, &first_waypoint_with_arrival_time_specified, - &entity_status, &distance_to_front_waypoint, step_time]() { + [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory, + &first_waypoint_with_arrival_time_specified, &distance_to_front_waypoint, step_time]() { /* Note for anyone working on adding support for followingMode follow to this function (FollowPolylineTrajectoryAction::tick) in the future: if followingMode is follow, this distance calculation may be inappropriate. */ - auto total_distance_to = [&polyline_trajectory, &distance_along_lanelet](auto last) { + auto total_distance_to = [&hdmap_utils, &entity_status, &matching_distance, + &polyline_trajectory](auto last) { auto total_distance = 0.0; for (auto iter = std::begin(polyline_trajectory.shape.vertices); 0 < std::distance(iter, last); ++iter) { - total_distance += - distance_along_lanelet(iter->position.position, std::next(iter)->position.position); + total_distance += distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, iter->position.position, + std::next(iter)->position.position); } return total_distance; }; @@ -255,7 +267,9 @@ auto makeUpdatedStatus( } }(); isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } else if (const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] std::isinf(acceleration) or std::isnan(acceleration)) { throw common::Error( @@ -410,7 +424,9 @@ auto makeUpdatedStatus( }(); (speed * step_time) > distance_to_front_waypoint && innerProduct(desired_velocity, current_velocity) < 0.0) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } else if (auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, speed); !std::isinf(remaining_time) && !predicted_state_opt.has_value()) { @@ -518,7 +534,9 @@ auto makeUpdatedStatus( */ if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, speed, distance_to_front_waypoint)) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } } else { /* @@ -527,7 +545,9 @@ auto makeUpdatedStatus( */ if (auto this_step_distance = (speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } } /* @@ -541,7 +561,9 @@ auto makeUpdatedStatus( } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, speed, distance_to_front_waypoint)) { - return discard_the_front_waypoint_and_recurse(); + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } else { throw common::SimulationError( "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, From 642d726714308fcf8536ec15cf6f2e77acf5d7c2 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 30 Oct 2024 11:44:12 +0100 Subject: [PATCH 03/82] remove a scary-looking lambda --- .../src/behavior/follow_trajectory.cpp | 23 ++++++++----------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 97f08785022..070ca0d7cea 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -31,16 +31,13 @@ namespace traffic_simulator { namespace follow_trajectory { -template -auto any(F f, T && x, Ts &&... xs) + +template +auto validate_vec3_like(const T & vec) -> bool { - if constexpr (math::geometry::IsLikeVector3>::value) { - return any(f, x.x, x.y, x.z); - } else if constexpr (0 < sizeof...(xs)) { - return f(x) or any(f, std::forward(xs)...); - } else { - return f(x); - } + static_assert(math::geometry::IsLikeVector3>::value); + return std::isinf(vec.x) or std::isnan(vec.x) or std::isinf(vec.y) or std::isnan(vec.y) or + std::isinf(vec.z) or std::isnan(vec.z); } auto distance_along_lanelet( @@ -131,8 +128,6 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; - auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; - auto first_waypoint_with_arrival_time_specified = [&polyline_trajectory]() { return std::find_if( polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), @@ -154,7 +149,7 @@ auto makeUpdatedStatus( */ if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; - } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) { + } else if (const auto position = entity_status.pose.position; validate_vec3_like(position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -167,7 +162,7 @@ auto makeUpdatedStatus( referenced only this once in this member function. */ const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - any(is_infinity_or_nan, target_position)) { + validate_vec3_like(target_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -404,7 +399,7 @@ auto makeUpdatedStatus( "The followingMode is only supported for position."); } }(); - any(is_infinity_or_nan, desired_velocity)) { + validate_vec3_like(desired_velocity)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", From e9baaea27e0bb1f5ffc9c8ee72c858083af20d4a Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 30 Oct 2024 17:42:04 +0100 Subject: [PATCH 04/82] further lambda extraction --- .../src/behavior/follow_trajectory.cpp | 374 +++++++++--------- 1 file changed, 197 insertions(+), 177 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 070ca0d7cea..6e60d40ac45 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -31,6 +31,95 @@ namespace traffic_simulator { namespace follow_trajectory { +void print_debug_info( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double acceleration, const double min_acceleration, const double max_acceleration, + const double desired_acceleration, const double speed, const double desired_speed, + const double distance, const double distance_to_front_waypoint, const double remaining_time, + const double remaining_time_to_arrival_to_front_waypoint, + const double remaining_time_to_front_waypoint, const double step_time) +{ + using math::arithmetic::isDefinitelyLessThan; + // clang-format off + std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; + + std::cout << "acceleration " + << "== " << acceleration + << std::endl; + + std::cout << "min_acceleration " + << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " + << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== " << min_acceleration + << std::endl; + + std::cout << "max_acceleration " + << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " + << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== " << max_acceleration + << std::endl; + + std::cout << "min_acceleration < acceleration < max_acceleration " + << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; + + std::cout << "desired_acceleration " + << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " + << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << speed << " / " << remaining_time << " " + << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * speed / remaining_time) << " " + << "== " << desired_acceleration << " " + << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" + << std::endl; + + std::cout << "desired_speed " + << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " + << "== " << speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " + << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " + << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " + << "== " << desired_speed + << std::endl; + + std::cout << "distance_to_front_waypoint " + << "== " << distance_to_front_waypoint + << std::endl; + + std::cout << "remaining_time_to_arrival_to_front_waypoint " + << "== " << remaining_time_to_arrival_to_front_waypoint + << std::endl; + + std::cout << "distance " + << "== " << distance + << std::endl; + + std::cout << "remaining_time " + << "== " << remaining_time + << std::endl; + + std::cout << "remaining_time_to_arrival_to_front_waypoint " + << "(" + << "== distance_to_front_waypoint / desired_speed " + << "== " << distance_to_front_waypoint << " / " << desired_speed << " " + << "== " << remaining_time_to_arrival_to_front_waypoint + << ")" + << std::endl; + + std::cout << "arrive during this frame? " + << "== remaining_time_to_arrival_to_front_waypoint < step_time " + << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " + << "== " << isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) + << std::endl; + + std::cout << "not too early? " + << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " + << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " + << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " + << "== " << (std::isnan(remaining_time_to_front_waypoint) or isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) + << std::endl; + // clang-format on +} template auto validate_vec3_like(const T & vec) -> bool @@ -47,13 +136,13 @@ auto distance_along_lanelet( { if (const auto from_lanelet_pose = hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); - from_lanelet_pose) { + from_lanelet_pose.has_value()) { if (const auto to_lanelet_pose = hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); - to_lanelet_pose) { + to_lanelet_pose.has_value()) { if (const auto distance = hdmap_utils->getLongitudinalDistance( from_lanelet_pose.value(), to_lanelet_pose.value()); - distance) { + distance.has_value()) { return distance.value(); } } @@ -105,6 +194,82 @@ auto discard_the_front_waypoint_and_recurse( matching_distance, target_speed); }; +auto first_waypoint_with_arrival_time_specified( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> auto +{ + return std::find_if( + polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), + [](const auto & vertex) { return not std::isnan(vertex.time); }); +}; + +auto make_updated_pose_orientation( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const geometry_msgs::msg::Vector3 & desired_velocity) +{ + if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { + // do not change orientation if there is no designed_velocity vector + return entity_status.pose.orientation; + } else { + // if there is a designed_velocity vector, set the orientation in the direction of it + const geometry_msgs::msg::Vector3 direction = + geometry_msgs::build() + .x(0.0) + .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) + .z(std::atan2(desired_velocity.y, desired_velocity.x)); + return math::geometry::convertEulerAngleToQuaternion(direction); + } +} + +auto calculate_desired_velocity( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const geometry_msgs::msg::Point & target_position, const geometry_msgs::msg::Point & position, + const double desired_speed) -> geometry_msgs::msg::Vector3 +{ + /* + Note: The followingMode in OpenSCENARIO is passed as + variable dynamic_constraints_ignorable. the value of the + variable is `followingMode == position`. + */ + if (polyline_trajectory.dynamic_constraints_ignorable) { + const double dx = target_position.x - position.x; + const double dy = target_position.y - position.y; + // if entity is on lane use pitch from lanelet, otherwise use pitch on target + const double pitch = + entity_status.lanelet_pose_valid + ? -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y + : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); + const double yaw = std::atan2(dy, dx); // Use yaw on target + return geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * desired_speed) + .y(std::cos(pitch) * std::sin(yaw) * desired_speed) + .z(std::sin(pitch) * desired_speed); + } else { + /* + Note: The vector returned if + dynamic_constraints_ignorable == true ignores parameters + such as the maximum rudder angle of the vehicle entry. In + this clause, such parameters must be respected and the + rotation angle difference of the z-axis center of the + vector must be kept below a certain value. + */ + throw common::SimulationError("The followingMode is only supported for position."); + } +} + +auto calculate_current_velocity( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) +{ + const double pitch = + -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; + const double yaw = + math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).z; + return geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * speed) + .y(std::cos(pitch) * std::sin(yaw) * speed) + .z(std::sin(pitch) * speed); +} + auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, @@ -128,16 +293,9 @@ auto makeUpdatedStatus( using math::geometry::normalize; using math::geometry::truncate; - auto first_waypoint_with_arrival_time_specified = [&polyline_trajectory]() { - return std::find_if( - polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), - [](auto && vertex) { return not std::isnan(vertex.time); }); - }; - - auto is_breaking_waypoint = [&first_waypoint_with_arrival_time_specified, - &polyline_trajectory]() { - return first_waypoint_with_arrival_time_specified() >= - std::prev(polyline_trajectory.shape.vertices.end()); + const auto is_breaking_waypoint = [&polyline_trajectory]() { + return first_waypoint_with_arrival_time_specified(polyline_trajectory) >= + std::prev(polyline_trajectory.shape.vertices.cend()); }; /* @@ -191,7 +349,7 @@ auto makeUpdatedStatus( } else if ( const auto [distance, remaining_time] = [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory, - &first_waypoint_with_arrival_time_specified, &distance_to_front_waypoint, step_time]() { + &distance_to_front_waypoint, step_time]() { /* Note for anyone working on adding support for followingMode follow to this function (FollowPolylineTrajectoryAction::tick) in the @@ -211,12 +369,13 @@ auto makeUpdatedStatus( }; if ( - first_waypoint_with_arrival_time_specified() != - std::end(polyline_trajectory.shape.vertices)) { + first_waypoint_with_arrival_time_specified(polyline_trajectory) != + std::cend(polyline_trajectory.shape.vertices)) { if (const auto remaining_time = (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - first_waypoint_with_arrival_time_specified()->time - entity_status.time; + first_waypoint_with_arrival_time_specified(polyline_trajectory)->time - + entity_status.time; /* The condition below should ideally be remaining_time < 0. @@ -244,15 +403,15 @@ auto makeUpdatedStatus( "Vehicle ", std::quoted(entity_status.name), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", - first_waypoint_with_arrival_time_specified()->time, " (in ", + first_waypoint_with_arrival_time_specified(polyline_trajectory)->time, " (in ", (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), " simulation time). This may be due to unrealistic conditions of arrival time " "specification compared to vehicle parameters and dynamic constraints."); } else { return std::make_tuple( distance_to_front_waypoint + - total_distance_to(first_waypoint_with_arrival_time_specified()), - remaining_time != 0 ? remaining_time : std::numeric_limits::epsilon()); + total_distance_to(first_waypoint_with_arrival_time_specified(polyline_trajectory)), + remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); } } else { return std::make_tuple( @@ -363,42 +522,8 @@ auto makeUpdatedStatus( "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); - } else if (const auto desired_velocity = - [&polyline_trajectory, &entity_status, &target_position, &position, - desired_speed]() { - /* - Note: The followingMode in OpenSCENARIO is passed as - variable dynamic_constraints_ignorable. the value of the - variable is `followingMode == position`. - */ - if (polyline_trajectory.dynamic_constraints_ignorable) { - const auto dx = target_position.x - position.x; - const auto dy = target_position.y - position.y; - // if entity is on lane use pitch from lanelet, otherwise use pitch on target - const auto pitch = - entity_status.lanelet_pose_valid - ? -math::geometry::convertQuaternionToEulerAngle( - entity_status.pose.orientation) - .y - : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); - const auto yaw = std::atan2(dy, dx); // Use yaw on target - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * desired_speed) - .y(std::cos(pitch) * std::sin(yaw) * desired_speed) - .z(std::sin(pitch) * desired_speed); - } else { - /* - Note: The vector returned if - dynamic_constraints_ignorable == true ignores parameters - such as the maximum rudder angle of the vehicle entry. In - this clause, such parameters must be respected and the - rotation angle difference of the z-axis center of the - vector must be kept below a certain value. - */ - throw common::SimulationError( - "The followingMode is only supported for position."); - } - }(); + } else if (const auto desired_velocity = calculate_desired_velocity( + polyline_trajectory, entity_status, target_position, position, desired_speed); validate_vec3_like(desired_velocity)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -406,24 +531,15 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); - } else if (const auto current_velocity = - [&entity_status, speed]() { - const auto pitch = - -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; - const auto yaw = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).z; - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); - }(); + } else if (const auto current_velocity = calculate_current_velocity(entity_status, speed); (speed * step_time) > distance_to_front_waypoint && innerProduct(desired_velocity, current_velocity) < 0.0) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else if (auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, acceleration, speed); + } else if (const auto predicted_state_opt = + follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time, distance, acceleration, speed); !std::isinf(remaining_time) && !predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -434,87 +550,12 @@ auto makeUpdatedStatus( ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", speed, ". ", follow_waypoint_controller); } else { - auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; if constexpr (false) { - // clang-format off - std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; - - std::cout << "acceleration " - << "== " << acceleration - << std::endl; - - std::cout << "min_acceleration " - << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== " << min_acceleration - << std::endl; - - std::cout << "max_acceleration " - << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== " << max_acceleration - << std::endl; - - std::cout << "min_acceleration < acceleration < max_acceleration " - << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; - - std::cout << "desired_acceleration " - << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " - << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << speed << " / " << remaining_time << " " - << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * speed / remaining_time) << " " - << "== " << desired_acceleration << " " - << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" - << std::endl; - - std::cout << "desired_speed " - << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " - << "== " << speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " - << "== " << desired_speed - << std::endl; - - std::cout << "distance_to_front_waypoint " - << "== " << distance_to_front_waypoint - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "== " << remaining_time_to_arrival_to_front_waypoint - << std::endl; - - std::cout << "distance " - << "== " << distance - << std::endl; - - std::cout << "remaining_time " - << "== " << remaining_time - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "(" - << "== distance_to_front_waypoint / desired_speed " - << "== " << distance_to_front_waypoint << " / " << desired_speed << " " - << "== " << remaining_time_to_arrival_to_front_waypoint - << ")" - << std::endl; - - std::cout << "arrive during this frame? " - << "== remaining_time_to_arrival_to_front_waypoint < step_time " - << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " - << "== " << isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) - << std::endl; - - std::cout << "not too early? " - << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " - << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " - << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " - << "== " << (std::isnan(remaining_time_to_front_waypoint) or isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) - << std::endl; - // clang-format on + const auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; + print_debug_info( + behavior_parameter, acceleration, min_acceleration, max_acceleration, desired_acceleration, + speed, desired_speed, distance, distance_to_front_waypoint, remaining_time, + remaining_time_to_arrival_to_front_waypoint, remaining_time_to_front_waypoint, step_time); } if (std::isnan(remaining_time_to_front_waypoint)) { @@ -522,7 +563,7 @@ auto makeUpdatedStatus( If the nearest waypoint is arrived at in this step without a specific arrival time, it will be considered as achieved */ - if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1) { + if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1UL) { /* If the trajectory has only waypoints with unspecified time, the last one is followed using maximum speed including braking - in this case accuracy of arrival is checked @@ -538,7 +579,8 @@ auto makeUpdatedStatus( If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is irrelevant */ - if (auto this_step_distance = (speed + desired_acceleration * step_time) * step_time; + if (const double this_step_distance = + (speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, @@ -578,45 +620,23 @@ auto makeUpdatedStatus( auto updated_status = entity_status; updated_status.pose.position += desired_velocity * step_time; - - updated_status.pose.orientation = [&entity_status, &desired_velocity]() { - if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) { - // do not change orientation if there is no designed_velocity vector - return entity_status.pose.orientation; - } else { - // if there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); - } - }(); - + updated_status.pose.orientation = + make_updated_pose_orientation(entity_status, desired_velocity); updated_status.action_status.twist.linear.x = norm(desired_velocity); - - updated_status.action_status.twist.linear.y = 0; - - updated_status.action_status.twist.linear.z = 0; - + updated_status.action_status.twist.linear.y = 0.0; + updated_status.action_status.twist.linear.z = 0.0; updated_status.action_status.twist.angular = math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( entity_status.pose.orientation, updated_status.pose.orientation)) / step_time; - updated_status.action_status.accel.linear = (updated_status.action_status.twist.linear - entity_status.action_status.twist.linear) / step_time; - updated_status.action_status.accel.angular = (updated_status.action_status.twist.angular - entity_status.action_status.twist.angular) / step_time; - updated_status.time = entity_status.time + step_time; - updated_status.lanelet_pose_valid = false; - return updated_status; } } From b573bf061b95347818c623ff3ac72b6d8b05672d Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 6 Nov 2024 15:03:49 +0100 Subject: [PATCH 05/82] const PolylineTrajectory, lambda extraction --- .../behavior/follow_trajectory.hpp | 2 +- .../src/behavior/follow_trajectory.cpp | 227 ++++++++++-------- 2 files changed, 128 insertions(+), 101 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index da3293fc67e..478b35722a2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -28,7 +28,7 @@ namespace follow_trajectory { auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus &, - traffic_simulator_msgs::msg::PolylineTrajectory &, + const traffic_simulator_msgs::msg::PolylineTrajectory &, const traffic_simulator_msgs::msg::BehaviorParameter &, const std::shared_ptr &, double, double, std::optional target_speed = std::nullopt) -> std::optional; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 6e60d40ac45..78c561a003a 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -151,7 +151,7 @@ auto distance_along_lanelet( }; auto discard_the_front_waypoint_and_recurse( - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::shared_ptr & hdmap_utils, const double step_time, @@ -175,27 +175,28 @@ auto discard_the_front_waypoint_and_recurse( means "The waypoint about to be popped is the waypoint with the specified arrival time". */ + auto polyline_trajectory_copy = polyline_trajectory; if ( - not std::isnan(polyline_trajectory.base_time) and - not std::isnan(polyline_trajectory.shape.vertices.front().time)) { - polyline_trajectory.base_time = entity_status.time; + not std::isnan(polyline_trajectory_copy.base_time) and + not std::isnan(polyline_trajectory_copy.shape.vertices.front().time)) { + polyline_trajectory_copy.base_time = entity_status.time; } if (std::rotate( - std::begin(polyline_trajectory.shape.vertices), - std::begin(polyline_trajectory.shape.vertices) + 1, - std::end(polyline_trajectory.shape.vertices)); - not polyline_trajectory.closed) { - polyline_trajectory.shape.vertices.pop_back(); + std::begin(polyline_trajectory_copy.shape.vertices), + std::begin(polyline_trajectory_copy.shape.vertices) + 1, + std::end(polyline_trajectory_copy.shape.vertices)); + not polyline_trajectory_copy.closed) { + polyline_trajectory_copy.shape.vertices.pop_back(); } return makeUpdatedStatus( - entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + entity_status, polyline_trajectory_copy, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); }; auto first_waypoint_with_arrival_time_specified( - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> auto + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> auto { return std::find_if( polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), @@ -270,9 +271,115 @@ auto calculate_current_velocity( .z(std::sin(pitch) * speed); } +auto calculate_desired_acceleration( + const traffic_simulator::follow_trajectory::FollowWaypointController & follow_waypoint_controller, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double remaining_time, const double distance, const double acceleration, const double speed) + -> double +{ + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). + + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ + + try { + return follow_waypoint_controller.getAcceleration( + remaining_time, distance, acceleration, speed); + } catch (const ControllerError & e) { + throw common::Error( + "Vehicle ", std::quoted(entity_status.name), " - controller operation problem encountered. ", + follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); + } +} + +auto calculate_distance_and_remaining_time( + const std::shared_ptr & hdmap_utils, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, double matching_distance, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double distance_to_front_waypoint, const double step_time) -> std::tuple +{ + /* + Note for anyone working on adding support for followingMode follow + to this function (FollowPolylineTrajectoryAction::tick) in the + future: if followingMode is follow, this distance calculation may be + inappropriate. + */ + const auto total_distance_to = [&hdmap_utils, &entity_status, &matching_distance, + &polyline_trajectory](auto last) { + double total_distance = 0.0; + for (auto iter = std::begin(polyline_trajectory.shape.vertices); 0 < std::distance(iter, last); + ++iter) { + total_distance += distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, iter->position.position, + std::next(iter)->position.position); + } + return total_distance; + }; + + if ( + first_waypoint_with_arrival_time_specified(polyline_trajectory) != + std::cend(polyline_trajectory.shape.vertices)) { + if (const auto remaining_time = + (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + first_waypoint_with_arrival_time_specified(polyline_trajectory)->time - + entity_status.time; + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ + remaining_time < -step_time) { + throw common::Error( + "Vehicle ", std::quoted(entity_status.name), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + first_waypoint_with_arrival_time_specified(polyline_trajectory)->time, " (in ", + (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + } else { + return std::make_tuple( + distance_to_front_waypoint + + total_distance_to(first_waypoint_with_arrival_time_specified(polyline_trajectory)), + remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); + } + } else { + return std::make_tuple( + distance_to_front_waypoint + + total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), + std::numeric_limits::infinity()); + } +} + auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::shared_ptr & hdmap_utils, const double step_time, double matching_distance, std::optional target_speed) -> std::optional @@ -346,81 +453,10 @@ auto makeUpdatedStatus( return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else if ( - const auto [distance, remaining_time] = - [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory, - &distance_to_front_waypoint, step_time]() { - /* - Note for anyone working on adding support for followingMode follow - to this function (FollowPolylineTrajectoryAction::tick) in the - future: if followingMode is follow, this distance calculation may be - inappropriate. - */ - auto total_distance_to = [&hdmap_utils, &entity_status, &matching_distance, - &polyline_trajectory](auto last) { - auto total_distance = 0.0; - for (auto iter = std::begin(polyline_trajectory.shape.vertices); - 0 < std::distance(iter, last); ++iter) { - total_distance += distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, iter->position.position, - std::next(iter)->position.position); - } - return total_distance; - }; - - if ( - first_waypoint_with_arrival_time_specified(polyline_trajectory) != - std::cend(polyline_trajectory.shape.vertices)) { - if (const auto remaining_time = - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time - : 0.0) + - first_waypoint_with_arrival_time_specified(polyline_trajectory)->time - - entity_status.time; - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - remaining_time < -step_time) { - throw common::Error( - "Vehicle ", std::quoted(entity_status.name), - " failed to reach the trajectory waypoint at the specified time. The specified time " - "is ", - first_waypoint_with_arrival_time_specified(polyline_trajectory)->time, " (in ", - (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), - " simulation time). This may be due to unrealistic conditions of arrival time " - "specification compared to vehicle parameters and dynamic constraints."); - } else { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(first_waypoint_with_arrival_time_specified(polyline_trajectory)), - remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); - } - } else { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), - std::numeric_limits::infinity()); - } - }(); - isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + } else if (const auto [distance, remaining_time] = calculate_distance_and_remaining_time( + hdmap_utils, entity_status, matching_distance, polyline_trajectory, + distance_to_front_waypoint, step_time); + isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); @@ -495,19 +531,10 @@ auto makeUpdatedStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const auto desired_acceleration = [&follow_waypoint_controller, &entity_status, - &polyline_trajectory, remaining_time, distance, acceleration, - speed]() -> double { - try { - return follow_waypoint_controller.getAcceleration( - remaining_time, distance, acceleration, speed); - } catch (const ControllerError & e) { - throw common::Error( - "Vehicle ", std::quoted(entity_status.name), - " - controller operation problem encountered. ", - follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); - } - }(); + + const auto desired_acceleration = calculate_desired_acceleration( + follow_waypoint_controller, entity_status, polyline_trajectory, remaining_time, distance, + acceleration, speed); std::isinf(desired_acceleration) or std::isnan(desired_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " From ca1331857bf3685832a2a90cf6b6692b4bcd5117 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 7 Nov 2024 16:10:42 +0100 Subject: [PATCH 06/82] flatten if-statements --- .../behavior/follow_trajectory.hpp | 11 +- .../src/behavior/follow_trajectory.cpp | 493 +++++++++--------- 2 files changed, 264 insertions(+), 240 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index 478b35722a2..7299941c550 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -27,11 +27,12 @@ namespace traffic_simulator namespace follow_trajectory { auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus &, - const traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, - const std::shared_ptr &, double, double, - std::optional target_speed = std::nullopt) -> std::optional; + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const std::shared_ptr & hdmap_utils, const double step_time, + const double matching_distance, const std::optional target_speed = std::nullopt) + -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 78c561a003a..f9f5818cfe7 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -122,11 +122,10 @@ void print_debug_info( } template -auto validate_vec3_like(const T & vec) -> bool +auto is_infinite_vec3_like(const T & vec) -> bool { static_assert(math::geometry::IsLikeVector3>::value); - return std::isinf(vec.x) or std::isnan(vec.x) or std::isinf(vec.y) or std::isnan(vec.y) or - std::isinf(vec.z) or std::isnan(vec.z); + return not std::isfinite(vec.x) or not std::isfinite(vec.y) or not std::isfinite(vec.z); } auto distance_along_lanelet( @@ -196,16 +195,17 @@ auto discard_the_front_waypoint_and_recurse( }; auto first_waypoint_with_arrival_time_specified( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> auto + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) + -> std::vector::const_iterator { return std::find_if( - polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), [](const auto & vertex) { return not std::isnan(vertex.time); }); }; auto make_updated_pose_orientation( const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const geometry_msgs::msg::Vector3 & desired_velocity) + const geometry_msgs::msg::Vector3 & desired_velocity) -> geometry_msgs::msg::Quaternion { if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { // do not change orientation if there is no designed_velocity vector @@ -325,56 +325,55 @@ auto calculate_distance_and_remaining_time( return total_distance; }; - if ( - first_waypoint_with_arrival_time_specified(polyline_trajectory) != - std::cend(polyline_trajectory.shape.vertices)) { - if (const auto remaining_time = - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - first_waypoint_with_arrival_time_specified(polyline_trajectory)->time - - entity_status.time; - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - remaining_time < -step_time) { - throw common::Error( - "Vehicle ", std::quoted(entity_status.name), - " failed to reach the trajectory waypoint at the specified time. The specified time " - "is ", - first_waypoint_with_arrival_time_specified(polyline_trajectory)->time, " (in ", - (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), - " simulation time). This may be due to unrealistic conditions of arrival time " - "specification compared to vehicle parameters and dynamic constraints."); - } else { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(first_waypoint_with_arrival_time_specified(polyline_trajectory)), - remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); - } - } else { + const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); + if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { return std::make_tuple( distance_to_front_waypoint + total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), std::numeric_limits::infinity()); } + + const auto remaining_time = + (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + waypoint_ptr->time - entity_status.time; + + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ + if (remaining_time < -step_time) { + throw common::Error( + "Vehicle ", std::quoted(entity_status.name), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + waypoint_ptr->time, " (in ", + (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + + } else { + return std::make_tuple( + distance_to_front_waypoint + total_distance_to(waypoint_ptr), + remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); + } } auto makeUpdatedStatus( @@ -382,8 +381,16 @@ auto makeUpdatedStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::shared_ptr & hdmap_utils, const double step_time, - double matching_distance, std::optional target_speed) -> std::optional + const double matching_distance, const std::optional target_speed /*= std::nullopt*/) + -> std::optional { + /* + The following code implements the steering behavior known as "seek". See + "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + information. + + See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters + */ using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; @@ -405,267 +412,283 @@ auto makeUpdatedStatus( std::prev(polyline_trajectory.shape.vertices.cend()); }; - /* - The following code implements the steering behavior known as "seek". See - "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more - information. - - See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters - */ if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; - } else if (const auto position = entity_status.pose.position; validate_vec3_like(position)) { + } + + const auto entity_position = entity_status.pose.position; + if (is_infinite_vec3_like(entity_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - position.x, ", ", position.y, ", ", position.z, "]."); - } else if ( - /* - We've made sure that polyline_trajectory.shape.vertices is not empty, so - a reference to vertices.front() always succeeds. vertices.front() is - referenced only this once in this member function. - */ - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - validate_vec3_like(target_position)) { + entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); + } + + /* + We've made sure that polyline_trajectory.shape.vertices is not empty, so + a reference to vertices.front() always succeeds. vertices.front() is + referenced only this once in this member function. + */ + const auto target_position = polyline_trajectory.shape.vertices.front().position.position; + if (is_infinite_vec3_like(target_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); - } else if ( - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple( - distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, position, target_position), - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - polyline_trajectory.shape.vertices.front().time - entity_status.time); - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { + } + + const auto distance_to_front_waypoint = distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, entity_position, target_position); + const auto remaining_time_to_front_waypoint = + (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + polyline_trajectory.shape.vertices.front().time - entity_status.time; + + /* + If not dynamic_constraints_ignorable, the linear distance should cause + problems. + */ + + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else if (const auto [distance, remaining_time] = calculate_distance_and_remaining_time( - hdmap_utils, entity_status, matching_distance, polyline_trajectory, - distance_to_front_waypoint, step_time); - isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + } + + const auto [distance, remaining_time] = calculate_distance_and_remaining_time( + hdmap_utils, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, + step_time); + if (isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else if (const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] - std::isinf(acceleration) or std::isnan(acceleration)) { + } + + const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] + if (not std::isfinite(acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", acceleration, ". "); - } else if (const auto max_acceleration = std::min( - acceleration /* [m/s^2] */ + - behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); - std::isinf(max_acceleration) or std::isnan(max_acceleration)) { + } + + const auto max_acceleration = std::min( + acceleration /* [m/s^2] */ + + behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * + step_time /* [s] */, + +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); + if (not std::isfinite(max_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); - } else if (const auto min_acceleration = std::max( - acceleration /* [m/s^2] */ - - behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); - std::isinf(min_acceleration) or std::isnan(min_acceleration)) { + } + + const auto min_acceleration = std::max( + acceleration /* [m/s^2] */ - + behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * + step_time /* [s] */, + -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); + if (not std::isfinite(min_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); - } else if (const auto speed = entity_status.action_status.twist.linear.x; // [m/s] - std::isinf(speed) or std::isnan(speed)) { + } + + const auto entity_speed = entity_status.action_status.twist.linear.x; // [m/s] + if (not std::isfinite(entity_speed)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed, - ". "); - } else if ( - /* - The controller provides the ability to calculate acceleration using constraints from the - behavior_parameter. The value is_breaking_waypoint() determines whether the calculated - acceleration takes braking into account - it is true if the nearest waypoint with the - specified time is the last waypoint or the nearest waypoint without the specified time is the - last waypoint. - - If an arrival time was specified for any of the remaining waypoints, priority is given to - meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can - be met. - - However, the controller allows passing target_speed as a speed which is followed by the - controller. target_speed is passed only if no arrival time was specified for any of the - remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is - not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the - behaviour_parameter. - */ - const auto follow_waypoint_controller = FollowWaypointController( - behavior_parameter, step_time, is_breaking_waypoint(), - std::isinf(remaining_time) ? target_speed : std::nullopt); - false) { - } else if ( - /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). - - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - It is calculated in such a way as to reach a constant linear speed as quickly as possible, - ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - More precisely, the controller selects acceleration to minimize the distance to the waypoint - that will be reached in a time step defined as the expected arrival time. - In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - with linear speed equal to zero and acceleration equal to zero. - */ + std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", + entity_speed, ". "); + } + + /* + The controller provides the ability to calculate acceleration using constraints from the + behavior_parameter. The value is_breaking_waypoint() determines whether the calculated + acceleration takes braking into account - it is true if the nearest waypoint with the + specified time is the last waypoint or the nearest waypoint without the specified time is the + last waypoint. + + If an arrival time was specified for any of the remaining waypoints, priority is given to + meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + be met. + + However, the controller allows passing target_speed as a speed which is followed by the + controller. target_speed is passed only if no arrival time was specified for any of the + remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + behaviour_parameter. + */ + const auto follow_waypoint_controller = FollowWaypointController( + behavior_parameter, step_time, is_breaking_waypoint(), + std::isinf(remaining_time) ? target_speed : std::nullopt); + + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). - const auto desired_acceleration = calculate_desired_acceleration( - follow_waypoint_controller, entity_status, polyline_trajectory, remaining_time, distance, - acceleration, speed); - std::isinf(desired_acceleration) or std::isnan(desired_acceleration)) { + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ + const auto desired_acceleration = calculate_desired_acceleration( + follow_waypoint_controller, entity_status, polyline_trajectory, remaining_time, distance, + acceleration, entity_speed); + if (not std::isfinite(desired_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, ". "); - } else if (const auto desired_speed = speed + desired_acceleration * step_time; - std::isinf(desired_speed) or std::isnan(desired_speed)) { + } + + const auto desired_speed = entity_speed + desired_acceleration * step_time; + if (not std::isfinite(desired_speed)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); - } else if (const auto desired_velocity = calculate_desired_velocity( - polyline_trajectory, entity_status, target_position, position, desired_speed); - validate_vec3_like(desired_velocity)) { + } + + const auto desired_velocity = calculate_desired_velocity( + polyline_trajectory, entity_status, target_position, entity_position, desired_speed); + if (is_infinite_vec3_like(desired_velocity)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); - } else if (const auto current_velocity = calculate_current_velocity(entity_status, speed); - (speed * step_time) > distance_to_front_waypoint && - innerProduct(desired_velocity, current_velocity) < 0.0) { + } + + const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); + + if ( + entity_speed * step_time > distance_to_front_waypoint && + innerProduct(desired_velocity, current_velocity) < 0.0) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else if (const auto predicted_state_opt = - follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, acceleration, speed); - !std::isinf(remaining_time) && !predicted_state_opt.has_value()) { + } + + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time, distance, acceleration, entity_speed); + if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", std::quoted(entity_status.name), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, - ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", speed, ". ", + ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", entity_speed, ". ", follow_waypoint_controller); - } else { - if constexpr (false) { - const auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; - print_debug_info( - behavior_parameter, acceleration, min_acceleration, max_acceleration, desired_acceleration, - speed, desired_speed, distance, distance_to_front_waypoint, remaining_time, - remaining_time_to_arrival_to_front_waypoint, remaining_time_to_front_waypoint, step_time); - } + } + + if constexpr (false) { + const auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; + print_debug_info( + behavior_parameter, acceleration, min_acceleration, max_acceleration, desired_acceleration, + entity_speed, desired_speed, distance, distance_to_front_waypoint, remaining_time, + remaining_time_to_arrival_to_front_waypoint, remaining_time_to_front_waypoint, step_time); + } - if (std::isnan(remaining_time_to_front_waypoint)) { + if (std::isnan(remaining_time_to_front_waypoint)) { + /* + If the nearest waypoint is arrived at in this step without a specific arrival time, it will + be considered as achieved + */ + if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1UL) { /* - If the nearest waypoint is arrived at in this step without a specific arrival time, it will - be considered as achieved + If the trajectory has only waypoints with unspecified time, the last one is followed using + maximum speed including braking - in this case accuracy of arrival is checked */ - if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1UL) { - /* - If the trajectory has only waypoints with unspecified time, the last one is followed using - maximum speed including braking - in this case accuracy of arrival is checked - */ - if (follow_waypoint_controller.areConditionsOfArrivalMet( - acceleration, speed, distance_to_front_waypoint)) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); - } - } else { - /* - If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is - irrelevant - */ - if (const double this_step_distance = - (speed + desired_acceleration * step_time) * step_time; - this_step_distance > distance_to_front_waypoint) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); - } + if (follow_waypoint_controller.areConditionsOfArrivalMet( + acceleration, entity_speed, distance_to_front_waypoint)) { + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); } + } else { /* - If there is insufficient time left for the next calculation step. - The value of step_time/2 is compared, as the remaining time is affected by floating point - inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - step is possible (remaining_time_to_front_waypoint is almost zero). + If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is + irrelevant */ - } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { - if (follow_waypoint_controller.areConditionsOfArrivalMet( - acceleration, speed, distance_to_front_waypoint)) { + if (const double this_step_distance = + (entity_speed + desired_acceleration * step_time) * step_time; + this_step_distance > distance_to_front_waypoint) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, matching_distance, target_speed); - } else { - throw common::SimulationError( - "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, - "s (remaining time is ", remaining_time_to_front_waypoint, - "s), has completed a trajectory to the nearest waypoint with", - " specified time equal to ", polyline_trajectory.shape.vertices.front().time, - "s at a distance equal to ", distance, - " from that waypoint which is greater than the accepted accuracy."); } } - /* - Note: If obstacle avoidance is to be implemented, the steering behavior - known by the name "collision avoidance" should be synthesized here into - steering. + If there is insufficient time left for the next calculation step. + The value of step_time/2 is compared, as the remaining time is affected by floating point + inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + step is possible (remaining_time_to_front_waypoint is almost zero). */ - auto updated_status = entity_status; - - updated_status.pose.position += desired_velocity * step_time; - updated_status.pose.orientation = - make_updated_pose_orientation(entity_status, desired_velocity); - updated_status.action_status.twist.linear.x = norm(desired_velocity); - updated_status.action_status.twist.linear.y = 0.0; - updated_status.action_status.twist.linear.z = 0.0; - updated_status.action_status.twist.angular = - math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( - entity_status.pose.orientation, updated_status.pose.orientation)) / - step_time; - updated_status.action_status.accel.linear = - (updated_status.action_status.twist.linear - entity_status.action_status.twist.linear) / - step_time; - updated_status.action_status.accel.angular = - (updated_status.action_status.twist.angular - entity_status.action_status.twist.angular) / - step_time; - updated_status.time = entity_status.time + step_time; - updated_status.lanelet_pose_valid = false; - return updated_status; + } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { + if (follow_waypoint_controller.areConditionsOfArrivalMet( + acceleration, entity_speed, distance_to_front_waypoint)) { + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, + matching_distance, target_speed); + } else { + throw common::SimulationError( + "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, + "s (remaining time is ", remaining_time_to_front_waypoint, + "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", + polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance, + " from that waypoint which is greater than the accepted accuracy."); + } } + + /* + Note: If obstacle avoidance is to be implemented, the steering behavior + known by the name "collision avoidance" should be synthesized here into + steering. + */ + auto updated_status = entity_status; + + updated_status.pose.position += desired_velocity * step_time; + updated_status.pose.orientation = make_updated_pose_orientation(entity_status, desired_velocity); + updated_status.action_status.twist.linear.x = norm(desired_velocity); + updated_status.action_status.twist.linear.y = 0.0; + updated_status.action_status.twist.linear.z = 0.0; + updated_status.action_status.twist.angular = + math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( + entity_status.pose.orientation, updated_status.pose.orientation)) / + step_time; + updated_status.action_status.accel.linear = + (updated_status.action_status.twist.linear - entity_status.action_status.twist.linear) / + step_time; + updated_status.action_status.accel.angular = + (updated_status.action_status.twist.angular - entity_status.action_status.twist.angular) / + step_time; + updated_status.time = entity_status.time + step_time; + updated_status.lanelet_pose_valid = false; + return updated_status; } } // namespace follow_trajectory } // namespace traffic_simulator From ffedcb81a4fd55a1b4f21ae955ca155c6c0a0325 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 11 Nov 2024 18:03:39 +0100 Subject: [PATCH 07/82] introduce a struct for follow_trajectory --- .../syntax/follow_trajectory_action.hpp | 2 +- .../follow_polyline_trajectory_action.cpp | 13 +- .../follow_polyline_trajectory_action.cpp | 13 +- simulation/traffic_simulator/CMakeLists.txt | 2 +- .../behavior/behavior_plugin_base.hpp | 2 +- .../behavior/follow_trajectory.hpp | 39 --- .../behavior/polyline_trajectory_follower.hpp | 61 ++++ .../traffic_simulator/entity/entity_base.hpp | 2 +- ...y.cpp => polyline_trajectory_follower.cpp} | 286 ++++++------------ .../src/entity/ego_entity.cpp | 17 +- 10 files changed, 190 insertions(+), 247 deletions(-) delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp rename simulation/traffic_simulator/src/behavior/{follow_trajectory.cpp => polyline_trajectory_follower.cpp} (73%) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp index d821cb079d5..4576706aeb9 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include namespace openscenario_interpreter { diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 8e722f5afb0..b0cc25def33 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -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(*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(*canonicalized_entity_status), + behavior_parameter, hdmap_utils) + .makeUpdatedStatus( + *polyline_trajectory, step_time, + 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())); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index c2c097a0da2..8887bd88806 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -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(*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(*canonicalized_entity_status), + behavior_parameter, hdmap_utils) + .makeUpdatedStatus( + *polyline_trajectory, step_time, + 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())); diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 638ddd73fcf..5630d6f662e 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/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 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 index f4162f3e9b0..5487a49dc3d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp deleted file mode 100644 index 7299941c550..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// 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_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ - -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -namespace follow_trajectory -{ -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time, - const double matching_distance, const std::optional target_speed = std::nullopt) - -> std::optional; -} // namespace follow_trajectory -} // namespace traffic_simulator - -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp new file mode 100644 index 00000000000..35bbf683b21 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -0,0 +1,61 @@ +// 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 +#include +#include +#include +#include +#include + +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); + + auto makeUpdatedStatus( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double step_time, const double matching_distance, + const std::optional target_speed /*= std::nullopt*/) const + -> std::optional; + +private: + const traffic_simulator_msgs::msg::EntityStatus entity_status; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + const std::shared_ptr hdmap_utils; + + auto discard_the_front_waypoint_and_recurse( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double step_time, const double matching_distance, + const std::optional target_speed) const -> std::optional; + // PolylineTrajectoryFollower(const PolylineTrajectoryFollower & other); + // PolylineTrajectoryFollower & operator=(const PolylineTrajectoryFollower & other); + // PolylineTrajectoryFollower(PolylineTrajectoryFollower && other) noexcept; + // PolylineTrajectoryFollower & operator=(PolylineTrajectoryFollower && other) noexcept; + // ~PolylineTrajectoryFollower(); +}; +} // namespace follow_trajectory +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 5d362585dfb..7ea882e0306 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -22,8 +22,8 @@ #include #include #include -#include #include +#include #include #include #include diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp similarity index 73% rename from simulation/traffic_simulator/src/behavior/follow_trajectory.cpp rename to simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index f9f5818cfe7..8f8a3a911e3 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -19,106 +19,24 @@ #include #include #include -#include #include -#include -#include -#include -#include +#include +#include #include +#include +#include namespace traffic_simulator { namespace follow_trajectory { -void print_debug_info( + +PolylineTrajectoryFollower::PolylineTrajectoryFollower( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const double acceleration, const double min_acceleration, const double max_acceleration, - const double desired_acceleration, const double speed, const double desired_speed, - const double distance, const double distance_to_front_waypoint, const double remaining_time, - const double remaining_time_to_arrival_to_front_waypoint, - const double remaining_time_to_front_waypoint, const double step_time) + const std::shared_ptr & hdmap_utils) +: entity_status(entity_status), behavior_parameter(behavior_parameter), hdmap_utils(hdmap_utils) { - using math::arithmetic::isDefinitelyLessThan; - // clang-format off - std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; - - std::cout << "acceleration " - << "== " << acceleration - << std::endl; - - std::cout << "min_acceleration " - << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== " << min_acceleration - << std::endl; - - std::cout << "max_acceleration " - << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== " << max_acceleration - << std::endl; - - std::cout << "min_acceleration < acceleration < max_acceleration " - << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; - - std::cout << "desired_acceleration " - << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " - << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << speed << " / " << remaining_time << " " - << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * speed / remaining_time) << " " - << "== " << desired_acceleration << " " - << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" - << std::endl; - - std::cout << "desired_speed " - << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " - << "== " << speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " - << "== " << desired_speed - << std::endl; - - std::cout << "distance_to_front_waypoint " - << "== " << distance_to_front_waypoint - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "== " << remaining_time_to_arrival_to_front_waypoint - << std::endl; - - std::cout << "distance " - << "== " << distance - << std::endl; - - std::cout << "remaining_time " - << "== " << remaining_time - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "(" - << "== distance_to_front_waypoint / desired_speed " - << "== " << distance_to_front_waypoint << " / " << desired_speed << " " - << "== " << remaining_time_to_arrival_to_front_waypoint - << ")" - << std::endl; - - std::cout << "arrive during this frame? " - << "== remaining_time_to_arrival_to_front_waypoint < step_time " - << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " - << "== " << isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) - << std::endl; - - std::cout << "not too early? " - << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " - << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " - << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " - << "== " << (std::isnan(remaining_time_to_front_waypoint) or isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) - << std::endl; - // clang-format on } template @@ -149,51 +67,6 @@ auto distance_along_lanelet( return math::geometry::hypot(from, to); }; -auto discard_the_front_waypoint_and_recurse( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time, - const double matching_distance, const std::optional target_speed) - -> std::optional -{ - /* - The OpenSCENARIO standard does not define the behavior when the value of - Timing.domainAbsoluteRelative is "relative". The standard only states - "Definition of time value context as either absolute or relative", and - it is completely unclear when the relative time starts. - - This implementation has interpreted the specification as follows: - Relative time starts from the start of FollowTrajectoryAction or from - the time of reaching the previous "waypoint with arrival time". - - Note: not std::isnan(polyline_trajectory.base_time) means - "Timing.domainAbsoluteRelative is relative". - - Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) - means "The waypoint about to be popped is the waypoint with the - specified arrival time". - */ - auto polyline_trajectory_copy = polyline_trajectory; - if ( - not std::isnan(polyline_trajectory_copy.base_time) and - not std::isnan(polyline_trajectory_copy.shape.vertices.front().time)) { - polyline_trajectory_copy.base_time = entity_status.time; - } - - if (std::rotate( - std::begin(polyline_trajectory_copy.shape.vertices), - std::begin(polyline_trajectory_copy.shape.vertices) + 1, - std::end(polyline_trajectory_copy.shape.vertices)); - not polyline_trajectory_copy.closed) { - polyline_trajectory_copy.shape.vertices.pop_back(); - } - - return makeUpdatedStatus( - entity_status, polyline_trajectory_copy, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); -}; - auto first_waypoint_with_arrival_time_specified( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> std::vector::const_iterator @@ -376,13 +249,50 @@ auto calculate_distance_and_remaining_time( } } -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, +auto PolylineTrajectoryFollower::discard_the_front_waypoint_and_recurse( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time, - const double matching_distance, const std::optional target_speed /*= std::nullopt*/) - -> std::optional + const double step_time, const double matching_distance, + const std::optional target_speed) const -> std::optional +{ + /* + The OpenSCENARIO standard does not define the behavior when the value of + Timing.domainAbsoluteRelative is "relative". The standard only states + "Definition of time value context as either absolute or relative", and + it is completely unclear when the relative time starts. + + This implementation has interpreted the specification as follows: + Relative time starts from the start of FollowTrajectoryAction or from + the time of reaching the previous "waypoint with arrival time". + + Note: not std::isnan(polyline_trajectory.base_time) means + "Timing.domainAbsoluteRelative is relative". + + Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) + means "The waypoint about to be popped is the waypoint with the + specified arrival time". + */ + auto polyline_trajectory_copy = polyline_trajectory; + if ( + not std::isnan(polyline_trajectory_copy.base_time) and + not std::isnan(polyline_trajectory_copy.shape.vertices.front().time)) { + polyline_trajectory_copy.base_time = entity_status.time; + } + + if (std::rotate( + std::begin(polyline_trajectory_copy.shape.vertices), + std::begin(polyline_trajectory_copy.shape.vertices) + 1, + std::end(polyline_trajectory_copy.shape.vertices)); + not polyline_trajectory_copy.closed) { + polyline_trajectory_copy.shape.vertices.pop_back(); + } + + return makeUpdatedStatus(polyline_trajectory_copy, step_time, matching_distance, target_speed); +}; + +auto PolylineTrajectoryFollower::makeUpdatedStatus( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double step_time, const double matching_distance, + const std::optional target_speed /*= std::nullopt*/) const -> std::optional { /* The following code implements the steering behavior known as "seek". See @@ -391,7 +301,6 @@ auto makeUpdatedStatus( See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters */ - using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; using math::geometry::operator+; @@ -401,11 +310,8 @@ auto makeUpdatedStatus( using math::geometry::operator+=; using math::geometry::CatmullRomSpline; - using math::geometry::hypot; using math::geometry::innerProduct; using math::geometry::norm; - using math::geometry::normalize; - using math::geometry::truncate; const auto is_breaking_waypoint = [&polyline_trajectory]() { return first_waypoint_with_arrival_time_specified(polyline_trajectory) >= @@ -458,8 +364,7 @@ auto makeUpdatedStatus( */ if (isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } const auto [distance, remaining_time] = calculate_distance_and_remaining_time( @@ -467,8 +372,7 @@ auto makeUpdatedStatus( step_time); if (isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] @@ -586,8 +490,7 @@ auto makeUpdatedStatus( entity_speed * step_time > distance_to_front_waypoint && innerProduct(desired_velocity, current_velocity) < 0.0) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( @@ -603,14 +506,6 @@ auto makeUpdatedStatus( follow_waypoint_controller); } - if constexpr (false) { - const auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; - print_debug_info( - behavior_parameter, acceleration, min_acceleration, max_acceleration, desired_acceleration, - entity_speed, desired_speed, distance, distance_to_front_waypoint, remaining_time, - remaining_time_to_arrival_to_front_waypoint, remaining_time_to_front_waypoint, step_time); - } - if (std::isnan(remaining_time_to_front_waypoint)) { /* If the nearest waypoint is arrived at in this step without a specific arrival time, it will @@ -624,8 +519,7 @@ auto makeUpdatedStatus( if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } } else { /* @@ -636,8 +530,7 @@ auto makeUpdatedStatus( (entity_speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } } /* @@ -652,8 +545,7 @@ auto makeUpdatedStatus( if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { return discard_the_front_waypoint_and_recurse( - polyline_trajectory, entity_status, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + polyline_trajectory, step_time, matching_distance, target_speed); } else { throw common::SimulationError( "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, @@ -669,26 +561,48 @@ auto makeUpdatedStatus( known by the name "collision avoidance" should be synthesized here into steering. */ - auto updated_status = entity_status; - - updated_status.pose.position += desired_velocity * step_time; - updated_status.pose.orientation = make_updated_pose_orientation(entity_status, desired_velocity); - updated_status.action_status.twist.linear.x = norm(desired_velocity); - updated_status.action_status.twist.linear.y = 0.0; - updated_status.action_status.twist.linear.z = 0.0; - updated_status.action_status.twist.angular = - math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( - entity_status.pose.orientation, updated_status.pose.orientation)) / - step_time; - updated_status.action_status.accel.linear = - (updated_status.action_status.twist.linear - entity_status.action_status.twist.linear) / + const auto updated_pose_orientation = + make_updated_pose_orientation(entity_status, desired_velocity); + const auto updated_pose = geometry_msgs::build() + .position(entity_status.pose.position + desired_velocity * step_time) + .orientation(updated_pose_orientation); + + const auto updated_action_status_twist_linear = + geometry_msgs::build().x(norm(desired_velocity)).y(0.0).z(0.0); + const auto updated_action_status_twist_angular = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / step_time; - updated_status.action_status.accel.angular = - (updated_status.action_status.twist.angular - entity_status.action_status.twist.angular) / - step_time; - updated_status.time = entity_status.time + step_time; - updated_status.lanelet_pose_valid = false; - return updated_status; + const auto updated_action_status_twist = geometry_msgs::build() + .linear(updated_action_status_twist_linear) + .angular(updated_action_status_twist_angular); + const auto updated_action_status_accel = + geometry_msgs::build() + .linear( + (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) + .angular( + (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / + step_time); + const auto updated_action_status = + traffic_simulator_msgs::build() + .current_action(entity_status.action_status.current_action) + .twist(updated_action_status_twist) + .accel(updated_action_status_accel) + .linear_jerk(entity_status.action_status.linear_jerk); + const auto updated_time = entity_status.time + step_time; + const auto updated_lanelet_pose_valid = false; + + return traffic_simulator_msgs::build() + .type(entity_status.type) + .subtype(entity_status.subtype) + .time(updated_time) + .name(entity_status.name) + .bounding_box(entity_status.bounding_box) + .action_status(updated_action_status) + .pose(updated_pose) + .lanelet_pose(entity_status.lanelet_pose) + .lanelet_pose_valid(updated_lanelet_pose_valid); } + } // namespace follow_trajectory -} // namespace traffic_simulator +} // namespace traffic_simulator \ No newline at end of file diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index fcd36c957f3..4c151087991 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -146,15 +146,16 @@ void EgoEntity::updateFieldOperatorApplication() const void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); - if (is_controlled_by_simulator_) { - if ( - const auto non_canonicalized_updated_status = - traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*status_), *polyline_trajectory_, - behavior_parameter_, hdmap_utils_ptr_, step_time, - getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { + if (const auto non_canonicalized_updated_status = + traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( + static_cast(*status_), behavior_parameter_, + hdmap_utils_ptr_) + .makeUpdatedStatus( + *polyline_trajectory_, step_time, + getDefaultMatchingDistanceForLaneletPoseCalculation(), + target_speed_ ? target_speed_.value() : status_->getTwist().linear.x); + non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { From a442c8e7d9ea4bb3370181023101621343999866 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 11 Nov 2024 19:04:45 +0100 Subject: [PATCH 08/82] buildUpdatedEntityStatus --- .../follow_polyline_trajectory_action.cpp | 2 +- .../follow_polyline_trajectory_action.cpp | 2 +- .../behavior/polyline_trajectory_follower.hpp | 5 +- .../behavior/polyline_trajectory_follower.cpp | 156 ++++++++++-------- .../src/entity/ego_entity.cpp | 2 +- 5 files changed, 94 insertions(+), 73 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index b0cc25def33..ba79b71c46f 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -78,7 +78,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*canonicalized_entity_status), behavior_parameter, hdmap_utils) - .makeUpdatedStatus( + .makeUpdatedEntityStatus( *polyline_trajectory, step_time, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed()); entity_status_updated.has_value()) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 8887bd88806..6d237adfad2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -78,7 +78,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*canonicalized_entity_status), behavior_parameter, hdmap_utils) - .makeUpdatedStatus( + .makeUpdatedEntityStatus( *polyline_trajectory, step_time, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed()); entity_status_updated.has_value()) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 35bbf683b21..427438a3c08 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -34,7 +34,7 @@ struct PolylineTrajectoryFollower const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::shared_ptr & hdmap_utils); - auto makeUpdatedStatus( + auto makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double step_time, const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const @@ -49,6 +49,9 @@ struct PolylineTrajectoryFollower const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double step_time, const double matching_distance, const std::optional target_speed) const -> std::optional; + auto buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + -> EntityStatus; // PolylineTrajectoryFollower(const PolylineTrajectoryFollower & other); // PolylineTrajectoryFollower & operator=(const PolylineTrajectoryFollower & other); // PolylineTrajectoryFollower(PolylineTrajectoryFollower && other) noexcept; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 8f8a3a911e3..722c3b30725 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -134,10 +134,10 @@ auto calculate_desired_velocity( auto calculate_current_velocity( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) { - const double pitch = - -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; - const double yaw = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).z; + const auto euler_angles = + math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); + const double pitch = -euler_angles.y; + const double yaw = euler_angles.z; return geometry_msgs::build() .x(std::cos(pitch) * std::cos(yaw) * speed) .y(std::cos(pitch) * std::sin(yaw) * speed) @@ -278,18 +278,75 @@ auto PolylineTrajectoryFollower::discard_the_front_waypoint_and_recurse( polyline_trajectory_copy.base_time = entity_status.time; } - if (std::rotate( - std::begin(polyline_trajectory_copy.shape.vertices), - std::begin(polyline_trajectory_copy.shape.vertices) + 1, - std::end(polyline_trajectory_copy.shape.vertices)); - not polyline_trajectory_copy.closed) { + std::rotate( + std::begin(polyline_trajectory_copy.shape.vertices), + std::begin(polyline_trajectory_copy.shape.vertices) + 1, + std::end(polyline_trajectory_copy.shape.vertices)); + + if (not polyline_trajectory_copy.closed) { polyline_trajectory_copy.shape.vertices.pop_back(); } - return makeUpdatedStatus(polyline_trajectory_copy, step_time, matching_distance, target_speed); + return makeUpdatedEntityStatus( + polyline_trajectory_copy, step_time, matching_distance, target_speed); }; -auto PolylineTrajectoryFollower::makeUpdatedStatus( +auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + -> EntityStatus +{ + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator*; + using math::geometry::operator/; + + const auto updated_pose_orientation = + make_updated_pose_orientation(entity_status, desired_velocity); + const auto updated_pose = geometry_msgs::build() + .position(entity_status.pose.position + desired_velocity * step_time) + .orientation(updated_pose_orientation); + + const auto updated_action_status_twist_linear = + geometry_msgs::build() + .x(math::geometry::norm(desired_velocity)) + .y(0.0) + .z(0.0); + const auto updated_action_status_twist_angular = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / + step_time; + const auto updated_action_status_twist = geometry_msgs::build() + .linear(updated_action_status_twist_linear) + .angular(updated_action_status_twist_angular); + const auto updated_action_status_accel = + geometry_msgs::build() + .linear( + (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) + .angular( + (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / + step_time); + const auto updated_action_status = + traffic_simulator_msgs::build() + .current_action(entity_status.action_status.current_action) + .twist(updated_action_status_twist) + .accel(updated_action_status_accel) + .linear_jerk(entity_status.action_status.linear_jerk); + const auto updated_time = entity_status.time + step_time; + const auto updated_lanelet_pose_valid = false; + + return traffic_simulator_msgs::build() + .type(entity_status.type) + .subtype(entity_status.subtype) + .time(updated_time) + .name(entity_status.name) + .bounding_box(entity_status.bounding_box) + .action_status(updated_action_status) + .pose(updated_pose) + .lanelet_pose(entity_status.lanelet_pose) + .lanelet_pose_valid(updated_lanelet_pose_valid); +} + +auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double step_time, const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const -> std::optional @@ -301,7 +358,6 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters */ - using math::arithmetic::isDefinitelyLessThan; using math::geometry::operator+; using math::geometry::operator-; @@ -309,20 +365,13 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( using math::geometry::operator/; using math::geometry::operator+=; - using math::geometry::CatmullRomSpline; - using math::geometry::innerProduct; - using math::geometry::norm; - - const auto is_breaking_waypoint = [&polyline_trajectory]() { - return first_waypoint_with_arrival_time_specified(polyline_trajectory) >= - std::prev(polyline_trajectory.shape.vertices.cend()); - }; - if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; } const auto entity_position = entity_status.pose.position; + const auto target_position = polyline_trajectory.shape.vertices.front().position.position; + if (is_infinite_vec3_like(entity_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -336,7 +385,6 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( a reference to vertices.front() always succeeds. vertices.front() is referenced only this once in this member function. */ - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; if (is_infinite_vec3_like(target_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -349,7 +397,7 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( const auto distance_to_front_waypoint = distance_along_lanelet( hdmap_utils, entity_status, matching_distance, entity_position, target_position); const auto remaining_time_to_front_waypoint = - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + polyline_trajectory.shape.vertices.front().time - entity_status.time; /* @@ -362,7 +410,8 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( distance_to_front_waypoint as the denominator if the distance miraculously becomes zero. */ - if (isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { + if (math::arithmetic::isDefinitelyLessThan( + distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, step_time, matching_distance, target_speed); } @@ -370,7 +419,7 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( const auto [distance, remaining_time] = calculate_distance_and_remaining_time( hdmap_utils, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, step_time); - if (isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, step_time, matching_distance, target_speed); } @@ -436,8 +485,11 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the behaviour_parameter. */ + const bool is_breaking_waypoint = + first_waypoint_with_arrival_time_specified(polyline_trajectory) >= + std::prev(polyline_trajectory.shape.vertices.cend()); const auto follow_waypoint_controller = FollowWaypointController( - behavior_parameter, step_time, is_breaking_waypoint(), + behavior_parameter, step_time, is_breaking_waypoint, std::isinf(remaining_time) ? target_speed : std::nullopt); /* @@ -488,7 +540,7 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( if ( entity_speed * step_time > distance_to_front_waypoint && - innerProduct(desired_velocity, current_velocity) < 0.0) { + math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, step_time, matching_distance, target_speed); } @@ -520,6 +572,8 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( acceleration, entity_speed, distance_to_front_waypoint)) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, step_time, matching_distance, target_speed); + } else { + return buildUpdatedEntityStatus(desired_velocity, step_time); } } else { /* @@ -531,6 +585,8 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( this_step_distance > distance_to_front_waypoint) { return discard_the_front_waypoint_and_recurse( polyline_trajectory, step_time, matching_distance, target_speed); + } else { + return buildUpdatedEntityStatus(desired_velocity, step_time); } } /* @@ -541,7 +597,8 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next step is possible (remaining_time_to_front_waypoint is almost zero). */ - } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { + } else if (math::arithmetic::isDefinitelyLessThan( + remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { return discard_the_front_waypoint_and_recurse( @@ -554,6 +611,8 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance, " from that waypoint which is greater than the accepted accuracy."); } + } else { + return buildUpdatedEntityStatus(desired_velocity, step_time); } /* @@ -561,47 +620,6 @@ auto PolylineTrajectoryFollower::makeUpdatedStatus( known by the name "collision avoidance" should be synthesized here into steering. */ - const auto updated_pose_orientation = - make_updated_pose_orientation(entity_status, desired_velocity); - const auto updated_pose = geometry_msgs::build() - .position(entity_status.pose.position + desired_velocity * step_time) - .orientation(updated_pose_orientation); - - const auto updated_action_status_twist_linear = - geometry_msgs::build().x(norm(desired_velocity)).y(0.0).z(0.0); - const auto updated_action_status_twist_angular = - math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / - step_time; - const auto updated_action_status_twist = geometry_msgs::build() - .linear(updated_action_status_twist_linear) - .angular(updated_action_status_twist_angular); - const auto updated_action_status_accel = - geometry_msgs::build() - .linear( - (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) - .angular( - (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / - step_time); - const auto updated_action_status = - traffic_simulator_msgs::build() - .current_action(entity_status.action_status.current_action) - .twist(updated_action_status_twist) - .accel(updated_action_status_accel) - .linear_jerk(entity_status.action_status.linear_jerk); - const auto updated_time = entity_status.time + step_time; - const auto updated_lanelet_pose_valid = false; - - return traffic_simulator_msgs::build() - .type(entity_status.type) - .subtype(entity_status.subtype) - .time(updated_time) - .name(entity_status.name) - .bounding_box(entity_status.bounding_box) - .action_status(updated_action_status) - .pose(updated_pose) - .lanelet_pose(entity_status.lanelet_pose) - .lanelet_pose_valid(updated_lanelet_pose_valid); } } // namespace follow_trajectory diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 4c151087991..71d20fdfb90 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -151,7 +151,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*status_), behavior_parameter_, hdmap_utils_ptr_) - .makeUpdatedStatus( + .makeUpdatedEntityStatus( *polyline_trajectory_, step_time, getDefaultMatchingDistanceForLaneletPoseCalculation(), target_speed_ ? target_speed_.value() : status_->getTwist().linear.x); From d3213a773a08162b0315c16b05f2478c268d756e Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 12 Nov 2024 11:02:38 +0100 Subject: [PATCH 09/82] validators --- .../behavior/polyline_trajectory_follower.hpp | 17 +- .../behavior/polyline_trajectory_follower.cpp | 287 +++++++++++------- 2 files changed, 184 insertions(+), 120 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 427438a3c08..fc7e44737f7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -51,12 +51,17 @@ struct PolylineTrajectoryFollower const std::optional target_speed) const -> std::optional; auto buildUpdatedEntityStatus( const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const - -> EntityStatus; - // PolylineTrajectoryFollower(const PolylineTrajectoryFollower & other); - // PolylineTrajectoryFollower & operator=(const PolylineTrajectoryFollower & other); - // PolylineTrajectoryFollower(PolylineTrajectoryFollower && other) noexcept; - // PolylineTrajectoryFollower & operator=(PolylineTrajectoryFollower && other) noexcept; - // ~PolylineTrajectoryFollower(); + noexcept(true) -> EntityStatus; + auto getValidatedEntityAcceleration() const noexcept(false) -> double; + auto getValidatedEntitySpeed() const noexcept(false) -> double; + auto getValidatedEntityMaxAcceleration(const double acceleration, const double step_time) const + noexcept(false) -> double; + auto getValidatedEntityMinAcceleration(const double acceleration, const double step_time) const + noexcept(false) -> double; + auto getValidatedEntityPosition() const noexcept(false) -> const geometry_msgs::msg::Point; + auto getValidatedEntityTargetPosition( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const + noexcept(false) -> const geometry_msgs::msg::Point; }; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 722c3b30725..e6856866169 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -186,23 +186,33 @@ auto calculate_distance_and_remaining_time( future: if followingMode is follow, this distance calculation may be inappropriate. */ - const auto total_distance_to = [&hdmap_utils, &entity_status, &matching_distance, - &polyline_trajectory](auto last) { - double total_distance = 0.0; - for (auto iter = std::begin(polyline_trajectory.shape.vertices); 0 < std::distance(iter, last); - ++iter) { - total_distance += distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, iter->position.position, - std::next(iter)->position.position); - } - return total_distance; - }; + const auto total_distance_to = + [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory]( + const std::vector::const_iterator last) { + return std::accumulate( + std::begin(polyline_trajectory.shape.vertices), last, 0.0, + [&hdmap_utils, &entity_status, &matching_distance]( + const double total_distance, const auto & vertex) { + const auto next = std::next(&vertex); + return total_distance + distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, + vertex.position.position, next->position.position); + }); + double total_distance = 0.0; + for (auto iter = std::begin(polyline_trajectory.shape.vertices); + 0 < std::distance(iter, last); ++iter) { + total_distance += distance_along_lanelet( + hdmap_utils, entity_status, matching_distance, iter->position.position, + std::next(iter)->position.position); + } + return total_distance; + }; const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { return std::make_tuple( distance_to_front_waypoint + - total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), + total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), std::numeric_limits::infinity()); } @@ -245,7 +255,7 @@ auto calculate_distance_and_remaining_time( } else { return std::make_tuple( distance_to_front_waypoint + total_distance_to(waypoint_ptr), - remaining_time != 0.0 ? remaining_time : std::numeric_limits::epsilon()); + remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time); } } @@ -292,7 +302,7 @@ auto PolylineTrajectoryFollower::discard_the_front_waypoint_and_recurse( }; auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const noexcept(true) -> EntityStatus { using math::geometry::operator+; @@ -369,104 +379,24 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return std::nullopt; } - const auto entity_position = entity_status.pose.position; - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - - if (is_infinite_vec3_like(entity_position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); - } - - /* - We've made sure that polyline_trajectory.shape.vertices is not empty, so - a reference to vertices.front() always succeeds. vertices.front() is - referenced only this once in this member function. - */ - if (is_infinite_vec3_like(target_position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s target position coordinate value contains NaN or infinity. The value is [", - target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); - } + const double entity_speed = getValidatedEntitySpeed(); + const double acceleration = getValidatedEntityAcceleration(); + [[maybe_unused]] const double max_acceleration = + getValidatedEntityMaxAcceleration(acceleration, step_time); + [[maybe_unused]] const double min_acceleration = + getValidatedEntityMinAcceleration(acceleration, step_time); + const auto entity_position = getValidatedEntityPosition(); + const auto target_position = getValidatedEntityTargetPosition(polyline_trajectory); - const auto distance_to_front_waypoint = distance_along_lanelet( + const double distance_to_front_waypoint = distance_along_lanelet( hdmap_utils, entity_status, matching_distance, entity_position, target_position); - const auto remaining_time_to_front_waypoint = + const double remaining_time_to_front_waypoint = (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + polyline_trajectory.shape.vertices.front().time - entity_status.time; - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - if (math::arithmetic::isDefinitelyLessThan( - distance_to_front_waypoint, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); - } - const auto [distance, remaining_time] = calculate_distance_and_remaining_time( hdmap_utils, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, step_time); - if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); - } - - const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] - if (not std::isfinite(acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", - acceleration, ". "); - } - - const auto max_acceleration = std::min( - acceleration /* [m/s^2] */ + - behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); - if (not std::isfinite(max_acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); - } - - const auto min_acceleration = std::max( - acceleration /* [m/s^2] */ - - behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); - if (not std::isfinite(min_acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); - } - - const auto entity_speed = entity_status.action_status.twist.linear.x; // [m/s] - if (not std::isfinite(entity_speed)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", - entity_speed, ". "); - } /* The controller provides the ability to calculate acceleration using constraints from the @@ -504,9 +434,58 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const auto desired_acceleration = calculate_desired_acceleration( - follow_waypoint_controller, entity_status, polyline_trajectory, remaining_time, distance, - acceleration, entity_speed); + const double desired_acceleration = follow_waypoint_controller.getAcceleration( + remaining_time, distance, acceleration, entity_speed); + const double desired_speed = entity_speed + desired_acceleration * step_time; + const auto desired_velocity = calculate_desired_velocity( + polyline_trajectory, entity_status, target_position, entity_position, desired_speed); + const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time, distance, acceleration, entity_speed); + + if (is_infinite_vec3_like(entity_position)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", + entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); + } + + /* + We've made sure that polyline_trajectory.shape.vertices is not empty, so + a reference to vertices.front() always succeeds. vertices.front() is + referenced only this once in this member function. + */ + if (is_infinite_vec3_like(target_position)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s target position coordinate value contains NaN or infinity. The value is [", + target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); + } + + /* + If not dynamic_constraints_ignorable, the linear distance should cause + problems. + */ + + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (math::arithmetic::isDefinitelyLessThan( + distance_to_front_waypoint, std::numeric_limits::epsilon())) { + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, step_time, matching_distance, target_speed); + } + + if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + return discard_the_front_waypoint_and_recurse( + polyline_trajectory, step_time, matching_distance, target_speed); + } + if (not std::isfinite(desired_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -516,7 +495,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( ". "); } - const auto desired_speed = entity_speed + desired_acceleration * step_time; if (not std::isfinite(desired_speed)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -525,8 +503,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( desired_speed, ". "); } - const auto desired_velocity = calculate_desired_velocity( - polyline_trajectory, entity_status, target_position, entity_position, desired_speed); if (is_infinite_vec3_like(desired_velocity)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -536,8 +512,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( desired_velocity.y, ", ", desired_velocity.z, "]."); } - const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); - if ( entity_speed * step_time > distance_to_front_waypoint && math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { @@ -545,8 +519,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( polyline_trajectory, step_time, matching_distance, target_speed); } - const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, acceleration, entity_speed); if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -622,5 +594,92 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ } +auto PolylineTrajectoryFollower::getValidatedEntityAcceleration() const noexcept(false) -> double +{ + const double acceleration = entity_status.action_status.accel.linear.x; + if (not std::isfinite(acceleration)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", + acceleration, ". "); + } + return acceleration; +} + +auto PolylineTrajectoryFollower::getValidatedEntitySpeed() const noexcept(false) -> double +{ + const double entity_speed = entity_status.action_status.twist.linear.x; // [m/s] + + if (not std::isfinite(entity_speed)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", + entity_speed, ". "); + } + return entity_speed; +} +auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration( + const double acceleration, const double step_time) const noexcept(false) -> double +{ + const double max_acceleration = std::min( + acceleration /* [m/s^2] */ + + behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * + step_time /* [s] */, + +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); + + if (not std::isfinite(max_acceleration)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); + } + return max_acceleration; +} +auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration( + const double acceleration, const double step_time) const noexcept(false) -> double +{ + const double min_acceleration = std::max( + acceleration /* [m/s^2] */ - + behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * + step_time /* [s] */, + -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); + + if (not std::isfinite(min_acceleration)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); + } + return min_acceleration; +} +auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(false) + -> const geometry_msgs::msg::Point +{ + const auto entity_position = entity_status.pose.position; + if (is_infinite_vec3_like(entity_position)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", + entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); + } + return entity_position; +} +auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) + -> const geometry_msgs::msg::Point +{ + if (polyline_trajectory.shape.vertices.empty()) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "attempted to dereference an element of an empty PolylineTrajectory"); + } + return polyline_trajectory.shape.vertices.front().position.position; +} + } // namespace follow_trajectory } // namespace traffic_simulator \ No newline at end of file From 9d895cbc5d876fc05be6e028d48e6aba1b62d34e Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 12 Nov 2024 16:47:09 +0100 Subject: [PATCH 10/82] PolylineTrajectoryFollower cleanup --- .../arithmetic/floating_point/comparison.hpp | 14 +- .../follow_polyline_trajectory_action.cpp | 6 +- .../follow_polyline_trajectory_action.cpp | 6 +- .../behavior/follow_waypoint_controller.hpp | 4 +- .../behavior/polyline_trajectory_follower.hpp | 37 ++- .../behavior/follow_waypoint_controller.cpp | 2 +- .../behavior/polyline_trajectory_follower.cpp | 275 +++++++++--------- .../src/entity/ego_entity.cpp | 5 +- 8 files changed, 172 insertions(+), 177 deletions(-) diff --git a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp index b720e58a09c..34c1c2586d1 100644 --- a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp +++ b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp @@ -36,18 +36,10 @@ auto isEssentiallyEqualTo(T a, T b) (std::numeric_limits::epsilon() * std::min(std::abs(a), std::abs(b))); } -template -auto isDefinitelyLessThan(T a, T b, Ts... xs) +template +auto isDefinitelyLessThan(T a, T b) { - auto compare = [](T a, T b) { - return (b - a) > (std::numeric_limits::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::epsilon() * std::max(std::abs(a), std::abs(b))); } template diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index ba79b71c46f..1ef3304ef9d 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -77,10 +77,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if (const auto entity_status_updated = traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*canonicalized_entity_status), - behavior_parameter, hdmap_utils) + behavior_parameter, hdmap_utils, step_time) .makeUpdatedEntityStatus( - *polyline_trajectory, step_time, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed()); + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, + getTargetSpeed()); entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 6d237adfad2..93a3206a244 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -77,10 +77,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if (const auto entity_status_updated = traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*canonicalized_entity_status), - behavior_parameter, hdmap_utils) + behavior_parameter, hdmap_utils, step_time) .makeUpdatedEntityStatus( - *polyline_trajectory, step_time, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed()); + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, + getTargetSpeed()); entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp index 761ce8bf2a5..d94ad394fb2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/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 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(xs)...)) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index fc7e44737f7..eba4d37bcfe 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ #include +#include #include #include #include @@ -32,11 +33,11 @@ struct PolylineTrajectoryFollower explicit PolylineTrajectoryFollower( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils); + const std::shared_ptr & hdmap_utils, const double step_time); auto makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double step_time, const double matching_distance, + const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const -> std::optional; @@ -44,24 +45,34 @@ struct PolylineTrajectoryFollower const traffic_simulator_msgs::msg::EntityStatus entity_status; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const std::shared_ptr hdmap_utils; + const double step_time; - auto discard_the_front_waypoint_and_recurse( + auto discardTheFrontWaypointAndRecurse( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double step_time, const double matching_distance, - const std::optional target_speed) const -> std::optional; - auto buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + const double matching_distance, const std::optional target_speed) const + -> std::optional; + 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 double step_time) const - noexcept(false) -> double; - auto getValidatedEntityMinAcceleration(const double acceleration, const double step_time) const - noexcept(false) -> double; - auto getValidatedEntityPosition() const noexcept(false) -> const geometry_msgs::msg::Point; + 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) -> const geometry_msgs::msg::Point; + 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 diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp index 96a23681a9a..16a2e02b31c 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp @@ -65,7 +65,7 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( auto FollowWaypointController::roundTimeToFullStepsWithTolerance( const double remaining_time_source, const double time_tolerance) const -> double { - if (std::isnan(remaining_time_source) || std::isinf(remaining_time_source)) { + if (not std::isfinite(remaining_time_source)) { throw ControllerError( "Value remaining_time_source (", remaining_time_source, ") is NaN or inf - it cannot be rounded."); diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index e6856866169..5484b161c3f 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -34,8 +33,11 @@ namespace follow_trajectory PolylineTrajectoryFollower::PolylineTrajectoryFollower( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils) -: entity_status(entity_status), behavior_parameter(behavior_parameter), hdmap_utils(hdmap_utils) + const std::shared_ptr & hdmap_utils, const double step_time) +: entity_status(entity_status), + behavior_parameter(behavior_parameter), + hdmap_utils(hdmap_utils), + step_time(step_time) { } @@ -94,31 +96,35 @@ auto make_updated_pose_orientation( } } -auto calculate_desired_velocity( +auto calculate_current_velocity( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) +{ + const auto euler_angles = + math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); + const double pitch = -euler_angles.y; + const double yaw = euler_angles.z; + return geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * speed) + .y(std::cos(pitch) * std::sin(yaw) * speed) + .z(std::sin(pitch) * speed); +} + +auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const geometry_msgs::msg::Point & target_position, const geometry_msgs::msg::Point & position, - const double desired_speed) -> geometry_msgs::msg::Vector3 + const double desired_speed) const noexcept(false) -> geometry_msgs::msg::Vector3 { + /* + If not dynamic_constraints_ignorable, the linear distance should cause + problems. + */ + /* Note: The followingMode in OpenSCENARIO is passed as variable dynamic_constraints_ignorable. the value of the variable is `followingMode == position`. */ if (polyline_trajectory.dynamic_constraints_ignorable) { - const double dx = target_position.x - position.x; - const double dy = target_position.y - position.y; - // if entity is on lane use pitch from lanelet, otherwise use pitch on target - const double pitch = - entity_status.lanelet_pose_valid - ? -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y - : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); - const double yaw = std::atan2(dy, dx); // Use yaw on target - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * desired_speed) - .y(std::cos(pitch) * std::sin(yaw) * desired_speed) - .z(std::sin(pitch) * desired_speed); - } else { /* Note: The vector returned if dynamic_constraints_ignorable == true ignores parameters @@ -129,27 +135,36 @@ auto calculate_desired_velocity( */ throw common::SimulationError("The followingMode is only supported for position."); } -} -auto calculate_current_velocity( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) -{ - const auto euler_angles = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); - const double pitch = -euler_angles.y; - const double yaw = euler_angles.z; - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); + const double dx = target_position.x - position.x; + const double dy = target_position.y - position.y; + // if entity is on lane use pitch from lanelet, otherwise use pitch on target + const double pitch = + entity_status.lanelet_pose_valid + ? -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y + : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); + const double yaw = std::atan2(dy, dx); // Use yaw on target + + const auto desired_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * desired_speed) + .y(std::cos(pitch) * std::sin(yaw) * desired_speed) + .z(std::sin(pitch) * desired_speed); + if (is_infinite_vec3_like(desired_velocity)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", + desired_velocity.y, ", ", desired_velocity.z, "]."); + } + return desired_velocity; } -auto calculate_desired_acceleration( +auto PolylineTrajectoryFollower::getValidatedEntityDesiredAcceleration( const traffic_simulator::follow_trajectory::FollowWaypointController & follow_waypoint_controller, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double remaining_time, const double distance, const double acceleration, const double speed) - -> double + const double remaining_time, const double distance, const double acceleration, + const double speed) const noexcept(false) -> double { /* The desired acceleration is the acceleration at which the destination @@ -165,8 +180,18 @@ auto calculate_desired_acceleration( */ try { - return follow_waypoint_controller.getAcceleration( - remaining_time, distance, acceleration, speed); + const double desired_acceleration = + follow_waypoint_controller.getAcceleration(remaining_time, distance, acceleration, speed); + + if (not std::isfinite(desired_acceleration)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s desired acceleration value contains NaN or infinity. The value is ", + desired_acceleration, ". "); + } + return desired_acceleration; } catch (const ControllerError & e) { throw common::Error( "Vehicle ", std::quoted(entity_status.name), " - controller operation problem encountered. ", @@ -259,11 +284,17 @@ auto calculate_distance_and_remaining_time( } } -auto PolylineTrajectoryFollower::discard_the_front_waypoint_and_recurse( +auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double step_time, const double matching_distance, - const std::optional target_speed) const -> std::optional + const double matching_distance, const std::optional target_speed) const + -> std::optional { + if (polyline_trajectory.shape.vertices.empty()) { + throw common::SimulationError( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: ", + "Attmpted to access an element of an empty vector"); + } /* The OpenSCENARIO standard does not define the behavior when the value of Timing.domainAbsoluteRelative is "relative". The standard only states @@ -297,13 +328,11 @@ auto PolylineTrajectoryFollower::discard_the_front_waypoint_and_recurse( polyline_trajectory_copy.shape.vertices.pop_back(); } - return makeUpdatedEntityStatus( - polyline_trajectory_copy, step_time, matching_distance, target_speed); + return makeUpdatedEntityStatus(polyline_trajectory_copy, matching_distance, target_speed); }; auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const noexcept(true) - -> EntityStatus + const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> EntityStatus { using math::geometry::operator+; using math::geometry::operator-; @@ -358,8 +387,8 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double step_time, const double matching_distance, - const std::optional target_speed /*= std::nullopt*/) const -> std::optional + const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const + -> std::optional { /* The following code implements the steering behavior known as "seek". See @@ -375,16 +404,21 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( using math::geometry::operator/; using math::geometry::operator+=; + if (step_time <= 0.0) { + throw common::SimulationError( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: ", + "non-positive step time provided"); + } + if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; } const double entity_speed = getValidatedEntitySpeed(); const double acceleration = getValidatedEntityAcceleration(); - [[maybe_unused]] const double max_acceleration = - getValidatedEntityMaxAcceleration(acceleration, step_time); - [[maybe_unused]] const double min_acceleration = - getValidatedEntityMinAcceleration(acceleration, step_time); + [[maybe_unused]] const double max_acceleration = getValidatedEntityMaxAcceleration(acceleration); + [[maybe_unused]] const double min_acceleration = getValidatedEntityMinAcceleration(acceleration); const auto entity_position = getValidatedEntityPosition(); const auto target_position = getValidatedEntityTargetPosition(polyline_trajectory); @@ -422,6 +456,20 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( behavior_parameter, step_time, is_breaking_waypoint, std::isinf(remaining_time) ? target_speed : std::nullopt); + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (math::arithmetic::isDefinitelyLessThan( + distance_to_front_waypoint, std::numeric_limits::epsilon())) { + return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); + } + + if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); + } + /* The desired acceleration is the acceleration at which the destination can be reached exactly at the specified time (= time remaining at zero). @@ -434,89 +482,20 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const double desired_acceleration = follow_waypoint_controller.getAcceleration( - remaining_time, distance, acceleration, entity_speed); - const double desired_speed = entity_speed + desired_acceleration * step_time; - const auto desired_velocity = calculate_desired_velocity( - polyline_trajectory, entity_status, target_position, entity_position, desired_speed); + const double desired_acceleration = getValidatedEntityDesiredAcceleration( + follow_waypoint_controller, polyline_trajectory, remaining_time, distance, acceleration, + entity_speed); + const double desired_speed = getValidatedEntityDesiredSpeed(entity_speed, desired_acceleration); + const auto desired_velocity = getValidatedEntityDesiredVelocity( + polyline_trajectory, target_position, entity_position, desired_speed); const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, entity_speed); - if (is_infinite_vec3_like(entity_position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); - } - - /* - We've made sure that polyline_trajectory.shape.vertices is not empty, so - a reference to vertices.front() always succeeds. vertices.front() is - referenced only this once in this member function. - */ - if (is_infinite_vec3_like(target_position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s target position coordinate value contains NaN or infinity. The value is [", - target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); - } - - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - if (math::arithmetic::isDefinitelyLessThan( - distance_to_front_waypoint, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); - } - - if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); - } - - if (not std::isfinite(desired_acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, - ". "); - } - - if (not std::isfinite(desired_speed)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", - desired_speed, ". "); - } - - if (is_infinite_vec3_like(desired_velocity)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", - desired_velocity.y, ", ", desired_velocity.z, "]."); - } - if ( entity_speed * step_time > distance_to_front_waypoint && math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); + return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { @@ -542,10 +521,10 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); + return discardTheFrontWaypointAndRecurse( + polyline_trajectory, matching_distance, target_speed); } else { - return buildUpdatedEntityStatus(desired_velocity, step_time); + return buildUpdatedEntityStatus(desired_velocity); } } else { /* @@ -555,10 +534,10 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (const double this_step_distance = (entity_speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); + return discardTheFrontWaypointAndRecurse( + polyline_trajectory, matching_distance, target_speed); } else { - return buildUpdatedEntityStatus(desired_velocity, step_time); + return buildUpdatedEntityStatus(desired_velocity); } } /* @@ -573,8 +552,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { - return discard_the_front_waypoint_and_recurse( - polyline_trajectory, step_time, matching_distance, target_speed); + return discardTheFrontWaypointAndRecurse( + polyline_trajectory, matching_distance, target_speed); } else { throw common::SimulationError( "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, @@ -584,7 +563,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( " from that waypoint which is greater than the accepted accuracy."); } } else { - return buildUpdatedEntityStatus(desired_velocity, step_time); + return buildUpdatedEntityStatus(desired_velocity); } /* @@ -620,8 +599,8 @@ auto PolylineTrajectoryFollower::getValidatedEntitySpeed() const noexcept(false) } return entity_speed; } -auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration( - const double acceleration, const double step_time) const noexcept(false) -> double +auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration(const double acceleration) const + noexcept(false) -> double { const double max_acceleration = std::min( acceleration /* [m/s^2] */ + @@ -638,8 +617,8 @@ auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration( } return max_acceleration; } -auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration( - const double acceleration, const double step_time) const noexcept(false) -> double +auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration(const double acceleration) const + noexcept(false) -> double { const double min_acceleration = std::max( acceleration /* [m/s^2] */ - @@ -657,7 +636,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration( return min_acceleration; } auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(false) - -> const geometry_msgs::msg::Point + -> geometry_msgs::msg::Point { const auto entity_position = entity_status.pose.position; if (is_infinite_vec3_like(entity_position)) { @@ -671,7 +650,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(fal } auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) - -> const geometry_msgs::msg::Point + -> geometry_msgs::msg::Point { if (polyline_trajectory.shape.vertices.empty()) { throw common::Error( @@ -680,6 +659,20 @@ auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( } return polyline_trajectory.shape.vertices.front().position.position; } +auto PolylineTrajectoryFollower::getValidatedEntityDesiredSpeed( + const double entity_speed, const double desired_acceleration) const noexcept(false) -> double +{ + const double desired_speed = entity_speed + desired_acceleration * step_time; + + if (not std::isfinite(desired_speed)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", + desired_speed, ". "); + } + return desired_speed; +} } // namespace follow_trajectory } // namespace traffic_simulator \ No newline at end of file diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 71d20fdfb90..35e2e5fdc15 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -150,10 +150,9 @@ void EgoEntity::onUpdate(double current_time, double step_time) if (const auto non_canonicalized_updated_status = traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( static_cast(*status_), behavior_parameter_, - hdmap_utils_ptr_) + hdmap_utils_ptr_, step_time) .makeUpdatedEntityStatus( - *polyline_trajectory_, step_time, - getDefaultMatchingDistanceForLaneletPoseCalculation(), + *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), target_speed_ ? target_speed_.value() : status_->getTwist().linear.x); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side From eba216884115519ce9ab37144288a525dac99bec Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 12 Nov 2024 17:38:28 +0100 Subject: [PATCH 11/82] add missing validator --- .../src/behavior/polyline_trajectory_follower.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 5484b161c3f..44677549d9a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -657,7 +657,16 @@ auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "attempted to dereference an element of an empty PolylineTrajectory"); } - return polyline_trajectory.shape.vertices.front().position.position; + const auto target_position = polyline_trajectory.shape.vertices.front().position.position; + if (is_infinite_vec3_like(target_position)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), + "'s target position coordinate value contains NaN or infinity. The value is [", + target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); + } + return target_position; } auto PolylineTrajectoryFollower::getValidatedEntityDesiredSpeed( const double entity_speed, const double desired_acceleration) const noexcept(false) -> double @@ -675,4 +684,4 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredSpeed( } } // namespace follow_trajectory -} // namespace traffic_simulator \ No newline at end of file +} // namespace traffic_simulator From 6bbba1eb0588d83998dda27443a0e9e0944388f8 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 12 Nov 2024 17:41:21 +0100 Subject: [PATCH 12/82] remove dead code --- .../src/behavior/polyline_trajectory_follower.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 44677549d9a..c83ee7e102a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -223,14 +223,6 @@ auto calculate_distance_and_remaining_time( hdmap_utils, entity_status, matching_distance, vertex.position.position, next->position.position); }); - double total_distance = 0.0; - for (auto iter = std::begin(polyline_trajectory.shape.vertices); - 0 < std::distance(iter, last); ++iter) { - total_distance += distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, iter->position.position, - std::next(iter)->position.position); - } - return total_distance; }; const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); From 39585689f32cb71f11183d1749a02d75a78bdd42 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 12 Nov 2024 17:43:00 +0100 Subject: [PATCH 13/82] use const iterator --- .../src/behavior/polyline_trajectory_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index c83ee7e102a..1d6edf4938a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -215,7 +215,7 @@ auto calculate_distance_and_remaining_time( [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory]( const std::vector::const_iterator last) { return std::accumulate( - std::begin(polyline_trajectory.shape.vertices), last, 0.0, + polyline_trajectory.shape.vertices.cbegin(), last, 0.0, [&hdmap_utils, &entity_status, &matching_distance]( const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); From ac535c342af0627f877af09f119855a645454a6f Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 13 Nov 2024 10:44:08 +0100 Subject: [PATCH 14/82] inversion isfinite_vec3 --- .../src/behavior/polyline_trajectory_follower.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 1d6edf4938a..8029655c0e8 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -42,10 +42,10 @@ PolylineTrajectoryFollower::PolylineTrajectoryFollower( } template -auto is_infinite_vec3_like(const T & vec) -> bool +auto isfinite_vec3(const T & vec) -> bool { static_assert(math::geometry::IsLikeVector3>::value); - return not std::isfinite(vec.x) or not std::isfinite(vec.y) or not std::isfinite(vec.z); + return std::isfinite(vec.x) and std::isfinite(vec.y) and std::isfinite(vec.z); } auto distance_along_lanelet( @@ -149,7 +149,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( .x(std::cos(pitch) * std::cos(yaw) * desired_speed) .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); - if (is_infinite_vec3_like(desired_velocity)) { + if (not isfinite_vec3(desired_velocity)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -631,7 +631,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(fal -> geometry_msgs::msg::Point { const auto entity_position = entity_status.pose.position; - if (is_infinite_vec3_like(entity_position)) { + if (not isfinite_vec3(entity_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -650,7 +650,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( "attempted to dereference an element of an empty PolylineTrajectory"); } const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - if (is_infinite_vec3_like(target_position)) { + if (not isfinite_vec3(target_position)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", From a5ed84da5bed4736172e25341fa3cd47ca56df40 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 13 Nov 2024 11:12:00 +0100 Subject: [PATCH 15/82] typed and constexpr comparisons --- .../include/arithmetic/floating_point/comparison.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp index 34c1c2586d1..d6287f625c3 100644 --- a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp +++ b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp @@ -23,27 +23,27 @@ namespace math namespace arithmetic { template -auto isApproximatelyEqualTo(T a, T b) +constexpr auto isApproximatelyEqualTo(T a, T b) -> bool { return std::abs(a - b) <= (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } template -auto isEssentiallyEqualTo(T a, T b) +constexpr auto isEssentiallyEqualTo(T a, T b) -> bool { return std::abs(a - b) <= (std::numeric_limits::epsilon() * std::min(std::abs(a), std::abs(b))); } template -auto isDefinitelyLessThan(T a, T b) +constexpr auto isDefinitelyLessThan(T a, T b) -> bool { return (b - a) > (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } template -auto isDefinitelyGreaterThan(T a, T b) +constexpr auto isDefinitelyGreaterThan(T a, T b) -> bool { return (a - b) > (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } From 834fb3a50d532dfececebf05733bfe5ebf11e7aa Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 13 Nov 2024 12:51:51 +0100 Subject: [PATCH 16/82] rename --- .../behavior/polyline_trajectory_follower.hpp | 4 +- .../behavior/polyline_trajectory_follower.cpp | 173 +++++++++--------- 2 files changed, 89 insertions(+), 88 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index eba4d37bcfe..a4dc5ba0344 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -33,7 +33,7 @@ struct PolylineTrajectoryFollower explicit PolylineTrajectoryFollower( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time); + const std::shared_ptr & hdmap_utils_ptr, const double step_time); auto makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, @@ -44,7 +44,7 @@ struct PolylineTrajectoryFollower private: const traffic_simulator_msgs::msg::EntityStatus entity_status; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - const std::shared_ptr hdmap_utils; + const std::shared_ptr hdmap_utils_ptr; const double step_time; auto discardTheFrontWaypointAndRecurse( diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 8029655c0e8..d4afde94b00 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -33,10 +33,10 @@ namespace follow_trajectory PolylineTrajectoryFollower::PolylineTrajectoryFollower( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time) + const std::shared_ptr & hdmap_utils_ptr, const double step_time) : entity_status(entity_status), behavior_parameter(behavior_parameter), - hdmap_utils(hdmap_utils), + hdmap_utils_ptr(hdmap_utils_ptr), step_time(step_time) { } @@ -49,17 +49,17 @@ auto isfinite_vec3(const T & vec) -> bool } auto distance_along_lanelet( - const std::shared_ptr & hdmap_utils, + const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { if (const auto from_lanelet_pose = - hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); + hdmap_utils_ptr->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); from_lanelet_pose.has_value()) { if (const auto to_lanelet_pose = - hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); + hdmap_utils_ptr->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); to_lanelet_pose.has_value()) { - if (const auto distance = hdmap_utils->getLongitudinalDistance( + if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance( from_lanelet_pose.value(), to_lanelet_pose.value()); distance.has_value()) { return distance.value(); @@ -98,6 +98,7 @@ auto make_updated_pose_orientation( auto calculate_current_velocity( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) + -> geometry_msgs::msg::Vector3 { const auto euler_angles = math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); @@ -109,6 +110,83 @@ auto calculate_current_velocity( .z(std::sin(pitch) * speed); } +auto calculate_distance_and_remaining_time( + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double distance_to_front_waypoint, const double step_time) -> std::tuple +{ + /* + Note for anyone working on adding support for followingMode follow + to this function (FollowPolylineTrajectoryAction::tick) in the + future: if followingMode is follow, this distance calculation may be + inappropriate. + */ + const auto total_distance_to = + [&hdmap_utils_ptr, &entity_status, &matching_distance, &polyline_trajectory]( + const std::vector::const_iterator last) { + return std::accumulate( + polyline_trajectory.shape.vertices.cbegin(), last, 0.0, + [&hdmap_utils_ptr, &entity_status, &matching_distance]( + const double total_distance, const auto & vertex) { + const auto next = std::next(&vertex); + return total_distance + distance_along_lanelet( + hdmap_utils_ptr, entity_status, matching_distance, + vertex.position.position, next->position.position); + }); + }; + + const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); + if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { + return std::make_tuple( + distance_to_front_waypoint + + total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), + std::numeric_limits::infinity()); + } + + const auto remaining_time = + (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + waypoint_ptr->time - entity_status.time; + + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ + if (remaining_time < -step_time) { + throw common::Error( + "Vehicle ", std::quoted(entity_status.name), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + waypoint_ptr->time, " (in ", + (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + + } else { + return std::make_tuple( + distance_to_front_waypoint + total_distance_to(waypoint_ptr), + remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time); + } +} + auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const geometry_msgs::msg::Point & target_position, const geometry_msgs::msg::Point & position, @@ -199,83 +277,6 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredAcceleration( } } -auto calculate_distance_and_remaining_time( - const std::shared_ptr & hdmap_utils, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, double matching_distance, - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double distance_to_front_waypoint, const double step_time) -> std::tuple -{ - /* - Note for anyone working on adding support for followingMode follow - to this function (FollowPolylineTrajectoryAction::tick) in the - future: if followingMode is follow, this distance calculation may be - inappropriate. - */ - const auto total_distance_to = - [&hdmap_utils, &entity_status, &matching_distance, &polyline_trajectory]( - const std::vector::const_iterator last) { - return std::accumulate( - polyline_trajectory.shape.vertices.cbegin(), last, 0.0, - [&hdmap_utils, &entity_status, &matching_distance]( - const double total_distance, const auto & vertex) { - const auto next = std::next(&vertex); - return total_distance + distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, - vertex.position.position, next->position.position); - }); - }; - - const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); - if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), - std::numeric_limits::infinity()); - } - - const auto remaining_time = - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - waypoint_ptr->time - entity_status.time; - - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - if (remaining_time < -step_time) { - throw common::Error( - "Vehicle ", std::quoted(entity_status.name), - " failed to reach the trajectory waypoint at the specified time. The specified time " - "is ", - waypoint_ptr->time, " (in ", - (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), - " simulation time). This may be due to unrealistic conditions of arrival time " - "specification compared to vehicle parameters and dynamic constraints."); - - } else { - return std::make_tuple( - distance_to_front_waypoint + total_distance_to(waypoint_ptr), - remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time); - } -} - auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const @@ -415,14 +416,14 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto target_position = getValidatedEntityTargetPosition(polyline_trajectory); const double distance_to_front_waypoint = distance_along_lanelet( - hdmap_utils, entity_status, matching_distance, entity_position, target_position); + hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); const double remaining_time_to_front_waypoint = (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + polyline_trajectory.shape.vertices.front().time - entity_status.time; const auto [distance, remaining_time] = calculate_distance_and_remaining_time( - hdmap_utils, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, - step_time); + hdmap_utils_ptr, entity_status, matching_distance, polyline_trajectory, + distance_to_front_waypoint, step_time); /* The controller provides the ability to calculate acceleration using constraints from the From 86471e76f982e82d6e047af84ccc5eecb1e10ea5 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 13 Nov 2024 15:12:18 +0100 Subject: [PATCH 17/82] typo --- .../src/behavior/polyline_trajectory_follower.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index d4afde94b00..1c9f289b763 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -286,7 +286,7 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( throw common::SimulationError( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: ", - "Attmpted to access an element of an empty vector"); + "Attempted to access an element of an empty vector"); } /* The OpenSCENARIO standard does not define the behavior when the value of From cc2908c5db53fa457ef4a5f005f841347e27fc98 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 14 Nov 2024 17:46:58 +0100 Subject: [PATCH 18/82] inverse if-statement, specify captures --- .../follow_polyline_trajectory_action.cpp | 2 +- .../follow_polyline_trajectory_action.cpp | 2 +- .../src/behavior/polyline_trajectory_follower.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 1ef3304ef9d..de9414d2bf7 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/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 { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 93a3206a244..32f4c9fa8c2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/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 { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 1c9f289b763..2499891055c 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -202,7 +202,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( variable dynamic_constraints_ignorable. the value of the variable is `followingMode == position`. */ - if (polyline_trajectory.dynamic_constraints_ignorable) { + if (not polyline_trajectory.dynamic_constraints_ignorable) { /* Note: The vector returned if dynamic_constraints_ignorable == true ignores parameters From 01ddaa9478575d6f40f3a373867c3d9c998efee9 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 20 Nov 2024 11:14:25 +0100 Subject: [PATCH 19/82] reorder algorithm steps --- .../behavior/polyline_trajectory_follower.cpp | 47 ++++++++++--------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 2499891055c..539fa24936f 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -408,23 +408,34 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return std::nullopt; } - const double entity_speed = getValidatedEntitySpeed(); - const double acceleration = getValidatedEntityAcceleration(); - [[maybe_unused]] const double max_acceleration = getValidatedEntityMaxAcceleration(acceleration); - [[maybe_unused]] const double min_acceleration = getValidatedEntityMinAcceleration(acceleration); const auto entity_position = getValidatedEntityPosition(); const auto target_position = getValidatedEntityTargetPosition(polyline_trajectory); const double distance_to_front_waypoint = distance_along_lanelet( hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); - const double remaining_time_to_front_waypoint = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - polyline_trajectory.shape.vertices.front().time - entity_status.time; + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (math::arithmetic::isDefinitelyLessThan( + distance_to_front_waypoint, std::numeric_limits::epsilon())) { + return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); + } const auto [distance, remaining_time] = calculate_distance_and_remaining_time( hdmap_utils_ptr, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, step_time); + if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); + } + + const double acceleration = getValidatedEntityAcceleration(); + [[maybe_unused]] const double max_acceleration = getValidatedEntityMaxAcceleration(acceleration); + [[maybe_unused]] const double min_acceleration = getValidatedEntityMinAcceleration(acceleration); + const double entity_speed = getValidatedEntitySpeed(); + /* The controller provides the ability to calculate acceleration using constraints from the behavior_parameter. The value is_breaking_waypoint() determines whether the calculated @@ -449,20 +460,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( behavior_parameter, step_time, is_breaking_waypoint, std::isinf(remaining_time) ? target_speed : std::nullopt); - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - if (math::arithmetic::isDefinitelyLessThan( - distance_to_front_waypoint, std::numeric_limits::epsilon())) { - return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); - } - - if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { - return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); - } - /* The desired acceleration is the acceleration at which the destination can be reached exactly at the specified time (= time remaining at zero). @@ -482,8 +479,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto desired_velocity = getValidatedEntityDesiredVelocity( polyline_trajectory, target_position, entity_position, desired_speed); const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); - const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, acceleration, entity_speed); if ( entity_speed * step_time > distance_to_front_waypoint && @@ -491,6 +486,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time, distance, acceleration, entity_speed); + const double remaining_time_to_front_waypoint = + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + polyline_trajectory.shape.vertices.front().time - entity_status.time; + if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " From abba124799f855af491fb6a487f4227744b8728a Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 20 Nov 2024 15:42:49 +0100 Subject: [PATCH 20/82] debug info --- .../behavior/polyline_trajectory_follower.cpp | 89 ++++++++++++++++++- 1 file changed, 85 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 539fa24936f..2997c6b0bec 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -413,6 +413,9 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const double distance_to_front_waypoint = distance_along_lanelet( hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); + const double remaining_time_to_front_waypoint = + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + polyline_trajectory.shape.vertices.front().time - entity_status.time; /* This clause is to avoid division-by-zero errors in later clauses with @@ -488,10 +491,88 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, entity_speed); - const double remaining_time_to_front_waypoint = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - polyline_trajectory.shape.vertices.front().time - entity_status.time; - + if constexpr (true) { + auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; + // clang-format off + std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; + + std::cout << "acceleration " + << "== " << acceleration + << std::endl; + + std::cout << "min_acceleration " + << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " + << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " + << "== " << min_acceleration + << std::endl; + + std::cout << "max_acceleration " + << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " + << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " + << "== " << max_acceleration + << std::endl; + + std::cout << "min_acceleration < acceleration < max_acceleration " + << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; + + std::cout << "desired_acceleration " + << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " + << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << entity_speed << " / " << remaining_time << " " + << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * entity_speed / remaining_time) << " " + << "== " << desired_acceleration << " " + << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" + << std::endl; + + std::cout << "desired_speed " + << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " + << "== " << entity_speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " + << "== " << entity_speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " + << "== " << entity_speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " + << "== " << desired_speed + << std::endl; + + std::cout << "distance_to_front_waypoint " + << "== " << distance_to_front_waypoint + << std::endl; + + std::cout << "remaining_time_to_arrival_to_front_waypoint " + << "== " << remaining_time_to_arrival_to_front_waypoint + << std::endl; + + std::cout << "distance " + << "== " << distance + << std::endl; + + std::cout << "remaining_time " + << "== " << remaining_time + << std::endl; + + std::cout << "remaining_time_to_arrival_to_front_waypoint " + << "(" + << "== distance_to_front_waypoint / desired_speed " + << "== " << distance_to_front_waypoint << " / " << desired_speed << " " + << "== " << remaining_time_to_arrival_to_front_waypoint + << ")" + << std::endl; + + std::cout << "arrive during this frame? " + << "== remaining_time_to_arrival_to_front_waypoint < step_time " + << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " + << "== " << math::arithmetic::isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) + << std::endl; + + std::cout << "not too early? " + << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " + << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " + << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << math::arithmetic::isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " + << "== " << (std::isnan(remaining_time_to_front_waypoint) or math::arithmetic::isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) + << std::endl; + // clang-format on + } if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " From edf1f6e92ec7eb61bcf4a9792a1fe03fd6699966 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 20 Nov 2024 18:17:10 +0100 Subject: [PATCH 21/82] debug info --- .../behavior/polyline_trajectory_follower.cpp | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 2997c6b0bec..fc60c0865e2 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -143,7 +143,8 @@ auto calculate_distance_and_remaining_time( total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), std::numeric_limits::infinity()); } - + std::cout << " waypoint_ptr->time " << waypoint_ptr->time << " " << polyline_trajectory.base_time + << std::endl; const auto remaining_time = (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + waypoint_ptr->time - entity_status.time; @@ -405,6 +406,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( } if (polyline_trajectory.shape.vertices.empty()) { + std::cout << __LINE__ << "empty" << std::endl; return std::nullopt; } @@ -424,6 +426,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ if (math::arithmetic::isDefinitelyLessThan( distance_to_front_waypoint, std::numeric_limits::epsilon())) { + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } const auto [distance, remaining_time] = calculate_distance_and_remaining_time( @@ -431,6 +434,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( distance_to_front_waypoint, step_time); if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } @@ -486,6 +490,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if ( entity_speed * step_time > distance_to_front_waypoint && math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { + std::cout << entity_speed << " " << step_time << " " << distance_to_front_waypoint << std::endl; + std::cout << desired_velocity.x << " " << desired_velocity.y << " " << desired_velocity.z << " " + << std::endl; + std::cout << current_velocity.x << " " << current_velocity.y << " " << current_velocity.z << " " + << std::endl; + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } @@ -495,7 +505,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; // clang-format off std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; - + std::cout << "entity time == " << entity_status.time << std::endl; std::cout << "acceleration " << "== " << acceleration << std::endl; @@ -596,9 +606,11 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { + std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } } else { @@ -609,9 +621,11 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (const double this_step_distance = (entity_speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { + std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } } @@ -627,6 +641,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { + std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { @@ -638,6 +653,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( " from that waypoint which is greater than the accepted accuracy."); } } else { + std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } From a8505103a2542a333e938065c4e44783bb4de676 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 20 Nov 2024 19:05:39 +0100 Subject: [PATCH 22/82] mutable polyline_trajectory --- .../behavior/polyline_trajectory_follower.hpp | 6 +- .../behavior/polyline_trajectory_follower.cpp | 123 ++---------------- 2 files changed, 16 insertions(+), 113 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index a4dc5ba0344..6572f2ba88f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -35,8 +35,9 @@ struct PolylineTrajectoryFollower const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::shared_ptr & hdmap_utils_ptr, const double step_time); + // side effects on polyline_trajectory auto makeUpdatedEntityStatus( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const -> std::optional; @@ -47,8 +48,9 @@ struct PolylineTrajectoryFollower const std::shared_ptr hdmap_utils_ptr; const double step_time; + // side effects on polyline_trajectory auto discardTheFrontWaypointAndRecurse( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const -> std::optional; auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index fc60c0865e2..6bd86a3b938 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -143,8 +143,6 @@ auto calculate_distance_and_remaining_time( total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), std::numeric_limits::infinity()); } - std::cout << " waypoint_ptr->time " << waypoint_ptr->time << " " << polyline_trajectory.base_time - << std::endl; const auto remaining_time = (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + waypoint_ptr->time - entity_status.time; @@ -279,7 +277,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredAcceleration( } auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const -> std::optional { @@ -306,23 +304,22 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( means "The waypoint about to be popped is the waypoint with the specified arrival time". */ - auto polyline_trajectory_copy = polyline_trajectory; if ( - not std::isnan(polyline_trajectory_copy.base_time) and - not std::isnan(polyline_trajectory_copy.shape.vertices.front().time)) { - polyline_trajectory_copy.base_time = entity_status.time; + not std::isnan(polyline_trajectory.base_time) and + not std::isnan(polyline_trajectory.shape.vertices.front().time)) { + polyline_trajectory.base_time = entity_status.time; } std::rotate( - std::begin(polyline_trajectory_copy.shape.vertices), - std::begin(polyline_trajectory_copy.shape.vertices) + 1, - std::end(polyline_trajectory_copy.shape.vertices)); + std::begin(polyline_trajectory.shape.vertices), + std::begin(polyline_trajectory.shape.vertices) + 1, + std::end(polyline_trajectory.shape.vertices)); - if (not polyline_trajectory_copy.closed) { - polyline_trajectory_copy.shape.vertices.pop_back(); + if (not polyline_trajectory.closed) { + polyline_trajectory.shape.vertices.pop_back(); } - return makeUpdatedEntityStatus(polyline_trajectory_copy, matching_distance, target_speed); + return makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); }; auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( @@ -380,7 +377,7 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( } auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const -> std::optional { @@ -406,7 +403,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( } if (polyline_trajectory.shape.vertices.empty()) { - std::cout << __LINE__ << "empty" << std::endl; return std::nullopt; } @@ -426,7 +422,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ if (math::arithmetic::isDefinitelyLessThan( distance_to_front_waypoint, std::numeric_limits::epsilon())) { - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } const auto [distance, remaining_time] = calculate_distance_and_remaining_time( @@ -434,7 +429,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( distance_to_front_waypoint, step_time); if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } @@ -490,99 +484,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if ( entity_speed * step_time > distance_to_front_waypoint && math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { - std::cout << entity_speed << " " << step_time << " " << distance_to_front_waypoint << std::endl; - std::cout << desired_velocity.x << " " << desired_velocity.y << " " << desired_velocity.z << " " - << std::endl; - std::cout << current_velocity.x << " " << current_velocity.y << " " << current_velocity.z << " " - << std::endl; - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, entity_speed); - if constexpr (true) { - auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; - // clang-format off - std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; - std::cout << "entity time == " << entity_status.time << std::endl; - std::cout << "acceleration " - << "== " << acceleration - << std::endl; - - std::cout << "min_acceleration " - << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== " << min_acceleration - << std::endl; - - std::cout << "max_acceleration " - << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== " << max_acceleration - << std::endl; - - std::cout << "min_acceleration < acceleration < max_acceleration " - << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; - - std::cout << "desired_acceleration " - << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " - << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << entity_speed << " / " << remaining_time << " " - << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * entity_speed / remaining_time) << " " - << "== " << desired_acceleration << " " - << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" - << std::endl; - - std::cout << "desired_speed " - << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " - << "== " << entity_speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " - << "== " << entity_speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " - << "== " << entity_speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " - << "== " << desired_speed - << std::endl; - - std::cout << "distance_to_front_waypoint " - << "== " << distance_to_front_waypoint - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "== " << remaining_time_to_arrival_to_front_waypoint - << std::endl; - - std::cout << "distance " - << "== " << distance - << std::endl; - - std::cout << "remaining_time " - << "== " << remaining_time - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "(" - << "== distance_to_front_waypoint / desired_speed " - << "== " << distance_to_front_waypoint << " / " << desired_speed << " " - << "== " << remaining_time_to_arrival_to_front_waypoint - << ")" - << std::endl; - - std::cout << "arrive during this frame? " - << "== remaining_time_to_arrival_to_front_waypoint < step_time " - << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " - << "== " << math::arithmetic::isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) - << std::endl; - - std::cout << "not too early? " - << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " - << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " - << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << math::arithmetic::isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " - << "== " << (std::isnan(remaining_time_to_front_waypoint) or math::arithmetic::isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) - << std::endl; - // clang-format on - } + if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -606,11 +513,9 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { - std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } } else { @@ -621,11 +526,9 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (const double this_step_distance = (entity_speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { - std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } } @@ -641,7 +544,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( acceleration, entity_speed, distance_to_front_waypoint)) { - std::cout << __LINE__ << "recursion" << std::endl; return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { @@ -653,7 +555,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( " from that waypoint which is greater than the accepted accuracy."); } } else { - std::cout << __LINE__ << "finish" << std::endl; return buildUpdatedEntityStatus(desired_velocity); } From 7d12a12ee2f018a1469114d4d191a90e2d729588 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 21 Nov 2024 16:03:39 +0100 Subject: [PATCH 23/82] acceleration validator --- .../behavior/polyline_trajectory_follower.hpp | 16 ++--- .../behavior/polyline_trajectory_follower.cpp | 69 ++++++++----------- 2 files changed, 36 insertions(+), 49 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 6572f2ba88f..5c0b9b68a82 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -55,25 +55,23 @@ struct PolylineTrajectoryFollower -> std::optional; 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( + auto validatedEntityAcceleration() const noexcept(false) -> double; + auto validatedEntitySpeed() const noexcept(false) -> double; + auto validatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point; + auto validatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) -> geometry_msgs::msg::Point; - auto getValidatedEntityDesiredAcceleration( + auto validatedEntityDesiredAcceleration( 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( + auto validatedEntityDesiredVelocity( 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( + auto validatedEntityDesiredSpeed( const double entity_speed, const double desired_acceleration) const noexcept(false) -> double; }; } // namespace follow_trajectory diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 6bd86a3b938..697067fc285 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -186,7 +186,7 @@ auto calculate_distance_and_remaining_time( } } -auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( +auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( 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 @@ -237,7 +237,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityDesiredVelocity( return desired_velocity; } -auto PolylineTrajectoryFollower::getValidatedEntityDesiredAcceleration( +auto PolylineTrajectoryFollower::validatedEntityDesiredAcceleration( 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, @@ -406,8 +406,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return std::nullopt; } - const auto entity_position = getValidatedEntityPosition(); - const auto target_position = getValidatedEntityTargetPosition(polyline_trajectory); + const auto entity_position = validatedEntityPosition(); + const auto target_position = validatedEntityTargetPosition(polyline_trajectory); const double distance_to_front_waypoint = distance_along_lanelet( hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); @@ -432,10 +432,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } - const double acceleration = getValidatedEntityAcceleration(); - [[maybe_unused]] const double max_acceleration = getValidatedEntityMaxAcceleration(acceleration); - [[maybe_unused]] const double min_acceleration = getValidatedEntityMinAcceleration(acceleration); - const double entity_speed = getValidatedEntitySpeed(); + const double acceleration = validatedEntityAcceleration(); + const double entity_speed = validatedEntitySpeed(); /* The controller provides the ability to calculate acceleration using constraints from the @@ -473,11 +471,11 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const double desired_acceleration = getValidatedEntityDesiredAcceleration( + const double desired_acceleration = validatedEntityDesiredAcceleration( follow_waypoint_controller, polyline_trajectory, remaining_time, distance, acceleration, entity_speed); - const double desired_speed = getValidatedEntityDesiredSpeed(entity_speed, desired_acceleration); - const auto desired_velocity = getValidatedEntityDesiredVelocity( + const double desired_speed = validatedEntityDesiredSpeed(entity_speed, desired_acceleration); + const auto desired_velocity = validatedEntityDesiredVelocity( polyline_trajectory, target_position, entity_position, desired_speed); const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); @@ -565,7 +563,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ } -auto PolylineTrajectoryFollower::getValidatedEntityAcceleration() const noexcept(false) -> double +auto PolylineTrajectoryFollower::validatedEntityAcceleration() const noexcept(false) -> double { const double acceleration = entity_status.action_status.accel.linear.x; if (not std::isfinite(acceleration)) { @@ -575,25 +573,6 @@ auto PolylineTrajectoryFollower::getValidatedEntityAcceleration() const noexcept std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", acceleration, ". "); } - return acceleration; -} - -auto PolylineTrajectoryFollower::getValidatedEntitySpeed() const noexcept(false) -> double -{ - const double entity_speed = entity_status.action_status.twist.linear.x; // [m/s] - - if (not std::isfinite(entity_speed)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", - entity_speed, ". "); - } - return entity_speed; -} -auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration(const double acceleration) const - noexcept(false) -> double -{ const double max_acceleration = std::min( acceleration /* [m/s^2] */ + behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * @@ -607,11 +586,6 @@ auto PolylineTrajectoryFollower::getValidatedEntityMaxAcceleration(const double std::quoted(entity_status.name), "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); } - return max_acceleration; -} -auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration(const double acceleration) const - noexcept(false) -> double -{ const double min_acceleration = std::max( acceleration /* [m/s^2] */ - behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * @@ -625,9 +599,24 @@ auto PolylineTrajectoryFollower::getValidatedEntityMinAcceleration(const double std::quoted(entity_status.name), "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); } - return min_acceleration; + return acceleration; +} + +auto PolylineTrajectoryFollower::validatedEntitySpeed() const noexcept(false) -> double +{ + const double entity_speed = entity_status.action_status.twist.linear.x; // [m/s] + + if (not std::isfinite(entity_speed)) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", + entity_speed, ". "); + } + return entity_speed; } -auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(false) + +auto PolylineTrajectoryFollower::validatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point { const auto entity_position = entity_status.pose.position; @@ -640,7 +629,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityPosition() const noexcept(fal } return entity_position; } -auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( +auto PolylineTrajectoryFollower::validatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) -> geometry_msgs::msg::Point { @@ -660,7 +649,7 @@ auto PolylineTrajectoryFollower::getValidatedEntityTargetPosition( } return target_position; } -auto PolylineTrajectoryFollower::getValidatedEntityDesiredSpeed( +auto PolylineTrajectoryFollower::validatedEntityDesiredSpeed( const double entity_speed, const double desired_acceleration) const noexcept(false) -> double { const double desired_speed = entity_speed + desired_acceleration * step_time; From 0ec90b2f8ebc45bd71649b0a79885058bc09fd03 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 10:47:17 +0100 Subject: [PATCH 24/82] clean up FollowWaypointController --- .../behavior/polyline_trajectory_follower.hpp | 3 +- .../behavior/follow_waypoint_controller.cpp | 59 ++++++++++--------- .../behavior/polyline_trajectory_follower.cpp | 36 +++++------ 3 files changed, 51 insertions(+), 47 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 5c0b9b68a82..3da3f396e99 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -38,8 +38,7 @@ struct PolylineTrajectoryFollower // side effects on polyline_trajectory auto makeUpdatedEntityStatus( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, - const std::optional target_speed /*= std::nullopt*/) const + const double matching_distance, const std::optional target_speed) const -> std::optional; private: diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp index 16a2e02b31c..9d17848a806 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp @@ -23,7 +23,7 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( const double speed) const -> double { if (remaining_time <= step_time * 2.0) { - if (!with_breaking || (std::abs(speed) < local_epsilon)) { + if (not with_breaking or (std::abs(speed) < local_epsilon)) { // Step in which the acceleration is set to 0.0. return clampAcceleration(0.0, acceleration, speed); } else { @@ -58,7 +58,7 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( throw common::SemanticError( "An analytical calculation of acceleration is not available for a time step greater than 4 (", - step_time * 4, "s) in the case of braking and greater than 3 (", step_time * 3, + step_time * 4.0, "s) in the case of braking and greater than 3 (", step_time * 3.0, "s) without braking."); } @@ -100,7 +100,7 @@ auto FollowWaypointController::getTimeRequiredForNonAcceleration(const double ac auto FollowWaypointController::getAccelerationLimits( const double acceleration, const double speed) const -> std::pair { - const auto time_for_non_acceleration = [&, acceleration]() { + const auto time_for_non_acceleration = [this, acceleration]() { if (std::abs(acceleration) < local_epsilon) { return 0.0; } else { @@ -109,8 +109,9 @@ auto FollowWaypointController::getAccelerationLimits( } }(); - const auto local_min_acceleration = [&]() { - const auto local_min_acceleration = acceleration - max_deceleration_rate * step_time; + const double local_min_acceleration = [this, speed, acceleration, + time_for_non_acceleration]() -> double { + const double local_min_acceleration = acceleration - max_deceleration_rate * step_time; if (std::abs(speed) < local_epsilon) { // If the speed is equal to 0.0, it should no longer be decreased. return std::max(0.0, local_min_acceleration); @@ -128,8 +129,9 @@ auto FollowWaypointController::getAccelerationLimits( } }(); - const auto local_max_acceleration = [&]() { - const auto local_max_acceleration = acceleration + max_acceleration_rate * step_time; + const double local_max_acceleration = [this, speed, acceleration, time_for_non_acceleration, + local_min_acceleration]() -> double { + const double local_max_acceleration = acceleration + max_acceleration_rate * step_time; if (std::abs(speed - target_speed) < local_epsilon) { // If the speed is equal to target_speed, it should no longer be increased. return std::min(0.0, local_max_acceleration); @@ -160,8 +162,8 @@ auto FollowWaypointController::getAccelerationLimits( const double speed_min = speed + local_min_acceleration * step_time; const double speed_max = speed + local_max_acceleration * step_time; if ( - speed_max < -local_epsilon || speed_max > std::max(max_speed, target_speed) + local_epsilon || - speed_min < -local_epsilon || speed_min > std::max(max_speed, target_speed) + local_epsilon) { + speed_max < -local_epsilon or speed_max > std::max(max_speed, target_speed) + local_epsilon or + speed_min < -local_epsilon or speed_min > std::max(max_speed, target_speed) + local_epsilon) { throw ControllerError( "Incorrect acceleration limits [", local_min_acceleration, ", ", local_max_acceleration, "] for acceleration: ", acceleration, " and speed: ", speed, " -> speed_min: ", speed_min, @@ -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] = getAccelerationLimits(acceleration, speed); return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration); } @@ -194,7 +196,7 @@ auto FollowWaypointController::getPredictedStopStateWithoutConsideringTime( { PredictedState breaking_check{acceleration, speed, 0.0, 0.0}; moveStraight(breaking_check, step_acceleration); - while (!breaking_check.isImmobile(local_epsilon)) { + while (not breaking_check.isImmobile(local_epsilon)) { if (breaking_check.traveled_distance > remaining_distance + predicted_distance_tolerance) { return std::nullopt; } else { @@ -208,8 +210,8 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( const double step_acceleration, const double remaining_time, const double remaining_distance, const double acceleration, const double speed) const -> std::optional { - const auto brakeUntilImmobility = [&](PredictedState & state) -> bool { - while (!state.isImmobile(local_epsilon)) { + const auto brakeUntilImmobility = [this, remaining_time](PredictedState & state) -> bool { + while (not state.isImmobile(local_epsilon)) { if (state.travel_time >= remaining_time) { return false; } else { @@ -230,7 +232,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( if (with_breaking) { // Predict the current (before acceleration zeroing) braking time required for stopping. PredictedState breaking_check = state; - if (!brakeUntilImmobility(breaking_check)) { + if (not brakeUntilImmobility(breaking_check)) { // If complete immobility is not possible - ignore this candidate. return std::nullopt; } else if (std::abs(breaking_check.travel_time - remaining_time) <= step_time) { @@ -262,7 +264,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( if (with_breaking) { // Predict the current (after acceleration zeroing) braking time required for stopping. - if (!brakeUntilImmobility(state)) { + if (not brakeUntilImmobility(state)) { // If complete immobility is not possible - ignore this candidate. return std::nullopt; } else if (std::abs(state.travel_time - remaining_time) <= step_time) { @@ -306,14 +308,14 @@ auto FollowWaypointController::getAcceleration( std::optional best_acceleration = std::nullopt; - for (std::size_t i = 0; i <= number_of_acceleration_candidates; ++i) { + for (std::size_t i = 0UL; i <= number_of_acceleration_candidates; ++i) { const double candidate_acceleration = local_min_acceleration + i * step_acceleration; 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; + (distance_diff >= 0.0 or min_distance_diff < 0.0) && (std::abs(distance_diff) < std::abs(min_distance_diff))) { min_distance_diff = distance_diff; best_acceleration = candidate_acceleration; @@ -363,10 +365,10 @@ auto FollowWaypointController::getAcceleration( /* If last steps, increase accuracy by using analytical calculations. */ - if (with_breaking && remaining_time <= step_time * 4) { + if (with_breaking && remaining_time <= step_time * 4.0) { return getAnalyticalAccelerationForLastSteps( remaining_time, remaining_distance, acceleration, speed); - } else if (remaining_time <= step_time * 3) { + } else if (remaining_time <= step_time * 3.0) { return getAnalyticalAccelerationForLastSteps( remaining_time, remaining_distance, acceleration, speed); } @@ -378,16 +380,19 @@ auto FollowWaypointController::getAcceleration( &min_distance_diff]( double time_diff, double distance_diff) -> bool { const bool same_time = std::abs(time_diff - min_time_diff) < local_epsilon; - const bool time_better = !same_time && (std::abs(time_diff) < std::abs(min_time_diff)); + const bool time_better = not same_time and (std::abs(time_diff) < std::abs(min_time_diff)); const bool distance_better = - distance_diff >= 0 && - (std::abs(distance_diff) < std::abs(min_distance_diff) || min_distance_diff < 0); - return time_better || (same_time && distance_better); + distance_diff >= 0.0 and + (std::abs(distance_diff) < std::abs(min_distance_diff) or min_distance_diff < 0.0); + return time_better or (same_time and distance_better); }; // Create a lambda for candidate selection std::optional best_acceleration = std::nullopt; - const auto considerCandidate = [&](double candidate_acceleration) -> void { + const auto considerCandidate = + [this, remaining_time, remaining_distance, acceleration, speed, isBetterCandidate, + &min_time_diff, &min_distance_diff, + &best_acceleration](const double candidate_acceleration) -> void { if ( const auto predicted_state_opt = getPredictedWaypointArrivalState( candidate_acceleration, remaining_time, remaining_distance, acceleration, speed)) { @@ -420,7 +425,7 @@ auto FollowWaypointController::getAcceleration( if (const double step_acceleration = (local_max_acceleration - local_min_acceleration) / number_of_acceleration_candidates; step_acceleration > local_epsilon) { - for (std::size_t i = 1; i < number_of_acceleration_candidates; ++i) { + for (std::size_t i = 1UL; i < number_of_acceleration_candidates; ++i) { considerCandidate(local_min_acceleration + i * step_acceleration); } } @@ -431,7 +436,7 @@ auto FollowWaypointController::getAcceleration( } else if (min_time_diff >= step_time - step_time_tolerance) { return local_min_acceleration; } else if ( - std::abs(min_time_diff) < step_time && min_distance_diff > predicted_distance_tolerance) { + std::abs(min_time_diff) < step_time and min_distance_diff > predicted_distance_tolerance) { throw ControllerError( "It is not possible to stop with the expected accuracy of the distance" " from the waypoint - the trajectory does not meet the constraints. Check if the " diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 697067fc285..ed0f17a5415 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -170,7 +170,7 @@ auto calculate_distance_and_remaining_time( of 1 step time is allowed. */ if (remaining_time < -step_time) { - throw common::Error( + THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(entity_status.name), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", @@ -210,7 +210,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( rotation angle difference of the z-axis center of the vector must be kept below a certain value. */ - throw common::SimulationError("The followingMode is only supported for position."); + THROW_SIMULATION_ERROR("The followingMode is only supported for position."); } const double dx = target_position.x - position.x; @@ -227,7 +227,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); if (not isfinite_vec3(desired_velocity)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), @@ -261,7 +261,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredAcceleration( follow_waypoint_controller.getAcceleration(remaining_time, distance, acceleration, speed); if (not std::isfinite(desired_acceleration)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), @@ -270,7 +270,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredAcceleration( } return desired_acceleration; } catch (const ControllerError & e) { - throw common::Error( + THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(entity_status.name), " - controller operation problem encountered. ", follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); } @@ -282,7 +282,7 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( -> std::optional { if (polyline_trajectory.shape.vertices.empty()) { - throw common::SimulationError( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: ", "Attempted to access an element of an empty vector"); @@ -378,7 +378,7 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed /*= std::nullopt*/) const + const double matching_distance, const std::optional target_speed) const -> std::optional { /* @@ -396,7 +396,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( using math::geometry::operator+=; if (step_time <= 0.0) { - throw common::SimulationError( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: ", "non-positive step time provided"); @@ -489,7 +489,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( desired_acceleration, remaining_time, distance, acceleration, entity_speed); if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", std::quoted(entity_status.name), @@ -545,7 +545,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { - throw common::SimulationError( + THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, "s (remaining time is ", remaining_time_to_front_waypoint, "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", @@ -567,7 +567,7 @@ auto PolylineTrajectoryFollower::validatedEntityAcceleration() const noexcept(fa { const double acceleration = entity_status.action_status.accel.linear.x; if (not std::isfinite(acceleration)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", @@ -580,7 +580,7 @@ auto PolylineTrajectoryFollower::validatedEntityAcceleration() const noexcept(fa +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); if (not std::isfinite(max_acceleration)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), @@ -593,7 +593,7 @@ auto PolylineTrajectoryFollower::validatedEntityAcceleration() const noexcept(fa -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); if (not std::isfinite(min_acceleration)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), @@ -607,7 +607,7 @@ auto PolylineTrajectoryFollower::validatedEntitySpeed() const noexcept(false) -> const double entity_speed = entity_status.action_status.twist.linear.x; // [m/s] if (not std::isfinite(entity_speed)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", @@ -621,7 +621,7 @@ auto PolylineTrajectoryFollower::validatedEntityPosition() const noexcept(false) { const auto entity_position = entity_status.pose.position; if (not isfinite_vec3(entity_position)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", @@ -634,13 +634,13 @@ auto PolylineTrajectoryFollower::validatedEntityTargetPosition( -> geometry_msgs::msg::Point { if (polyline_trajectory.shape.vertices.empty()) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "attempted to dereference an element of an empty PolylineTrajectory"); } const auto target_position = polyline_trajectory.shape.vertices.front().position.position; if (not isfinite_vec3(target_position)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), @@ -655,7 +655,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredSpeed( const double desired_speed = entity_speed + desired_acceleration * step_time; if (not std::isfinite(desired_speed)) { - throw common::Error( + THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", From 26b87db83c12aaf4643ccc5a621a13e9ff17940e Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 10:57:54 +0100 Subject: [PATCH 25/82] clean up FollowWaypointController --- .../behavior/follow_waypoint_controller.hpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp index d94ad394fb2..4335b25494a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp @@ -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 @@ -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 " @@ -290,8 +289,8 @@ class FollowWaypointController auto areConditionsOfArrivalMet( const double acceleration, const double speed, const double distance) const -> double { - 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 From 179601d63167cc880c9f803cdea85946057644ee Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 12:07:15 +0100 Subject: [PATCH 26/82] noSnakeCase --- .../behavior/polyline_trajectory_follower.cpp | 38 +++++++++---------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index ed0f17a5415..1b04e3b8ca6 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -42,13 +42,13 @@ PolylineTrajectoryFollower::PolylineTrajectoryFollower( } template -auto isfinite_vec3(const T & vec) -> bool +auto isFiniteVector3(const T & vec) -> bool { static_assert(math::geometry::IsLikeVector3>::value); return std::isfinite(vec.x) and std::isfinite(vec.y) and std::isfinite(vec.z); } -auto distance_along_lanelet( +auto distanceAlongLanelet( const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double @@ -69,16 +69,16 @@ auto distance_along_lanelet( return math::geometry::hypot(from, to); }; -auto first_waypoint_with_arrival_time_specified( +auto firstWaypointWithArrivalTimeSpecified( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) -> std::vector::const_iterator { return std::find_if( polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), [](const auto & vertex) { return not std::isnan(vertex.time); }); -}; +} -auto make_updated_pose_orientation( +auto makeUpdatedPoseOrientation( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const geometry_msgs::msg::Vector3 & desired_velocity) -> geometry_msgs::msg::Quaternion { @@ -96,7 +96,7 @@ auto make_updated_pose_orientation( } } -auto calculate_current_velocity( +auto calculateCurrentVelocity( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) -> geometry_msgs::msg::Vector3 { @@ -110,7 +110,7 @@ auto calculate_current_velocity( .z(std::sin(pitch) * speed); } -auto calculate_distance_and_remaining_time( +auto calculateDistanceAndRemainingTime( const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, @@ -130,13 +130,13 @@ auto calculate_distance_and_remaining_time( [&hdmap_utils_ptr, &entity_status, &matching_distance]( const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); - return total_distance + distance_along_lanelet( + return total_distance + distanceAlongLanelet( hdmap_utils_ptr, entity_status, matching_distance, vertex.position.position, next->position.position); }); }; - const auto waypoint_ptr = first_waypoint_with_arrival_time_specified(polyline_trajectory); + const auto waypoint_ptr = firstWaypointWithArrivalTimeSpecified(polyline_trajectory); if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { return std::make_tuple( distance_to_front_waypoint + @@ -226,7 +226,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( .x(std::cos(pitch) * std::cos(yaw) * desired_speed) .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); - if (not isfinite_vec3(desired_velocity)) { + if (not isFiniteVector3(desired_velocity)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -330,8 +330,7 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( using math::geometry::operator*; using math::geometry::operator/; - const auto updated_pose_orientation = - make_updated_pose_orientation(entity_status, desired_velocity); + const auto updated_pose_orientation = makeUpdatedPoseOrientation(entity_status, desired_velocity); const auto updated_pose = geometry_msgs::build() .position(entity_status.pose.position + desired_velocity * step_time) .orientation(updated_pose_orientation); @@ -409,7 +408,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto entity_position = validatedEntityPosition(); const auto target_position = validatedEntityTargetPosition(polyline_trajectory); - const double distance_to_front_waypoint = distance_along_lanelet( + const double distance_to_front_waypoint = distanceAlongLanelet( hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); const double remaining_time_to_front_waypoint = (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + @@ -424,7 +423,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } - const auto [distance, remaining_time] = calculate_distance_and_remaining_time( + const auto [distance, remaining_time] = calculateDistanceAndRemainingTime( hdmap_utils_ptr, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, step_time); @@ -452,9 +451,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the behaviour_parameter. */ - const bool is_breaking_waypoint = - first_waypoint_with_arrival_time_specified(polyline_trajectory) >= - std::prev(polyline_trajectory.shape.vertices.cend()); + const bool is_breaking_waypoint = firstWaypointWithArrivalTimeSpecified(polyline_trajectory) >= + std::prev(polyline_trajectory.shape.vertices.cend()); const auto follow_waypoint_controller = FollowWaypointController( behavior_parameter, step_time, is_breaking_waypoint, std::isinf(remaining_time) ? target_speed : std::nullopt); @@ -477,7 +475,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const double desired_speed = validatedEntityDesiredSpeed(entity_speed, desired_acceleration); const auto desired_velocity = validatedEntityDesiredVelocity( polyline_trajectory, target_position, entity_position, desired_speed); - const auto current_velocity = calculate_current_velocity(entity_status, entity_speed); + const auto current_velocity = calculateCurrentVelocity(entity_status, entity_speed); if ( entity_speed * step_time > distance_to_front_waypoint && @@ -620,7 +618,7 @@ auto PolylineTrajectoryFollower::validatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point { const auto entity_position = entity_status.pose.position; - if (not isfinite_vec3(entity_position)) { + if (not isFiniteVector3(entity_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -639,7 +637,7 @@ auto PolylineTrajectoryFollower::validatedEntityTargetPosition( "attempted to dereference an element of an empty PolylineTrajectory"); } const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - if (not isfinite_vec3(target_position)) { + if (not isFiniteVector3(target_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", From 1c3e1361aa5850fee7863df73e42405223f991be Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 13:50:48 +0100 Subject: [PATCH 27/82] clarify comment --- .../src/behavior/polyline_trajectory_follower.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 1b04e3b8ca6..3e81b0a3c57 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -311,9 +311,9 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( } std::rotate( - std::begin(polyline_trajectory.shape.vertices), - std::begin(polyline_trajectory.shape.vertices) + 1, - std::end(polyline_trajectory.shape.vertices)); + std::cbegin(polyline_trajectory.shape.vertices), + std::cbegin(polyline_trajectory.shape.vertices) + 1UL, + std::cend(polyline_trajectory.shape.vertices)); if (not polyline_trajectory.closed) { polyline_trajectory.shape.vertices.pop_back(); @@ -423,7 +423,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } - const auto [distance, remaining_time] = calculateDistanceAndRemainingTime( + const auto && [distance, remaining_time] = calculateDistanceAndRemainingTime( hdmap_utils_ptr, entity_status, matching_distance, polyline_trajectory, distance_to_front_waypoint, step_time); @@ -478,7 +478,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto current_velocity = calculateCurrentVelocity(entity_status, entity_speed); if ( - entity_speed * step_time > distance_to_front_waypoint && + entity_speed * step_time > distance_to_front_waypoint and math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } @@ -502,7 +502,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( If the nearest waypoint is arrived at in this step without a specific arrival time, it will be considered as achieved */ - if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1UL) { + if (std::isinf(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { /* If the trajectory has only waypoints with unspecified time, the last one is followed using maximum speed including braking - in this case accuracy of arrival is checked From 6f25291bb1fd9ecc6862d159b277fd5a94de81cb Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 14:33:26 +0100 Subject: [PATCH 28/82] mutable iterators in std::rotate --- .../behavior/polyline_trajectory_follower.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 3e81b0a3c57..78e6668787c 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -311,9 +311,9 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( } std::rotate( - std::cbegin(polyline_trajectory.shape.vertices), - std::cbegin(polyline_trajectory.shape.vertices) + 1UL, - std::cend(polyline_trajectory.shape.vertices)); + std::begin(polyline_trajectory.shape.vertices), + std::begin(polyline_trajectory.shape.vertices) + 1, + std::end(polyline_trajectory.shape.vertices)); if (not polyline_trajectory.closed) { polyline_trajectory.shape.vertices.pop_back(); @@ -410,9 +410,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const double distance_to_front_waypoint = distanceAlongLanelet( hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); - const double remaining_time_to_front_waypoint = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - polyline_trajectory.shape.vertices.front().time - entity_status.time; /* This clause is to avoid division-by-zero errors in later clauses with @@ -438,8 +435,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( The controller provides the ability to calculate acceleration using constraints from the behavior_parameter. The value is_breaking_waypoint() determines whether the calculated acceleration takes braking into account - it is true if the nearest waypoint with the - specified time is the last waypoint or the nearest waypoint without the specified time is the - last waypoint. + specified time is the last waypoint or there is no waypoint with a specified time. If an arrival time was specified for any of the remaining waypoints, priority is given to meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can @@ -483,6 +479,10 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } + const double remaining_time_to_front_waypoint = + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + polyline_trajectory.shape.vertices.front().time - entity_status.time; + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, entity_speed); From fe7c1acbef6cfa99fd04998bd1dd579db8305515 Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 22 Nov 2024 15:27:39 +0100 Subject: [PATCH 29/82] move --- .../src/behavior/follow_waypoint_controller.cpp | 6 +++--- .../src/behavior/polyline_trajectory_follower.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp index 9d17848a806..f8e129f74c4 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp @@ -178,7 +178,7 @@ auto FollowWaypointController::clampAcceleration( const double candidate_acceleration, const double acceleration, const double speed) const -> double { - const auto [local_min_acceleration, local_max_acceleration] = + const auto && [local_min_acceleration, local_max_acceleration] = getAccelerationLimits(acceleration, speed); return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration); } @@ -336,7 +336,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] = getAccelerationLimits(acceleration, speed); if ((speed + local_min_acceleration * step_time) * step_time > remaining_distance) { @@ -408,7 +408,7 @@ auto FollowWaypointController::getAcceleration( // Consider the borderline values and precise value of 0 considerCandidate(local_min_acceleration); - if (local_min_acceleration < 0.0 && local_max_acceleration > 0.0) { + if (local_min_acceleration < 0.0 and local_max_acceleration > 0.0) { considerCandidate(0.0); } considerCandidate(local_max_acceleration); diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 78e6668787c..ba72dcaef99 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -143,8 +143,8 @@ auto calculateDistanceAndRemainingTime( total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), std::numeric_limits::infinity()); } - const auto remaining_time = - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + const double remaining_time = + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + waypoint_ptr->time - entity_status.time; /* From 41711f220a0272acaeeb97e289a4174fc4517a13 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 27 Nov 2024 16:12:05 +0100 Subject: [PATCH 30/82] free function separation, condition simplification --- .../include/geometry/vector3/is_finite.hpp | 32 +++++ .../behavior/polyline_trajectory_follower.hpp | 7 + .../traffic_simulator/utils/distance.hpp | 5 + .../behavior/polyline_trajectory_follower.cpp | 131 +++++++----------- .../traffic_simulator/src/utils/distance.cpp | 22 +++ 5 files changed, 115 insertions(+), 82 deletions(-) create mode 100644 common/math/geometry/include/geometry/vector3/is_finite.hpp diff --git a/common/math/geometry/include/geometry/vector3/is_finite.hpp b/common/math/geometry/include/geometry/vector3/is_finite.hpp new file mode 100644 index 00000000000..d3f8cca8a3f --- /dev/null +++ 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 + +namespace math +{ +namespace geometry +{ +template ::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_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 3da3f396e99..8c1d7631706 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -52,6 +52,13 @@ struct PolylineTrajectoryFollower traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const -> std::optional; + auto calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; + auto calculateDistanceAndRemainingTime( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const double distance_to_front_waypoint, + const double step_time) const -> std::tuple; + auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const + noexcept(true) -> geometry_msgs::msg::Quaternion; auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> EntityStatus; auto validatedEntityAcceleration() const noexcept(false) -> double; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 7919a140491..181860c13db 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -112,6 +112,11 @@ auto distanceToStopLine( const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, const lanelet::Id target_stop_line_id, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + +auto distanceAlongLanelet( + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const double matching_distance, + const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index ba72dcaef99..3b8946517c2 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -18,11 +18,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include namespace traffic_simulator @@ -41,46 +43,9 @@ PolylineTrajectoryFollower::PolylineTrajectoryFollower( { } -template -auto isFiniteVector3(const T & vec) -> bool -{ - static_assert(math::geometry::IsLikeVector3>::value); - return std::isfinite(vec.x) and std::isfinite(vec.y) and std::isfinite(vec.z); -} - -auto distanceAlongLanelet( - const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, - const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double -{ - if (const auto from_lanelet_pose = - hdmap_utils_ptr->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); - from_lanelet_pose.has_value()) { - if (const auto to_lanelet_pose = - hdmap_utils_ptr->toLaneletPose(to, entity_status.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, to); -}; - -auto firstWaypointWithArrivalTimeSpecified( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) - -> std::vector::const_iterator -{ - return std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), - [](const auto & vertex) { return not std::isnan(vertex.time); }); -} - -auto makeUpdatedPoseOrientation( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const geometry_msgs::msg::Vector3 & desired_velocity) -> geometry_msgs::msg::Quaternion +auto PolylineTrajectoryFollower::buildUpdatedPoseOrientation( + const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) + -> geometry_msgs::msg::Quaternion { if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { // do not change orientation if there is no designed_velocity vector @@ -96,8 +61,7 @@ auto makeUpdatedPoseOrientation( } } -auto calculateCurrentVelocity( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double speed) +auto PolylineTrajectoryFollower::calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3 { const auto euler_angles = @@ -110,11 +74,10 @@ auto calculateCurrentVelocity( .z(std::sin(pitch) * speed); } -auto calculateDistanceAndRemainingTime( - const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::EntityStatus & entity_status, const double matching_distance, +auto PolylineTrajectoryFollower::calculateDistanceAndRemainingTime( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double distance_to_front_waypoint, const double step_time) -> std::tuple + const double matching_distance, const double distance_to_front_waypoint, + const double step_time) const -> std::tuple { /* Note for anyone working on adding support for followingMode follow @@ -123,20 +86,21 @@ auto calculateDistanceAndRemainingTime( inappropriate. */ const auto total_distance_to = - [&hdmap_utils_ptr, &entity_status, &matching_distance, &polyline_trajectory]( + [this, matching_distance, &polyline_trajectory]( const std::vector::const_iterator last) { return std::accumulate( polyline_trajectory.shape.vertices.cbegin(), last, 0.0, - [&hdmap_utils_ptr, &entity_status, &matching_distance]( - const double total_distance, const auto & vertex) { + [this, matching_distance](const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); return total_distance + distanceAlongLanelet( - hdmap_utils_ptr, entity_status, matching_distance, + hdmap_utils_ptr, entity_status.bounding_box, matching_distance, vertex.position.position, next->position.position); }); }; - const auto waypoint_ptr = firstWaypointWithArrivalTimeSpecified(polyline_trajectory); + const auto waypoint_ptr = std::find_if( + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + [](const auto & vertex) { return std::isfinite(vertex.time); }); if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { return std::make_tuple( distance_to_front_waypoint + @@ -144,7 +108,7 @@ auto calculateDistanceAndRemainingTime( std::numeric_limits::infinity()); } const double remaining_time = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + waypoint_ptr->time - entity_status.time; /* @@ -175,14 +139,13 @@ auto calculateDistanceAndRemainingTime( " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", waypoint_ptr->time, " (in ", - (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + (std::isfinite(polyline_trajectory.base_time) ? "absolute" : "relative"), " simulation time). This may be due to unrealistic conditions of arrival time " "specification compared to vehicle parameters and dynamic constraints."); } else { return std::make_tuple( - distance_to_front_waypoint + total_distance_to(waypoint_ptr), - remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time); + distance_to_front_waypoint + total_distance_to(waypoint_ptr), remaining_time); } } @@ -226,7 +189,7 @@ auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( .x(std::cos(pitch) * std::cos(yaw) * desired_speed) .y(std::cos(pitch) * std::sin(yaw) * desired_speed) .z(std::sin(pitch) * desired_speed); - if (not isFiniteVector3(desired_velocity)) { + if (not math::geometry::isFinite(desired_velocity)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -297,16 +260,16 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( Relative time starts from the start of FollowTrajectoryAction or from the time of reaching the previous "waypoint with arrival time". - Note: not std::isnan(polyline_trajectory.base_time) means + Note: std::isfinite(polyline_trajectory.base_time) means "Timing.domainAbsoluteRelative is relative". - Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) + Note: std::isfinite(polyline_trajectory.shape.vertices.front().time) means "The waypoint about to be popped is the waypoint with the specified arrival time". */ if ( - not std::isnan(polyline_trajectory.base_time) and - not std::isnan(polyline_trajectory.shape.vertices.front().time)) { + std::isfinite(polyline_trajectory.base_time) and + std::isfinite(polyline_trajectory.shape.vertices.front().time)) { polyline_trajectory.base_time = entity_status.time; } @@ -330,7 +293,7 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( using math::geometry::operator*; using math::geometry::operator/; - const auto updated_pose_orientation = makeUpdatedPoseOrientation(entity_status, desired_velocity); + const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); const auto updated_pose = geometry_msgs::build() .position(entity_status.pose.position + desired_velocity * step_time) .orientation(updated_pose_orientation); @@ -361,7 +324,7 @@ auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( .accel(updated_action_status_accel) .linear_jerk(entity_status.action_status.linear_jerk); const auto updated_time = entity_status.time + step_time; - const auto updated_lanelet_pose_valid = false; + constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() .type(entity_status.type) @@ -408,23 +371,22 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const auto entity_position = validatedEntityPosition(); const auto target_position = validatedEntityTargetPosition(polyline_trajectory); - const double distance_to_front_waypoint = distanceAlongLanelet( - hdmap_utils_ptr, entity_status, matching_distance, entity_position, target_position); + const double distance_to_front_waypoint = traffic_simulator::distance::distanceAlongLanelet( + hdmap_utils_ptr, entity_status.bounding_box, matching_distance, entity_position, + target_position); /* This clause is to avoid division-by-zero errors in later clauses with distance_to_front_waypoint as the denominator if the distance miraculously becomes zero. */ - if (math::arithmetic::isDefinitelyLessThan( - distance_to_front_waypoint, std::numeric_limits::epsilon())) { + if (distance_to_front_waypoint <= 0.0) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } const auto && [distance, remaining_time] = calculateDistanceAndRemainingTime( - hdmap_utils_ptr, entity_status, matching_distance, polyline_trajectory, - distance_to_front_waypoint, step_time); + polyline_trajectory, matching_distance, distance_to_front_waypoint, step_time); - if (math::arithmetic::isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { + if (distance <= 0) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } @@ -447,11 +409,14 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the behaviour_parameter. */ - const bool is_breaking_waypoint = firstWaypointWithArrivalTimeSpecified(polyline_trajectory) >= - std::prev(polyline_trajectory.shape.vertices.cend()); + const bool is_breaking_waypoint = + std::find_if( + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + [](const auto & vertex) { return std::isfinite(vertex.time); }) >= + std::prev(polyline_trajectory.shape.vertices.cend()); const auto follow_waypoint_controller = FollowWaypointController( behavior_parameter, step_time, is_breaking_waypoint, - std::isinf(remaining_time) ? target_speed : std::nullopt); + std::isfinite(remaining_time) ? std::nullopt : target_speed); /* The desired acceleration is the acceleration at which the destination @@ -471,22 +436,24 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const double desired_speed = validatedEntityDesiredSpeed(entity_speed, desired_acceleration); const auto desired_velocity = validatedEntityDesiredVelocity( polyline_trajectory, target_position, entity_position, desired_speed); - const auto current_velocity = calculateCurrentVelocity(entity_status, entity_speed); - if ( - entity_speed * step_time > distance_to_front_waypoint and - math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0) { + const auto current_velocity = calculateCurrentVelocity(entity_speed); + + if (const bool target_passed = + entity_speed * step_time > distance_to_front_waypoint and + math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0; + target_passed) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } const double remaining_time_to_front_waypoint = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + polyline_trajectory.shape.vertices.front().time - entity_status.time; const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, remaining_time, distance, acceleration, entity_speed); - if (not std::isinf(remaining_time) and not predicted_state_opt.has_value()) { + if (std::isfinite(remaining_time) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", @@ -497,12 +464,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( follow_waypoint_controller); } - if (std::isnan(remaining_time_to_front_waypoint)) { + if (not std::isfinite(remaining_time_to_front_waypoint)) { /* If the nearest waypoint is arrived at in this step without a specific arrival time, it will be considered as achieved */ - if (std::isinf(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { + if (not std::isfinite(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { /* If the trajectory has only waypoints with unspecified time, the last one is followed using maximum speed including braking - in this case accuracy of arrival is checked @@ -618,7 +585,7 @@ auto PolylineTrajectoryFollower::validatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point { const auto entity_position = entity_status.pose.position; - if (not isFiniteVector3(entity_position)) { + if (not math::geometry::isFinite(entity_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -637,7 +604,7 @@ auto PolylineTrajectoryFollower::validatedEntityTargetPosition( "attempted to dereference an element of an empty PolylineTrajectory"); } const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - if (not isFiniteVector3(target_position)) { + if (not math::geometry::isFinite(target_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 98bac102b88..75f7e1379fc 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -311,5 +312,26 @@ auto distanceToStopLine( return spline.getCollisionPointIn2D(polygon); } } + +auto distanceAlongLanelet( + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const double matching_distance, + const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double +{ + if (const auto from_lanelet_pose = + hdmap_utils_ptr->toLaneletPose(from, bounding_box, false, matching_distance); + from_lanelet_pose.has_value()) { + if (const auto to_lanelet_pose = + hdmap_utils_ptr->toLaneletPose(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, to); +} } // namespace distance } // namespace traffic_simulator From fb6fd7d9e18319910a6ab9af6f9032cbaef79965 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 3 Dec 2024 15:37:15 +0100 Subject: [PATCH 31/82] separation; EntityStatus validator/builder --- simulation/traffic_simulator/CMakeLists.txt | 1 + .../behavior/polyline_trajectory_follower.hpp | 9 +- .../behavior/validated_entity_status.hpp | 62 ++++++ .../behavior/polyline_trajectory_follower.cpp | 182 +++--------------- .../src/behavior/validated_entity_status.cpp | 180 +++++++++++++++++ 5 files changed, 268 insertions(+), 166 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp create mode 100644 simulation/traffic_simulator/src/behavior/validated_entity_status.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index a81dd779d0e..0a9de6d940e 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -31,6 +31,7 @@ ament_auto_add_library(traffic_simulator SHARED src/behavior/follow_waypoint_controller.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp + src/behavior/validated_entity_status.cpp src/color_utils/color_utils.cpp src/data_type/behavior.cpp src/data_type/entity_status.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index 8c1d7631706..f6cd15923ad 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -27,6 +28,7 @@ namespace traffic_simulator { namespace follow_trajectory { + struct PolylineTrajectoryFollower { public: @@ -57,13 +59,6 @@ struct PolylineTrajectoryFollower const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const double distance_to_front_waypoint, const double step_time) const -> std::tuple; - auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const - noexcept(true) -> geometry_msgs::msg::Quaternion; - auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const - noexcept(true) -> EntityStatus; - auto validatedEntityAcceleration() const noexcept(false) -> double; - auto validatedEntitySpeed() const noexcept(false) -> double; - auto validatedEntityPosition() const noexcept(false) -> geometry_msgs::msg::Point; auto validatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) -> geometry_msgs::msg::Point; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp new file mode 100644 index 00000000000..63c9a586066 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp @@ -0,0 +1,62 @@ +// 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__VALIDATED_ENTITY_STATUS_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ + +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +struct ValidatedEntityStatus +{ +public: + ValidatedEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time); + + auto buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + -> traffic_simulator_msgs::msg::EntityStatus; + + const std::string name; + const double time; + const geometry_msgs::msg::Point position; + const double linear_speed; + const double linear_acceleration; + const bool lanelet_pose_valid; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + +private: + auto validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point; + + auto validatedLinearSpeed() const noexcept(false) -> double; + + auto validatedLinearAcceleration(const double step_time) const noexcept(false) -> double; + + auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const + noexcept(true) -> geometry_msgs::msg::Quaternion; + + const traffic_simulator_msgs::msg::EntityStatus entity_status; +}; +} // namespace follow_trajectory +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ \ No newline at end of file diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 3b8946517c2..96bb4b3d9e4 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -43,24 +43,6 @@ PolylineTrajectoryFollower::PolylineTrajectoryFollower( { } -auto PolylineTrajectoryFollower::buildUpdatedPoseOrientation( - const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) - -> geometry_msgs::msg::Quaternion -{ - if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { - // do not change orientation if there is no designed_velocity vector - return entity_status.pose.orientation; - } else { - // if there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); - } -} - auto PolylineTrajectoryFollower::calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3 { @@ -285,59 +267,6 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( return makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); }; -auto PolylineTrajectoryFollower::buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> EntityStatus -{ - using math::geometry::operator+; - using math::geometry::operator-; - using math::geometry::operator*; - using math::geometry::operator/; - - const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); - const auto updated_pose = geometry_msgs::build() - .position(entity_status.pose.position + desired_velocity * step_time) - .orientation(updated_pose_orientation); - - const auto updated_action_status_twist_linear = - geometry_msgs::build() - .x(math::geometry::norm(desired_velocity)) - .y(0.0) - .z(0.0); - const auto updated_action_status_twist_angular = - math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / - step_time; - const auto updated_action_status_twist = geometry_msgs::build() - .linear(updated_action_status_twist_linear) - .angular(updated_action_status_twist_angular); - const auto updated_action_status_accel = - geometry_msgs::build() - .linear( - (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) - .angular( - (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / - step_time); - const auto updated_action_status = - traffic_simulator_msgs::build() - .current_action(entity_status.action_status.current_action) - .twist(updated_action_status_twist) - .accel(updated_action_status_accel) - .linear_jerk(entity_status.action_status.linear_jerk); - const auto updated_time = entity_status.time + step_time; - constexpr bool updated_lanelet_pose_valid = false; - - return traffic_simulator_msgs::build() - .type(entity_status.type) - .subtype(entity_status.subtype) - .time(updated_time) - .name(entity_status.name) - .bounding_box(entity_status.bounding_box) - .action_status(updated_action_status) - .pose(updated_pose) - .lanelet_pose(entity_status.lanelet_pose) - .lanelet_pose_valid(updated_lanelet_pose_valid); -} - auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const @@ -367,13 +296,13 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (polyline_trajectory.shape.vertices.empty()) { return std::nullopt; } - - const auto entity_position = validatedEntityPosition(); + const auto validated_entity_status = + ValidatedEntityStatus(entity_status, behavior_parameter, step_time); const auto target_position = validatedEntityTargetPosition(polyline_trajectory); const double distance_to_front_waypoint = traffic_simulator::distance::distanceAlongLanelet( - hdmap_utils_ptr, entity_status.bounding_box, matching_distance, entity_position, - target_position); + hdmap_utils_ptr, entity_status.bounding_box, matching_distance, + validated_entity_status.position, target_position); /* This clause is to avoid division-by-zero errors in later clauses with @@ -390,9 +319,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); } - const double acceleration = validatedEntityAcceleration(); - const double entity_speed = validatedEntitySpeed(); - /* The controller provides the ability to calculate acceleration using constraints from the behavior_parameter. The value is_breaking_waypoint() determines whether the calculated @@ -431,16 +357,17 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( with linear speed equal to zero and acceleration equal to zero. */ const double desired_acceleration = validatedEntityDesiredAcceleration( - follow_waypoint_controller, polyline_trajectory, remaining_time, distance, acceleration, - entity_speed); - const double desired_speed = validatedEntityDesiredSpeed(entity_speed, desired_acceleration); + follow_waypoint_controller, polyline_trajectory, remaining_time, distance, + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); + const double desired_speed = + validatedEntityDesiredSpeed(validated_entity_status.linear_speed, desired_acceleration); const auto desired_velocity = validatedEntityDesiredVelocity( - polyline_trajectory, target_position, entity_position, desired_speed); + polyline_trajectory, target_position, validated_entity_status.position, desired_speed); - const auto current_velocity = calculateCurrentVelocity(entity_speed); + const auto current_velocity = calculateCurrentVelocity(validated_entity_status.linear_speed); if (const bool target_passed = - entity_speed * step_time > distance_to_front_waypoint and + validated_entity_status.linear_speed * step_time > distance_to_front_waypoint and math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0; target_passed) { return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); @@ -451,7 +378,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( polyline_trajectory.shape.vertices.front().time - entity_status.time; const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, acceleration, entity_speed); + desired_acceleration, remaining_time, distance, validated_entity_status.linear_acceleration, + validated_entity_status.linear_speed); if (std::isfinite(remaining_time) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( @@ -460,8 +388,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( std::quoted(entity_status.name), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, - ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", entity_speed, ". ", - follow_waypoint_controller); + ", distance: ", distance, ", acceleration: ", validated_entity_status.linear_acceleration, + ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); } if (not std::isfinite(remaining_time_to_front_waypoint)) { @@ -475,11 +403,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( maximum speed including braking - in this case accuracy of arrival is checked */ if (follow_waypoint_controller.areConditionsOfArrivalMet( - acceleration, entity_speed, distance_to_front_waypoint)) { + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + distance_to_front_waypoint)) { return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { - return buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); } } else { /* @@ -487,12 +416,12 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( irrelevant */ if (const double this_step_distance = - (entity_speed + desired_acceleration * step_time) * step_time; + (validated_entity_status.linear_speed + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_front_waypoint) { return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { - return buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); } } /* @@ -506,7 +435,8 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( } else if (math::arithmetic::isDefinitelyLessThan( remaining_time_to_front_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( - acceleration, entity_speed, distance_to_front_waypoint)) { + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + distance_to_front_waypoint)) { return discardTheFrontWaypointAndRecurse( polyline_trajectory, matching_distance, target_speed); } else { @@ -518,7 +448,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( " from that waypoint which is greater than the accepted accuracy."); } } else { - return buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); } /* @@ -528,72 +458,6 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( */ } -auto PolylineTrajectoryFollower::validatedEntityAcceleration() const noexcept(false) -> double -{ - const double acceleration = entity_status.action_status.accel.linear.x; - if (not std::isfinite(acceleration)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", - acceleration, ". "); - } - const double max_acceleration = std::min( - acceleration /* [m/s^2] */ + - behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); - - if (not std::isfinite(max_acceleration)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); - } - const double min_acceleration = std::max( - acceleration /* [m/s^2] */ - - behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); - - if (not std::isfinite(min_acceleration)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); - } - return acceleration; -} - -auto PolylineTrajectoryFollower::validatedEntitySpeed() const noexcept(false) -> double -{ - const double entity_speed = entity_status.action_status.twist.linear.x; // [m/s] - - if (not std::isfinite(entity_speed)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", - entity_speed, ". "); - } - return entity_speed; -} - -auto PolylineTrajectoryFollower::validatedEntityPosition() const noexcept(false) - -> geometry_msgs::msg::Point -{ - const auto entity_position = entity_status.pose.position; - if (not math::geometry::isFinite(entity_position)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); - } - return entity_position; -} auto PolylineTrajectoryFollower::validatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) -> geometry_msgs::msg::Point diff --git a/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp new file mode 100644 index 00000000000..11e46a34385 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp @@ -0,0 +1,180 @@ +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +ValidatedEntityStatus::ValidatedEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) +: name(entity_status.name), + time(entity_status.time), + position(validatedPosition()), + linear_speed(validatedLinearSpeed()), + linear_acceleration(validatedLinearAcceleration(step_time)), + lanelet_pose_valid(entity_status.lanelet_pose_valid), + bounding_box(entity_status.bounding_box), + behavior_parameter(behavior_parameter), + entity_status(entity_status) +{ +} + +auto ValidatedEntityStatus::buildUpdatedPoseOrientation( + const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) + -> geometry_msgs::msg::Quaternion +{ + if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { + // do not change orientation if there is no designed_velocity vector + return entity_status.pose.orientation; + } else { + // if there is a designed_velocity vector, set the orientation in the direction of it + const geometry_msgs::msg::Vector3 direction = + geometry_msgs::build() + .x(0.0) + .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) + .z(std::atan2(desired_velocity.y, desired_velocity.x)); + return math::geometry::convertEulerAngleToQuaternion(direction); + } +} + +auto ValidatedEntityStatus::buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + -> traffic_simulator_msgs::msg::EntityStatus +{ + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator*; + using math::geometry::operator/; + + const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); + const auto updated_pose = geometry_msgs::build() + .position(entity_status.pose.position + desired_velocity * step_time) + .orientation(updated_pose_orientation); + + const auto updated_action_status_twist_linear = + geometry_msgs::build() + .x(math::geometry::norm(desired_velocity)) + .y(0.0) + .z(0.0); + const auto updated_action_status_twist_angular = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / + step_time; + const auto updated_action_status_twist = geometry_msgs::build() + .linear(updated_action_status_twist_linear) + .angular(updated_action_status_twist_angular); + const auto updated_action_status_accel = + geometry_msgs::build() + .linear( + (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) + .angular( + (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / + step_time); + const auto updated_action_status = + traffic_simulator_msgs::build() + .current_action(entity_status.action_status.current_action) + .twist(updated_action_status_twist) + .accel(updated_action_status_accel) + .linear_jerk(entity_status.action_status.linear_jerk); + const auto updated_time = entity_status.time + step_time; + constexpr bool updated_lanelet_pose_valid = false; + + return traffic_simulator_msgs::build() + .type(entity_status.type) + .subtype(entity_status.subtype) + .time(updated_time) + .name(entity_status.name) + .bounding_box(entity_status.bounding_box) + .action_status(updated_action_status) + .pose(updated_pose) + .lanelet_pose(entity_status.lanelet_pose) + .lanelet_pose_valid(updated_lanelet_pose_valid); +} + +auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point +{ + const auto entity_position = entity_status.pose.position; + if (not math::geometry::isFinite(entity_position)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", + entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); + } + return entity_position; +} + +auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> double +{ + const double entity_speed = entity_status.action_status.twist.linear.x; + + if (not std::isfinite(entity_speed)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", + entity_speed, ". "); + } else { + return entity_speed; + } +} + +auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) const + noexcept(false) -> double +{ + const auto throwDetailedException = [this](const std::string & text, const double value) { + std::stringstream ss; + ss << "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Entity " + << std::quoted(name) << "'s"; + ss << text << " value is NaN or infinity. The value is " << value << "."; + THROW_SIMULATION_ERROR(ss.str()); + }; + + const double acceleration = entity_status.action_status.accel.linear.x; + const double max_acceleration = std::min( + acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, + +behavior_parameter.dynamic_constraints.max_acceleration); + const double min_acceleration = std::max( + acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time, + -behavior_parameter.dynamic_constraints.max_deceleration); + if (not std::isfinite(acceleration)) { + throwDetailedException("acceleration", acceleration); + } else if (not std::isfinite(max_acceleration)) { + throwDetailedException("maximum acceleration", max_acceleration); + } else if (not std::isfinite(min_acceleration)) { + throwDetailedException("minimum acceleration", min_acceleration); + } + return acceleration; +} + +} // namespace follow_trajectory +} // namespace traffic_simulator \ No newline at end of file From 9abf110034a288e4b56803fe91af6fc83de959cd Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 14:29:33 +0100 Subject: [PATCH 32/82] recursion removal --- .../follow_polyline_trajectory_action.cpp | 15 +- .../follow_polyline_trajectory_action.cpp | 15 +- simulation/traffic_simulator/CMakeLists.txt | 1 + .../behavior/follow_waypoint_controller.hpp | 2 +- .../behavior/polyline_trajectory_follower.hpp | 45 +- .../polyline_trajectory_follower_step.hpp | 76 +++ .../behavior/validated_entity_status.hpp | 3 +- .../behavior/polyline_trajectory_follower.cpp | 443 +----------------- .../polyline_trajectory_follower_step.cpp | 439 +++++++++++++++++ .../src/entity/ego_entity.cpp | 14 +- 10 files changed, 572 insertions(+), 481 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp create mode 100644 simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index de9414d2bf7..7f92854600a 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -74,13 +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::PolylineTrajectoryFollower( - static_cast(*canonicalized_entity_status), - behavior_parameter, hdmap_utils, step_time) - .makeUpdatedEntityStatus( - *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, - getTargetSpeed()); + } else if (const auto entity_status_updated = traffic_simulator::follow_trajectory:: + PolylineTrajectoryFollower::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status), + behavior_parameter, step_time), + hdmap_utils, behavior_parameter, *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()); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 32f4c9fa8c2..20d14cb03a7 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -74,13 +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::PolylineTrajectoryFollower( - static_cast(*canonicalized_entity_status), - behavior_parameter, hdmap_utils, step_time) - .makeUpdatedEntityStatus( - *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, - getTargetSpeed()); + } else if (const auto entity_status_updated = traffic_simulator::follow_trajectory:: + PolylineTrajectoryFollower::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status), + behavior_parameter, step_time), + hdmap_utils, behavior_parameter, *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()); diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 0a9de6d940e..c1319f8162d 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -28,6 +28,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(traffic_simulator SHARED src/api/api.cpp src/behavior/polyline_trajectory_follower.cpp + src/behavior/polyline_trajectory_follower_step.cpp src/behavior/follow_waypoint_controller.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp index 4335b25494a..4f414eaf29f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/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. diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp index f6cd15923ad..a59c294fc07 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -32,48 +33,20 @@ 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_ptr, const double step_time); - // side effects on polyline_trajectory - auto makeUpdatedEntityStatus( + static auto makeUpdatedEntityStatus( + const ValidatedEntityStatus & validated_entity_status, + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional; + const double matching_distance, const std::optional target_speed, + const double step_time) -> std::optional; private: - const traffic_simulator_msgs::msg::EntityStatus entity_status; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - const std::shared_ptr hdmap_utils_ptr; - const double step_time; - // side effects on polyline_trajectory - auto discardTheFrontWaypointAndRecurse( + static auto discardTheFrontWaypoint( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional; - auto calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; - auto calculateDistanceAndRemainingTime( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const double distance_to_front_waypoint, - const double step_time) const -> std::tuple; - auto validatedEntityTargetPosition( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const - noexcept(false) -> geometry_msgs::msg::Point; - auto validatedEntityDesiredAcceleration( - 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 validatedEntityDesiredVelocity( - 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 validatedEntityDesiredSpeed( - const double entity_speed, const double desired_acceleration) const noexcept(false) -> double; + const double current_time) -> void; }; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp new file mode 100644 index 00000000000..081bf8c97fe --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.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__POLYLINE_TRAJECTORY_FOLLOWER_STEP_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_STEP_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +struct PolylineTrajectoryFollowerStep +{ +public: + explicit PolylineTrajectoryFollowerStep( + const ValidatedEntityStatus & validated_entity_status, + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time); + + auto makeUpdatedEntityStatus( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed) const + -> std::optional; + +private: + const ValidatedEntityStatus validated_entity_status; + const std::shared_ptr hdmap_utils_ptr; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + const double step_time; + + auto calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; + auto calculateDistanceAndRemainingTime( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const double distance_to_front_waypoint, + const double step_time) const -> std::tuple; + auto validatedEntityTargetPosition( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const + noexcept(false) -> geometry_msgs::msg::Point; + auto validatedEntityDesiredAcceleration( + 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 validatedEntityDesiredVelocity( + 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 validatedEntityDesiredSpeed( + 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_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp index 63c9a586066..b92993a72da 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp @@ -43,6 +43,7 @@ struct ValidatedEntityStatus const bool lanelet_pose_valid; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + const traffic_simulator_msgs::msg::EntityStatus entity_status; private: auto validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point; @@ -53,8 +54,6 @@ struct ValidatedEntityStatus auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; - - const traffic_simulator_msgs::msg::EntityStatus entity_status; }; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp index 96bb4b3d9e4..cb0031d90d3 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp @@ -32,206 +32,33 @@ namespace traffic_simulator namespace follow_trajectory { -PolylineTrajectoryFollower::PolylineTrajectoryFollower( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, +auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( + const ValidatedEntityStatus & validated_entity_status, + const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils_ptr, const double step_time) -: entity_status(entity_status), - behavior_parameter(behavior_parameter), - hdmap_utils_ptr(hdmap_utils_ptr), - step_time(step_time) -{ -} - -auto PolylineTrajectoryFollower::calculateCurrentVelocity(const double speed) const - -> geometry_msgs::msg::Vector3 -{ - const auto euler_angles = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); - const double pitch = -euler_angles.y; - const double yaw = euler_angles.z; - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); -} - -auto PolylineTrajectoryFollower::calculateDistanceAndRemainingTime( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const double distance_to_front_waypoint, - const double step_time) const -> std::tuple -{ - /* - Note for anyone working on adding support for followingMode follow - to this function (FollowPolylineTrajectoryAction::tick) in the - future: if followingMode is follow, this distance calculation may be - inappropriate. - */ - const auto total_distance_to = - [this, matching_distance, &polyline_trajectory]( - const std::vector::const_iterator last) { - return std::accumulate( - polyline_trajectory.shape.vertices.cbegin(), last, 0.0, - [this, matching_distance](const double total_distance, const auto & vertex) { - const auto next = std::next(&vertex); - return total_distance + distanceAlongLanelet( - hdmap_utils_ptr, entity_status.bounding_box, matching_distance, - vertex.position.position, next->position.position); - }); - }; - - const auto waypoint_ptr = std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), - [](const auto & vertex) { return std::isfinite(vertex.time); }); - if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), - std::numeric_limits::infinity()); - } - const double remaining_time = - (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - waypoint_ptr->time - entity_status.time; - - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - if (remaining_time < -step_time) { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(entity_status.name), - " failed to reach the trajectory waypoint at the specified time. The specified time " - "is ", - waypoint_ptr->time, " (in ", - (std::isfinite(polyline_trajectory.base_time) ? "absolute" : "relative"), - " simulation time). This may be due to unrealistic conditions of arrival time " - "specification compared to vehicle parameters and dynamic constraints."); - - } else { - return std::make_tuple( - distance_to_front_waypoint + total_distance_to(waypoint_ptr), remaining_time); - } -} - -auto PolylineTrajectoryFollower::validatedEntityDesiredVelocity( - 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 -{ - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - - /* - Note: The followingMode in OpenSCENARIO is passed as - variable dynamic_constraints_ignorable. the value of the - variable is `followingMode == position`. - */ - if (not polyline_trajectory.dynamic_constraints_ignorable) { - /* - Note: The vector returned if - dynamic_constraints_ignorable == true ignores parameters - such as the maximum rudder angle of the vehicle entry. In - this clause, such parameters must be respected and the - rotation angle difference of the z-axis center of the - vector must be kept below a certain value. - */ - THROW_SIMULATION_ERROR("The followingMode is only supported for position."); - } - - const double dx = target_position.x - position.x; - const double dy = target_position.y - position.y; - // if entity is on lane use pitch from lanelet, otherwise use pitch on target - const double pitch = - entity_status.lanelet_pose_valid - ? -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y - : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); - const double yaw = std::atan2(dy, dx); // Use yaw on target - - const auto desired_velocity = geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * desired_speed) - .y(std::cos(pitch) * std::sin(yaw) * desired_speed) - .z(std::sin(pitch) * desired_speed); - if (not math::geometry::isFinite(desired_velocity)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", - desired_velocity.y, ", ", desired_velocity.z, "]."); - } - return desired_velocity; -} - -auto PolylineTrajectoryFollower::validatedEntityDesiredAcceleration( - 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 + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed, const double step_time) + -> std::optional { - /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). - - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - It is calculated in such a way as to reach a constant linear speed as quickly as possible, - ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - More precisely, the controller selects acceleration to minimize the distance to the waypoint - that will be reached in a time step defined as the expected arrival time. - In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - with linear speed equal to zero and acceleration equal to zero. - */ - - try { - const double desired_acceleration = - follow_waypoint_controller.getAcceleration(remaining_time, distance, acceleration, speed); - - if (not std::isfinite(desired_acceleration)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s desired acceleration value contains NaN or infinity. The value is ", - desired_acceleration, ". "); + while (not polyline_trajectory.shape.vertices.empty()) { + const auto updated_entity_opt = + PolylineTrajectoryFollowerStep( + validated_entity_status, hdmap_utils_ptr, behavior_parameter, step_time) + .makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); + if (updated_entity_opt.has_value()) { + return updated_entity_opt; + } else { + discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.entity_status.time); } - return desired_acceleration; - } catch (const ControllerError & e) { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(entity_status.name), " - controller operation problem encountered. ", - follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); } + return std::nullopt; } -auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional +auto PolylineTrajectoryFollower::discardTheFrontWaypoint( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) + -> void { - if (polyline_trajectory.shape.vertices.empty()) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: ", - "Attempted to access an element of an empty vector"); - } + assert(not polyline_trajectory.shape.vertices.empty()); /* The OpenSCENARIO standard does not define the behavior when the value of Timing.domainAbsoluteRelative is "relative". The standard only states @@ -252,7 +79,7 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( if ( std::isfinite(polyline_trajectory.base_time) and std::isfinite(polyline_trajectory.shape.vertices.front().time)) { - polyline_trajectory.base_time = entity_status.time; + polyline_trajectory.base_time = current_time; } std::rotate( @@ -263,235 +90,7 @@ auto PolylineTrajectoryFollower::discardTheFrontWaypointAndRecurse( if (not polyline_trajectory.closed) { polyline_trajectory.shape.vertices.pop_back(); } - - return makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); }; -auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional -{ - /* - The following code implements the steering behavior known as "seek". See - "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more - information. - - See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters - */ - - using math::geometry::operator+; - using math::geometry::operator-; - using math::geometry::operator*; - using math::geometry::operator/; - using math::geometry::operator+=; - - if (step_time <= 0.0) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: ", - "non-positive step time provided"); - } - - if (polyline_trajectory.shape.vertices.empty()) { - return std::nullopt; - } - const auto validated_entity_status = - ValidatedEntityStatus(entity_status, behavior_parameter, step_time); - const auto target_position = validatedEntityTargetPosition(polyline_trajectory); - - const double distance_to_front_waypoint = traffic_simulator::distance::distanceAlongLanelet( - hdmap_utils_ptr, entity_status.bounding_box, matching_distance, - validated_entity_status.position, target_position); - - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - if (distance_to_front_waypoint <= 0.0) { - return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); - } - const auto && [distance, remaining_time] = calculateDistanceAndRemainingTime( - polyline_trajectory, matching_distance, distance_to_front_waypoint, step_time); - - if (distance <= 0) { - return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); - } - - /* - The controller provides the ability to calculate acceleration using constraints from the - behavior_parameter. The value is_breaking_waypoint() determines whether the calculated - acceleration takes braking into account - it is true if the nearest waypoint with the - specified time is the last waypoint or there is no waypoint with a specified time. - - If an arrival time was specified for any of the remaining waypoints, priority is given to - meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can - be met. - - However, the controller allows passing target_speed as a speed which is followed by the - controller. target_speed is passed only if no arrival time was specified for any of the - remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is - not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the - behaviour_parameter. - */ - const bool is_breaking_waypoint = - std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), - [](const auto & vertex) { return std::isfinite(vertex.time); }) >= - std::prev(polyline_trajectory.shape.vertices.cend()); - const auto follow_waypoint_controller = FollowWaypointController( - behavior_parameter, step_time, is_breaking_waypoint, - std::isfinite(remaining_time) ? std::nullopt : target_speed); - - /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). - - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - It is calculated in such a way as to reach a constant linear speed as quickly as possible, - ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - More precisely, the controller selects acceleration to minimize the distance to the waypoint - that will be reached in a time step defined as the expected arrival time. - In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - with linear speed equal to zero and acceleration equal to zero. - */ - const double desired_acceleration = validatedEntityDesiredAcceleration( - follow_waypoint_controller, polyline_trajectory, remaining_time, distance, - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); - const double desired_speed = - validatedEntityDesiredSpeed(validated_entity_status.linear_speed, desired_acceleration); - const auto desired_velocity = validatedEntityDesiredVelocity( - polyline_trajectory, target_position, validated_entity_status.position, desired_speed); - - const auto current_velocity = calculateCurrentVelocity(validated_entity_status.linear_speed); - - if (const bool target_passed = - validated_entity_status.linear_speed * step_time > distance_to_front_waypoint and - math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0; - target_passed) { - return discardTheFrontWaypointAndRecurse(polyline_trajectory, matching_distance, target_speed); - } - - const double remaining_time_to_front_waypoint = - (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - polyline_trajectory.shape.vertices.front().time - entity_status.time; - - const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, validated_entity_status.linear_acceleration, - validated_entity_status.linear_speed); - - if (std::isfinite(remaining_time) and not predicted_state_opt.has_value()) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: FollowWaypointController for vehicle ", - std::quoted(entity_status.name), - " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, - ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, - ", distance: ", distance, ", acceleration: ", validated_entity_status.linear_acceleration, - ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); - } - - if (not std::isfinite(remaining_time_to_front_waypoint)) { - /* - If the nearest waypoint is arrived at in this step without a specific arrival time, it will - be considered as achieved - */ - if (not std::isfinite(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { - /* - If the trajectory has only waypoints with unspecified time, the last one is followed using - maximum speed including braking - in this case accuracy of arrival is checked - */ - if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, - distance_to_front_waypoint)) { - return discardTheFrontWaypointAndRecurse( - polyline_trajectory, matching_distance, target_speed); - } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); - } - } else { - /* - If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is - irrelevant - */ - if (const double this_step_distance = - (validated_entity_status.linear_speed + desired_acceleration * step_time) * step_time; - this_step_distance > distance_to_front_waypoint) { - return discardTheFrontWaypointAndRecurse( - polyline_trajectory, matching_distance, target_speed); - } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); - } - } - /* - If there is insufficient time left for the next calculation step. - The value of step_time/2 is compared, as the remaining time is affected by floating point - inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - step is possible (remaining_time_to_front_waypoint is almost zero). - */ - } else if (math::arithmetic::isDefinitelyLessThan( - remaining_time_to_front_waypoint, step_time / 2.0)) { - if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, - distance_to_front_waypoint)) { - return discardTheFrontWaypointAndRecurse( - polyline_trajectory, matching_distance, target_speed); - } else { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, - "s (remaining time is ", remaining_time_to_front_waypoint, - "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", - polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance, - " from that waypoint which is greater than the accepted accuracy."); - } - } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); - } - - /* - Note: If obstacle avoidance is to be implemented, the steering behavior - known by the name "collision avoidance" should be synthesized here into - steering. - */ -} - -auto PolylineTrajectoryFollower::validatedEntityTargetPosition( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) - -> geometry_msgs::msg::Point -{ - if (polyline_trajectory.shape.vertices.empty()) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "attempted to dereference an element of an empty PolylineTrajectory"); - } - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - if (not math::geometry::isFinite(target_position)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s target position coordinate value contains NaN or infinity. The value is [", - target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); - } - return target_position; -} -auto PolylineTrajectoryFollower::validatedEntityDesiredSpeed( - const double entity_speed, const double desired_acceleration) const noexcept(false) -> double -{ - const double desired_speed = entity_speed + desired_acceleration * step_time; - - if (not std::isfinite(desired_speed)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", - desired_speed, ". "); - } - return desired_speed; -} - } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp new file mode 100644 index 00000000000..cd9961abd84 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp @@ -0,0 +1,439 @@ +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +PolylineTrajectoryFollowerStep::PolylineTrajectoryFollowerStep( + const ValidatedEntityStatus & validated_entity_status, + const std::shared_ptr & hdmap_utils_ptr, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) +: validated_entity_status(validated_entity_status), + hdmap_utils_ptr(hdmap_utils_ptr), + behavior_parameter(behavior_parameter), + step_time(step_time) +{ +} + +auto PolylineTrajectoryFollowerStep::calculateCurrentVelocity(const double speed) const + -> geometry_msgs::msg::Vector3 +{ + const auto euler_angles = math::geometry::convertQuaternionToEulerAngle( + validated_entity_status.entity_status.pose.orientation); + const double pitch = -euler_angles.y; + const double yaw = euler_angles.z; + return geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * speed) + .y(std::cos(pitch) * std::sin(yaw) * speed) + .z(std::sin(pitch) * speed); +} + +auto PolylineTrajectoryFollowerStep::calculateDistanceAndRemainingTime( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const double distance_to_front_waypoint, + const double step_time) const -> std::tuple +{ + /* + Note for anyone working on adding support for followingMode follow + to this function (FollowPolylineTrajectoryAction::tick) in the + future: if followingMode is follow, this distance calculation may be + inappropriate. + */ + const auto total_distance_to = + [this, matching_distance, &polyline_trajectory]( + const std::vector::const_iterator last) { + return std::accumulate( + polyline_trajectory.shape.vertices.cbegin(), last, 0.0, + [this, matching_distance](const double total_distance, const auto & vertex) { + const auto next = std::next(&vertex); + return total_distance + distanceAlongLanelet( + hdmap_utils_ptr, validated_entity_status.bounding_box, + matching_distance, vertex.position.position, + next->position.position); + }); + }; + + const auto waypoint_ptr = std::find_if( + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + [](const auto & vertex) { return std::isfinite(vertex.time); }); + if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { + return std::make_tuple( + distance_to_front_waypoint + + total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), + std::numeric_limits::infinity()); + } + const double remaining_time = + (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + waypoint_ptr->time - validated_entity_status.time; + + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ + if (remaining_time < -step_time) { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status.name), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + waypoint_ptr->time, " (in ", + (std::isfinite(polyline_trajectory.base_time) ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + + } else { + return std::make_tuple( + distance_to_front_waypoint + total_distance_to(waypoint_ptr), remaining_time); + } +} + +auto PolylineTrajectoryFollowerStep::validatedEntityDesiredVelocity( + 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 +{ + /* + If not dynamic_constraints_ignorable, the linear distance should cause + problems. + */ + + /* + Note: The followingMode in OpenSCENARIO is passed as + variable dynamic_constraints_ignorable. the value of the + variable is `followingMode == position`. + */ + if (not polyline_trajectory.dynamic_constraints_ignorable) { + /* + Note: The vector returned if + dynamic_constraints_ignorable == true ignores parameters + such as the maximum rudder angle of the vehicle entry. In + this clause, such parameters must be respected and the + rotation angle difference of the z-axis center of the + vector must be kept below a certain value. + */ + THROW_SIMULATION_ERROR("The followingMode is only supported for position."); + } + + const double dx = target_position.x - position.x; + const double dy = target_position.y - position.y; + // if entity is on lane use pitch from lanelet, otherwise use pitch on target + const double pitch = validated_entity_status.lanelet_pose_valid + ? -math::geometry::convertQuaternionToEulerAngle( + validated_entity_status.entity_status.pose.orientation) + .y + : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); + const double yaw = std::atan2(dy, dx); // Use yaw on target + + const auto desired_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * desired_speed) + .y(std::cos(pitch) * std::sin(yaw) * desired_speed) + .z(std::sin(pitch) * desired_speed); + if (not math::geometry::isFinite(desired_velocity)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status.name), + "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", + desired_velocity.y, ", ", desired_velocity.z, "]."); + } + return desired_velocity; +} + +auto PolylineTrajectoryFollowerStep::validatedEntityDesiredAcceleration( + 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 +{ + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). + + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ + + try { + const double desired_acceleration = + follow_waypoint_controller.getAcceleration(remaining_time, distance, acceleration, speed); + + if (not std::isfinite(desired_acceleration)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status.name), + "'s desired acceleration value contains NaN or infinity. The value is ", + desired_acceleration, ". "); + } + return desired_acceleration; + } catch (const ControllerError & e) { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status.name), + " - controller operation problem encountered. ", + follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); + } +} + +auto PolylineTrajectoryFollowerStep::makeUpdatedEntityStatus( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed) const + -> std::optional +{ + /* + The following code implements the steering behavior known as "seek". See + "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + information. + + See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters + */ + + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator*; + using math::geometry::operator/; + using math::geometry::operator+=; + + const auto target_position = validatedEntityTargetPosition(polyline_trajectory); + + const double distance_to_front_waypoint = traffic_simulator::distance::distanceAlongLanelet( + hdmap_utils_ptr, validated_entity_status.bounding_box, matching_distance, + validated_entity_status.position, target_position); + + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_front_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (distance_to_front_waypoint <= 0.0) { + return std::nullopt; + } + const auto && [distance, remaining_time] = calculateDistanceAndRemainingTime( + polyline_trajectory, matching_distance, distance_to_front_waypoint, step_time); + + if (distance <= 0) { + return std::nullopt; + } + + /* + The controller provides the ability to calculate acceleration using constraints from the + behavior_parameter. The value is_breaking_waypoint() determines whether the calculated + acceleration takes braking into account - it is true if the nearest waypoint with the + specified time is the last waypoint or there is no waypoint with a specified time. + + If an arrival time was specified for any of the remaining waypoints, priority is given to + meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + be met. + + However, the controller allows passing target_speed as a speed which is followed by the + controller. target_speed is passed only if no arrival time was specified for any of the + remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + behaviour_parameter. + */ + const bool is_breaking_waypoint = + std::find_if( + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + [](const auto & vertex) { return std::isfinite(vertex.time); }) >= + std::prev(polyline_trajectory.shape.vertices.cend()); + const auto follow_waypoint_controller = FollowWaypointController( + behavior_parameter, step_time, is_breaking_waypoint, + std::isfinite(remaining_time) ? std::nullopt : target_speed); + + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). + + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ + const double desired_acceleration = validatedEntityDesiredAcceleration( + follow_waypoint_controller, polyline_trajectory, remaining_time, distance, + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); + const double desired_speed = + validatedEntityDesiredSpeed(validated_entity_status.linear_speed, desired_acceleration); + const auto desired_velocity = validatedEntityDesiredVelocity( + polyline_trajectory, target_position, validated_entity_status.position, desired_speed); + + const auto current_velocity = calculateCurrentVelocity(validated_entity_status.linear_speed); + + if (const bool target_passed = + validated_entity_status.linear_speed * step_time > distance_to_front_waypoint and + math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0; + target_passed) { + return std::nullopt; + } + + const double remaining_time_to_front_waypoint = + (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + + polyline_trajectory.shape.vertices.front().time - validated_entity_status.time; + + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time, distance, validated_entity_status.linear_acceleration, + validated_entity_status.linear_speed); + + if (std::isfinite(remaining_time) and not predicted_state_opt.has_value()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: FollowWaypointController for vehicle ", + std::quoted(validated_entity_status.name), + " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, + ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, + ", distance: ", distance, ", acceleration: ", validated_entity_status.linear_acceleration, + ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); + } + + if (not std::isfinite(remaining_time_to_front_waypoint)) { + /* + If the nearest waypoint is arrived at in this step without a specific arrival time, it will + be considered as achieved + */ + if (not std::isfinite(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { + /* + If the trajectory has only waypoints with unspecified time, the last one is followed using + maximum speed including braking - in this case accuracy of arrival is checked + */ + if (follow_waypoint_controller.areConditionsOfArrivalMet( + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + distance_to_front_waypoint)) { + return std::nullopt; + } else { + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + } + } else { + /* + If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is + irrelevant + */ + if (const double this_step_distance = + (validated_entity_status.linear_speed + desired_acceleration * step_time) * step_time; + this_step_distance > distance_to_front_waypoint) { + return std::nullopt; + } else { + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + } + } + /* + If there is insufficient time left for the next calculation step. + The value of step_time/2 is compared, as the remaining time is affected by floating point + inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + step is possible (remaining_time_to_front_waypoint is almost zero). + */ + } else if (math::arithmetic::isDefinitelyLessThan( + remaining_time_to_front_waypoint, step_time / 2.0)) { + if (follow_waypoint_controller.areConditionsOfArrivalMet( + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + distance_to_front_waypoint)) { + return std::nullopt; + } else { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status.name), " at time ", + validated_entity_status.time, "s (remaining time is ", remaining_time_to_front_waypoint, + "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", + polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance, + " from that waypoint which is greater than the accepted accuracy."); + } + } else { + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + } + + /* + Note: If obstacle avoidance is to be implemented, the steering behavior + known by the name "collision avoidance" should be synthesized here into + steering. + */ +} + +auto PolylineTrajectoryFollowerStep::validatedEntityTargetPosition( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) + -> geometry_msgs::msg::Point +{ + if (polyline_trajectory.shape.vertices.empty()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "attempted to dereference an element of an empty PolylineTrajectory"); + } + const auto target_position = polyline_trajectory.shape.vertices.front().position.position; + if (not math::geometry::isFinite(target_position)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status.name), + "'s target position coordinate value contains NaN or infinity. The value is [", + target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); + } + return target_position; +} +auto PolylineTrajectoryFollowerStep::validatedEntityDesiredSpeed( + const double entity_speed, const double desired_acceleration) const noexcept(false) -> double +{ + const double desired_speed = entity_speed + desired_acceleration * step_time; + + if (not std::isfinite(desired_speed)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status.name), + "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); + } + return desired_speed; +} + +} // namespace follow_trajectory +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 65290efc847..605d1454ee4 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -148,12 +148,14 @@ void EgoEntity::onUpdate(double current_time, double step_time) EntityBase::onUpdate(current_time, step_time); if (is_controlled_by_simulator_) { if (const auto non_canonicalized_updated_status = - traffic_simulator::follow_trajectory::PolylineTrajectoryFollower( - static_cast(*status_), behavior_parameter_, - hdmap_utils_ptr_, step_time) - .makeUpdatedEntityStatus( - *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_->getTwist().linear.x); + traffic_simulator::follow_trajectory::PolylineTrajectoryFollower::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*status_), behavior_parameter_, + step_time), + hdmap_utils_ptr_, behavior_parameter_, *polyline_trajectory_, + getDefaultMatchingDistanceForLaneletPoseCalculation(), + target_speed_.has_value() ? target_speed_.value() : status_->getTwist().linear.x, + step_time); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); From 22791bf8cb2cd571c86f509ae8b57e3a85e614e4 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 14:32:13 +0100 Subject: [PATCH 33/82] line lint --- .../traffic_simulator/behavior/validated_entity_status.hpp | 2 +- .../traffic_simulator/src/behavior/validated_entity_status.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp index b92993a72da..5d84957724f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp @@ -58,4 +58,4 @@ struct ValidatedEntityStatus } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ \ No newline at end of file +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp index 11e46a34385..e7fffd12a6b 100644 --- a/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp @@ -177,4 +177,4 @@ auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) } } // namespace follow_trajectory -} // namespace traffic_simulator \ No newline at end of file +} // namespace traffic_simulator From d07cca19613c48096baa69247230fde6085675cb Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 15:08:18 +0100 Subject: [PATCH 34/82] restructure polyline_trajectory namespace --- simulation/traffic_simulator/CMakeLists.txt | 8 ++++---- .../follow_waypoint_controller.hpp | 8 ++++---- .../polyline_trajectory_follower.hpp | 12 ++++++------ .../polyline_trajectory_positioner.hpp} | 14 +++++++------- .../validated_entity_status.hpp | 6 +++--- .../follow_waypoint_controller.cpp | 2 +- .../polyline_trajectory_follower.cpp | 4 ++-- .../polyline_trajectory_positioner.cpp} | 18 +++++++++--------- .../validated_entity_status.cpp | 2 +- 9 files changed, 37 insertions(+), 37 deletions(-) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{ => polyline_trajectory_follower}/follow_waypoint_controller.hpp (97%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{ => polyline_trajectory_follower}/polyline_trajectory_follower.hpp (85%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{polyline_trajectory_follower_step.hpp => polyline_trajectory_follower/polyline_trajectory_positioner.hpp} (89%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{ => polyline_trajectory_follower}/validated_entity_status.hpp (87%) rename simulation/traffic_simulator/src/behavior/{ => polyline_trajectory_follower}/follow_waypoint_controller.cpp (99%) rename simulation/traffic_simulator/src/behavior/{ => polyline_trajectory_follower}/polyline_trajectory_follower.cpp (98%) rename simulation/traffic_simulator/src/behavior/{polyline_trajectory_follower_step.cpp => polyline_trajectory_follower/polyline_trajectory_positioner.cpp} (97%) rename simulation/traffic_simulator/src/behavior/{ => polyline_trajectory_follower}/validated_entity_status.cpp (98%) diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index c1319f8162d..08635c3c907 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -27,12 +27,12 @@ ament_auto_find_build_dependencies() ament_auto_add_library(traffic_simulator SHARED src/api/api.cpp - src/behavior/polyline_trajectory_follower.cpp - src/behavior/polyline_trajectory_follower_step.cpp - src/behavior/follow_waypoint_controller.cpp + src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp + src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp + src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp + src/behavior/polyline_trajectory_follower/validated_entity_status.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp - src/behavior/validated_entity_status.cpp src/color_utils/color_utils.cpp src/data_type/behavior.cpp src/data_type/entity_status.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/follow_waypoint_controller.hpp similarity index 97% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/follow_waypoint_controller.hpp index 4f414eaf29f..6b92eb96b95 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/follow_waypoint_controller.hpp @@ -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__POLYLINE_TRAJECTORY_FOLLOWER__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__FOLLOW_WAYPOINT_CONTROLLER_HPP_ #include #include @@ -287,7 +287,7 @@ 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 (not with_breaking or std::abs(speed) < local_epsilon) and std::abs(acceleration) < local_epsilon and distance < finish_distance_tolerance; @@ -296,4 +296,4 @@ class FollowWaypointController } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__FOLLOW_WAYPOINT_CONTROLLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp similarity index 85% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp index a59c294fc07..19767102cce 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp @@ -12,13 +12,13 @@ // 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_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -51,4 +51,4 @@ struct PolylineTrajectoryFollower } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp similarity index 89% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index 081bf8c97fe..1847420b7bd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower_step.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_STEP_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_STEP_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ #include -#include -#include +#include +#include #include #include #include @@ -29,10 +29,10 @@ namespace traffic_simulator namespace follow_trajectory { -struct PolylineTrajectoryFollowerStep +struct PolylineTrajectoryPositioner { public: - explicit PolylineTrajectoryFollowerStep( + explicit PolylineTrajectoryPositioner( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, @@ -73,4 +73,4 @@ struct PolylineTrajectoryFollowerStep } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp similarity index 87% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 5d84957724f..734068c48b5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ #include #include @@ -58,4 +58,4 @@ struct ValidatedEntityStatus } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__VALIDATED_ENTITY_STATUS_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp similarity index 99% rename from simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp rename to simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp index f8e129f74c4..3fe9162893d 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp similarity index 98% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp rename to simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index cb0031d90d3..aa6236920d9 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include @@ -42,7 +42,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( { while (not polyline_trajectory.shape.vertices.empty()) { const auto updated_entity_opt = - PolylineTrajectoryFollowerStep( + PolylineTrajectoryPositioner( validated_entity_status, hdmap_utils_ptr, behavior_parameter, step_time) .makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); if (updated_entity_opt.has_value()) { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp similarity index 97% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp rename to simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index cd9961abd84..63f995717a5 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower_step.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include @@ -32,7 +32,7 @@ namespace traffic_simulator namespace follow_trajectory { -PolylineTrajectoryFollowerStep::PolylineTrajectoryFollowerStep( +PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) @@ -43,7 +43,7 @@ PolylineTrajectoryFollowerStep::PolylineTrajectoryFollowerStep( { } -auto PolylineTrajectoryFollowerStep::calculateCurrentVelocity(const double speed) const +auto PolylineTrajectoryPositioner::calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3 { const auto euler_angles = math::geometry::convertQuaternionToEulerAngle( @@ -56,7 +56,7 @@ auto PolylineTrajectoryFollowerStep::calculateCurrentVelocity(const double speed .z(std::sin(pitch) * speed); } -auto PolylineTrajectoryFollowerStep::calculateDistanceAndRemainingTime( +auto PolylineTrajectoryPositioner::calculateDistanceAndRemainingTime( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const double distance_to_front_waypoint, const double step_time) const -> std::tuple @@ -132,7 +132,7 @@ auto PolylineTrajectoryFollowerStep::calculateDistanceAndRemainingTime( } } -auto PolylineTrajectoryFollowerStep::validatedEntityDesiredVelocity( +auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity( 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 @@ -184,7 +184,7 @@ auto PolylineTrajectoryFollowerStep::validatedEntityDesiredVelocity( return desired_velocity; } -auto PolylineTrajectoryFollowerStep::validatedEntityDesiredAcceleration( +auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration( 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, @@ -224,7 +224,7 @@ auto PolylineTrajectoryFollowerStep::validatedEntityDesiredAcceleration( } } -auto PolylineTrajectoryFollowerStep::makeUpdatedEntityStatus( +auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed) const -> std::optional @@ -400,7 +400,7 @@ auto PolylineTrajectoryFollowerStep::makeUpdatedEntityStatus( */ } -auto PolylineTrajectoryFollowerStep::validatedEntityTargetPosition( +auto PolylineTrajectoryPositioner::validatedEntityTargetPosition( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) -> geometry_msgs::msg::Point { @@ -420,7 +420,7 @@ auto PolylineTrajectoryFollowerStep::validatedEntityTargetPosition( } return target_position; } -auto PolylineTrajectoryFollowerStep::validatedEntityDesiredSpeed( +auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( const double entity_speed, const double desired_acceleration) const noexcept(false) -> double { const double desired_speed = entity_speed + desired_acceleration * step_time; diff --git a/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp similarity index 98% rename from simulation/traffic_simulator/src/behavior/validated_entity_status.cpp rename to simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index e7fffd12a6b..213aa7c560f 100644 --- a/simulation/traffic_simulator/src/behavior/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include namespace traffic_simulator From 78e1f7622b931ed6cf3367561eaddeb0736c0c83 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 16:40:09 +0100 Subject: [PATCH 35/82] cleaner error throwing --- .../syntax/follow_trajectory_action.hpp | 2 +- .../behavior/behavior_plugin_base.hpp | 2 +- .../polyline_trajectory_positioner.hpp | 1 - .../validated_entity_status.hpp | 26 +++++++++ .../traffic_simulator/entity/entity_base.hpp | 2 +- .../polyline_trajectory_follower.cpp | 1 + .../polyline_trajectory_positioner.cpp | 18 +----- .../validated_entity_status.cpp | 58 +++++++++---------- 8 files changed, 61 insertions(+), 49 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp index 4576706aeb9..ccbe6b79609 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include namespace openscenario_interpreter { 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 index 5487a49dc3d..8b1d7823eb6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index 1847420b7bd..6b71c06240a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -49,7 +49,6 @@ struct PolylineTrajectoryPositioner const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const double step_time; - auto calculateCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; auto calculateDistanceAndRemainingTime( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const double distance_to_front_waypoint, diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 734068c48b5..98bf710ba1f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -15,6 +15,7 @@ #ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ #define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ +#include #include #include #include @@ -41,6 +42,7 @@ struct ValidatedEntityStatus const double linear_speed; const double linear_acceleration; const bool lanelet_pose_valid; + const geometry_msgs::msg::Vector3 current_velocity; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const traffic_simulator_msgs::msg::EntityStatus entity_status; @@ -54,7 +56,31 @@ struct ValidatedEntityStatus auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; + + auto buildValidatedCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; + + template < + typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> + auto throwDetailedError(const std::string & variable_name, const T variable) const noexcept(false) + -> void + { + THROW_SIMULATION_ERROR( + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status.name), + ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", + "Values: [", variable.x, ", ", variable.y, ", ", variable.z, "]."); + } + + template , std::nullptr_t> = nullptr> + auto throwDetailedError(const std::string & variable_name, const T variable) const noexcept(false) + -> void + { + THROW_SIMULATION_ERROR( + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status.name), + ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", + "Value: ", variable); + } }; + } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 7ea882e0306..8ed9468f97e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index aa6236920d9..7cfe4316703 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -40,6 +40,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const double matching_distance, const std::optional target_speed, const double step_time) -> std::optional { + assert(step_time > 0.0); while (not polyline_trajectory.shape.vertices.empty()) { const auto updated_entity_opt = PolylineTrajectoryPositioner( diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 63f995717a5..f01c5e9bd95 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -43,19 +43,6 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( { } -auto PolylineTrajectoryPositioner::calculateCurrentVelocity(const double speed) const - -> geometry_msgs::msg::Vector3 -{ - const auto euler_angles = math::geometry::convertQuaternionToEulerAngle( - validated_entity_status.entity_status.pose.orientation); - const double pitch = -euler_angles.y; - const double yaw = euler_angles.z; - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); -} - auto PolylineTrajectoryPositioner::calculateDistanceAndRemainingTime( const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const double distance_to_front_waypoint, @@ -309,11 +296,10 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( const auto desired_velocity = validatedEntityDesiredVelocity( polyline_trajectory, target_position, validated_entity_status.position, desired_speed); - const auto current_velocity = calculateCurrentVelocity(validated_entity_status.linear_speed); - if (const bool target_passed = validated_entity_status.linear_speed * step_time > distance_to_front_waypoint and - math::geometry::innerProduct(desired_velocity, current_velocity) < 0.0; + math::geometry::innerProduct(desired_velocity, validated_entity_status.current_velocity) < + 0.0; target_passed) { return std::nullopt; } diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 213aa7c560f..0ac1827939f 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -41,6 +41,7 @@ ValidatedEntityStatus::ValidatedEntityStatus( linear_speed(validatedLinearSpeed()), linear_acceleration(validatedLinearAcceleration(step_time)), lanelet_pose_valid(entity_status.lanelet_pose_valid), + current_velocity(buildValidatedCurrentVelocity(linear_speed)), bounding_box(entity_status.bounding_box), behavior_parameter(behavior_parameter), entity_status(entity_status) @@ -74,11 +75,8 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( using math::geometry::operator*; using math::geometry::operator/; + const auto updated_time = entity_status.time + step_time; const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); - const auto updated_pose = geometry_msgs::build() - .position(entity_status.pose.position + desired_velocity * step_time) - .orientation(updated_pose_orientation); - const auto updated_action_status_twist_linear = geometry_msgs::build() .x(math::geometry::norm(desired_velocity)) @@ -104,7 +102,10 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .twist(updated_action_status_twist) .accel(updated_action_status_accel) .linear_jerk(entity_status.action_status.linear_jerk); - const auto updated_time = entity_status.time + step_time; + const auto updated_pose = geometry_msgs::build() + .position(entity_status.pose.position + desired_velocity * step_time) + .orientation(updated_pose_orientation); + constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() @@ -123,11 +124,7 @@ auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometr { const auto entity_position = entity_status.pose.position; if (not math::geometry::isFinite(entity_position)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - entity_position.x, ", ", entity_position.y, ", ", entity_position.z, "]."); + throwDetailedError("entity_position", entity_position); } return entity_position; } @@ -137,28 +134,14 @@ auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> doub const double entity_speed = entity_status.action_status.twist.linear.x; if (not std::isfinite(entity_speed)) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", - entity_speed, ". "); - } else { - return entity_speed; + throwDetailedError("entity_speed", entity_speed); } + return entity_speed; } auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) const noexcept(false) -> double { - const auto throwDetailedException = [this](const std::string & text, const double value) { - std::stringstream ss; - ss << "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Entity " - << std::quoted(name) << "'s"; - ss << text << " value is NaN or infinity. The value is " << value << "."; - THROW_SIMULATION_ERROR(ss.str()); - }; - const double acceleration = entity_status.action_status.accel.linear.x; const double max_acceleration = std::min( acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, @@ -167,14 +150,31 @@ auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time, -behavior_parameter.dynamic_constraints.max_deceleration); if (not std::isfinite(acceleration)) { - throwDetailedException("acceleration", acceleration); + throwDetailedError("acceleration", acceleration); } else if (not std::isfinite(max_acceleration)) { - throwDetailedException("maximum acceleration", max_acceleration); + throwDetailedError("maximum acceleration", max_acceleration); } else if (not std::isfinite(min_acceleration)) { - throwDetailedException("minimum acceleration", min_acceleration); + throwDetailedError("minimum acceleration", min_acceleration); } return acceleration; } +auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) const + -> geometry_msgs::msg::Vector3 +{ + const auto euler_angles = + math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); + const double pitch = -euler_angles.y; + const double yaw = euler_angles.z; + const auto entity_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * speed) + .y(std::cos(pitch) * std::sin(yaw) * speed) + .z(std::sin(pitch) * speed); + if (not math::geometry::isFinite(entity_velocity)) { + throwDetailedError("entity_velocity", entity_velocity); + } + return entity_velocity; +} + } // namespace follow_trajectory } // namespace traffic_simulator From 80547c940f409ce6c2dc9758c15115a88f6afe14 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 16:46:37 +0100 Subject: [PATCH 36/82] refer to private untity_status_ field --- .../validated_entity_status.hpp | 10 ++-- .../polyline_trajectory_follower.cpp | 2 +- .../polyline_trajectory_positioner.cpp | 2 +- .../validated_entity_status.cpp | 47 ++++++++++--------- 4 files changed, 31 insertions(+), 30 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 98bf710ba1f..dc24764e0fa 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -36,7 +36,8 @@ struct ValidatedEntityStatus const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const -> traffic_simulator_msgs::msg::EntityStatus; - const std::string name; + const traffic_simulator_msgs::msg::EntityStatus entity_status_; + const std::string & name; const double time; const geometry_msgs::msg::Point position; const double linear_speed; @@ -44,8 +45,7 @@ struct ValidatedEntityStatus const bool lanelet_pose_valid; const geometry_msgs::msg::Vector3 current_velocity; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - const traffic_simulator_msgs::msg::EntityStatus entity_status; + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter; private: auto validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point; @@ -65,7 +65,7 @@ struct ValidatedEntityStatus -> void { THROW_SIMULATION_ERROR( - "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status.name), + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", "Values: [", variable.x, ", ", variable.y, ", ", variable.z, "]."); } @@ -75,7 +75,7 @@ struct ValidatedEntityStatus -> void { THROW_SIMULATION_ERROR( - "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status.name), + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", "Value: ", variable); } diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 7cfe4316703..056713b15db 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -49,7 +49,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (updated_entity_opt.has_value()) { return updated_entity_opt; } else { - discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.entity_status.time); + discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.entity_status_.time); } } return std::nullopt; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index f01c5e9bd95..7f664ec19c8 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -151,7 +151,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity( // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = validated_entity_status.lanelet_pose_valid ? -math::geometry::convertQuaternionToEulerAngle( - validated_entity_status.entity_status.pose.orientation) + validated_entity_status.entity_status_.pose.orientation) .y : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); const double yaw = std::atan2(dy, dx); // Use yaw on target diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 0ac1827939f..839a892ef8a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -35,16 +35,16 @@ namespace follow_trajectory ValidatedEntityStatus::ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) -: name(entity_status.name), - time(entity_status.time), +: entity_status_(entity_status), + name(entity_status_.name), + time(entity_status_.time), position(validatedPosition()), linear_speed(validatedLinearSpeed()), linear_acceleration(validatedLinearAcceleration(step_time)), - lanelet_pose_valid(entity_status.lanelet_pose_valid), + lanelet_pose_valid(entity_status_.lanelet_pose_valid), current_velocity(buildValidatedCurrentVelocity(linear_speed)), - bounding_box(entity_status.bounding_box), - behavior_parameter(behavior_parameter), - entity_status(entity_status) + bounding_box(entity_status_.bounding_box), + behavior_parameter(behavior_parameter) { } @@ -54,7 +54,7 @@ auto ValidatedEntityStatus::buildUpdatedPoseOrientation( { if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { // do not change orientation if there is no designed_velocity vector - return entity_status.pose.orientation; + return entity_status_.pose.orientation; } else { // if there is a designed_velocity vector, set the orientation in the direction of it const geometry_msgs::msg::Vector3 direction = @@ -75,7 +75,7 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( using math::geometry::operator*; using math::geometry::operator/; - const auto updated_time = entity_status.time + step_time; + const auto updated_time = entity_status_.time + step_time; const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); const auto updated_action_status_twist_linear = geometry_msgs::build() @@ -84,7 +84,7 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .z(0.0); const auto updated_action_status_twist_angular = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(entity_status.pose.orientation, updated_pose_orientation)) / + math::geometry::getRotation(entity_status_.pose.orientation, updated_pose_orientation)) / step_time; const auto updated_action_status_twist = geometry_msgs::build() .linear(updated_action_status_twist_linear) @@ -92,37 +92,38 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( const auto updated_action_status_accel = geometry_msgs::build() .linear( - (updated_action_status_twist_linear - entity_status.action_status.twist.linear) / step_time) + (updated_action_status_twist_linear - entity_status_.action_status.twist.linear) / + step_time) .angular( - (updated_action_status_twist_angular - entity_status.action_status.twist.angular) / + (updated_action_status_twist_angular - entity_status_.action_status.twist.angular) / step_time); const auto updated_action_status = traffic_simulator_msgs::build() - .current_action(entity_status.action_status.current_action) + .current_action(entity_status_.action_status.current_action) .twist(updated_action_status_twist) .accel(updated_action_status_accel) - .linear_jerk(entity_status.action_status.linear_jerk); + .linear_jerk(entity_status_.action_status.linear_jerk); const auto updated_pose = geometry_msgs::build() - .position(entity_status.pose.position + desired_velocity * step_time) + .position(entity_status_.pose.position + desired_velocity * step_time) .orientation(updated_pose_orientation); constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() - .type(entity_status.type) - .subtype(entity_status.subtype) + .type(entity_status_.type) + .subtype(entity_status_.subtype) .time(updated_time) - .name(entity_status.name) - .bounding_box(entity_status.bounding_box) + .name(entity_status_.name) + .bounding_box(entity_status_.bounding_box) .action_status(updated_action_status) .pose(updated_pose) - .lanelet_pose(entity_status.lanelet_pose) + .lanelet_pose(entity_status_.lanelet_pose) .lanelet_pose_valid(updated_lanelet_pose_valid); } auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point { - const auto entity_position = entity_status.pose.position; + const auto entity_position = entity_status_.pose.position; if (not math::geometry::isFinite(entity_position)) { throwDetailedError("entity_position", entity_position); } @@ -131,7 +132,7 @@ auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometr auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> double { - const double entity_speed = entity_status.action_status.twist.linear.x; + const double entity_speed = entity_status_.action_status.twist.linear.x; if (not std::isfinite(entity_speed)) { throwDetailedError("entity_speed", entity_speed); @@ -142,7 +143,7 @@ auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> doub auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) const noexcept(false) -> double { - const double acceleration = entity_status.action_status.accel.linear.x; + const double acceleration = entity_status_.action_status.accel.linear.x; const double max_acceleration = std::min( acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, +behavior_parameter.dynamic_constraints.max_acceleration); @@ -163,7 +164,7 @@ auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) co -> geometry_msgs::msg::Vector3 { const auto euler_angles = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation); + math::geometry::convertQuaternionToEulerAngle(entity_status_.pose.orientation); const double pitch = -euler_angles.y; const double yaw = euler_angles.z; const auto entity_velocity = geometry_msgs::build() From de3524c15e818375eb97aab4dc9d540534a456ed Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 4 Dec 2024 17:03:20 +0100 Subject: [PATCH 37/82] special memeber function definitions --- .../validated_entity_status.hpp | 17 ++++++++++++----- .../polyline_trajectory_positioner.cpp | 6 +++--- .../validated_entity_status.cpp | 13 +++++++++---- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index dc24764e0fa..34f9a00f008 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -27,32 +27,39 @@ namespace follow_trajectory struct ValidatedEntityStatus { public: - ValidatedEntityStatus( + explicit ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time); - auto buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const std::string & name; const double time; + const double step_time; const geometry_msgs::msg::Point position; const double linear_speed; const double linear_acceleration; const bool lanelet_pose_valid; const geometry_msgs::msg::Vector3 current_velocity; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + + ValidatedEntityStatus() = delete; + ValidatedEntityStatus(const ValidatedEntityStatus & other); + ValidatedEntityStatus & operator=(const ValidatedEntityStatus & other) = delete; + ValidatedEntityStatus(ValidatedEntityStatus && other) noexcept(true) = delete; + ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) noexcept(true) = delete; + ~ValidatedEntityStatus() = default; private: auto validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point; auto validatedLinearSpeed() const noexcept(false) -> double; - auto validatedLinearAcceleration(const double step_time) const noexcept(false) -> double; + auto validatedLinearAcceleration() const noexcept(false) -> double; auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 7f664ec19c8..e11a0bd3997 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -338,7 +338,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( distance_to_front_waypoint)) { return std::nullopt; } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); } } else { /* @@ -350,7 +350,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( this_step_distance > distance_to_front_waypoint) { return std::nullopt; } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); } } /* @@ -376,7 +376,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( " from that waypoint which is greater than the accepted accuracy."); } } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity, step_time); + return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); } /* diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 839a892ef8a..315f5859bc9 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,9 +38,10 @@ ValidatedEntityStatus::ValidatedEntityStatus( : entity_status_(entity_status), name(entity_status_.name), time(entity_status_.time), + step_time(step_time), position(validatedPosition()), linear_speed(validatedLinearSpeed()), - linear_acceleration(validatedLinearAcceleration(step_time)), + linear_acceleration(validatedLinearAcceleration()), lanelet_pose_valid(entity_status_.lanelet_pose_valid), current_velocity(buildValidatedCurrentVelocity(linear_speed)), bounding_box(entity_status_.bounding_box), @@ -48,6 +49,11 @@ ValidatedEntityStatus::ValidatedEntityStatus( { } +ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) +: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter, other.step_time) +{ +} + auto ValidatedEntityStatus::buildUpdatedPoseOrientation( const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion @@ -67,7 +73,7 @@ auto ValidatedEntityStatus::buildUpdatedPoseOrientation( } auto ValidatedEntityStatus::buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity, const double step_time) const + const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus { using math::geometry::operator+; @@ -140,8 +146,7 @@ auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> doub return entity_speed; } -auto ValidatedEntityStatus::validatedLinearAcceleration(const double step_time) const - noexcept(false) -> double +auto ValidatedEntityStatus::validatedLinearAcceleration() const noexcept(false) -> double { const double acceleration = entity_status_.action_status.accel.linear.x; const double max_acceleration = std::min( From b6ade200f313e6ff62a35df0eb090591132d477f Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Dec 2024 09:39:02 +0100 Subject: [PATCH 38/82] dyn constraints assertions --- .../polyline_trajectory_follower.hpp | 4 ++-- .../validated_entity_status.hpp | 2 +- .../validated_entity_status.cpp | 10 +++++++--- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp index 19767102cce..0f8c8ba7bea 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp @@ -33,7 +33,7 @@ namespace follow_trajectory struct PolylineTrajectoryFollower { public: - // side effects on polyline_trajectory + /// @note side effects on polyline_trajectory static auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, @@ -43,7 +43,7 @@ struct PolylineTrajectoryFollower const double step_time) -> std::optional; private: - // side effects on polyline_trajectory + /// @note side effects on polyline_trajectory static auto discardTheFrontWaypoint( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 34f9a00f008..d68721e5957 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -30,7 +30,7 @@ struct ValidatedEntityStatus explicit ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const double step_time); + const double step_time) noexcept(false); auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 315f5859bc9..1ed3aee98bd 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -34,7 +34,8 @@ namespace follow_trajectory ValidatedEntityStatus::ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) noexcept(false) : entity_status_(entity_status), name(entity_status_.name), time(entity_status_.time), @@ -47,6 +48,10 @@ ValidatedEntityStatus::ValidatedEntityStatus( bounding_box(entity_status_.bounding_box), behavior_parameter(behavior_parameter) { + assert(std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration_rate)); + assert(std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration_rate)); + assert(std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration)); + assert(std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration)); } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) @@ -58,7 +63,7 @@ auto ValidatedEntityStatus::buildUpdatedPoseOrientation( const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion { - if (desired_velocity.y == 0.0 && desired_velocity.x == 0.0 && desired_velocity.z == 0.0) { + if (desired_velocity.x == 0.0 and desired_velocity.y == 0.0 and desired_velocity.z == 0.0) { // do not change orientation if there is no designed_velocity vector return entity_status_.pose.orientation; } else { @@ -112,7 +117,6 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( const auto updated_pose = geometry_msgs::build() .position(entity_status_.pose.position + desired_velocity * step_time) .orientation(updated_pose_orientation); - constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() From a70d31841233387c36d8fa38fbd4d4f8717526f2 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Dec 2024 15:20:14 +0100 Subject: [PATCH 39/82] PolylineTrajectoryPositioner refactor --- .../polyline_trajectory_positioner.hpp | 56 +-- .../validated_entity_status.hpp | 15 +- .../polyline_trajectory_follower.cpp | 5 +- .../polyline_trajectory_positioner.cpp | 325 +++++++++--------- .../validated_entity_status.cpp | 46 ++- .../src/entity/ego_entity.cpp | 11 +- 6 files changed, 256 insertions(+), 202 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index 6b71c06240a..578cbcfa928 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -33,41 +33,47 @@ struct PolylineTrajectoryPositioner { public: explicit PolylineTrajectoryPositioner( - const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, + const ValidatedEntityStatus & validated_entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const std::optional target_speed, const double matching_distance, const double step_time); - auto makeUpdatedEntityStatus( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional; + auto makeUpdatedEntityStatus() const -> std::optional; private: - const ValidatedEntityStatus validated_entity_status; const std::shared_ptr hdmap_utils_ptr; + const ValidatedEntityStatus validated_entity_status; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory; const double step_time; + const double matching_distance; - auto calculateDistanceAndRemainingTime( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const double distance_to_front_waypoint, - const double step_time) const -> std::tuple; - auto validatedEntityTargetPosition( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const - noexcept(false) -> geometry_msgs::msg::Point; - auto validatedEntityDesiredAcceleration( - 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 validatedEntityDesiredVelocity( - 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 validatedEntityDesiredSpeed( - const double entity_speed, const double desired_acceleration) const noexcept(false) -> double; + const std::vector::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_remining_distance; + const double time_to_nearest_waypoint; + const double total_remaining_time; + + const FollowWaypointController follow_waypoint_controller; + + auto totalRemainingDistance( + const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) const -> double; + auto totalRemainingTime() const noexcept(false) -> double; + auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; + auto nearestWaypointWithSpecifiedTimeIterator() const + -> std::vector::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 diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index d68721e5957..5071d80c6e4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -61,15 +61,20 @@ struct ValidatedEntityStatus auto validatedLinearAcceleration() const noexcept(false) -> double; + auto validatedBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> traffic_simulator_msgs::msg::BehaviorParameter; + auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; - auto buildValidatedCurrentVelocity(const double speed) const -> geometry_msgs::msg::Vector3; + auto buildValidatedCurrentVelocity(const double speed) const noexcept(false) + -> geometry_msgs::msg::Vector3; template < typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> - auto throwDetailedError(const std::string & variable_name, const T variable) const noexcept(false) - -> void + auto throwDetailedValidationError(const std::string & variable_name, const T variable) const + noexcept(false) -> void { THROW_SIMULATION_ERROR( "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), @@ -78,8 +83,8 @@ struct ValidatedEntityStatus } template , std::nullptr_t> = nullptr> - auto throwDetailedError(const std::string & variable_name, const T variable) const noexcept(false) - -> void + auto throwDetailedValidationError(const std::string & variable_name, const T variable) const + noexcept(false) -> void { THROW_SIMULATION_ERROR( "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 056713b15db..9e47d549f55 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -44,8 +44,9 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( while (not polyline_trajectory.shape.vertices.empty()) { const auto updated_entity_opt = PolylineTrajectoryPositioner( - validated_entity_status, hdmap_utils_ptr, behavior_parameter, step_time) - .makeUpdatedEntityStatus(polyline_trajectory, matching_distance, target_speed); + hdmap_utils_ptr, validated_entity_status, polyline_trajectory, behavior_parameter, + target_speed, matching_distance, step_time) + .makeUpdatedEntityStatus(); if (updated_entity_opt.has_value()) { return updated_entity_opt; } else { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index e11a0bd3997..5df3f85adb8 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -33,20 +33,69 @@ namespace follow_trajectory { PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( - const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) -: validated_entity_status(validated_entity_status), - hdmap_utils_ptr(hdmap_utils_ptr), + const ValidatedEntityStatus & validated_entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const std::optional target_speed, const double matching_distance, const double step_time) +: hdmap_utils_ptr(hdmap_utils_ptr), + validated_entity_status(validated_entity_status), behavior_parameter(behavior_parameter), - step_time(step_time) + polyline_trajectory(polyline_trajectory), + step_time(step_time), + matching_distance(matching_distance), + nearest_waypoint_with_specified_time_it(nearestWaypointWithSpecifiedTimeIterator()), + nearest_waypoint_position(validatedEntityTargetPosition()), + distance_to_nearest_waypoint(distanceAlongLanelet( + hdmap_utils_ptr, validated_entity_status.bounding_box, matching_distance, + validated_entity_status.entity_status_.pose.position, nearest_waypoint_position)), + total_remining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), + time_to_nearest_waypoint( + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + polyline_trajectory.shape.vertices.front().time - validated_entity_status.time), + total_remaining_time(totalRemainingTime()), + follow_waypoint_controller(FollowWaypointController( + validated_entity_status.behavior_parameter, step_time, + isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint(), + std::isinf(total_remaining_time) ? target_speed : std::nullopt)) { } -auto PolylineTrajectoryPositioner::calculateDistanceAndRemainingTime( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const double distance_to_front_waypoint, - const double step_time) const -> std::tuple +auto PolylineTrajectoryPositioner::nearestWaypointWithSpecifiedTimeIterator() const + -> std::vector::const_iterator +{ + return std::find_if( + polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + [](const auto & vertex) { return not std::isnan(vertex.time); }); +} + +/* + The controller provides the ability to calculate acceleration using constraints from the + behavior_parameter. The value isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() + determines whether the calculated acceleration takes braking into account - it is true + if the nearest waypoint with the specified time is the last waypoint or + there is no waypoint with a specified time. + + If an arrival time was specified for any of the remaining waypoints, priority is given to + meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + be met. + + However, the controller allows passing target_speed as a speed which is followed by the + controller. target_speed is passed only if no arrival time was specified for any of the + remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + behaviour_parameter. +*/ +auto PolylineTrajectoryPositioner::isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const + -> bool +{ + return nearest_waypoint_with_specified_time_it >= + std::prev(polyline_trajectory.shape.vertices.cend()); +} + +auto PolylineTrajectoryPositioner::totalRemainingDistance( + const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) const -> double { /* Note for anyone working on adding support for followingMode follow @@ -55,11 +104,12 @@ auto PolylineTrajectoryPositioner::calculateDistanceAndRemainingTime( inappropriate. */ const auto total_distance_to = - [this, matching_distance, &polyline_trajectory]( + [this, &matching_distance, &hdmap_utils_ptr]( const std::vector::const_iterator last) { return std::accumulate( polyline_trajectory.shape.vertices.cbegin(), last, 0.0, - [this, matching_distance](const double total_distance, const auto & vertex) { + [this, &matching_distance, &hdmap_utils_ptr]( + const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); return total_distance + distanceAlongLanelet( hdmap_utils_ptr, validated_entity_status.bounding_box, @@ -68,61 +118,63 @@ auto PolylineTrajectoryPositioner::calculateDistanceAndRemainingTime( }); }; - const auto waypoint_ptr = std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), - [](const auto & vertex) { return std::isfinite(vertex.time); }); - if (waypoint_ptr == std::cend(polyline_trajectory.shape.vertices)) { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1), - std::numeric_limits::infinity()); + if (nearest_waypoint_with_specified_time_it == std::cend(polyline_trajectory.shape.vertices)) { + return distance_to_nearest_waypoint + + total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1); + } else { + return distance_to_nearest_waypoint + + total_distance_to(nearest_waypoint_with_specified_time_it); } - const double remaining_time = - (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - waypoint_ptr->time - validated_entity_status.time; - - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - if (remaining_time < -step_time) { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name), - " failed to reach the trajectory waypoint at the specified time. The specified time " - "is ", - waypoint_ptr->time, " (in ", - (std::isfinite(polyline_trajectory.base_time) ? "absolute" : "relative"), - " simulation time). This may be due to unrealistic conditions of arrival time " - "specification compared to vehicle parameters and dynamic constraints."); +} +auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> double +{ + if (nearest_waypoint_with_specified_time_it == std::cend(polyline_trajectory.shape.vertices)) { + return std::numeric_limits::infinity(); } else { - return std::make_tuple( - distance_to_front_waypoint + total_distance_to(waypoint_ptr), remaining_time); + const double remaining_time = + (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + + nearest_waypoint_with_specified_time_it->time - validated_entity_status.time; + + /* + The condition below should ideally be remaining_time < 0. + + The simulator runs at a constant frame rate, so the step time is + 1/FPS. If the simulation time is an accumulation of step times + expressed as rational numbers, times that are integer multiples + of the frame rate will always be exact integer seconds. + Therefore, the timing of remaining_time == 0 always exists, and + the velocity planning of this member function (tick) aims to + reach the waypoint exactly at that timing. So the ideal timeout + condition is remaining_time < 0. + + But actually the step time is expressed as a float and the + simulation time is its accumulation. As a result, it is not + guaranteed that there will be times when the simulation time is + exactly zero. For example, remaining_time == -0.00006 and it was + judged to be out of time. + + For the above reasons, the condition is remaining_time < + -step_time. In other words, the conditions are such that a delay + of 1 step time is allowed. + */ + if (remaining_time < -step_time) { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status.name), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + nearest_waypoint_with_specified_time_it->time, " (in ", + (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + } else { + return remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time; + } } } -auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity( - 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 PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double desired_speed) const + noexcept(false) -> geometry_msgs::msg::Vector3 { /* If not dynamic_constraints_ignorable, the linear distance should cause @@ -146,14 +198,16 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity( THROW_SIMULATION_ERROR("The followingMode is only supported for position."); } - const double dx = target_position.x - position.x; - const double dy = target_position.y - position.y; + const double dx = nearest_waypoint_position.x - validated_entity_status.position.x; + const double dy = nearest_waypoint_position.y - validated_entity_status.position.y; // if entity is on lane use pitch from lanelet, otherwise use pitch on target - const double pitch = validated_entity_status.lanelet_pose_valid - ? -math::geometry::convertQuaternionToEulerAngle( - validated_entity_status.entity_status_.pose.orientation) - .y - : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); + const double pitch = + validated_entity_status.lanelet_pose_valid + ? -math::geometry::convertQuaternionToEulerAngle( + validated_entity_status.entity_status_.pose.orientation) + .y + : std::atan2( + nearest_waypoint_position.z - validated_entity_status.position.z, std::hypot(dy, dx)); const double yaw = std::atan2(dy, dx); // Use yaw on target const auto desired_velocity = geometry_msgs::build() @@ -171,11 +225,8 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity( return desired_velocity; } -auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration( - 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 PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const noexcept(false) + -> double { /* The desired acceleration is the acceleration at which the destination @@ -191,8 +242,9 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration( */ try { - const double desired_acceleration = - follow_waypoint_controller.getAcceleration(remaining_time, distance, acceleration, speed); + const double desired_acceleration = follow_waypoint_controller.getAcceleration( + total_remaining_time, total_remining_distance, validated_entity_status.linear_acceleration, + validated_entity_status.linear_speed); if (not std::isfinite(desired_acceleration)) { THROW_SIMULATION_ERROR( @@ -211,10 +263,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration( } } -auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed) const - -> std::optional +auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optional { /* The following code implements the steering behavior known as "seek". See @@ -230,52 +279,19 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( using math::geometry::operator/; using math::geometry::operator+=; - const auto target_position = validatedEntityTargetPosition(polyline_trajectory); - - const double distance_to_front_waypoint = traffic_simulator::distance::distanceAlongLanelet( - hdmap_utils_ptr, validated_entity_status.bounding_box, matching_distance, - validated_entity_status.position, target_position); - /* This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance + distance_to_nearest_waypoint as the denominator if the distance miraculously becomes zero. */ - if (distance_to_front_waypoint <= 0.0) { + if (distance_to_nearest_waypoint <= 0.0) { return std::nullopt; } - const auto && [distance, remaining_time] = calculateDistanceAndRemainingTime( - polyline_trajectory, matching_distance, distance_to_front_waypoint, step_time); - if (distance <= 0) { + if (total_remining_distance <= 0) { return std::nullopt; } - /* - The controller provides the ability to calculate acceleration using constraints from the - behavior_parameter. The value is_breaking_waypoint() determines whether the calculated - acceleration takes braking into account - it is true if the nearest waypoint with the - specified time is the last waypoint or there is no waypoint with a specified time. - - If an arrival time was specified for any of the remaining waypoints, priority is given to - meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can - be met. - - However, the controller allows passing target_speed as a speed which is followed by the - controller. target_speed is passed only if no arrival time was specified for any of the - remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is - not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the - behaviour_parameter. - */ - const bool is_breaking_waypoint = - std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), - [](const auto & vertex) { return std::isfinite(vertex.time); }) >= - std::prev(polyline_trajectory.shape.vertices.cend()); - const auto follow_waypoint_controller = FollowWaypointController( - behavior_parameter, step_time, is_breaking_waypoint, - std::isfinite(remaining_time) ? std::nullopt : target_speed); - /* The desired acceleration is the acceleration at which the destination can be reached exactly at the specified time (= time remaining at zero). @@ -288,54 +304,34 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, with linear speed equal to zero and acceleration equal to zero. */ - const double desired_acceleration = validatedEntityDesiredAcceleration( - follow_waypoint_controller, polyline_trajectory, remaining_time, distance, - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); - const double desired_speed = - validatedEntityDesiredSpeed(validated_entity_status.linear_speed, desired_acceleration); - const auto desired_velocity = validatedEntityDesiredVelocity( - polyline_trajectory, target_position, validated_entity_status.position, desired_speed); + const double desired_acceleration = validatedEntityDesiredAcceleration(); + const double desired_speed = validatedEntityDesiredSpeed(desired_acceleration); + const auto desired_velocity = validatedEntityDesiredVelocity(desired_speed); if (const bool target_passed = - validated_entity_status.linear_speed * step_time > distance_to_front_waypoint and + validated_entity_status.linear_speed * step_time > distance_to_nearest_waypoint and math::geometry::innerProduct(desired_velocity, validated_entity_status.current_velocity) < 0.0; target_passed) { return std::nullopt; } - const double remaining_time_to_front_waypoint = - (std::isfinite(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - polyline_trajectory.shape.vertices.front().time - validated_entity_status.time; - - const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, distance, validated_entity_status.linear_acceleration, - validated_entity_status.linear_speed); - - if (std::isfinite(remaining_time) and not predicted_state_opt.has_value()) { - THROW_SIMULATION_ERROR( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: FollowWaypointController for vehicle ", - std::quoted(validated_entity_status.name), - " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, - ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, - ", distance: ", distance, ", acceleration: ", validated_entity_status.linear_acceleration, - ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); - } - - if (not std::isfinite(remaining_time_to_front_waypoint)) { + validatePredictedState(desired_acceleration); + if (not std::isfinite(time_to_nearest_waypoint)) { /* If the nearest waypoint is arrived at in this step without a specific arrival time, it will be considered as achieved */ - if (not std::isfinite(remaining_time) and polyline_trajectory.shape.vertices.size() == 1UL) { + if ( + not std::isfinite(total_remaining_time) and + polyline_trajectory.shape.vertices.size() == 1UL) { /* If the trajectory has only waypoints with unspecified time, the last one is followed using maximum speed including braking - in this case accuracy of arrival is checked */ if (follow_waypoint_controller.areConditionsOfArrivalMet( validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, - distance_to_front_waypoint)) { + distance_to_nearest_waypoint)) { return std::nullopt; } else { return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); @@ -347,7 +343,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( */ if (const double this_step_distance = (validated_entity_status.linear_speed + desired_acceleration * step_time) * step_time; - this_step_distance > distance_to_front_waypoint) { + this_step_distance > distance_to_nearest_waypoint) { return std::nullopt; } else { return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); @@ -359,20 +355,20 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - step is possible (remaining_time_to_front_waypoint is almost zero). + step is possible (time_to_nearest_waypoint is almost zero). */ - } else if (math::arithmetic::isDefinitelyLessThan( - remaining_time_to_front_waypoint, step_time / 2.0)) { + } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, - distance_to_front_waypoint)) { + distance_to_nearest_waypoint)) { return std::nullopt; } else { THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(validated_entity_status.name), " at time ", - validated_entity_status.time, "s (remaining time is ", remaining_time_to_front_waypoint, + validated_entity_status.time, "s (remaining time is ", time_to_nearest_waypoint, "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", - polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance, + polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", + distance_to_nearest_waypoint, " from that waypoint which is greater than the accepted accuracy."); } } else { @@ -386,8 +382,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus( */ } -auto PolylineTrajectoryPositioner::validatedEntityTargetPosition( - const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const noexcept(false) +auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point { if (polyline_trajectory.shape.vertices.empty()) { @@ -407,9 +402,10 @@ auto PolylineTrajectoryPositioner::validatedEntityTargetPosition( return target_position; } auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( - const double entity_speed, const double desired_acceleration) const noexcept(false) -> double + const double desired_acceleration) const noexcept(false) -> double { - const double desired_speed = entity_speed + desired_acceleration * step_time; + const double desired_speed = + validated_entity_status.linear_speed + desired_acceleration * step_time; if (not std::isfinite(desired_speed)) { THROW_SIMULATION_ERROR( @@ -421,5 +417,24 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( return desired_speed; } +auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_acceleration) const + noexcept(false) -> void +{ + const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, total_remaining_time, total_remining_distance, + validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); + if (not std::isinf(total_remaining_time) and not predicted_state_opt.has_value()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: FollowWaypointController for vehicle ", + std::quoted(validated_entity_status.name), + " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, + ", total_remaining_time: ", total_remaining_time, + ", total_remining_distance: ", total_remining_distance, + ", acceleration: ", validated_entity_status.linear_acceleration, + ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); + } +} + } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 1ed3aee98bd..f57c3b3c683 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -46,12 +46,8 @@ ValidatedEntityStatus::ValidatedEntityStatus( lanelet_pose_valid(entity_status_.lanelet_pose_valid), current_velocity(buildValidatedCurrentVelocity(linear_speed)), bounding_box(entity_status_.bounding_box), - behavior_parameter(behavior_parameter) + behavior_parameter(validatedBehaviorParameter(behavior_parameter)) { - assert(std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration_rate)); - assert(std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration_rate)); - assert(std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration)); - assert(std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration)); } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) @@ -135,7 +131,7 @@ auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometr { const auto entity_position = entity_status_.pose.position; if (not math::geometry::isFinite(entity_position)) { - throwDetailedError("entity_position", entity_position); + throwDetailedValidationError("entity_position", entity_position); } return entity_position; } @@ -145,7 +141,7 @@ auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> doub const double entity_speed = entity_status_.action_status.twist.linear.x; if (not std::isfinite(entity_speed)) { - throwDetailedError("entity_speed", entity_speed); + throwDetailedValidationError("entity_speed", entity_speed); } return entity_speed; } @@ -160,16 +156,16 @@ auto ValidatedEntityStatus::validatedLinearAcceleration() const noexcept(false) acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time, -behavior_parameter.dynamic_constraints.max_deceleration); if (not std::isfinite(acceleration)) { - throwDetailedError("acceleration", acceleration); + throwDetailedValidationError("acceleration", acceleration); } else if (not std::isfinite(max_acceleration)) { - throwDetailedError("maximum acceleration", max_acceleration); + throwDetailedValidationError("maximum acceleration", max_acceleration); } else if (not std::isfinite(min_acceleration)) { - throwDetailedError("minimum acceleration", min_acceleration); + throwDetailedValidationError("minimum acceleration", min_acceleration); } return acceleration; } -auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) const +auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) const noexcept(false) -> geometry_msgs::msg::Vector3 { const auto euler_angles = @@ -181,10 +177,36 @@ auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) co .y(std::cos(pitch) * std::sin(yaw) * speed) .z(std::sin(pitch) * speed); if (not math::geometry::isFinite(entity_velocity)) { - throwDetailedError("entity_velocity", entity_velocity); + throwDetailedValidationError("entity_velocity", entity_velocity); } return entity_velocity; } +auto ValidatedEntityStatus::validatedBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> traffic_simulator_msgs::msg::BehaviorParameter +{ + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration_rate)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration_rate)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + return behavior_parameter; +} } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 605d1454ee4..bb855b3948a 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -145,6 +145,13 @@ void EgoEntity::updateFieldOperatorApplication() const void EgoEntity::onUpdate(double current_time, double step_time) { + const auto getTargetSpeed = [this]() -> double { + if (target_speed_.has_value()) { + return target_speed_.value(); + } else { + return status_->getTwist().linear.x; + } + }; EntityBase::onUpdate(current_time, step_time); if (is_controlled_by_simulator_) { if (const auto non_canonicalized_updated_status = @@ -153,9 +160,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) static_cast(*status_), behavior_parameter_, step_time), hdmap_utils_ptr_, behavior_parameter_, *polyline_trajectory_, - getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_.has_value() ? target_speed_.value() : status_->getTwist().linear.x, - step_time); + getDefaultMatchingDistanceForLaneletPoseCalculation(), getTargetSpeed(), step_time); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); From f9bc6474a49d0c743b09a0a127f71767396c07ac Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Dec 2024 15:43:28 +0100 Subject: [PATCH 40/82] spell check --- .../polyline_trajectory_positioner.hpp | 2 +- .../follow_waypoint_controller.cpp | 2 +- .../polyline_trajectory_positioner.cpp | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index 578cbcfa928..bbe5b9f2a9a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -54,7 +54,7 @@ struct PolylineTrajectoryPositioner nearest_waypoint_with_specified_time_it; const geometry_msgs::msg::Point nearest_waypoint_position; const double distance_to_nearest_waypoint; - const double total_remining_distance; + const double total_remaining_distance; const double time_to_nearest_waypoint; const double total_remaining_time; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp index 3fe9162893d..aa5b34be669 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp @@ -275,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::max() * const_speed_value) { + const_speed_distance == std::numeric_limits::infinity()) { throw ControllerError( "Exceeded the range of the variable type (", const_speed_distance, "/", const_speed_value, ") - the value is too large. Probably the distance", diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 5df3f85adb8..d71772117a3 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -49,7 +49,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( distance_to_nearest_waypoint(distanceAlongLanelet( hdmap_utils_ptr, validated_entity_status.bounding_box, matching_distance, validated_entity_status.entity_status_.pose.position, nearest_waypoint_position)), - total_remining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), + total_remaining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), time_to_nearest_waypoint( (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + polyline_trajectory.shape.vertices.front().time - validated_entity_status.time), @@ -243,7 +243,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no try { const double desired_acceleration = follow_waypoint_controller.getAcceleration( - total_remaining_time, total_remining_distance, validated_entity_status.linear_acceleration, + total_remaining_time, total_remaining_distance, validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); if (not std::isfinite(desired_acceleration)) { @@ -288,7 +288,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio return std::nullopt; } - if (total_remining_distance <= 0) { + if (total_remaining_distance <= 0) { return std::nullopt; } @@ -421,7 +421,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a noexcept(false) -> void { const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, total_remaining_time, total_remining_distance, + desired_acceleration, total_remaining_time, total_remaining_distance, validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); if (not std::isinf(total_remaining_time) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( @@ -430,7 +430,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a std::quoted(validated_entity_status.name), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", total_remaining_time: ", total_remaining_time, - ", total_remining_distance: ", total_remining_distance, + ", total_remaining_distance: ", total_remaining_distance, ", acceleration: ", validated_entity_status.linear_acceleration, ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); } From 1075845b4ed8c99b7c29773f64905975b0c7e475 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 17:31:49 +0100 Subject: [PATCH 41/82] Fix ValidatedEntityStatus member initialization order Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 8 ++++---- .../validated_entity_status.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 5071d80c6e4..0b3bd3ace08 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -35,17 +35,17 @@ struct ValidatedEntityStatus auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus; + const double step_time; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const std::string & name; const double time; - const double step_time; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box; + const bool lanelet_pose_valid; const geometry_msgs::msg::Point position; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const double linear_speed; const double linear_acceleration; - const bool lanelet_pose_valid; const geometry_msgs::msg::Vector3 current_velocity; - const traffic_simulator_msgs::msg::BoundingBox & bounding_box; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; ValidatedEntityStatus() = delete; ValidatedEntityStatus(const ValidatedEntityStatus & other); diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index f57c3b3c683..73ae6d49ce9 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -36,17 +36,17 @@ ValidatedEntityStatus::ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) noexcept(false) -: entity_status_(entity_status), +: step_time(step_time), + entity_status_(entity_status), name(entity_status_.name), time(entity_status_.time), - step_time(step_time), + bounding_box(entity_status_.bounding_box), + lanelet_pose_valid(entity_status_.lanelet_pose_valid), position(validatedPosition()), + behavior_parameter(validatedBehaviorParameter(behavior_parameter)), linear_speed(validatedLinearSpeed()), linear_acceleration(validatedLinearAcceleration()), - lanelet_pose_valid(entity_status_.lanelet_pose_valid), - current_velocity(buildValidatedCurrentVelocity(linear_speed)), - bounding_box(entity_status_.bounding_box), - behavior_parameter(validatedBehaviorParameter(behavior_parameter)) + current_velocity(buildValidatedCurrentVelocity(linear_speed)) { } From d72ab82b01012f0f12ab55276400918ed48f43bf Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:02:43 +0100 Subject: [PATCH 42/82] Refactor ValidatedEntityStatus to use arguments instead of class members This was the source of a bug. In initialization it was hidden which functions used which members and wrong initialization order was used Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 15 ++++++--- .../validated_entity_status.cpp | 32 ++++++++++--------- 2 files changed, 27 insertions(+), 20 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 0b3bd3ace08..1eafd0260c3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -55,11 +55,15 @@ struct ValidatedEntityStatus ~ValidatedEntityStatus() = default; private: - auto validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point; + auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) + -> geometry_msgs::msg::Point; - auto validatedLinearSpeed() const noexcept(false) -> double; + auto validatedLinearSpeed(const double entity_speed) const noexcept(false) -> double; - auto validatedLinearAcceleration() const noexcept(false) -> double; + auto validatedLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> double; auto validatedBehaviorParameter( const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) @@ -68,8 +72,9 @@ struct ValidatedEntityStatus auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; - auto buildValidatedCurrentVelocity(const double speed) const noexcept(false) - -> geometry_msgs::msg::Vector3; + auto buildValidatedCurrentVelocity( + const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const + noexcept(false) -> geometry_msgs::msg::Vector3; template < typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 73ae6d49ce9..a6762bb7894 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -42,11 +42,12 @@ ValidatedEntityStatus::ValidatedEntityStatus( time(entity_status_.time), bounding_box(entity_status_.bounding_box), lanelet_pose_valid(entity_status_.lanelet_pose_valid), - position(validatedPosition()), + position(validatedPosition(entity_status_.pose.position)), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), - linear_speed(validatedLinearSpeed()), - linear_acceleration(validatedLinearAcceleration()), - current_velocity(buildValidatedCurrentVelocity(linear_speed)) + linear_speed(validatedLinearSpeed(entity_status_.action_status.twist.linear.x)), + linear_acceleration(validatedLinearAcceleration( + entity_status_.action_status.accel.linear.x, behavior_parameter, step_time)), + current_velocity(buildValidatedCurrentVelocity(linear_speed, entity_status_.pose.orientation)) { } @@ -127,28 +128,29 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .lanelet_pose_valid(updated_lanelet_pose_valid); } -auto ValidatedEntityStatus::validatedPosition() const noexcept(false) -> geometry_msgs::msg::Point +auto ValidatedEntityStatus::validatedPosition(const geometry_msgs::msg::Point & entity_position) + const noexcept(false) -> geometry_msgs::msg::Point { - const auto entity_position = entity_status_.pose.position; if (not math::geometry::isFinite(entity_position)) { throwDetailedValidationError("entity_position", entity_position); } return entity_position; } -auto ValidatedEntityStatus::validatedLinearSpeed() const noexcept(false) -> double +auto ValidatedEntityStatus::validatedLinearSpeed(const double entity_speed) const noexcept(false) + -> double { - const double entity_speed = entity_status_.action_status.twist.linear.x; - if (not std::isfinite(entity_speed)) { throwDetailedValidationError("entity_speed", entity_speed); } return entity_speed; } -auto ValidatedEntityStatus::validatedLinearAcceleration() const noexcept(false) -> double +auto ValidatedEntityStatus::validatedLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> double { - const double acceleration = entity_status_.action_status.accel.linear.x; const double max_acceleration = std::min( acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, +behavior_parameter.dynamic_constraints.max_acceleration); @@ -165,11 +167,11 @@ auto ValidatedEntityStatus::validatedLinearAcceleration() const noexcept(false) return acceleration; } -auto ValidatedEntityStatus::buildValidatedCurrentVelocity(const double speed) const noexcept(false) - -> geometry_msgs::msg::Vector3 +auto ValidatedEntityStatus::buildValidatedCurrentVelocity( + const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const + noexcept(false) -> geometry_msgs::msg::Vector3 { - const auto euler_angles = - math::geometry::convertQuaternionToEulerAngle(entity_status_.pose.orientation); + const auto euler_angles = math::geometry::convertQuaternionToEulerAngle(entity_orientation); const double pitch = -euler_angles.y; const double yaw = euler_angles.z; const auto entity_velocity = geometry_msgs::build() From 87e5581dd7ac7d6fcd372d558ab63e58b00e76b7 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:08:09 +0100 Subject: [PATCH 43/82] Refactor ValidatedEntityStatus: step_time -> step_time_ Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 2 +- .../validated_entity_status.cpp | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 1eafd0260c3..65605b9cde7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -35,7 +35,7 @@ struct ValidatedEntityStatus auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus; - const double step_time; + const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const std::string & name; const double time; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index a6762bb7894..5a5bfb4fecf 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -36,7 +36,7 @@ ValidatedEntityStatus::ValidatedEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) noexcept(false) -: step_time(step_time), +: step_time_(step_time), entity_status_(entity_status), name(entity_status_.name), time(entity_status_.time), @@ -46,13 +46,13 @@ ValidatedEntityStatus::ValidatedEntityStatus( behavior_parameter(validatedBehaviorParameter(behavior_parameter)), linear_speed(validatedLinearSpeed(entity_status_.action_status.twist.linear.x)), linear_acceleration(validatedLinearAcceleration( - entity_status_.action_status.accel.linear.x, behavior_parameter, step_time)), + entity_status_.action_status.accel.linear.x, behavior_parameter, step_time_)), current_velocity(buildValidatedCurrentVelocity(linear_speed, entity_status_.pose.orientation)) { } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) -: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter, other.step_time) +: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter, other.step_time_) { } @@ -83,7 +83,7 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( using math::geometry::operator*; using math::geometry::operator/; - const auto updated_time = entity_status_.time + step_time; + const auto updated_time = entity_status_.time + step_time_; const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); const auto updated_action_status_twist_linear = geometry_msgs::build() @@ -93,7 +93,7 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( const auto updated_action_status_twist_angular = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(entity_status_.pose.orientation, updated_pose_orientation)) / - step_time; + step_time_; const auto updated_action_status_twist = geometry_msgs::build() .linear(updated_action_status_twist_linear) .angular(updated_action_status_twist_angular); @@ -101,19 +101,20 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( geometry_msgs::build() .linear( (updated_action_status_twist_linear - entity_status_.action_status.twist.linear) / - step_time) + step_time_) .angular( (updated_action_status_twist_angular - entity_status_.action_status.twist.angular) / - step_time); + step_time_); const auto updated_action_status = traffic_simulator_msgs::build() .current_action(entity_status_.action_status.current_action) .twist(updated_action_status_twist) .accel(updated_action_status_accel) .linear_jerk(entity_status_.action_status.linear_jerk); - const auto updated_pose = geometry_msgs::build() - .position(entity_status_.pose.position + desired_velocity * step_time) - .orientation(updated_pose_orientation); + const auto updated_pose = + geometry_msgs::build() + .position(entity_status_.pose.position + desired_velocity * step_time_) + .orientation(updated_pose_orientation); constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() From d9099ed0a74c7e38e0737f9941539aaefd0dcc93 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:19:06 +0100 Subject: [PATCH 44/82] Refactor ValidatedEntityStatus: remove duplicated data: name Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 3 ++- .../polyline_trajectory_positioner.cpp | 16 ++++++++-------- .../validated_entity_status.cpp | 1 - 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 65605b9cde7..c579cf65497 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const std::string & name; const double time; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; const bool lanelet_pose_valid; @@ -54,6 +53,8 @@ struct ValidatedEntityStatus ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) noexcept(true) = delete; ~ValidatedEntityStatus() = default; + auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } + private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) -> geometry_msgs::msg::Point; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index d71772117a3..ed3c6b383ea 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -160,7 +160,7 @@ auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> */ if (remaining_time < -step_time) { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name), + "Vehicle ", std::quoted(validated_entity_status.name()), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", nearest_waypoint_with_specified_time_it->time, " (in ", @@ -218,7 +218,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name), + std::quoted(validated_entity_status.name()), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); } @@ -250,14 +250,14 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name), + std::quoted(validated_entity_status.name()), "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, ". "); } return desired_acceleration; } catch (const ControllerError & e) { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name), + "Vehicle ", std::quoted(validated_entity_status.name()), " - controller operation problem encountered. ", follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); } @@ -364,7 +364,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio return std::nullopt; } else { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name), " at time ", + "Vehicle ", std::quoted(validated_entity_status.name()), " at time ", validated_entity_status.time, "s (remaining time is ", time_to_nearest_waypoint, "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", @@ -395,7 +395,7 @@ auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcep THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name), + std::quoted(validated_entity_status.name()), "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); } @@ -411,7 +411,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name), + std::quoted(validated_entity_status.name()), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); } return desired_speed; @@ -427,7 +427,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", - std::quoted(validated_entity_status.name), + std::quoted(validated_entity_status.name()), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", total_remaining_time: ", total_remaining_time, ", total_remaining_distance: ", total_remaining_distance, diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 5a5bfb4fecf..86b6c06e34a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,7 +38,6 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - name(entity_status_.name), time(entity_status_.time), bounding_box(entity_status_.bounding_box), lanelet_pose_valid(entity_status_.lanelet_pose_valid), From b47dbebe455eb789b149cd301ceeaffc5ef16599 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:35:43 +0100 Subject: [PATCH 45/82] Refactor ValidatedEntityStatus: remove duplicated data: time Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 2 +- .../polyline_trajectory_positioner.cpp | 6 +++--- .../validated_entity_status.cpp | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index c579cf65497..03348ebef18 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const double time; const traffic_simulator_msgs::msg::BoundingBox & bounding_box; const bool lanelet_pose_valid; const geometry_msgs::msg::Point position; @@ -54,6 +53,7 @@ struct ValidatedEntityStatus ~ValidatedEntityStatus() = default; auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } + auto time() const noexcept(true) -> double { return entity_status_.time; } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index ed3c6b383ea..c82a1fadce1 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -52,7 +52,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( total_remaining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), time_to_nearest_waypoint( (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - polyline_trajectory.shape.vertices.front().time - validated_entity_status.time), + polyline_trajectory.shape.vertices.front().time - validated_entity_status.time()), total_remaining_time(totalRemainingTime()), follow_waypoint_controller(FollowWaypointController( validated_entity_status.behavior_parameter, step_time, @@ -134,7 +134,7 @@ auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> } else { const double remaining_time = (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - nearest_waypoint_with_specified_time_it->time - validated_entity_status.time; + nearest_waypoint_with_specified_time_it->time - validated_entity_status.time(); /* The condition below should ideally be remaining_time < 0. @@ -365,7 +365,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio } else { THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(validated_entity_status.name()), " at time ", - validated_entity_status.time, "s (remaining time is ", time_to_nearest_waypoint, + validated_entity_status.time(), "s (remaining time is ", time_to_nearest_waypoint, "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", distance_to_nearest_waypoint, diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 86b6c06e34a..6eea81ebda4 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,7 +38,6 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - time(entity_status_.time), bounding_box(entity_status_.bounding_box), lanelet_pose_valid(entity_status_.lanelet_pose_valid), position(validatedPosition(entity_status_.pose.position)), From 8d7ca687f81716dc89677a7236743efb69e0af88 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:36:56 +0100 Subject: [PATCH 46/82] Refactor ValidatedEntityStatus: remove duplicated data: bounding_box Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_follower/validated_entity_status.hpp | 5 ++++- .../polyline_trajectory_positioner.cpp | 4 ++-- .../polyline_trajectory_follower/validated_entity_status.cpp | 1 - 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 03348ebef18..61122bed3f1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const traffic_simulator_msgs::msg::BoundingBox & bounding_box; const bool lanelet_pose_valid; const geometry_msgs::msg::Point position; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; @@ -54,6 +53,10 @@ struct ValidatedEntityStatus auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } auto time() const noexcept(true) -> double { return entity_status_.time; } + auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & + { + return entity_status_.bounding_box; + } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index c82a1fadce1..fbef3096e56 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -47,7 +47,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( nearest_waypoint_with_specified_time_it(nearestWaypointWithSpecifiedTimeIterator()), nearest_waypoint_position(validatedEntityTargetPosition()), distance_to_nearest_waypoint(distanceAlongLanelet( - hdmap_utils_ptr, validated_entity_status.bounding_box, matching_distance, + hdmap_utils_ptr, validated_entity_status.boundingBox(), matching_distance, validated_entity_status.entity_status_.pose.position, nearest_waypoint_position)), total_remaining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), time_to_nearest_waypoint( @@ -112,7 +112,7 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance( const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); return total_distance + distanceAlongLanelet( - hdmap_utils_ptr, validated_entity_status.bounding_box, + hdmap_utils_ptr, validated_entity_status.boundingBox(), matching_distance, vertex.position.position, next->position.position); }); diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 6eea81ebda4..082ad426a4e 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,7 +38,6 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - bounding_box(entity_status_.bounding_box), lanelet_pose_valid(entity_status_.lanelet_pose_valid), position(validatedPosition(entity_status_.pose.position)), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), From 24cf34f30e5d207c532e35d84a6bf011cfb96891 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:45:20 +0100 Subject: [PATCH 47/82] Refactor ValidatedEntityStatus: remove duplicated data: lanelet_pose_valid Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_follower/validated_entity_status.hpp | 2 +- .../polyline_trajectory_positioner.cpp | 2 +- .../polyline_trajectory_follower/validated_entity_status.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 61122bed3f1..d4114fa0f19 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const bool lanelet_pose_valid; const geometry_msgs::msg::Point position; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const double linear_speed; @@ -57,6 +56,7 @@ struct ValidatedEntityStatus { return entity_status_.bounding_box; } + auto laneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index fbef3096e56..10e07b3dd55 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -202,7 +202,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d const double dy = nearest_waypoint_position.y - validated_entity_status.position.y; // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = - validated_entity_status.lanelet_pose_valid + validated_entity_status.laneletPoseValid() ? -math::geometry::convertQuaternionToEulerAngle( validated_entity_status.entity_status_.pose.orientation) .y diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 082ad426a4e..7d508201301 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,7 +38,6 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - lanelet_pose_valid(entity_status_.lanelet_pose_valid), position(validatedPosition(entity_status_.pose.position)), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), linear_speed(validatedLinearSpeed(entity_status_.action_status.twist.linear.x)), From a79602a52208b197f38e95f7ecb0bf113df5999b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:51:59 +0100 Subject: [PATCH 48/82] Refactor ValidatedEntityStatus: remove duplicated data: position Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 5 ++++- .../polyline_trajectory_positioner.cpp | 6 +++--- .../validated_entity_status.cpp | 1 - 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index d4114fa0f19..2350bbdf98a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const geometry_msgs::msg::Point position; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const double linear_speed; const double linear_acceleration; @@ -57,6 +56,10 @@ struct ValidatedEntityStatus return entity_status_.bounding_box; } auto laneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } + auto position() const noexcept(true) -> const geometry_msgs::msg::Point & + { + return entity_status_.pose.position; + } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 10e07b3dd55..673b8662ba1 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -198,8 +198,8 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d THROW_SIMULATION_ERROR("The followingMode is only supported for position."); } - const double dx = nearest_waypoint_position.x - validated_entity_status.position.x; - const double dy = nearest_waypoint_position.y - validated_entity_status.position.y; + const double dx = nearest_waypoint_position.x - validated_entity_status.position().x; + const double dy = nearest_waypoint_position.y - validated_entity_status.position().y; // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = validated_entity_status.laneletPoseValid() @@ -207,7 +207,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d validated_entity_status.entity_status_.pose.orientation) .y : std::atan2( - nearest_waypoint_position.z - validated_entity_status.position.z, std::hypot(dy, dx)); + nearest_waypoint_position.z - validated_entity_status.position().z, std::hypot(dy, dx)); const double yaw = std::atan2(dy, dx); // Use yaw on target const auto desired_velocity = geometry_msgs::build() diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 7d508201301..6953cb6de6c 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,7 +38,6 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - position(validatedPosition(entity_status_.pose.position)), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), linear_speed(validatedLinearSpeed(entity_status_.action_status.twist.linear.x)), linear_acceleration(validatedLinearAcceleration( From 64bab3351288f5b3c971d9f9e096081760d9792a Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 18:56:38 +0100 Subject: [PATCH 49/82] Refactor ValidatedEntityStatus: remove duplicated data: linear_speed Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 7 +++++-- .../polyline_trajectory_positioner.cpp | 16 ++++++++-------- .../validated_entity_status.cpp | 9 ++++----- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 2350bbdf98a..2699a6a5dd9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -38,7 +38,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - const double linear_speed; const double linear_acceleration; const geometry_msgs::msg::Vector3 current_velocity; @@ -60,12 +59,16 @@ struct ValidatedEntityStatus { return entity_status_.pose.position; } + auto linearSpeed() const noexcept(true) -> double + { + return entity_status_.action_status.twist.linear.x; + } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) -> geometry_msgs::msg::Point; - auto validatedLinearSpeed(const double entity_speed) const noexcept(false) -> double; + auto validateLinearSpeed(const double entity_speed) const noexcept(false) -> void; auto validatedLinearAcceleration( const double acceleration, diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 673b8662ba1..d21f922ef8f 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -244,7 +244,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no try { const double desired_acceleration = follow_waypoint_controller.getAcceleration( total_remaining_time, total_remaining_distance, validated_entity_status.linear_acceleration, - validated_entity_status.linear_speed); + validated_entity_status.linearSpeed()); if (not std::isfinite(desired_acceleration)) { THROW_SIMULATION_ERROR( @@ -309,7 +309,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio const auto desired_velocity = validatedEntityDesiredVelocity(desired_speed); if (const bool target_passed = - validated_entity_status.linear_speed * step_time > distance_to_nearest_waypoint and + validated_entity_status.linearSpeed() * step_time > distance_to_nearest_waypoint and math::geometry::innerProduct(desired_velocity, validated_entity_status.current_velocity) < 0.0; target_passed) { @@ -330,7 +330,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio maximum speed including braking - in this case accuracy of arrival is checked */ if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed(), distance_to_nearest_waypoint)) { return std::nullopt; } else { @@ -342,7 +342,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio irrelevant */ if (const double this_step_distance = - (validated_entity_status.linear_speed + desired_acceleration * step_time) * step_time; + (validated_entity_status.linearSpeed() + desired_acceleration * step_time) * step_time; this_step_distance > distance_to_nearest_waypoint) { return std::nullopt; } else { @@ -359,7 +359,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio */ } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed, + validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed(), distance_to_nearest_waypoint)) { return std::nullopt; } else { @@ -405,7 +405,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( const double desired_acceleration) const noexcept(false) -> double { const double desired_speed = - validated_entity_status.linear_speed + desired_acceleration * step_time; + validated_entity_status.linearSpeed() + desired_acceleration * step_time; if (not std::isfinite(desired_speed)) { THROW_SIMULATION_ERROR( @@ -422,7 +422,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a { const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, total_remaining_time, total_remaining_distance, - validated_entity_status.linear_acceleration, validated_entity_status.linear_speed); + validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed()); if (not std::isinf(total_remaining_time) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -432,7 +432,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a ", total_remaining_time: ", total_remaining_time, ", total_remaining_distance: ", total_remaining_distance, ", acceleration: ", validated_entity_status.linear_acceleration, - ", speed: ", validated_entity_status.linear_speed, ". ", follow_waypoint_controller); + ", speed: ", validated_entity_status.linearSpeed(), ". ", follow_waypoint_controller); } } diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 6953cb6de6c..1a5a0a7e7ac 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -39,11 +39,11 @@ ValidatedEntityStatus::ValidatedEntityStatus( : step_time_(step_time), entity_status_(entity_status), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), - linear_speed(validatedLinearSpeed(entity_status_.action_status.twist.linear.x)), linear_acceleration(validatedLinearAcceleration( entity_status_.action_status.accel.linear.x, behavior_parameter, step_time_)), - current_velocity(buildValidatedCurrentVelocity(linear_speed, entity_status_.pose.orientation)) + current_velocity(buildValidatedCurrentVelocity(linearSpeed(), entity_status_.pose.orientation)) { + validateLinearSpeed(linearSpeed()); } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) @@ -133,13 +133,12 @@ auto ValidatedEntityStatus::validatedPosition(const geometry_msgs::msg::Point & return entity_position; } -auto ValidatedEntityStatus::validatedLinearSpeed(const double entity_speed) const noexcept(false) - -> double +auto ValidatedEntityStatus::validateLinearSpeed(const double entity_speed) const noexcept(false) + -> void { if (not std::isfinite(entity_speed)) { throwDetailedValidationError("entity_speed", entity_speed); } - return entity_speed; } auto ValidatedEntityStatus::validatedLinearAcceleration( From 2eb721da8ed4a9f78758b55482bee297446196f7 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:08:45 +0100 Subject: [PATCH 50/82] Refactor ValidatedEntityStatus: remove duplicated data: linear_acceleration Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 9 ++++++--- .../polyline_trajectory_positioner.cpp | 10 +++++----- .../validated_entity_status.cpp | 8 +++----- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 2699a6a5dd9..0dff88fec4f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -38,7 +38,6 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; - const double linear_acceleration; const geometry_msgs::msg::Vector3 current_velocity; ValidatedEntityStatus() = delete; @@ -63,6 +62,10 @@ struct ValidatedEntityStatus { return entity_status_.action_status.twist.linear.x; } + auto linearAcceleration() const noexcept(true) -> double + { + return entity_status_.action_status.accel.linear.x; + } private: auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) @@ -70,10 +73,10 @@ struct ValidatedEntityStatus auto validateLinearSpeed(const double entity_speed) const noexcept(false) -> void; - auto validatedLinearAcceleration( + auto validateLinearAcceleration( const double acceleration, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const double step_time) const noexcept(false) -> double; + const double step_time) const noexcept(false) -> void; auto validatedBehaviorParameter( const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index d21f922ef8f..c0450218d2c 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -243,7 +243,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no try { const double desired_acceleration = follow_waypoint_controller.getAcceleration( - total_remaining_time, total_remaining_distance, validated_entity_status.linear_acceleration, + total_remaining_time, total_remaining_distance, validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed()); if (not std::isfinite(desired_acceleration)) { @@ -330,7 +330,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio maximum speed including braking - in this case accuracy of arrival is checked */ if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed(), + validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed(), distance_to_nearest_waypoint)) { return std::nullopt; } else { @@ -359,7 +359,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio */ } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint, step_time / 2.0)) { if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed(), + validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed(), distance_to_nearest_waypoint)) { return std::nullopt; } else { @@ -422,7 +422,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a { const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( desired_acceleration, total_remaining_time, total_remaining_distance, - validated_entity_status.linear_acceleration, validated_entity_status.linearSpeed()); + validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed()); if (not std::isinf(total_remaining_time) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -431,7 +431,7 @@ auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_a " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", total_remaining_time: ", total_remaining_time, ", total_remaining_distance: ", total_remaining_distance, - ", acceleration: ", validated_entity_status.linear_acceleration, + ", acceleration: ", validated_entity_status.linearAcceleration(), ", speed: ", validated_entity_status.linearSpeed(), ". ", follow_waypoint_controller); } } diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 1a5a0a7e7ac..00618e1fd28 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -39,11 +39,10 @@ ValidatedEntityStatus::ValidatedEntityStatus( : step_time_(step_time), entity_status_(entity_status), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), - linear_acceleration(validatedLinearAcceleration( - entity_status_.action_status.accel.linear.x, behavior_parameter, step_time_)), current_velocity(buildValidatedCurrentVelocity(linearSpeed(), entity_status_.pose.orientation)) { validateLinearSpeed(linearSpeed()); + validateLinearAcceleration(linearAcceleration(), behavior_parameter, step_time_); } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) @@ -141,10 +140,10 @@ auto ValidatedEntityStatus::validateLinearSpeed(const double entity_speed) const } } -auto ValidatedEntityStatus::validatedLinearAcceleration( +auto ValidatedEntityStatus::validateLinearAcceleration( const double acceleration, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const double step_time) const noexcept(false) -> double + const double step_time) const noexcept(false) -> void { const double max_acceleration = std::min( acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, @@ -159,7 +158,6 @@ auto ValidatedEntityStatus::validatedLinearAcceleration( } else if (not std::isfinite(min_acceleration)) { throwDetailedValidationError("minimum acceleration", min_acceleration); } - return acceleration; } auto ValidatedEntityStatus::buildValidatedCurrentVelocity( From 8b9369d81ba7ef348b31b5b9f5b1e9901b88cd16 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:13:29 +0100 Subject: [PATCH 51/82] Refactor ValidatedEntityStatus: add orientation getter Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_follower/validated_entity_status.hpp | 4 ++++ .../polyline_trajectory_positioner.cpp | 4 +--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 0dff88fec4f..2a44f03b006 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -58,6 +58,10 @@ struct ValidatedEntityStatus { return entity_status_.pose.position; } + auto orientation() const noexcept(true) -> const geometry_msgs::msg::Quaternion & + { + return entity_status_.pose.orientation; + } auto linearSpeed() const noexcept(true) -> double { return entity_status_.action_status.twist.linear.x; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index c0450218d2c..28bb94e46be 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -203,9 +203,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = validated_entity_status.laneletPoseValid() - ? -math::geometry::convertQuaternionToEulerAngle( - validated_entity_status.entity_status_.pose.orientation) - .y + ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status.orientation()).y : std::atan2( nearest_waypoint_position.z - validated_entity_status.position().z, std::hypot(dy, dx)); const double yaw = std::atan2(dy, dx); // Use yaw on target From 4b7b9a4d73cfef5861892a40f45a2891d4947e3d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:13:45 +0100 Subject: [PATCH 52/82] Refactor ValidatedEntityStatus: use position getter Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_positioner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 28bb94e46be..94a302874b6 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -48,7 +48,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( nearest_waypoint_position(validatedEntityTargetPosition()), distance_to_nearest_waypoint(distanceAlongLanelet( hdmap_utils_ptr, validated_entity_status.boundingBox(), matching_distance, - validated_entity_status.entity_status_.pose.position, nearest_waypoint_position)), + validated_entity_status.position(), nearest_waypoint_position)), total_remaining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), time_to_nearest_waypoint( (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + From c4fd21502fe7cf5497588aed437a8cfeed25a65a Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:16:11 +0100 Subject: [PATCH 53/82] Fix position validation Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 4 ++-- .../validated_entity_status.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 2a44f03b006..7cdacb1cef7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -72,8 +72,8 @@ struct ValidatedEntityStatus } private: - auto validatedPosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) - -> geometry_msgs::msg::Point; + auto validatePosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) + -> void; auto validateLinearSpeed(const double entity_speed) const noexcept(false) -> void; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 00618e1fd28..7660d01754a 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -41,6 +41,7 @@ ValidatedEntityStatus::ValidatedEntityStatus( behavior_parameter(validatedBehaviorParameter(behavior_parameter)), current_velocity(buildValidatedCurrentVelocity(linearSpeed(), entity_status_.pose.orientation)) { + validatePosition(position()); validateLinearSpeed(linearSpeed()); validateLinearAcceleration(linearAcceleration(), behavior_parameter, step_time_); } @@ -123,13 +124,12 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .lanelet_pose_valid(updated_lanelet_pose_valid); } -auto ValidatedEntityStatus::validatedPosition(const geometry_msgs::msg::Point & entity_position) - const noexcept(false) -> geometry_msgs::msg::Point +auto ValidatedEntityStatus::validatePosition( + const geometry_msgs::msg::Point & entity_position) const noexcept(false) -> void { if (not math::geometry::isFinite(entity_position)) { throwDetailedValidationError("entity_position", entity_position); } - return entity_position; } auto ValidatedEntityStatus::validateLinearSpeed(const double entity_speed) const noexcept(false) From f5ba27ae7460877efd8dfedc9f35adcd8427c6f8 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:20:04 +0100 Subject: [PATCH 54/82] Use getters Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 7660d01754a..af594fefd11 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -39,7 +39,7 @@ ValidatedEntityStatus::ValidatedEntityStatus( : step_time_(step_time), entity_status_(entity_status), behavior_parameter(validatedBehaviorParameter(behavior_parameter)), - current_velocity(buildValidatedCurrentVelocity(linearSpeed(), entity_status_.pose.orientation)) + current_velocity(buildValidatedCurrentVelocity(linearSpeed(), orientation())) { validatePosition(position()); validateLinearSpeed(linearSpeed()); @@ -57,7 +57,7 @@ auto ValidatedEntityStatus::buildUpdatedPoseOrientation( { if (desired_velocity.x == 0.0 and desired_velocity.y == 0.0 and desired_velocity.z == 0.0) { // do not change orientation if there is no designed_velocity vector - return entity_status_.pose.orientation; + return orientation(); } else { // if there is a designed_velocity vector, set the orientation in the direction of it const geometry_msgs::msg::Vector3 direction = @@ -87,7 +87,7 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .z(0.0); const auto updated_action_status_twist_angular = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(entity_status_.pose.orientation, updated_pose_orientation)) / + math::geometry::getRotation(orientation(), updated_pose_orientation)) / step_time_; const auto updated_action_status_twist = geometry_msgs::build() .linear(updated_action_status_twist_linear) @@ -106,10 +106,9 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .twist(updated_action_status_twist) .accel(updated_action_status_accel) .linear_jerk(entity_status_.action_status.linear_jerk); - const auto updated_pose = - geometry_msgs::build() - .position(entity_status_.pose.position + desired_velocity * step_time_) - .orientation(updated_pose_orientation); + const auto updated_pose = geometry_msgs::build() + .position(position() + desired_velocity * step_time_) + .orientation(updated_pose_orientation); constexpr bool updated_lanelet_pose_valid = false; return traffic_simulator_msgs::build() From b4b7cf2ab11d658de56f5390a626f6c7542ba842 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:28:30 +0100 Subject: [PATCH 55/82] Refactor ValidatedEntityStatus: add behavior parameter getter Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 11 ++++++++--- .../polyline_trajectory_positioner.cpp | 2 +- .../validated_entity_status.cpp | 12 ++++++------ 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 7cdacb1cef7..082d5f4e9f7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -37,7 +37,7 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; const geometry_msgs::msg::Vector3 current_velocity; ValidatedEntityStatus() = delete; @@ -70,6 +70,11 @@ struct ValidatedEntityStatus { return entity_status_.action_status.accel.linear.x; } + auto behaviorParameter() const noexcept(true) + -> const traffic_simulator_msgs::msg::BehaviorParameter & + { + return behavior_parameter_; + } private: auto validatePosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) @@ -82,9 +87,9 @@ struct ValidatedEntityStatus const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) const noexcept(false) -> void; - auto validatedBehaviorParameter( + auto validateBehaviorParameter( const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) - -> traffic_simulator_msgs::msg::BehaviorParameter; + -> void; auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) -> geometry_msgs::msg::Quaternion; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 94a302874b6..ff27e41a895 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -55,7 +55,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( polyline_trajectory.shape.vertices.front().time - validated_entity_status.time()), total_remaining_time(totalRemainingTime()), follow_waypoint_controller(FollowWaypointController( - validated_entity_status.behavior_parameter, step_time, + validated_entity_status.behaviorParameter(), step_time, isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint(), std::isinf(total_remaining_time) ? target_speed : std::nullopt)) { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index af594fefd11..6f5a9c10423 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -38,16 +38,17 @@ ValidatedEntityStatus::ValidatedEntityStatus( const double step_time) noexcept(false) : step_time_(step_time), entity_status_(entity_status), - behavior_parameter(validatedBehaviorParameter(behavior_parameter)), + behavior_parameter_(behavior_parameter), current_velocity(buildValidatedCurrentVelocity(linearSpeed(), orientation())) { + validateBehaviorParameter(behaviorParameter()); validatePosition(position()); validateLinearSpeed(linearSpeed()); - validateLinearAcceleration(linearAcceleration(), behavior_parameter, step_time_); + validateLinearAcceleration(linearAcceleration(), behaviorParameter(), step_time_); } ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) -: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter, other.step_time_) +: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter_, other.step_time_) { } @@ -176,9 +177,9 @@ auto ValidatedEntityStatus::buildValidatedCurrentVelocity( return entity_velocity; } -auto ValidatedEntityStatus::validatedBehaviorParameter( +auto ValidatedEntityStatus::validateBehaviorParameter( const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) - -> traffic_simulator_msgs::msg::BehaviorParameter + -> void { if (not std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration_rate)) { throwDetailedValidationError( @@ -200,7 +201,6 @@ auto ValidatedEntityStatus::validatedBehaviorParameter( "behavior_parameter.dynamic_constraints.max_acceleration_rate", behavior_parameter.dynamic_constraints.max_acceleration_rate); } - return behavior_parameter; } } // namespace follow_trajectory } // namespace traffic_simulator From 74e3591378bb5f6041f26c0748812280a41b02bb Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:31:42 +0100 Subject: [PATCH 56/82] Refactor ValidatedEntityStatus: add current velocity getter Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 6 +++++- .../polyline_trajectory_positioner.cpp | 2 +- .../validated_entity_status.cpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 082d5f4e9f7..2b2e699a974 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -38,7 +38,7 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; - const geometry_msgs::msg::Vector3 current_velocity; + const geometry_msgs::msg::Vector3 current_velocity_; ValidatedEntityStatus() = delete; ValidatedEntityStatus(const ValidatedEntityStatus & other); @@ -75,6 +75,10 @@ struct ValidatedEntityStatus { return behavior_parameter_; } + auto currentVelocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & + { + return current_velocity_; + } private: auto validatePosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index ff27e41a895..1d067f30116 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -308,7 +308,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio if (const bool target_passed = validated_entity_status.linearSpeed() * step_time > distance_to_nearest_waypoint and - math::geometry::innerProduct(desired_velocity, validated_entity_status.current_velocity) < + math::geometry::innerProduct(desired_velocity, validated_entity_status.currentVelocity()) < 0.0; target_passed) { return std::nullopt; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp index 6f5a9c10423..4e03b1c8f59 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp @@ -39,7 +39,7 @@ ValidatedEntityStatus::ValidatedEntityStatus( : step_time_(step_time), entity_status_(entity_status), behavior_parameter_(behavior_parameter), - current_velocity(buildValidatedCurrentVelocity(linearSpeed(), orientation())) + current_velocity_(buildValidatedCurrentVelocity(linearSpeed(), orientation())) { validateBehaviorParameter(behaviorParameter()); validatePosition(position()); From e12c4b02be84fb614cb84527d8a8b55764256edf Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 18 Dec 2024 19:38:22 +0100 Subject: [PATCH 57/82] Formatting of all getters Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 44 +++++-------------- 1 file changed, 12 insertions(+), 32 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index 2b2e699a974..ee275415c51 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -47,38 +47,18 @@ struct ValidatedEntityStatus ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) noexcept(true) = delete; ~ValidatedEntityStatus() = default; - auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } - auto time() const noexcept(true) -> double { return entity_status_.time; } - auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & - { - return entity_status_.bounding_box; - } - auto laneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } - auto position() const noexcept(true) -> const geometry_msgs::msg::Point & - { - return entity_status_.pose.position; - } - auto orientation() const noexcept(true) -> const geometry_msgs::msg::Quaternion & - { - return entity_status_.pose.orientation; - } - auto linearSpeed() const noexcept(true) -> double - { - return entity_status_.action_status.twist.linear.x; - } - auto linearAcceleration() const noexcept(true) -> double - { - return entity_status_.action_status.accel.linear.x; - } - auto behaviorParameter() const noexcept(true) - -> const traffic_simulator_msgs::msg::BehaviorParameter & - { - return behavior_parameter_; - } - auto currentVelocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & - { - return current_velocity_; - } + // clang-format off + auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } + auto time() const noexcept(true) -> double { return entity_status_.time; } + auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } + auto laneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } + auto position() const noexcept(true) -> const geometry_msgs::msg::Point & { return entity_status_.pose.position; } + auto orientation() const noexcept(true) -> const geometry_msgs::msg::Quaternion & { return entity_status_.pose.orientation; } + auto linearSpeed() const noexcept(true) -> double { return entity_status_.action_status.twist.linear.x; } + auto linearAcceleration() const noexcept(true) -> double { return entity_status_.action_status.accel.linear.x; } + auto behaviorParameter() const noexcept(true) -> const traffic_simulator_msgs::msg::BehaviorParameter & { return behavior_parameter_; } + auto currentVelocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & { return current_velocity_; } + // clang-format on private: auto validatePosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) From 96318b49a1fe4c3a553f4d9350f002318881069b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 09:56:40 +0100 Subject: [PATCH 58/82] Make ValidatedEntityStatus data members private Signed-off-by: Mateusz Palczuk --- .../validated_entity_status.hpp | 10 +++++----- .../polyline_trajectory_follower.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp index ee275415c51..ebde9168cb3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp @@ -35,11 +35,6 @@ struct ValidatedEntityStatus auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const -> traffic_simulator_msgs::msg::EntityStatus; - const double step_time_; - const traffic_simulator_msgs::msg::EntityStatus entity_status_; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; - const geometry_msgs::msg::Vector3 current_velocity_; - ValidatedEntityStatus() = delete; ValidatedEntityStatus(const ValidatedEntityStatus & other); ValidatedEntityStatus & operator=(const ValidatedEntityStatus & other) = delete; @@ -102,6 +97,11 @@ struct ValidatedEntityStatus ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", "Value: ", variable); } + + const double step_time_; + const traffic_simulator_msgs::msg::EntityStatus entity_status_; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; + const geometry_msgs::msg::Vector3 current_velocity_; }; } // namespace follow_trajectory diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 9e47d549f55..56538a0cd0d 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -50,7 +50,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( if (updated_entity_opt.has_value()) { return updated_entity_opt; } else { - discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.entity_status_.time); + discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.time()); } } return std::nullopt; From 08c2505a71dde869798c0f90988cfd92b3bcf3be Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 10:38:18 +0100 Subject: [PATCH 59/82] Refactor: remove duplicated data: behavior_parameter Signed-off-by: Mateusz Palczuk --- .../follow_polyline_trajectory_action.cpp | 2 +- .../follow_polyline_trajectory_action.cpp | 2 +- .../polyline_trajectory_follower.hpp | 2 -- .../polyline_trajectory_positioner.hpp | 3 --- .../polyline_trajectory_follower.cpp | 10 ++++------ .../polyline_trajectory_positioner.cpp | 2 -- simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +- 7 files changed, 7 insertions(+), 16 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 7f92854600a..9eac0b9155b 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, behavior_parameter, *polyline_trajectory, + hdmap_utils, *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), step_time); entity_status_updated.has_value()) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 20d14cb03a7..b1c2a3d77da 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, behavior_parameter, *polyline_trajectory, + hdmap_utils, *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), step_time); entity_status_updated.has_value()) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp index 0f8c8ba7bea..f7fc16e0123 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -37,7 +36,6 @@ struct PolylineTrajectoryFollower static auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed, const double step_time) -> std::optional; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index bbe5b9f2a9a..876d86a8700 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -36,7 +35,6 @@ struct PolylineTrajectoryPositioner const std::shared_ptr & hdmap_utils_ptr, const ValidatedEntityStatus & validated_entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::optional target_speed, const double matching_distance, const double step_time); @@ -45,7 +43,6 @@ struct PolylineTrajectoryPositioner private: const std::shared_ptr hdmap_utils_ptr; const ValidatedEntityStatus validated_entity_status; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory; const double step_time; const double matching_distance; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 56538a0cd0d..23eee90ed02 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -35,18 +35,16 @@ namespace follow_trajectory auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed, const double step_time) -> std::optional { assert(step_time > 0.0); while (not polyline_trajectory.shape.vertices.empty()) { - const auto updated_entity_opt = - PolylineTrajectoryPositioner( - hdmap_utils_ptr, validated_entity_status, polyline_trajectory, behavior_parameter, - target_speed, matching_distance, step_time) - .makeUpdatedEntityStatus(); + const auto updated_entity_opt = PolylineTrajectoryPositioner( + hdmap_utils_ptr, validated_entity_status, polyline_trajectory, + target_speed, matching_distance, step_time) + .makeUpdatedEntityStatus(); if (updated_entity_opt.has_value()) { return updated_entity_opt; } else { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 1d067f30116..7f689e57b91 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -36,11 +36,9 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( const std::shared_ptr & hdmap_utils_ptr, const ValidatedEntityStatus & validated_entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::optional target_speed, const double matching_distance, const double step_time) : hdmap_utils_ptr(hdmap_utils_ptr), validated_entity_status(validated_entity_status), - behavior_parameter(behavior_parameter), polyline_trajectory(polyline_trajectory), step_time(step_time), matching_distance(matching_distance), diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index bb855b3948a..77a8257265f 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -159,7 +159,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*status_), behavior_parameter_, step_time), - hdmap_utils_ptr_, behavior_parameter_, *polyline_trajectory_, + hdmap_utils_ptr_, *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), getTargetSpeed(), step_time); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side From b68d9bb2b59c21237bb3822fed61ffdd657abf69 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 11:04:52 +0100 Subject: [PATCH 60/82] Refactor: remove pointless class PolylineTrajectoryFollower Signed-off-by: Mateusz Palczuk --- .../follow_polyline_trajectory_action.cpp | 4 ++-- .../follow_polyline_trajectory_action.cpp | 4 ++-- .../polyline_trajectory_follower.hpp | 24 ++++++------------- .../polyline_trajectory_follower.cpp | 9 +++++-- .../src/entity/ego_entity.cpp | 2 +- 5 files changed, 19 insertions(+), 24 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 9eac0b9155b..f7511615483 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -74,8 +74,8 @@ 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:: - PolylineTrajectoryFollower::makeUpdatedEntityStatus( + } else if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index b1c2a3d77da..995e300b3af 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -74,8 +74,8 @@ 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:: - PolylineTrajectoryFollower::makeUpdatedEntityStatus( + } else if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp index f7fc16e0123..b6ceafd0d8f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp @@ -28,24 +28,14 @@ namespace traffic_simulator { namespace follow_trajectory { +/// @note side effects on polyline_trajectory +auto makeUpdatedEntityStatus( + const ValidatedEntityStatus & validated_entity_status, + const std::shared_ptr & hdmap_utils_ptr, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed, const double step_time) + -> std::optional; -struct PolylineTrajectoryFollower -{ -public: - /// @note side effects on polyline_trajectory - static auto makeUpdatedEntityStatus( - const ValidatedEntityStatus & validated_entity_status, - const std::shared_ptr & hdmap_utils_ptr, - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed, - const double step_time) -> std::optional; - -private: - /// @note side effects on polyline_trajectory - static auto discardTheFrontWaypoint( - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double current_time) -> void; -}; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 23eee90ed02..cdeb936e7a0 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -32,7 +32,12 @@ namespace traffic_simulator namespace follow_trajectory { -auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( +/// @note side effects on polyline_trajectory +auto discardTheFrontWaypoint( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) + -> void; + +auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, @@ -54,7 +59,7 @@ auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( return std::nullopt; } -auto PolylineTrajectoryFollower::discardTheFrontWaypoint( +auto discardTheFrontWaypoint( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) -> void { diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 77a8257265f..95349935605 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -155,7 +155,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) EntityBase::onUpdate(current_time, step_time); if (is_controlled_by_simulator_) { if (const auto non_canonicalized_updated_status = - traffic_simulator::follow_trajectory::PolylineTrajectoryFollower::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*status_), behavior_parameter_, step_time), From 0b7b59a916ec5b79e26e7546ee07918abb88dc60 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 11:54:02 +0100 Subject: [PATCH 61/82] Refactor: rename whole polyline_trajectory_follower directory + rename polyline_trajectory_follower file to follow_trajectory Signed-off-by: Mateusz Palczuk --- .../follow_trajectory.hpp} | 0 .../follow_waypoint_controller.hpp | 0 .../polyline_trajectory_positioner.hpp | 0 .../validated_entity_status.hpp | 0 .../follow_trajectory.cpp} | 0 .../follow_waypoint_controller.cpp | 0 .../polyline_trajectory_positioner.cpp | 0 .../validated_entity_status.cpp | 0 8 files changed, 0 insertions(+), 0 deletions(-) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{polyline_trajectory_follower/polyline_trajectory_follower.hpp => follow_trajectory/follow_trajectory.hpp} (100%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{polyline_trajectory_follower => follow_trajectory}/follow_waypoint_controller.hpp (100%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{polyline_trajectory_follower => follow_trajectory}/polyline_trajectory_positioner.hpp (100%) rename simulation/traffic_simulator/include/traffic_simulator/behavior/{polyline_trajectory_follower => follow_trajectory}/validated_entity_status.hpp (100%) rename simulation/traffic_simulator/src/behavior/{polyline_trajectory_follower/polyline_trajectory_follower.cpp => follow_trajectory/follow_trajectory.cpp} (100%) rename simulation/traffic_simulator/src/behavior/{polyline_trajectory_follower => follow_trajectory}/follow_waypoint_controller.cpp (100%) rename simulation/traffic_simulator/src/behavior/{polyline_trajectory_follower => follow_trajectory}/polyline_trajectory_positioner.cpp (100%) rename simulation/traffic_simulator/src/behavior/{polyline_trajectory_follower => follow_trajectory}/validated_entity_status.cpp (100%) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp similarity index 100% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp similarity index 100% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/follow_waypoint_controller.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp similarity index 100% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp similarity index 100% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp similarity index 100% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp similarity index 100% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp similarity index 100% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp similarity index 100% rename from simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/validated_entity_status.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp From 251d94a463e65c222f5cc00835664649728b3943 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 12:07:15 +0100 Subject: [PATCH 62/82] Refactor: rename whole polyline_trajectory_follower directory + rename polyline_trajectory_follower file to follow_trajectory Change include paths and include guards Signed-off-by: Mateusz Palczuk --- .../syntax/follow_trajectory_action.hpp | 2 +- simulation/traffic_simulator/CMakeLists.txt | 8 ++++---- .../behavior/behavior_plugin_base.hpp | 16 ++++++++-------- .../follow_trajectory/follow_trajectory.hpp | 12 ++++++------ .../follow_waypoint_controller.hpp | 6 +++--- .../polyline_trajectory_positioner.hpp | 10 +++++----- .../validated_entity_status.hpp | 6 +++--- .../traffic_simulator/entity/entity_base.hpp | 8 ++++---- .../follow_trajectory/follow_trajectory.cpp | 2 +- .../follow_waypoint_controller.cpp | 2 +- .../polyline_trajectory_positioner.cpp | 2 +- .../validated_entity_status.cpp | 2 +- 12 files changed, 38 insertions(+), 38 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp index ccbe6b79609..f998aa9d8c3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include namespace openscenario_interpreter { diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 08635c3c907..8c25f2aa807 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -27,10 +27,10 @@ ament_auto_find_build_dependencies() ament_auto_add_library(traffic_simulator SHARED src/api/api.cpp - src/behavior/polyline_trajectory_follower/follow_waypoint_controller.cpp - src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp - src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp - src/behavior/polyline_trajectory_follower/validated_entity_status.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 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 index 8b1d7823eb6..829eb6fd2c3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp index b6ceafd0d8f..8ca41f235e7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,4 +39,4 @@ auto makeUpdatedEntityStatus( } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_FOLLOWER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp index 6b92eb96b95..547d8d357e9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__FOLLOW_WAYPOINT_CONTROLLER_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__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 #include @@ -296,4 +296,4 @@ class FollowWaypointController } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp index 876d86a8700..09cee3c9680 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ #include -#include -#include +#include +#include #include #include #include @@ -75,4 +75,4 @@ struct PolylineTrajectoryPositioner } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp index ebde9168cb3..b7bb02d8c39 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ #include #include @@ -107,4 +107,4 @@ struct ValidatedEntityStatus } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__POLYLINE_TRAJECTORY_FOLLOWER__VALIDATED_ENTITY_STATUS_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 8ed9468f97e..0261a5f0dfd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -22,8 +22,8 @@ #include #include #include +#include #include -#include #include #include #include @@ -144,7 +144,7 @@ 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, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -152,7 +152,7 @@ class EntityBase virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &) { /** - * @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, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -182,7 +182,7 @@ class EntityBase virtual void requestClearRoute() { /** - * @note Since there are Entities such as MiscObjectEntity that do not perform routing, + * @note Since there are Entities such as MiscObjectEntity that do not perform routing, * and routing methods differ from Entity to Entity, this function performs no operation in the base type. */ } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp index cdeb936e7a0..6b571932661 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index aa5b34be669..a4b0f78c13f 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 7f689e57b91..034d29fc2bf 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp index 4e03b1c8f59..16d513f0f3f 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include namespace traffic_simulator From 3cecfb113cddc399c907369e8d8354769abd41f5 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 15:09:11 +0100 Subject: [PATCH 63/82] Refactor PolylineTrajectoryPositioner & distance 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 --- .../polyline_trajectory_positioner.hpp | 28 ++- .../traffic_simulator/utils/distance.hpp | 7 +- .../polyline_trajectory_positioner.cpp | 192 +++++++++--------- .../traffic_simulator/src/utils/distance.cpp | 15 +- 4 files changed, 124 insertions(+), 118 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp index 09cee3c9680..a5578eba8b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -41,25 +41,23 @@ struct PolylineTrajectoryPositioner auto makeUpdatedEntityStatus() const -> std::optional; private: - const std::shared_ptr 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::shared_ptr 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::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; + 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; + const FollowWaypointController follow_waypoint_controller_; - auto totalRemainingDistance( - const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) const -> double; + auto totalRemainingDistance() const -> double; auto totalRemainingTime() const noexcept(false) -> double; auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; auto nearestWaypointWithSpecifiedTimeIterator() const diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 181860c13db..6a7da67102b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -115,8 +115,11 @@ auto distanceToStopLine( auto distanceAlongLanelet( const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const double matching_distance, - const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double; + const geometry_msgs::msg::Point & from_position, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Point & to_position, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> double; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 034d29fc2bf..4b861134ad1 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -37,25 +37,29 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( const ValidatedEntityStatus & validated_entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const std::optional target_speed, const double matching_distance, const double step_time) -: hdmap_utils_ptr(hdmap_utils_ptr), - validated_entity_status(validated_entity_status), - polyline_trajectory(polyline_trajectory), - step_time(step_time), - matching_distance(matching_distance), - nearest_waypoint_with_specified_time_it(nearestWaypointWithSpecifiedTimeIterator()), - nearest_waypoint_position(validatedEntityTargetPosition()), - distance_to_nearest_waypoint(distanceAlongLanelet( - hdmap_utils_ptr, validated_entity_status.boundingBox(), matching_distance, - validated_entity_status.position(), nearest_waypoint_position)), - total_remaining_distance(totalRemainingDistance(matching_distance, hdmap_utils_ptr)), - time_to_nearest_waypoint( - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - polyline_trajectory.shape.vertices.front().time - validated_entity_status.time()), - total_remaining_time(totalRemainingTime()), - follow_waypoint_controller(FollowWaypointController( - validated_entity_status.behaviorParameter(), step_time, - isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint(), - std::isinf(total_remaining_time) ? target_speed : std::nullopt)) +: hdmap_utils_ptr_(hdmap_utils_ptr), + validated_entity_status_(validated_entity_status), + polyline_trajectory_(polyline_trajectory), + step_time_(step_time), + matching_distance_(matching_distance), + nearest_waypoint_with_specified_time_it_( + nearestWaypointWithSpecifiedTimeIterator()), // implicitly requires: polyline_trajectory_ + nearest_waypoint_position_( + validatedEntityTargetPosition()), // implicitly requires: polyline_trajectory_ + distance_to_nearest_waypoint_(distanceAlongLanelet( + hdmap_utils_ptr_, validated_entity_status_.position(), validated_entity_status_.boundingBox(), + nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_)), + total_remaining_distance_( + totalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ + time_to_nearest_waypoint_( + (std::isnan(polyline_trajectory_.base_time) ? 0.0 : polyline_trajectory_.base_time) + + polyline_trajectory_.shape.vertices.front().time - validated_entity_status_.time()), + total_remaining_time_( + totalRemainingTime()), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_, validated_entity_status_, step_time_ + follow_waypoint_controller_( + validated_entity_status_.behaviorParameter(), step_time_, + isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint(), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_ + std::isinf(total_remaining_time_) ? target_speed : std::nullopt) { } @@ -63,7 +67,7 @@ auto PolylineTrajectoryPositioner::nearestWaypointWithSpecifiedTimeIterator() co -> std::vector::const_iterator { return std::find_if( - polyline_trajectory.shape.vertices.cbegin(), polyline_trajectory.shape.vertices.cend(), + polyline_trajectory_.shape.vertices.cbegin(), polyline_trajectory_.shape.vertices.cend(), [](const auto & vertex) { return not std::isnan(vertex.time); }); } @@ -87,13 +91,11 @@ auto PolylineTrajectoryPositioner::nearestWaypointWithSpecifiedTimeIterator() co auto PolylineTrajectoryPositioner::isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool { - return nearest_waypoint_with_specified_time_it >= - std::prev(polyline_trajectory.shape.vertices.cend()); + return nearest_waypoint_with_specified_time_it_ >= + std::prev(polyline_trajectory_.shape.vertices.cend()); } -auto PolylineTrajectoryPositioner::totalRemainingDistance( - const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) const -> double +auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double { /* Note for anyone working on adding support for followingMode follow @@ -102,37 +104,35 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance( inappropriate. */ const auto total_distance_to = - [this, &matching_distance, &hdmap_utils_ptr]( - const std::vector::const_iterator last) { + [this](const std::vector::const_iterator last) { return std::accumulate( - polyline_trajectory.shape.vertices.cbegin(), last, 0.0, - [this, &matching_distance, &hdmap_utils_ptr]( - const double total_distance, const auto & vertex) { + polyline_trajectory_.shape.vertices.cbegin(), last, 0.0, + [this](const double total_distance, const auto & vertex) { const auto next = std::next(&vertex); return total_distance + distanceAlongLanelet( - hdmap_utils_ptr, validated_entity_status.boundingBox(), - matching_distance, vertex.position.position, - next->position.position); + hdmap_utils_ptr_, vertex.position.position, + validated_entity_status_.boundingBox(), next->position.position, + validated_entity_status_.boundingBox(), matching_distance_); }); }; - if (nearest_waypoint_with_specified_time_it == std::cend(polyline_trajectory.shape.vertices)) { - return distance_to_nearest_waypoint + - total_distance_to(std::cend(polyline_trajectory.shape.vertices) - 1); + if (nearest_waypoint_with_specified_time_it_ == std::cend(polyline_trajectory_.shape.vertices)) { + return distance_to_nearest_waypoint_ + + total_distance_to(std::cend(polyline_trajectory_.shape.vertices) - 1); } else { - return distance_to_nearest_waypoint + - total_distance_to(nearest_waypoint_with_specified_time_it); + return distance_to_nearest_waypoint_ + + total_distance_to(nearest_waypoint_with_specified_time_it_); } } auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> double { - if (nearest_waypoint_with_specified_time_it == std::cend(polyline_trajectory.shape.vertices)) { + if (nearest_waypoint_with_specified_time_it_ == std::cend(polyline_trajectory_.shape.vertices)) { return std::numeric_limits::infinity(); } else { const double remaining_time = - (std::isnan(polyline_trajectory.base_time) ? 0.0 : polyline_trajectory.base_time) + - nearest_waypoint_with_specified_time_it->time - validated_entity_status.time(); + (std::isnan(polyline_trajectory_.base_time) ? 0.0 : polyline_trajectory_.base_time) + + nearest_waypoint_with_specified_time_it_->time - validated_entity_status_.time(); /* The condition below should ideally be remaining_time < 0. @@ -156,13 +156,13 @@ auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> -step_time. In other words, the conditions are such that a delay of 1 step time is allowed. */ - if (remaining_time < -step_time) { + if (remaining_time < -step_time_) { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name()), + "Vehicle ", std::quoted(validated_entity_status_.name()), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", - nearest_waypoint_with_specified_time_it->time, " (in ", - (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), + nearest_waypoint_with_specified_time_it_->time, " (in ", + (not std::isnan(polyline_trajectory_.base_time) ? "absolute" : "relative"), " simulation time). This may be due to unrealistic conditions of arrival time " "specification compared to vehicle parameters and dynamic constraints."); } else { @@ -184,7 +184,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d variable dynamic_constraints_ignorable. the value of the variable is `followingMode == position`. */ - if (not polyline_trajectory.dynamic_constraints_ignorable) { + if (not polyline_trajectory_.dynamic_constraints_ignorable) { /* Note: The vector returned if dynamic_constraints_ignorable == true ignores parameters @@ -196,14 +196,14 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d THROW_SIMULATION_ERROR("The followingMode is only supported for position."); } - const double dx = nearest_waypoint_position.x - validated_entity_status.position().x; - const double dy = nearest_waypoint_position.y - validated_entity_status.position().y; + const double dx = nearest_waypoint_position_.x - validated_entity_status_.position().x; + const double dy = nearest_waypoint_position_.y - validated_entity_status_.position().y; // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = - validated_entity_status.laneletPoseValid() - ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status.orientation()).y + validated_entity_status_.laneletPoseValid() + ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status_.orientation()).y : std::atan2( - nearest_waypoint_position.z - validated_entity_status.position().z, std::hypot(dy, dx)); + nearest_waypoint_position_.z - validated_entity_status_.position().z, std::hypot(dy, dx)); const double yaw = std::atan2(dy, dx); // Use yaw on target const auto desired_velocity = geometry_msgs::build() @@ -214,7 +214,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name()), + std::quoted(validated_entity_status_.name()), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); } @@ -238,24 +238,24 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no */ try { - const double desired_acceleration = follow_waypoint_controller.getAcceleration( - total_remaining_time, total_remaining_distance, validated_entity_status.linearAcceleration(), - validated_entity_status.linearSpeed()); + const double desired_acceleration = follow_waypoint_controller_.getAcceleration( + total_remaining_time_, total_remaining_distance_, + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); if (not std::isfinite(desired_acceleration)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name()), + std::quoted(validated_entity_status_.name()), "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, ". "); } return desired_acceleration; } catch (const ControllerError & e) { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name()), + "Vehicle ", std::quoted(validated_entity_status_.name()), " - controller operation problem encountered. ", - follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); + follow_waypoint_controller_.getFollowedWaypointDetails(polyline_trajectory_), e.what()); } } @@ -280,11 +280,11 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio distance_to_nearest_waypoint as the denominator if the distance miraculously becomes zero. */ - if (distance_to_nearest_waypoint <= 0.0) { + if (distance_to_nearest_waypoint_ <= 0.0) { return std::nullopt; } - if (total_remaining_distance <= 0) { + if (total_remaining_distance_ <= 0) { return std::nullopt; } @@ -305,32 +305,32 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio const auto desired_velocity = validatedEntityDesiredVelocity(desired_speed); if (const bool target_passed = - validated_entity_status.linearSpeed() * step_time > distance_to_nearest_waypoint and - math::geometry::innerProduct(desired_velocity, validated_entity_status.currentVelocity()) < + validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and + math::geometry::innerProduct(desired_velocity, validated_entity_status_.currentVelocity()) < 0.0; target_passed) { return std::nullopt; } validatePredictedState(desired_acceleration); - if (not std::isfinite(time_to_nearest_waypoint)) { + if (not std::isfinite(time_to_nearest_waypoint_)) { /* If the nearest waypoint is arrived at in this step without a specific arrival time, it will be considered as achieved */ if ( - not std::isfinite(total_remaining_time) and - polyline_trajectory.shape.vertices.size() == 1UL) { + not std::isfinite(total_remaining_time_) and + polyline_trajectory_.shape.vertices.size() == 1UL) { /* If the trajectory has only waypoints with unspecified time, the last one is followed using maximum speed including braking - in this case accuracy of arrival is checked */ - if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed(), - distance_to_nearest_waypoint)) { + if (follow_waypoint_controller_.areConditionsOfArrivalMet( + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), + distance_to_nearest_waypoint_)) { return std::nullopt; } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); } } else { /* @@ -338,11 +338,12 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio irrelevant */ if (const double this_step_distance = - (validated_entity_status.linearSpeed() + desired_acceleration * step_time) * step_time; - this_step_distance > distance_to_nearest_waypoint) { + (validated_entity_status_.linearSpeed() + desired_acceleration * step_time_) * + step_time_; + this_step_distance > distance_to_nearest_waypoint_) { return std::nullopt; } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); } } /* @@ -353,22 +354,22 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next step is possible (time_to_nearest_waypoint is almost zero). */ - } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint, step_time / 2.0)) { - if (follow_waypoint_controller.areConditionsOfArrivalMet( - validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed(), - distance_to_nearest_waypoint)) { + } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { + if (follow_waypoint_controller_.areConditionsOfArrivalMet( + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), + distance_to_nearest_waypoint_)) { return std::nullopt; } else { THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status.name()), " at time ", - validated_entity_status.time(), "s (remaining time is ", time_to_nearest_waypoint, + "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", + validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", - polyline_trajectory.shape.vertices.front().time, "s at a distance equal to ", - distance_to_nearest_waypoint, + polyline_trajectory_.shape.vertices.front().time, "s at a distance equal to ", + distance_to_nearest_waypoint_, " from that waypoint which is greater than the accepted accuracy."); } } else { - return validated_entity_status.buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); } /* @@ -381,33 +382,34 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point { - if (polyline_trajectory.shape.vertices.empty()) { + if (polyline_trajectory_.shape.vertices.empty()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "attempted to dereference an element of an empty PolylineTrajectory"); } - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; + const auto & target_position = polyline_trajectory_.shape.vertices.front().position.position; if (not math::geometry::isFinite(target_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name()), + std::quoted(validated_entity_status_.name()), "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); } return target_position; } + auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( const double desired_acceleration) const noexcept(false) -> double { const double desired_speed = - validated_entity_status.linearSpeed() + desired_acceleration * step_time; + validated_entity_status_.linearSpeed() + desired_acceleration * step_time_; if (not std::isfinite(desired_speed)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", - std::quoted(validated_entity_status.name()), + std::quoted(validated_entity_status_.name()), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); } return desired_speed; @@ -416,19 +418,19 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_acceleration) const noexcept(false) -> void { - const auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, total_remaining_time, total_remaining_distance, - validated_entity_status.linearAcceleration(), validated_entity_status.linearSpeed()); - if (not std::isinf(total_remaining_time) and not predicted_state_opt.has_value()) { + const auto predicted_state_opt = follow_waypoint_controller_.getPredictedWaypointArrivalState( + desired_acceleration, total_remaining_time_, total_remaining_distance_, + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); + if (not std::isinf(total_remaining_time_) and not predicted_state_opt.has_value()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", - std::quoted(validated_entity_status.name()), + std::quoted(validated_entity_status_.name()), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, - ", total_remaining_time: ", total_remaining_time, - ", total_remaining_distance: ", total_remaining_distance, - ", acceleration: ", validated_entity_status.linearAcceleration(), - ", speed: ", validated_entity_status.linearSpeed(), ". ", follow_waypoint_controller); + ", total_remaining_time: ", total_remaining_time_, + ", total_remaining_distance: ", total_remaining_distance_, + ", acceleration: ", validated_entity_status_.linearAcceleration(), + ", speed: ", validated_entity_status_.linearSpeed(), ". ", follow_waypoint_controller_); } } diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 75f7e1379fc..cc0cae7c54b 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -93,7 +93,7 @@ auto longitudinalDistance( /** * @brief A matching distance of about 1.5*lane widths is given as the matching distance to match the * Entity present on the adjacent Lanelet. - * The length of the horizontal bar must intersect with the adjacent lanelet, + * The length of the horizontal bar must intersect with the adjacent lanelet, * so it is always 10m regardless of the entity type. */ constexpr double matching_distance = 5.0; @@ -315,14 +315,17 @@ auto distanceToStopLine( auto distanceAlongLanelet( const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const double matching_distance, - const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double + const geometry_msgs::msg::Point & from_position, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Point & to_position, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> double { if (const auto from_lanelet_pose = - hdmap_utils_ptr->toLaneletPose(from, bounding_box, false, matching_distance); + 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, bounding_box, false, matching_distance); + 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()); @@ -331,7 +334,7 @@ auto distanceAlongLanelet( } } } - return math::geometry::hypot(from, to); + return math::geometry::hypot(from_position, to_position); } } // namespace distance } // namespace traffic_simulator From 1ac46c41cc93b15f92184527b798b8176bbccf0e Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 15:40:12 +0100 Subject: [PATCH 64/82] Refactor PolylineTrajectoryPositioner: Style changes Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_positioner.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 4b861134ad1..7a9c902a764 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -108,17 +108,18 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double return std::accumulate( polyline_trajectory_.shape.vertices.cbegin(), last, 0.0, [this](const double total_distance, const auto & vertex) { - const auto next = std::next(&vertex); + const auto next_vertex = std::next(&vertex); return total_distance + distanceAlongLanelet( hdmap_utils_ptr_, vertex.position.position, - validated_entity_status_.boundingBox(), next->position.position, + validated_entity_status_.boundingBox(), + next_vertex->position.position, validated_entity_status_.boundingBox(), matching_distance_); }); }; - if (nearest_waypoint_with_specified_time_it_ == std::cend(polyline_trajectory_.shape.vertices)) { + if (nearest_waypoint_with_specified_time_it_ == polyline_trajectory_.shape.vertices.cend()) { return distance_to_nearest_waypoint_ + - total_distance_to(std::cend(polyline_trajectory_.shape.vertices) - 1); + total_distance_to(std::prev(polyline_trajectory_.shape.vertices.cend())); } else { return distance_to_nearest_waypoint_ + total_distance_to(nearest_waypoint_with_specified_time_it_); @@ -127,7 +128,7 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> double { - if (nearest_waypoint_with_specified_time_it_ == std::cend(polyline_trajectory_.shape.vertices)) { + if (nearest_waypoint_with_specified_time_it_ == polyline_trajectory_.shape.vertices.cend()) { return std::numeric_limits::infinity(); } else { const double remaining_time = @@ -385,7 +386,8 @@ auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcep if (polyline_trajectory_.shape.vertices.empty()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "attempted to dereference an element of an empty PolylineTrajectory"); + "attempted to dereference an element of an empty PolylineTrajectory for vehicle ", + std::quoted(validated_entity_status_.name())); } const auto & target_position = polyline_trajectory_.shape.vertices.front().position.position; if (not math::geometry::isFinite(target_position)) { From b3a28b5c5d0893f1337a211754280b5a1978157c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 16:28:12 +0100 Subject: [PATCH 65/82] Restore previous BehaviorPluginBase getter setter macro formatting that is wrong, but passes github CI test Signed-off-by: Mateusz Palczuk --- .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 index 829eb6fd2c3..a5617849759 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From ffcc0c09a6e91688ef705e92948f65b1c0fd069d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 7 Jan 2025 11:02:38 +0100 Subject: [PATCH 66/82] Refactor: Use value_or Signed-off-by: Mateusz Palczuk --- .../behavior/follow_trajectory/follow_waypoint_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp index 547d8d357e9..778653cab58 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/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.has_value() ? target_speed.value() : max_speed} + target_speed{target_speed.value_or(max_speed)} { } From 3c07074fa599935a9561bfe9e81967c30a63c232 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 7 Jan 2025 11:04:15 +0100 Subject: [PATCH 67/82] Refactor: Move initialization to if Signed-off-by: Mateusz Palczuk --- .../follow_trajectory/follow_waypoint_controller.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp index 778653cab58..27197eeb1ed 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp @@ -245,10 +245,11 @@ class FollowWaypointController if (const auto & vertices = polyline_trajectory.shape.vertices; not vertices.empty()) { std::stringstream waypoint_details; waypoint_details << "Currently followed waypoint: "; - 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()) { + + if (const auto first_waypoint_with_arrival_time_specified = std::find_if( + vertices.cbegin(), vertices.cend(), + [](const auto & vertex) { return not std::isnan(vertex.time); }); + 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 " From f91f80abf9b3d3209b560ec009ba6475c3f8c69b Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Tue, 7 Jan 2025 11:23:49 +0100 Subject: [PATCH 68/82] Refactor: Remove implicit conversion Signed-off-by: Mateusz Palczuk --- .../follow_trajectory/follow_waypoint_controller.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index a4b0f78c13f..9e2fc1a03db 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -78,7 +78,7 @@ auto FollowWaypointController::roundTimeToFullStepsWithTolerance( " large and the predicted time exceeds the range."); } else { const auto number_of_steps_source = static_cast(remaining_time_source / step_time); - const double remaining_time = number_of_steps_source * step_time; + const double remaining_time = static_cast(number_of_steps_source) * step_time; if (const double time_difference = (remaining_time_source / step_time) - (remaining_time / step_time); time_difference > 1.0 - time_tolerance) { @@ -309,7 +309,8 @@ auto FollowWaypointController::getAcceleration( std::optional best_acceleration = std::nullopt; for (std::size_t i = 0UL; i <= number_of_acceleration_candidates; ++i) { - const double candidate_acceleration = local_min_acceleration + i * step_acceleration; + const double candidate_acceleration = + local_min_acceleration + static_cast(i) * step_acceleration; if (const auto predicted_state_opt = getPredictedStopStateWithoutConsideringTime( candidate_acceleration, remaining_distance, acceleration, speed); @@ -426,7 +427,7 @@ auto FollowWaypointController::getAcceleration( (local_max_acceleration - local_min_acceleration) / number_of_acceleration_candidates; step_acceleration > local_epsilon) { for (std::size_t i = 1UL; i < number_of_acceleration_candidates; ++i) { - considerCandidate(local_min_acceleration + i * step_acceleration); + considerCandidate(local_min_acceleration + static_cast(i) * step_acceleration); } } From 7b30cee73476eebd818d5fb9211db30cd1619857 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 23 Jan 2025 14:14:36 +0100 Subject: [PATCH 69/82] ref(traffic_simulator, geometry): add rotate and direction_to_quaternion to geometry, reformat ValidatedEntityStatus using it --- .../quaternion/direction_to_quaternion.hpp | 46 +++++++ .../include/geometry/vector3/rotate.hpp | 44 ++++++ .../validated_entity_status.hpp | 39 +++--- .../polyline_trajectory_positioner.cpp | 5 +- .../validated_entity_status.cpp | 129 ++++++++---------- 5 files changed, 166 insertions(+), 97 deletions(-) create mode 100644 common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp create mode 100644 common/math/geometry/include/geometry/vector3/rotate.hpp diff --git a/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp new file mode 100644 index 00000000000..7a31f056a7f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp @@ -0,0 +1,46 @@ +// 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__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ +#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ + +#include +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +auto convertDirectionToQuaternion(const double dx, const double dy, const double dz) +{ + const auto euler_angles = geometry_msgs::build() + .x(0.0) + .y(std::atan2(-dz, std::hypot(dx, dy))) + .z(std::atan2(dy, dx)); + return math::geometry::convertEulerAngleToQuaternion(euler_angles); +} + +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertDirectionToQuaternion(const T & v) +{ + return convertDirectionToQuaternion(v.x, v.y, v.z); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp new file mode 100644 index 00000000000..19cd492ddeb --- /dev/null +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -0,0 +1,44 @@ +// 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__ROTATE_HPP_ +#define GEOMETRY__VECTOR3__ROTATE_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename V, typename Q, + std::enable_if_t, IsLikeQuaternion>, std::nullptr_t> = + nullptr> +auto rotate(const V & vector, const Q & quaternion) +{ + const Eigen::Quaterniond eigen_quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + const Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z); + const Eigen::Vector3d eigen_rotated_vector = eigen_quaternion * eigen_vector; + V rotated_vector; + rotated_vector.x = eigen_rotated_vector.x(); + rotated_vector.y = eigen_rotated_vector.y(); + rotated_vector.z = eigen_rotated_vector.z(); + return rotated_vector; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__ROTATE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp index b7bb02d8c39..50e82ea6239 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +// #include namespace traffic_simulator { @@ -31,35 +31,36 @@ struct ValidatedEntityStatus const traffic_simulator_msgs::msg::EntityStatus & entity_status, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time) noexcept(false); - - auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const - -> traffic_simulator_msgs::msg::EntityStatus; + ValidatedEntityStatus(const ValidatedEntityStatus & other); + ~ValidatedEntityStatus() = default; ValidatedEntityStatus() = delete; - ValidatedEntityStatus(const ValidatedEntityStatus & other); ValidatedEntityStatus & operator=(const ValidatedEntityStatus & other) = delete; - ValidatedEntityStatus(ValidatedEntityStatus && other) noexcept(true) = delete; - ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) noexcept(true) = delete; - ~ValidatedEntityStatus() = default; + ValidatedEntityStatus(ValidatedEntityStatus && other) = delete; + ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) = delete; // clang-format off auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } auto time() const noexcept(true) -> double { return entity_status_.time; } - auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } - auto laneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } + auto isLaneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } auto position() const noexcept(true) -> const geometry_msgs::msg::Point & { return entity_status_.pose.position; } auto orientation() const noexcept(true) -> const geometry_msgs::msg::Quaternion & { return entity_status_.pose.orientation; } + auto velocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & { return velocity_; } auto linearSpeed() const noexcept(true) -> double { return entity_status_.action_status.twist.linear.x; } auto linearAcceleration() const noexcept(true) -> double { return entity_status_.action_status.accel.linear.x; } + auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } auto behaviorParameter() const noexcept(true) -> const traffic_simulator_msgs::msg::BehaviorParameter & { return behavior_parameter_; } - auto currentVelocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & { return current_velocity_; } // clang-format on + auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const + -> traffic_simulator_msgs::msg::EntityStatus; + private: - auto validatePosition(const geometry_msgs::msg::Point & entity_position) const noexcept(false) - -> void; + auto validatePosition(const geometry_msgs::msg::Point & position) const noexcept(false) -> void; + + auto validateLinearSpeed(const double speed) const noexcept(false) -> void; - auto validateLinearSpeed(const double entity_speed) const noexcept(false) -> void; + auto validateVelocity(const geometry_msgs::msg::Vector3 & velocity) const noexcept(false) -> void; auto validateLinearAcceleration( const double acceleration, @@ -70,13 +71,6 @@ struct ValidatedEntityStatus const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) -> void; - auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const - noexcept(true) -> geometry_msgs::msg::Quaternion; - - auto buildValidatedCurrentVelocity( - const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const - noexcept(false) -> geometry_msgs::msg::Vector3; - template < typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> auto throwDetailedValidationError(const std::string & variable_name, const T variable) const @@ -101,7 +95,8 @@ struct ValidatedEntityStatus const double step_time_; const traffic_simulator_msgs::msg::EntityStatus entity_status_; const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; - const geometry_msgs::msg::Vector3 current_velocity_; + /// @note velocity_ is the global velocity vector (local velocity rotated using the global orientation). + const geometry_msgs::msg::Vector3 velocity_; }; } // namespace follow_trajectory diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 7a9c902a764..082e5eb9c3e 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -201,7 +201,7 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d const double dy = nearest_waypoint_position_.y - validated_entity_status_.position().y; // if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = - validated_entity_status_.laneletPoseValid() + validated_entity_status_.isLaneletPoseValid() ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status_.orientation()).y : std::atan2( nearest_waypoint_position_.z - validated_entity_status_.position().z, std::hypot(dy, dx)); @@ -307,8 +307,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio if (const bool target_passed = validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and - math::geometry::innerProduct(desired_velocity, validated_entity_status_.currentVelocity()) < - 0.0; + math::geometry::innerProduct(desired_velocity, validated_entity_status_.velocity()) < 0.0; target_passed) { return std::nullopt; } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp index 16d513f0f3f..4fe204cd847 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -20,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -39,11 +41,14 @@ ValidatedEntityStatus::ValidatedEntityStatus( : step_time_(step_time), entity_status_(entity_status), behavior_parameter_(behavior_parameter), - current_velocity_(buildValidatedCurrentVelocity(linearSpeed(), orientation())) + velocity_(math::geometry::rotate( + entity_status_.action_status.twist.linear, entity_status_.pose.orientation)) { + /// @todo add validate time, add validate orientation validateBehaviorParameter(behaviorParameter()); validatePosition(position()); validateLinearSpeed(linearSpeed()); + validateVelocity(velocity()); validateLinearAcceleration(linearAcceleration(), behaviorParameter(), step_time_); } @@ -52,26 +57,10 @@ ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other { } -auto ValidatedEntityStatus::buildUpdatedPoseOrientation( - const geometry_msgs::msg::Vector3 & desired_velocity) const noexcept(true) - -> geometry_msgs::msg::Quaternion -{ - if (desired_velocity.x == 0.0 and desired_velocity.y == 0.0 and desired_velocity.z == 0.0) { - // do not change orientation if there is no designed_velocity vector - return orientation(); - } else { - // if there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); - } -} - +/// @brief this method updates the EntityStatus by applying a local velocity vector, +/// during the update the position on the lanelet is lost (it is set as invalid) auto ValidatedEntityStatus::buildUpdatedEntityStatus( - const geometry_msgs::msg::Vector3 & desired_velocity) const + const geometry_msgs::msg::Vector3 & desired_local_velocity) const -> traffic_simulator_msgs::msg::EntityStatus { using math::geometry::operator+; @@ -80,37 +69,43 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( using math::geometry::operator/; const auto updated_time = entity_status_.time + step_time_; - const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); - const auto updated_action_status_twist_linear = - geometry_msgs::build() - .x(math::geometry::norm(desired_velocity)) - .y(0.0) - .z(0.0); - const auto updated_action_status_twist_angular = + + const auto updated_pose_position = + entity_status_.pose.position + desired_local_velocity * step_time_; + const auto updated_pose_orientation = + desired_local_velocity == geometry_msgs::msg::Vector3() + ? entity_status_.pose.orientation + : math::geometry::convertDirectionToQuaternion(desired_local_velocity); + const auto updated_pose = geometry_msgs::build() + .position(updated_pose_position) + .orientation(updated_pose_orientation); + + const auto updated_twist_linear = geometry_msgs::build() + .x(math::geometry::norm(desired_local_velocity)) + .y(0.0) + .z(0.0); + const auto updated_twist_angular = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(orientation(), updated_pose_orientation)) / + math::geometry::getRotation(entity_status_.pose.orientation, updated_pose_orientation)) / step_time_; - const auto updated_action_status_twist = geometry_msgs::build() - .linear(updated_action_status_twist_linear) - .angular(updated_action_status_twist_angular); - const auto updated_action_status_accel = + const auto updated_twist = geometry_msgs::build() + .linear(updated_twist_linear) + .angular(updated_twist_angular); + + const auto updated_accel = geometry_msgs::build() - .linear( - (updated_action_status_twist_linear - entity_status_.action_status.twist.linear) / - step_time_) - .angular( - (updated_action_status_twist_angular - entity_status_.action_status.twist.angular) / - step_time_); + .linear((updated_twist_linear - entity_status_.action_status.twist.linear) / step_time_) + .angular((updated_twist_angular - entity_status_.action_status.twist.angular) / step_time_); + + const auto updated_linear_jerk = + (updated_accel.linear.x - entity_status_.action_status.accel.linear.x) / step_time_; + const auto updated_action_status = traffic_simulator_msgs::build() .current_action(entity_status_.action_status.current_action) - .twist(updated_action_status_twist) - .accel(updated_action_status_accel) - .linear_jerk(entity_status_.action_status.linear_jerk); - const auto updated_pose = geometry_msgs::build() - .position(position() + desired_velocity * step_time_) - .orientation(updated_pose_orientation); - constexpr bool updated_lanelet_pose_valid = false; + .twist(updated_twist) + .accel(updated_accel) + .linear_jerk(updated_linear_jerk); return traffic_simulator_msgs::build() .type(entity_status_.type) @@ -120,23 +115,30 @@ auto ValidatedEntityStatus::buildUpdatedEntityStatus( .bounding_box(entity_status_.bounding_box) .action_status(updated_action_status) .pose(updated_pose) - .lanelet_pose(entity_status_.lanelet_pose) - .lanelet_pose_valid(updated_lanelet_pose_valid); + .lanelet_pose(traffic_simulator_msgs::msg::LaneletPose()) + .lanelet_pose_valid(false); } -auto ValidatedEntityStatus::validatePosition( - const geometry_msgs::msg::Point & entity_position) const noexcept(false) -> void +auto ValidatedEntityStatus::validatePosition(const geometry_msgs::msg::Point & position) const + noexcept(false) -> void { - if (not math::geometry::isFinite(entity_position)) { - throwDetailedValidationError("entity_position", entity_position); + if (not math::geometry::isFinite(position)) { + throwDetailedValidationError("position", position); } } -auto ValidatedEntityStatus::validateLinearSpeed(const double entity_speed) const noexcept(false) - -> void +auto ValidatedEntityStatus::validateLinearSpeed(const double speed) const noexcept(false) -> void { - if (not std::isfinite(entity_speed)) { - throwDetailedValidationError("entity_speed", entity_speed); + if (not std::isfinite(speed)) { + throwDetailedValidationError("speed", speed); + } +} + +auto ValidatedEntityStatus::validateVelocity(const geometry_msgs::msg::Vector3 & velocity) const + noexcept(false) -> void +{ + if (not math::geometry::isFinite(velocity)) { + throwDetailedValidationError("velocity", velocity); } } @@ -160,23 +162,6 @@ auto ValidatedEntityStatus::validateLinearAcceleration( } } -auto ValidatedEntityStatus::buildValidatedCurrentVelocity( - const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const - noexcept(false) -> geometry_msgs::msg::Vector3 -{ - const auto euler_angles = math::geometry::convertQuaternionToEulerAngle(entity_orientation); - const double pitch = -euler_angles.y; - const double yaw = euler_angles.z; - const auto entity_velocity = geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); - if (not math::geometry::isFinite(entity_velocity)) { - throwDetailedValidationError("entity_velocity", entity_velocity); - } - return entity_velocity; -} - auto ValidatedEntityStatus::validateBehaviorParameter( const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) -> void From 4c9ddd52fd8c496d37c394f08db4836f6ab8aead Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 24 Jan 2025 15:46:16 +0100 Subject: [PATCH 70/82] ref(traffic_simulator) : refactor/improve PolylineTrajectoryPositioner --- .../polyline_trajectory_positioner.hpp | 49 ++- .../follow_trajectory/follow_trajectory.cpp | 1 + .../polyline_trajectory_positioner.cpp | 327 +++++++++--------- 3 files changed, 197 insertions(+), 180 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp index a5578eba8b0..2649e7fa47d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -41,6 +41,42 @@ struct PolylineTrajectoryPositioner auto makeUpdatedEntityStatus() const -> std::optional; private: + // getters + auto getWaypoints() const noexcept(true) + -> const std::vector &; + + auto getNearestWaypointWithSpecifiedTimeIterator() const + -> std::vector::const_iterator; + + // checks + auto areConditionsOfArrivalMet() const noexcept(true) -> bool; + + auto isAbsoluteBaseTime() const noexcept(true) -> bool; + + auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; + + // helpers to the constructor + auto totalRemainingDistance() const -> double; + + auto totalRemainingTime() const noexcept(false) -> double; + + auto validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point; + + // other validators + auto validatedEntityDesiredLinearAcceleration() const noexcept(false) -> double; + + auto validatedEntityDesiredSpeed(const double desired_local_acceleration) const noexcept(false) + -> double; + + auto validatedEntityDesiredLocalVelocity(const double desired_speed) const noexcept(false) + -> geometry_msgs::msg::Vector3; + + auto validatePredictedState(const double desired_local_acceleration) const noexcept(false) + -> void; + + /// @todo add note + constexpr static double ABSOLUTE_BASE_TIME{0.0}; + const std::shared_ptr hdmap_utils_ptr_; const ValidatedEntityStatus validated_entity_status_; const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory_; @@ -56,19 +92,6 @@ struct PolylineTrajectoryPositioner 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::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 diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp index 6b571932661..183044560b6 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -33,6 +33,7 @@ namespace follow_trajectory { /// @note side effects on polyline_trajectory +/// @todo let Dawid know to analyse these free functions auto discardTheFrontWaypoint( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) -> void; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 082e5eb9c3e..d07ecbb6950 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -43,7 +43,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( step_time_(step_time), matching_distance_(matching_distance), nearest_waypoint_with_specified_time_it_( - nearestWaypointWithSpecifiedTimeIterator()), // implicitly requires: polyline_trajectory_ + getNearestWaypointWithSpecifiedTimeIterator()), // implicitly requires: polyline_trajectory_ nearest_waypoint_position_( validatedEntityTargetPosition()), // implicitly requires: polyline_trajectory_ distance_to_nearest_waypoint_(distanceAlongLanelet( @@ -51,9 +51,10 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_)), total_remaining_distance_( totalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ + /// @todo nearest_waypoint_ does not always have time defined, so is this correct (getWaypoints().front().time)? time_to_nearest_waypoint_( - (std::isnan(polyline_trajectory_.base_time) ? 0.0 : polyline_trajectory_.base_time) + - polyline_trajectory_.shape.vertices.front().time - validated_entity_status_.time()), + (isAbsoluteBaseTime() ? ABSOLUTE_BASE_TIME : polyline_trajectory_.base_time) + + getWaypoints().front().time - validated_entity_status_.time()), total_remaining_time_( totalRemainingTime()), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_, validated_entity_status_, step_time_ follow_waypoint_controller_( @@ -63,20 +64,37 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( { } -auto PolylineTrajectoryPositioner::nearestWaypointWithSpecifiedTimeIterator() const +auto PolylineTrajectoryPositioner::getWaypoints() const noexcept(true) + -> const std::vector & +{ + return polyline_trajectory_.shape.vertices; +} + +auto PolylineTrajectoryPositioner::getNearestWaypointWithSpecifiedTimeIterator() const -> std::vector::const_iterator { - return std::find_if( - polyline_trajectory_.shape.vertices.cbegin(), polyline_trajectory_.shape.vertices.cend(), - [](const auto & vertex) { return not std::isnan(vertex.time); }); + return std::find_if(getWaypoints().cbegin(), getWaypoints().cend(), [](const auto & vertex) { + return not std::isnan(vertex.time); + }); +} + +auto PolylineTrajectoryPositioner::areConditionsOfArrivalMet() const noexcept(true) -> bool +{ + return follow_waypoint_controller_.areConditionsOfArrivalMet( + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), + distance_to_nearest_waypoint_); +} + +auto PolylineTrajectoryPositioner::isAbsoluteBaseTime() const noexcept(true) -> bool +{ + return std::isnan(polyline_trajectory_.base_time); } /* - The controller provides the ability to calculate acceleration using constraints from the - behavior_parameter. The value isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() - determines whether the calculated acceleration takes braking into account - it is true - if the nearest waypoint with the specified time is the last waypoint or - there is no waypoint with a specified time. + The follow_waypoint_controller calculates accelerations based on the timestamps (arrival time) + defined in waypoints. isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() determines if braking + should be taken into account - it return true if the nearest waypoint with a specified time is + the last waypoint or there is no timed waypoint. If an arrival time was specified for any of the remaining waypoints, priority is given to meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can @@ -91,8 +109,7 @@ auto PolylineTrajectoryPositioner::nearestWaypointWithSpecifiedTimeIterator() co auto PolylineTrajectoryPositioner::isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool { - return nearest_waypoint_with_specified_time_it_ >= - std::prev(polyline_trajectory_.shape.vertices.cend()); + return nearest_waypoint_with_specified_time_it_ >= std::prev(getWaypoints().cend()); } auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double @@ -106,7 +123,7 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double const auto total_distance_to = [this](const std::vector::const_iterator last) { return std::accumulate( - polyline_trajectory_.shape.vertices.cbegin(), last, 0.0, + getWaypoints().cbegin(), last, 0.0, [this](const double total_distance, const auto & vertex) { const auto next_vertex = std::next(&vertex); return total_distance + distanceAlongLanelet( @@ -117,9 +134,9 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double }); }; - if (nearest_waypoint_with_specified_time_it_ == polyline_trajectory_.shape.vertices.cend()) { - return distance_to_nearest_waypoint_ + - total_distance_to(std::prev(polyline_trajectory_.shape.vertices.cend())); + /// @todo think about what if nearest_waypoint_==nearest_waypoint_with_specified_time_it_ + if (nearest_waypoint_with_specified_time_it_ == getWaypoints().cend()) { + return distance_to_nearest_waypoint_ + total_distance_to(std::prev(getWaypoints().cend())); } else { return distance_to_nearest_waypoint_ + total_distance_to(nearest_waypoint_with_specified_time_it_); @@ -128,13 +145,9 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> double { - if (nearest_waypoint_with_specified_time_it_ == polyline_trajectory_.shape.vertices.cend()) { + if (nearest_waypoint_with_specified_time_it_ == getWaypoints().cend()) { return std::numeric_limits::infinity(); } else { - const double remaining_time = - (std::isnan(polyline_trajectory_.base_time) ? 0.0 : polyline_trajectory_.base_time) + - nearest_waypoint_with_specified_time_it_->time - validated_entity_status_.time(); - /* The condition below should ideally be remaining_time < 0. @@ -157,13 +170,17 @@ auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> -step_time. In other words, the conditions are such that a delay of 1 step time is allowed. */ - if (remaining_time < -step_time_) { + /// @todo compare to time_to_nearest_waypoint_ + if (const double remaining_time = + (isAbsoluteBaseTime() ? ABSOLUTE_BASE_TIME : polyline_trajectory_.base_time) + + nearest_waypoint_with_specified_time_it_->time - validated_entity_status_.time(); + remaining_time < -step_time_) { THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(validated_entity_status_.name()), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", nearest_waypoint_with_specified_time_it_->time, " (in ", - (not std::isnan(polyline_trajectory_.base_time) ? "absolute" : "relative"), + (isAbsoluteBaseTime() ? "absolute" : "relative"), " simulation time). This may be due to unrealistic conditions of arrival time " "specification compared to vehicle parameters and dynamic constraints."); } else { @@ -172,26 +189,18 @@ auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> } } -auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double desired_speed) const - noexcept(false) -> geometry_msgs::msg::Vector3 +auto PolylineTrajectoryPositioner::validatedEntityDesiredLocalVelocity( + const double desired_speed) const noexcept(false) -> geometry_msgs::msg::Vector3 { /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - - /* - Note: The followingMode in OpenSCENARIO is passed as - variable dynamic_constraints_ignorable. the value of the - variable is `followingMode == position`. + The followingMode in OpenSCENARIO is passed as variable dynamic_constraints_ignorable. + the value of the variable is `followingMode == position`. */ if (not polyline_trajectory_.dynamic_constraints_ignorable) { /* - Note: The vector returned if - dynamic_constraints_ignorable == true ignores parameters - such as the maximum rudder angle of the vehicle entry. In - this clause, such parameters must be respected and the - rotation angle difference of the z-axis center of the + The vector returned if dynamic_constraints_ignorable == true ignores parameters + such as the maximum rudder angle of the vehicle entry. In this clause, such parameters + must be respected and the rotation angle difference of the z-axis center of the vector must be kept below a certain value. */ THROW_SIMULATION_ERROR("The followingMode is only supported for position."); @@ -199,37 +208,41 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredVelocity(const double d const double dx = nearest_waypoint_position_.x - validated_entity_status_.position().x; const double dy = nearest_waypoint_position_.y - validated_entity_status_.position().y; - // if entity is on lane use pitch from lanelet, otherwise use pitch on target + + /// @note if entity is on lane use pitch from lanelet, otherwise use pitch on target const double pitch = validated_entity_status_.isLaneletPoseValid() ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status_.orientation()).y : std::atan2( nearest_waypoint_position_.z - validated_entity_status_.position().z, std::hypot(dy, dx)); - const double yaw = std::atan2(dy, dx); // Use yaw on target + /// @note use yaw on target + const double yaw = std::atan2(dy, dx); + + const auto desired_local_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * desired_speed) + .y(std::cos(pitch) * std::sin(yaw) * desired_speed) + .z(std::sin(pitch) * desired_speed); - const auto desired_velocity = geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * desired_speed) - .y(std::cos(pitch) * std::sin(yaw) * desired_speed) - .z(std::sin(pitch) * desired_speed); - if (not math::geometry::isFinite(desired_velocity)) { + if (math::geometry::isFinite(desired_local_velocity)) { + return desired_local_velocity; + } else { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(validated_entity_status_.name()), - "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", - desired_velocity.y, ", ", desired_velocity.z, "]."); + "'s desired velocity contains NaN or infinity. The value is [", desired_local_velocity.x, + ", ", desired_local_velocity.y, ", ", desired_local_velocity.z, "]."); } - return desired_velocity; } -auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const noexcept(false) +auto PolylineTrajectoryPositioner::validatedEntityDesiredLinearAcceleration() const noexcept(false) -> double { /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). + The desired acceleration is the acceleration to reach the nearest_waypoint (as accurately as possible) + in the specified time (total_remaining_time_as close as possible to 0.0). - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + The desired acceleration is calculated to the nearest_waypoint with a specified arrival time. It is calculated in such a way as to reach a constant linear speed as quickly as possible, ensuring arrival at a waypoint at the precise time and with the shortest possible distance. More precisely, the controller selects acceleration to minimize the distance to the waypoint @@ -239,19 +252,21 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no */ try { - const double desired_acceleration = follow_waypoint_controller_.getAcceleration( + const double desired_linear_acceleration = follow_waypoint_controller_.getAcceleration( total_remaining_time_, total_remaining_distance_, validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); - if (not std::isfinite(desired_acceleration)) { + if (std::isfinite(desired_linear_acceleration)) { + return desired_linear_acceleration; + } else { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(validated_entity_status_.name()), "'s desired acceleration value contains NaN or infinity. The value is ", - desired_acceleration, ". "); + desired_linear_acceleration, ". "); } - return desired_acceleration; + } catch (const ControllerError & e) { THROW_SIMULATION_ERROR( "Vehicle ", std::quoted(validated_entity_status_.name()), @@ -260,174 +275,152 @@ auto PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() const no } } -auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optional -{ - /* - The following code implements the steering behavior known as "seek". See - "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more - information. +/* + The following code implements the steering behavior known as "seek". See + "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + information. - See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters - */ + See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters + Note: If obstacle avoidance is to be implemented, the steering behavior + known by the name "collision avoidance" should be synthesized here into + steering. +*/ +/// @todo add more explanations on how this method works (here) +auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optional +{ using math::geometry::operator+; using math::geometry::operator-; using math::geometry::operator*; using math::geometry::operator/; using math::geometry::operator+=; - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_nearest_waypoint as the denominator if the distance - miraculously becomes zero. + /** + * @note This clause is to avoid division-by-zero errors in later clauses with distance_to_nearest_waypoint + * as the denominator if the distance miraculously becomes zero. + * @todo extendhis comment about < 0.0 */ - if (distance_to_nearest_waypoint_ <= 0.0) { + if (distance_to_nearest_waypoint_ <= 0.0 || total_remaining_distance_ <= 0) { return std::nullopt; } - if (total_remaining_distance_ <= 0) { - return std::nullopt; - } + const auto desired_local_acceleration = validatedEntityDesiredLinearAcceleration(); + const auto desired_speed = validatedEntityDesiredSpeed(desired_local_acceleration); + const auto desired_local_velocity = validatedEntityDesiredLocalVelocity(desired_speed); - /* - The desired acceleration is the acceleration at which the destination - can be reached exactly at the specified time (= time remaining at zero). - - The desired acceleration is calculated to the nearest waypoint with a specified arrival time. - It is calculated in such a way as to reach a constant linear speed as quickly as possible, - ensuring arrival at a waypoint at the precise time and with the shortest possible distance. - More precisely, the controller selects acceleration to minimize the distance to the waypoint - that will be reached in a time step defined as the expected arrival time. - In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, - with linear speed equal to zero and acceleration equal to zero. - */ - const double desired_acceleration = validatedEntityDesiredAcceleration(); - const double desired_speed = validatedEntityDesiredSpeed(desired_acceleration); - const auto desired_velocity = validatedEntityDesiredVelocity(desired_speed); - - if (const bool target_passed = - validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and - math::geometry::innerProduct(desired_velocity, validated_entity_status_.velocity()) < 0.0; - target_passed) { + /// @note cancel step if is close to the nearest waypoint and requires a large change of direction + if ( + validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and + math::geometry::innerProduct(desired_local_velocity, validated_entity_status_.velocity()) < + 0.0) { return std::nullopt; } - validatePredictedState(desired_acceleration); + validatePredictedState(desired_local_acceleration); + + /// @note if the nearest waypoint has no timestamp specified if (not std::isfinite(time_to_nearest_waypoint_)) { - /* - If the nearest waypoint is arrived at in this step without a specific arrival time, it will - be considered as achieved - */ - if ( - not std::isfinite(total_remaining_time_) and - polyline_trajectory_.shape.vertices.size() == 1UL) { - /* - If the trajectory has only waypoints with unspecified time, the last one is followed using - maximum speed including braking - in this case accuracy of arrival is checked - */ - if (follow_waypoint_controller_.areConditionsOfArrivalMet( - validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), - distance_to_nearest_waypoint_)) { + /// @note if the nearest waypoint is the last one + /// @todo check if 'not std::isfinite(total_remaining_time_)' is necessary here + if (getWaypoints().size() == 1UL and not std::isfinite(total_remaining_time_)) { + /// @note if nearest waypoint should be reached in this step - check accuracy (meeting of arrival conditions) + // the last one waypoint is followed using maximum speed including braking (accuracy of the arrival is relevant) + if (areConditionsOfArrivalMet()) { return std::nullopt; } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); } + /// @note if nearest waypoint is reached in this step and it is an intermediate waypoint without a timestamp specified + // just consider it as reached (accuracy of the arrival is irrelevant) + } else if (const double this_step_speed = + (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); + this_step_speed * step_time_ > distance_to_nearest_waypoint_) { + return std::nullopt; + /// @note if nearest waypoint is not reached in this step - update and return state after this step } else { - /* - If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is - irrelevant - */ - if (const double this_step_distance = - (validated_entity_status_.linearSpeed() + desired_acceleration * step_time_) * - step_time_; - this_step_distance > distance_to_nearest_waypoint_) { + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); + } + + /// @note if the nearest waypoint has a timestamp specified + } else { + /** + * @note if nearest waypoint should be reached in this step - check accuracy (meeting of arrival conditions) + * + * About step_time_ / 2.0: + * The value of step_time/2 is compared, as the remaining time is affected by floating point + * inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + * 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + * here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + * step is possible (time_to_nearest_waypoint is almost zero). + * + */ + if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { + if (areConditionsOfArrivalMet()) { return std::nullopt; } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", + validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, + "s), has completed a trajectory to the nearest waypoint with specified time equal to ", + getWaypoints().front().time, "s at a distance equal to ", distance_to_nearest_waypoint_, + " from that waypoint which is greater than the accepted accuracy."); } - } - /* - If there is insufficient time left for the next calculation step. - The value of step_time/2 is compared, as the remaining time is affected by floating point - inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - step is possible (time_to_nearest_waypoint is almost zero). - */ - } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { - if (follow_waypoint_controller_.areConditionsOfArrivalMet( - validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), - distance_to_nearest_waypoint_)) { - return std::nullopt; + /// @note if should not be reached in this step - update and return state after this step } else { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", - validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, - "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", - polyline_trajectory_.shape.vertices.front().time, "s at a distance equal to ", - distance_to_nearest_waypoint_, - " from that waypoint which is greater than the accepted accuracy."); + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); } - } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); } - - /* - Note: If obstacle avoidance is to be implemented, the steering behavior - known by the name "collision avoidance" should be synthesized here into - steering. - */ } auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point { - if (polyline_trajectory_.shape.vertices.empty()) { + if (getWaypoints().empty()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "attempted to dereference an element of an empty PolylineTrajectory for vehicle ", std::quoted(validated_entity_status_.name())); - } - const auto & target_position = polyline_trajectory_.shape.vertices.front().position.position; - if (not math::geometry::isFinite(target_position)) { + } else if (const auto & target_position = getWaypoints().front().position.position; + not math::geometry::isFinite(target_position)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(validated_entity_status_.name()), "'s target position coordinate value contains NaN or infinity. The value is [", target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); + } else { + return target_position; } - return target_position; } auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( - const double desired_acceleration) const noexcept(false) -> double + const double desired_local_acceleration) const noexcept(false) -> double { - const double desired_speed = - validated_entity_status_.linearSpeed() + desired_acceleration * step_time_; - - if (not std::isfinite(desired_speed)) { + if (const double desired_speed = + validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_; + not std::isfinite(desired_speed)) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", std::quoted(validated_entity_status_.name()), "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); + } else { + return desired_speed; } - return desired_speed; } -auto PolylineTrajectoryPositioner::validatePredictedState(const double desired_acceleration) const - noexcept(false) -> void +auto PolylineTrajectoryPositioner::validatePredictedState( + const double desired_local_acceleration) const noexcept(false) -> void { - const auto predicted_state_opt = follow_waypoint_controller_.getPredictedWaypointArrivalState( - desired_acceleration, total_remaining_time_, total_remaining_distance_, - validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); - if (not std::isinf(total_remaining_time_) and not predicted_state_opt.has_value()) { + if (const auto predicted_state = follow_waypoint_controller_.getPredictedWaypointArrivalState( + desired_local_acceleration, total_remaining_time_, total_remaining_distance_, + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); + not std::isinf(total_remaining_time_) and not predicted_state.has_value()) { THROW_SIMULATION_ERROR( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: FollowWaypointController for vehicle ", - std::quoted(validated_entity_status_.name()), - " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, + std::quoted(validated_entity_status_.name()), " calculated invalid acceleration:", + " desired_local_acceleration: ", desired_local_acceleration, ", total_remaining_time: ", total_remaining_time_, ", total_remaining_distance: ", total_remaining_distance_, ", acceleration: ", validated_entity_status_.linearAcceleration(), From 310c30607203945560c8001eecc6f71b27758b34 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 24 Jan 2025 16:34:14 +0100 Subject: [PATCH 71/82] ref(traffic_simulator): improve time_to_nerest and updateEntity in PolylineTrajectoryPositioner --- .../polyline_trajectory_positioner.hpp | 2 + .../polyline_trajectory_positioner.cpp | 76 +++++++++++-------- 2 files changed, 47 insertions(+), 31 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp index 2649e7fa47d..6f8fd328bcb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -56,6 +56,8 @@ struct PolylineTrajectoryPositioner auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; // helpers to the constructor + auto timeToNearestWaypoint() const noexcept(false) -> double; + auto totalRemainingDistance() const -> double; auto totalRemainingTime() const noexcept(false) -> double; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index d07ecbb6950..eff7f2b0d9d 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -52,9 +52,7 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( total_remaining_distance_( totalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ /// @todo nearest_waypoint_ does not always have time defined, so is this correct (getWaypoints().front().time)? - time_to_nearest_waypoint_( - (isAbsoluteBaseTime() ? ABSOLUTE_BASE_TIME : polyline_trajectory_.base_time) + - getWaypoints().front().time - validated_entity_status_.time()), + time_to_nearest_waypoint_(timeToNearestWaypoint()), total_remaining_time_( totalRemainingTime()), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_, validated_entity_status_, step_time_ follow_waypoint_controller_( @@ -143,6 +141,16 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double } } +auto PolylineTrajectoryPositioner::timeToNearestWaypoint() const noexcept(false) -> double +{ + if (not std::isfinite(getWaypoints().front().time)) { + return std::numeric_limits::infinity(); + } else { + return (isAbsoluteBaseTime() ? ABSOLUTE_BASE_TIME : polyline_trajectory_.base_time) + + getWaypoints().front().time - validated_entity_status_.time(); + } +}; + auto PolylineTrajectoryPositioner::totalRemainingTime() const noexcept(false) -> double { if (nearest_waypoint_with_specified_time_it_ == getWaypoints().cend()) { @@ -318,42 +326,49 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio validatePredictedState(desired_local_acceleration); - /// @note if the nearest waypoint has no timestamp specified + /** + * + * 1. if **Nearest waypoint without a timestamp:** + * - 1.1 If it is the last waypoint: + * - Checks if the arrival conditions are met. If so, ends the movement (cancel step). + * - Otherwise, updates and returns the status with a new velocity. + * - 1.2 If it is an intermediate waypoint: + * - Checks if it can be reached within the current step. + * If so, considers it as reached (cancel step). + * - Otherwise, updates and returns the status with a new velocity. + * + * 2. else **Nearest waypoint with a timestamp:** + * - 2.1 If the waypoint should be reached in this step (time remaining is less than step time/2): + * - Checks if the arrival conditions are met. If so, ends the movement (cancel step). + * - Throws an error if the distance exceeds the accepted accuracy. + * - 2.2 If the waypoint should not be reached in this step: + * - Updates and returns the status with a new velocity. + * + * Note 2.1, about step_time_ / 2.0: + * The value of step_time/2 is compared, as the remaining time is affected by floating point + * inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + * 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + * here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + * step is possible (time_to_nearest_waypoint is almost zero). + */ + if (not std::isfinite(time_to_nearest_waypoint_)) { - /// @note if the nearest waypoint is the last one - /// @todo check if 'not std::isfinite(total_remaining_time_)' is necessary here - if (getWaypoints().size() == 1UL and not std::isfinite(total_remaining_time_)) { - /// @note if nearest waypoint should be reached in this step - check accuracy (meeting of arrival conditions) - // the last one waypoint is followed using maximum speed including braking (accuracy of the arrival is relevant) + if (getWaypoints().size() == 1UL) { if (areConditionsOfArrivalMet()) { return std::nullopt; } else { return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); } - /// @note if nearest waypoint is reached in this step and it is an intermediate waypoint without a timestamp specified - // just consider it as reached (accuracy of the arrival is irrelevant) - } else if (const double this_step_speed = - (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); - this_step_speed * step_time_ > distance_to_nearest_waypoint_) { - return std::nullopt; - /// @note if nearest waypoint is not reached in this step - update and return state after this step } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); + if (const double this_step_speed = + (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); + this_step_speed * step_time_ > distance_to_nearest_waypoint_) { + return std::nullopt; + } else { + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); + } } - - /// @note if the nearest waypoint has a timestamp specified } else { - /** - * @note if nearest waypoint should be reached in this step - check accuracy (meeting of arrival conditions) - * - * About step_time_ / 2.0: - * The value of step_time/2 is compared, as the remaining time is affected by floating point - * inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - * 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - * here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - * step is possible (time_to_nearest_waypoint is almost zero). - * - */ if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { if (areConditionsOfArrivalMet()) { return std::nullopt; @@ -365,7 +380,6 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio getWaypoints().front().time, "s at a distance equal to ", distance_to_nearest_waypoint_, " from that waypoint which is greater than the accepted accuracy."); } - /// @note if should not be reached in this step - update and return state after this step } else { return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); } From 75b04721be4c0b91af4f9ad6d7bda7a450b2f502 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Fri, 24 Jan 2025 16:50:35 +0100 Subject: [PATCH 72/82] ref(traffic_simulator): separate isNearestWaypointReachable in PolylineTrajectoryPositioner --- .../polyline_trajectory_positioner.hpp | 2 + .../polyline_trajectory_positioner.cpp | 124 ++++++++++-------- 2 files changed, 68 insertions(+), 58 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp index 6f8fd328bcb..3e6fea0e602 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -55,6 +55,8 @@ struct PolylineTrajectoryPositioner auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; + auto isNearestWaypointReachable(const auto desired_local_acceleration) const -> bool; + // helpers to the constructor auto timeToNearestWaypoint() const noexcept(false) -> double; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index eff7f2b0d9d..eb7c48c9db7 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -110,6 +110,69 @@ auto PolylineTrajectoryPositioner::isNearestWaypointWithSpecifiedTimeSameAsLastW return nearest_waypoint_with_specified_time_it_ >= std::prev(getWaypoints().cend()); } +/** + * @note isNearestWaypointReachable works as follows + * + * 1. if **Nearest waypoint without a timestamp:** + * - 1.1 If it is the last waypoint: + * - Checks if the arrival conditions are met. If so, ends the movement (return false). + * - Otherwise, updates and returns the status with a new velocity. + * - 1.2 If it is an intermediate waypoint: + * - Checks if it can be reached within the current step. + * If so, considers it as reached (return false). + * - Otherwise, updates and returns the status with a new velocity. + * + * 2. else **Nearest waypoint with a timestamp:** + * - 2.1 If the waypoint should be reached in this step (time remaining is less than step time/2): + * - Checks if the arrival conditions are met. If so, ends the movement (return false). + * - Throws an error if the distance exceeds the accepted accuracy. + * - 2.2 If the waypoint should not be reached in this step: + * - Updates and returns the status with a new velocity. + * + * Note 2.1, about step_time_ / 2.0: + * The value of step_time/2 is compared, as the remaining time is affected by floating point + * inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + * 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + * here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + * step is possible (time_to_nearest_waypoint is almost zero). + */ +auto PolylineTrajectoryPositioner::isNearestWaypointReachable( + const auto desired_local_acceleration) const -> bool +{ + if (not std::isfinite(time_to_nearest_waypoint_)) { + if (getWaypoints().size() == 1UL) { + if (areConditionsOfArrivalMet()) { + return false; + } else { + return true; + } + } else { + if (const double this_step_speed = + (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); + this_step_speed * step_time_ > distance_to_nearest_waypoint_) { + return false; + } else { + return true; + } + } + } else { + if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { + if (areConditionsOfArrivalMet()) { + return false; + } else { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", + validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, + "s), has completed a trajectory to the nearest waypoint with specified time equal to ", + getWaypoints().front().time, "s at a distance equal to ", distance_to_nearest_waypoint_, + " from that waypoint which is greater than the accepted accuracy."); + } + } else { + return true; + } + } +} + auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double { /* @@ -325,64 +388,10 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio } validatePredictedState(desired_local_acceleration); - - /** - * - * 1. if **Nearest waypoint without a timestamp:** - * - 1.1 If it is the last waypoint: - * - Checks if the arrival conditions are met. If so, ends the movement (cancel step). - * - Otherwise, updates and returns the status with a new velocity. - * - 1.2 If it is an intermediate waypoint: - * - Checks if it can be reached within the current step. - * If so, considers it as reached (cancel step). - * - Otherwise, updates and returns the status with a new velocity. - * - * 2. else **Nearest waypoint with a timestamp:** - * - 2.1 If the waypoint should be reached in this step (time remaining is less than step time/2): - * - Checks if the arrival conditions are met. If so, ends the movement (cancel step). - * - Throws an error if the distance exceeds the accepted accuracy. - * - 2.2 If the waypoint should not be reached in this step: - * - Updates and returns the status with a new velocity. - * - * Note 2.1, about step_time_ / 2.0: - * The value of step_time/2 is compared, as the remaining time is affected by floating point - * inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - - * 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value - * here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next - * step is possible (time_to_nearest_waypoint is almost zero). - */ - - if (not std::isfinite(time_to_nearest_waypoint_)) { - if (getWaypoints().size() == 1UL) { - if (areConditionsOfArrivalMet()) { - return std::nullopt; - } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); - } - } else { - if (const double this_step_speed = - (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); - this_step_speed * step_time_ > distance_to_nearest_waypoint_) { - return std::nullopt; - } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); - } - } + if (isNearestWaypointReachable(desired_local_acceleration)) { + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); } else { - if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { - if (areConditionsOfArrivalMet()) { - return std::nullopt; - } else { - THROW_SIMULATION_ERROR( - "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", - validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, - "s), has completed a trajectory to the nearest waypoint with specified time equal to ", - getWaypoints().front().time, "s at a distance equal to ", distance_to_nearest_waypoint_, - " from that waypoint which is greater than the accepted accuracy."); - } - } else { - return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); - } + return std::nullopt; } } @@ -441,6 +450,5 @@ auto PolylineTrajectoryPositioner::validatePredictedState( ", speed: ", validated_entity_status_.linearSpeed(), ". ", follow_waypoint_controller_); } } - } // namespace follow_trajectory } // namespace traffic_simulator From 15f3a83057b460730a87462a8213e90f9472208f Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:18:21 +0100 Subject: [PATCH 73/82] Revert trailing whitespace deletion Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/entity/entity_base.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index fd4a9777391..b35c18dcb02 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -144,7 +144,7 @@ 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, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -152,7 +152,7 @@ class EntityBase virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &) { /** - * @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, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -182,7 +182,7 @@ class EntityBase virtual void requestClearRoute() { /** - * @note Since there are Entities such as MiscObjectEntity that do not perform routing, + * @note Since there are Entities such as MiscObjectEntity that do not perform routing, * and routing methods differ from Entity to Entity, this function performs no operation in the base type. */ } From 436aba0f6266b64b4cd76d2018ab386fe36d67c6 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:20:27 +0100 Subject: [PATCH 74/82] Pass HdMapUtils ptr as the last argument Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/utils/distance.hpp | 5 ++--- simulation/traffic_simulator/src/utils/distance.cpp | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 6a7da67102b..65e90d8548f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -114,12 +114,11 @@ auto distanceToStopLine( const std::shared_ptr & hdmap_utils_ptr) -> std::optional; auto distanceAlongLanelet( - const std::shared_ptr & hdmap_utils_ptr, const geometry_msgs::msg::Point & from_position, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const geometry_msgs::msg::Point & to_position, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) - -> double; + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> double; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index cc0cae7c54b..269b7d40bf9 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -314,12 +314,11 @@ auto distanceToStopLine( } auto distanceAlongLanelet( - const std::shared_ptr & hdmap_utils_ptr, const geometry_msgs::msg::Point & from_position, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const geometry_msgs::msg::Point & to_position, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) - -> double + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> double { if (const auto from_lanelet_pose = hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, false, matching_distance); From f542ffeb7872575a43886363d5c043df8267bfef Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:29:56 +0100 Subject: [PATCH 75/82] Get double type by value Signed-off-by: Mateusz Palczuk --- .../behavior/follow_trajectory/follow_waypoint_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index 9e2fc1a03db..affc9ddfb23 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -178,7 +178,7 @@ auto FollowWaypointController::clampAcceleration( const double candidate_acceleration, const double acceleration, const double speed) const -> double { - const auto && [local_min_acceleration, local_max_acceleration] = + const auto [local_min_acceleration, local_max_acceleration] = getAccelerationLimits(acceleration, speed); return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration); } From 3173b3d2bbf55577328792e6d2ab0bc3bf2d7f52 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:31:49 +0100 Subject: [PATCH 76/82] Revert previous condition Signed-off-by: Mateusz Palczuk --- .../behavior/follow_trajectory/follow_waypoint_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index affc9ddfb23..e5285988372 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -275,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::infinity()) { + const_speed_distance >= std::numeric_limits::max() * const_speed_value) { throw ControllerError( "Exceeded the range of the variable type (", const_speed_distance, "/", const_speed_value, ") - the value is too large. Probably the distance", From 520e005ed9d38004fc6e93b1f83f29a2d8fd8cf5 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:32:30 +0100 Subject: [PATCH 77/82] Get double type by value Signed-off-by: Mateusz Palczuk --- .../behavior/follow_trajectory/follow_waypoint_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index e5285988372..541b7d3e81e 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -337,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] = getAccelerationLimits(acceleration, speed); if ((speed + local_min_acceleration * step_time) * step_time > remaining_distance) { From 4baf03dfcde0265b67700c3ad822c4cfef51f8d1 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:43:55 +0100 Subject: [PATCH 78/82] Adjust code to pass HdMapUtils as the last argument Signed-off-by: Mateusz Palczuk --- .../polyline_trajectory_positioner.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index eb7c48c9db7..c1e910f7ddb 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -47,8 +47,9 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( nearest_waypoint_position_( validatedEntityTargetPosition()), // implicitly requires: polyline_trajectory_ distance_to_nearest_waypoint_(distanceAlongLanelet( - hdmap_utils_ptr_, validated_entity_status_.position(), validated_entity_status_.boundingBox(), - nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_)), + validated_entity_status_.position(), validated_entity_status_.boundingBox(), + nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_, + hdmap_utils_ptr_)), total_remaining_distance_( totalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ /// @todo nearest_waypoint_ does not always have time defined, so is this correct (getWaypoints().front().time)? @@ -187,11 +188,11 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double getWaypoints().cbegin(), last, 0.0, [this](const double total_distance, const auto & vertex) { const auto next_vertex = std::next(&vertex); - return total_distance + distanceAlongLanelet( - hdmap_utils_ptr_, vertex.position.position, - validated_entity_status_.boundingBox(), - next_vertex->position.position, - validated_entity_status_.boundingBox(), matching_distance_); + return total_distance + + distanceAlongLanelet( + vertex.position.position, validated_entity_status_.boundingBox(), + next_vertex->position.position, validated_entity_status_.boundingBox(), + matching_distance_, hdmap_utils_ptr_); }); }; From c4ac964b2e26eb626a22adfbb4dd9bae4ddd1801 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 15:44:38 +0100 Subject: [PATCH 79/82] Remove "magic values" Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/utils/distance.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 269b7d40bf9..699d4af04d8 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -320,11 +320,14 @@ auto distanceAlongLanelet( const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) -> double { - if (const auto from_lanelet_pose = - hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, false, matching_distance); + /// @note due to this hardcoded value, the method cannot be used for calculations along a crosswalk (for pedestrians) + constexpr bool include_crosswalk{false}; + + if (const auto from_lanelet_pose = hdmap_utils_ptr->toLaneletPose( + from_position, from_bounding_box, include_crosswalk, 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); + if (const auto to_lanelet_pose = hdmap_utils_ptr->toLaneletPose( + to_position, to_bounding_box, include_crosswalk, matching_distance); to_lanelet_pose.has_value()) { if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance( from_lanelet_pose.value(), to_lanelet_pose.value()); From ef1594c094ad841e85949e1b76e0c6313d49fa43 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 16:01:31 +0100 Subject: [PATCH 80/82] Change the `distance::distanceAlongLanelet` to not have fallback into euclidean distance Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/utils/distance.hpp | 2 +- .../polyline_trajectory_positioner.cpp | 17 +++++++++++------ .../traffic_simulator/src/utils/distance.cpp | 13 ++++++------- 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 65e90d8548f..59d53f45c79 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -118,7 +118,7 @@ auto distanceAlongLanelet( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const geometry_msgs::msg::Point & to_position, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const std::shared_ptr & hdmap_utils_ptr) -> std::optional; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index c1e910f7ddb..b1f950993d2 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -46,10 +46,13 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( getNearestWaypointWithSpecifiedTimeIterator()), // implicitly requires: polyline_trajectory_ nearest_waypoint_position_( validatedEntityTargetPosition()), // implicitly requires: polyline_trajectory_ - distance_to_nearest_waypoint_(distanceAlongLanelet( - validated_entity_status_.position(), validated_entity_status_.boundingBox(), - nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_, - hdmap_utils_ptr_)), + distance_to_nearest_waypoint_( + distance::distanceAlongLanelet( + validated_entity_status_.position(), validated_entity_status_.boundingBox(), + nearest_waypoint_position_, validated_entity_status_.boundingBox(), matching_distance_, + hdmap_utils_ptr_) + .value_or( + math::geometry::hypot(validated_entity_status_.position(), nearest_waypoint_position_))), total_remaining_distance_( totalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ /// @todo nearest_waypoint_ does not always have time defined, so is this correct (getWaypoints().front().time)? @@ -189,10 +192,12 @@ auto PolylineTrajectoryPositioner::totalRemainingDistance() const -> double [this](const double total_distance, const auto & vertex) { const auto next_vertex = std::next(&vertex); return total_distance + - distanceAlongLanelet( + distance::distanceAlongLanelet( vertex.position.position, validated_entity_status_.boundingBox(), next_vertex->position.position, validated_entity_status_.boundingBox(), - matching_distance_, hdmap_utils_ptr_); + matching_distance_, hdmap_utils_ptr_) + .value_or(math::geometry::hypot( + vertex.position.position, next_vertex->position.position)); }); }; diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 699d4af04d8..fd534fc887c 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -318,7 +318,7 @@ auto distanceAlongLanelet( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const geometry_msgs::msg::Point & to_position, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> double + const std::shared_ptr & hdmap_utils_ptr) -> std::optional { /// @note due to this hardcoded value, the method cannot be used for calculations along a crosswalk (for pedestrians) constexpr bool include_crosswalk{false}; @@ -329,14 +329,13 @@ auto distanceAlongLanelet( if (const auto to_lanelet_pose = hdmap_utils_ptr->toLaneletPose( to_position, to_bounding_box, include_crosswalk, 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 hdmap_utils_ptr->getLongitudinalDistance( + from_lanelet_pose.value(), to_lanelet_pose.value()); } } - return math::geometry::hypot(from_position, to_position); + return std::nullopt; +} + } } // namespace distance } // namespace traffic_simulator From 8a139f41d9975583557d3a22a545dbf32142ce63 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 12 Feb 2025 16:16:18 +0100 Subject: [PATCH 81/82] Adjust code to pass HdMapUtils as the last argument Signed-off-by: Mateusz Palczuk --- .../follow_polyline_trajectory_action.cpp | 5 ++--- .../follow_polyline_trajectory_action.cpp | 5 ++--- .../behavior/follow_trajectory/follow_trajectory.hpp | 5 ++--- .../src/behavior/follow_trajectory/follow_trajectory.cpp | 5 ++--- simulation/traffic_simulator/src/entity/ego_entity.cpp | 4 ++-- 5 files changed, 10 insertions(+), 14 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index f7511615483..e65fea793a7 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,9 +79,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, *polyline_trajectory, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), - step_time); + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, + getTargetSpeed(), step_time, hdmap_utils); entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 995e300b3af..fcf6fd40a68 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,9 +79,8 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, *polyline_trajectory, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), - step_time); + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, + getTargetSpeed(), step_time, hdmap_utils); entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp index 8ca41f235e7..1c2ea1fc646 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp @@ -31,10 +31,9 @@ namespace follow_trajectory /// @note side effects on polyline_trajectory auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, - const std::shared_ptr & hdmap_utils_ptr, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed, const double step_time) - -> std::optional; + const double matching_distance, const std::optional target_speed, const double step_time, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp index 183044560b6..84308585966 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -40,10 +40,9 @@ auto discardTheFrontWaypoint( auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, - const std::shared_ptr & hdmap_utils_ptr, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const double matching_distance, const std::optional target_speed, const double step_time) - -> std::optional + const double matching_distance, const std::optional target_speed, const double step_time, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional { assert(step_time > 0.0); while (not polyline_trajectory.shape.vertices.empty()) { diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index ef6d8186826..44cf3338cbb 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -156,8 +156,8 @@ void EgoEntity::onUpdate(double current_time, double step_time) traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*status_), behavior_parameter_, step_time), - hdmap_utils_ptr_, *polyline_trajectory_, - getDefaultMatchingDistanceForLaneletPoseCalculation(), getTargetSpeed(), step_time); + *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), + getTargetSpeed(), step_time, hdmap_utils_ptr_); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); From 8119a9901c7b59a354754eee871436fb19829ea9 Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 12 Feb 2025 16:19:44 +0100 Subject: [PATCH 82/82] spellcheck --- .../src/behavior/follow_trajectory/follow_trajectory.cpp | 1 - .../follow_trajectory/polyline_trajectory_positioner.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp index 183044560b6..6b571932661 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -33,7 +33,6 @@ namespace follow_trajectory { /// @note side effects on polyline_trajectory -/// @todo let Dawid know to analyse these free functions auto discardTheFrontWaypoint( traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) -> void; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp index 8fb40aa9aad..5b505e0cec2 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -368,7 +368,7 @@ auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optio /** * @note This clause is to avoid division-by-zero errors in later clauses with distance_to_nearest_waypoint * as the denominator if the distance miraculously becomes zero. - * @todo extendhis comment about < 0.0 + * @todo extend this comment about < 0.0 */ if (distance_to_nearest_waypoint_ <= 0.0 || total_remaining_distance_ <= 0) { return std::nullopt;