From 02b9faff617aa30f8ca3262b9352ee325306d13e Mon Sep 17 00:00:00 2001 From: robomic Date: Wed, 30 Oct 2024 10:39:08 +0100 Subject: [PATCH 01/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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/68] 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); } }