diff --git a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp index b720e58a09c..d6287f625c3 100644 --- a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp +++ b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp @@ -23,35 +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, Ts... xs) +template +constexpr auto isDefinitelyLessThan(T a, T b) -> bool { - 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 -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))); } 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/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..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/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..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 @@ -56,7 +56,7 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus { - auto getTargetSpeed = [&]() -> double { + const auto getTargetSpeed = [this]() -> double { if (target_speed.has_value()) { return target_speed.value(); } else { @@ -74,11 +74,15 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus THROW_SIMULATION_ERROR( "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " "case."); - } else if ( - const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*canonicalized_entity_status), - *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + } else if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status), + behavior_parameter, step_time), + hdmap_utils, *polyline_trajectory, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), + step_time); + entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); 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..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 @@ -56,7 +56,7 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus { - auto getTargetSpeed = [&]() -> double { + const auto getTargetSpeed = [this]() -> double { if (target_speed.has_value()) { return target_speed.value(); } else { @@ -74,11 +74,15 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus THROW_SIMULATION_ERROR( "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " "case."); - } else if ( - const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*canonicalized_entity_status), - *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, - default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + } else if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status), + behavior_parameter, step_time), + hdmap_utils, *polyline_trajectory, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), + step_time); + entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 9394ec74af8..8c25f2aa807 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -27,8 +27,10 @@ ament_auto_find_build_dependencies() ament_auto_add_library(traffic_simulator SHARED src/api/api.cpp - src/behavior/follow_trajectory.cpp - src/behavior/follow_waypoint_controller.cpp + src/behavior/follow_trajectory/follow_waypoint_controller.cpp + src/behavior/follow_trajectory/follow_trajectory.cpp + src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp + src/behavior/follow_trajectory/validated_entity_status.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp src/color_utils/color_utils.cpp 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..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 @@ -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/follow_trajectory.hpp similarity index 52% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp index da3293fc67e..8ca41f235e7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ #include +#include +#include +#include #include #include -#include #include #include @@ -26,13 +28,15 @@ namespace traffic_simulator { namespace follow_trajectory { -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus &, - traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, - const std::shared_ptr &, double, double, - std::optional target_speed = std::nullopt) -> std::optional; +/// @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; + } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp similarity index 91% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp index 761ce8bf2a5..547d8d357e9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ #include #include @@ -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)...)) @@ -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 " @@ -288,13 +287,13 @@ class FollowWaypointController const double speed) const -> double; auto areConditionsOfArrivalMet( - const double acceleration, const double speed, const double distance) const -> double + const double acceleration, const double speed, const double distance) const -> bool { - return (!with_breaking || std::abs(speed) < local_epsilon) && - std::abs(acceleration) < local_epsilon && distance < finish_distance_tolerance; + return (not with_breaking or std::abs(speed) < local_epsilon) and + std::abs(acceleration) < local_epsilon and distance < finish_distance_tolerance; } }; } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ 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 new file mode 100644 index 00000000000..a5578eba8b0 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -0,0 +1,76 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +struct PolylineTrajectoryPositioner +{ +public: + explicit PolylineTrajectoryPositioner( + const std::shared_ptr & hdmap_utils_ptr, + 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); + + 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::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_; + + const FollowWaypointController follow_waypoint_controller_; + + auto totalRemainingDistance() const -> double; + auto totalRemainingTime() const noexcept(false) -> double; + auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; + auto nearestWaypointWithSpecifiedTimeIterator() const + -> std::vector::const_iterator; + auto validatedEntityTargetPosition() const noexcept(false) -> geometry_msgs::msg::Point; + auto validatedEntityDesiredAcceleration() const noexcept(false) -> double; + auto validatedEntityDesiredVelocity(const double desired_speed) const noexcept(false) + -> geometry_msgs::msg::Vector3; + auto validatedEntityDesiredSpeed(const double desired_acceleration) const noexcept(false) + -> double; + auto validatePredictedState(const double desired_acceleration) const noexcept(false) -> void; +}; +} // namespace follow_trajectory +} // namespace traffic_simulator + +#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 new file mode 100644 index 00000000000..b7bb02d8c39 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp @@ -0,0 +1,110 @@ +// 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__VALIDATED_ENTITY_STATUS_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ + +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +struct ValidatedEntityStatus +{ +public: + explicit ValidatedEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) noexcept(false); + + auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const + -> traffic_simulator_msgs::msg::EntityStatus; + + ValidatedEntityStatus() = 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; + + // 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) + -> void; + + auto validateLinearSpeed(const double entity_speed) const noexcept(false) -> void; + + auto validateLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> void; + + auto validateBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> void; + + auto buildUpdatedPoseOrientation(const geometry_msgs::msg::Vector3 & desired_velocity) const + noexcept(true) -> geometry_msgs::msg::Quaternion; + + auto buildValidatedCurrentVelocity( + const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const + noexcept(false) -> geometry_msgs::msg::Vector3; + + template < + typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> + auto throwDetailedValidationError(const std::string & variable_name, const T variable) const + 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 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), + ", 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 +} // namespace traffic_simulator + +#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 5d362585dfb..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,7 +22,7 @@ #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/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 7919a140491..6a7da67102b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -112,6 +112,14 @@ auto distanceToStopLine( const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, const lanelet::Id target_stop_line_id, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + +auto distanceAlongLanelet( + const std::shared_ptr & hdmap_utils_ptr, + const geometry_msgs::msg::Point & from_position, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Point & to_position, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> double; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp deleted file mode 100644 index da2ced4710c..00000000000 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ /dev/null @@ -1,599 +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. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -namespace follow_trajectory -{ -template -auto any(F f, T && x, Ts &&... xs) -{ - 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); - } -} - -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - 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 -{ - using math::arithmetic::isApproximatelyEqualTo; - using math::arithmetic::isDefinitelyLessThan; - - using math::geometry::operator+; - using math::geometry::operator-; - using math::geometry::operator*; - using math::geometry::operator/; - 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; - - auto distance_along_lanelet = - [&](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 = [&]() { - /* - 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 = [&]() { - 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 = [&]() { - return first_waypoint_with_arrival_time_specified() >= - std::prev(polyline_trajectory.shape.vertices.end()); - }; - - /* - 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; any(is_infinity_or_nan, 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; - any(is_infinity_or_nan, 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(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())) { - return discard_the_front_waypoint_and_recurse(); - } else if ( - const auto [distance, remaining_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 = 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); - } - return total_distance; - }; - - if ( - first_waypoint_with_arrival_time_specified() != - std::end(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; - /* - 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()->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()); - } - } 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())) { - return discard_the_front_waypoint_and_recurse(); - } 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( - "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)) { - 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)) { - 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)) { - 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. - */ - const auto desired_acceleration = [&]() -> 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()); - } - }(); - std::isinf(desired_acceleration) or std::isnan(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)) { - 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 = - [&]() { - /* - 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."); - } - }(); - any(is_infinity_or_nan, 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 = - [&]() { - 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); - }(); - (speed * step_time) > distance_to_front_waypoint && - innerProduct(desired_velocity, current_velocity) < 0.0) { - return discard_the_front_waypoint_and_recurse(); - } 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()) { - 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, ". ", - 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 - } - - 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() == 1) { - /* - 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(); - } - } else { - /* - 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; - this_step_distance > distance_to_front_waypoint) { - return discard_the_front_waypoint_and_recurse(); - } - } - /* - 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 (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(); - } 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 = [&]() { - 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.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.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 diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp new file mode 100644 index 00000000000..6b571932661 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -0,0 +1,101 @@ +// 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 +{ + +/// @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, + 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, + target_speed, matching_distance, step_time) + .makeUpdatedEntityStatus(); + if (updated_entity_opt.has_value()) { + return updated_entity_opt; + } else { + discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.time()); + } + } + return std::nullopt; +} + +auto discardTheFrontWaypoint( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) + -> void +{ + 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 + "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: std::isfinite(polyline_trajectory.base_time) means + "Timing.domainAbsoluteRelative is relative". + + 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 ( + std::isfinite(polyline_trajectory.base_time) and + std::isfinite(polyline_trajectory.shape.vertices.front().time)) { + polyline_trajectory.base_time = current_time; + } + + std::rotate( + 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(); + } +}; + +} // 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_trajectory/follow_waypoint_controller.cpp similarity index 86% rename from simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index 96a23681a9a..a4b0f78c13f 100644 --- a/simulation/traffic_simulator/src/behavior/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 { @@ -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,14 +58,14 @@ 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."); } 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."); @@ -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) { @@ -273,7 +275,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( // Count the distance and time of movement with constant speed, use this to prediction. if (const double const_speed_distance = remaining_distance - state.traveled_distance; - const_speed_distance >= std::numeric_limits::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", @@ -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; @@ -334,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) { @@ -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)) { @@ -403,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); @@ -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/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp new file mode 100644 index 00000000000..7a9c902a764 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -0,0 +1,440 @@ +// 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 +{ + +PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( + const std::shared_ptr & hdmap_utils_ptr, + 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()), // 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) +{ +} + +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 +{ + /* + 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](const std::vector::const_iterator last) { + return std::accumulate( + polyline_trajectory_.shape.vertices.cbegin(), last, 0.0, + [this](const double total_distance, const auto & vertex) { + const auto next_vertex = std::next(&vertex); + return total_distance + distanceAlongLanelet( + hdmap_utils_ptr_, vertex.position.position, + validated_entity_status_.boundingBox(), + next_vertex->position.position, + validated_entity_status_.boundingBox(), matching_distance_); + }); + }; + + if (nearest_waypoint_with_specified_time_it_ == polyline_trajectory_.shape.vertices.cend()) { + return distance_to_nearest_waypoint_ + + total_distance_to(std::prev(polyline_trajectory_.shape.vertices.cend())); + } else { + 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_ == polyline_trajectory_.shape.vertices.cend()) { + return std::numeric_limits::infinity(); + } else { + const double remaining_time = + (std::isnan(polyline_trajectory_.base_time) ? 0.0 : polyline_trajectory_.base_time) + + nearest_waypoint_with_specified_time_it_->time - validated_entity_status_.time(); + + /* + The condition below should ideally be remaining_time < 0. + + 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 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 = 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 + : 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() + .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 PolylineTrajectoryPositioner::validatedEntityDesiredAcceleration() 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( + 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()), + "'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 PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optional +{ + /* + The following code implements the steering behavior known as "seek". See + "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more + information. + + 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+=; + + /* + This clause is to avoid division-by-zero errors in later clauses with + distance_to_nearest_waypoint as the denominator if the distance + miraculously becomes zero. + */ + if (distance_to_nearest_waypoint_ <= 0.0) { + return std::nullopt; + } + + if (total_remaining_distance_ <= 0) { + return std::nullopt; + } + + /* + The desired acceleration is the acceleration at which the destination + can be reached exactly at the specified time (= time remaining at zero). + + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. + */ + const double desired_acceleration = validatedEntityDesiredAcceleration(); + const double desired_speed = validatedEntityDesiredSpeed(desired_acceleration); + const auto desired_velocity = validatedEntityDesiredVelocity(desired_speed); + + if (const bool target_passed = + validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and + math::geometry::innerProduct(desired_velocity, validated_entity_status_.currentVelocity()) < + 0.0; + target_passed) { + return std::nullopt; + } + + 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(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_)) { + return std::nullopt; + } else { + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); + } + } else { + /* + If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is + irrelevant + */ + if (const double this_step_distance = + (validated_entity_status_.linearSpeed() + desired_acceleration * step_time_) * + step_time_; + this_step_distance > distance_to_nearest_waypoint_) { + return std::nullopt; + } else { + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); + } + } + /* + If there is insufficient time left for the next calculation step. + The value of step_time/2 is compared, as the remaining time is affected by floating point + inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + step is possible (time_to_nearest_waypoint is almost zero). + */ + } else if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { + if (follow_waypoint_controller_.areConditionsOfArrivalMet( + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), + distance_to_nearest_waypoint_)) { + return std::nullopt; + } else { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", + validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, + "s), has completed a trajectory to the nearest waypoint with", " specified time equal to ", + polyline_trajectory_.shape.vertices.front().time, "s at a distance equal to ", + distance_to_nearest_waypoint_, + " from that waypoint which is greater than the accepted accuracy."); + } + } else { + return validated_entity_status_.buildUpdatedEntityStatus(desired_velocity); + } + + /* + Note: If obstacle avoidance is to be implemented, the steering behavior + known by the name "collision avoidance" should be synthesized here into + steering. + */ +} + +auto PolylineTrajectoryPositioner::validatedEntityTargetPosition() const noexcept(false) + -> geometry_msgs::msg::Point +{ + if (polyline_trajectory_.shape.vertices.empty()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "attempted to dereference an element of an empty PolylineTrajectory for vehicle ", + std::quoted(validated_entity_status_.name())); + } + const auto & target_position = polyline_trajectory_.shape.vertices.front().position.position; + if (not math::geometry::isFinite(target_position)) { + 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 PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( + const double desired_acceleration) const noexcept(false) -> double +{ + const double desired_speed = + 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()), + "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); + } + 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_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()), + " 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_); + } +} + +} // namespace follow_trajectory +} // namespace traffic_simulator 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 new file mode 100644 index 00000000000..16d513f0f3f --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp @@ -0,0 +1,206 @@ +// 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) noexcept(false) +: step_time_(step_time), + entity_status_(entity_status), + behavior_parameter_(behavior_parameter), + current_velocity_(buildValidatedCurrentVelocity(linearSpeed(), orientation())) +{ + validateBehaviorParameter(behaviorParameter()); + validatePosition(position()); + validateLinearSpeed(linearSpeed()); + validateLinearAcceleration(linearAcceleration(), behaviorParameter(), step_time_); +} + +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 +{ + if (desired_velocity.x == 0.0 and desired_velocity.y == 0.0 and desired_velocity.z == 0.0) { + // do not change orientation if there is no designed_velocity vector + return orientation(); + } else { + // if there is a designed_velocity vector, set the orientation in the direction of it + const geometry_msgs::msg::Vector3 direction = + geometry_msgs::build() + .x(0.0) + .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) + .z(std::atan2(desired_velocity.y, desired_velocity.x)); + return math::geometry::convertEulerAngleToQuaternion(direction); + } +} + +auto ValidatedEntityStatus::buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_velocity) 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_time = entity_status_.time + step_time_; + const auto updated_pose_orientation = buildUpdatedPoseOrientation(desired_velocity); + const auto updated_action_status_twist_linear = + geometry_msgs::build() + .x(math::geometry::norm(desired_velocity)) + .y(0.0) + .z(0.0); + const auto updated_action_status_twist_angular = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(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_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() + .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::validatePosition( + const geometry_msgs::msg::Point & entity_position) const noexcept(false) -> void +{ + if (not math::geometry::isFinite(entity_position)) { + throwDetailedValidationError("entity_position", entity_position); + } +} + +auto ValidatedEntityStatus::validateLinearSpeed(const double entity_speed) const noexcept(false) + -> void +{ + if (not std::isfinite(entity_speed)) { + throwDetailedValidationError("entity_speed", entity_speed); + } +} + +auto ValidatedEntityStatus::validateLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> void +{ + 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)) { + throwDetailedValidationError("acceleration", acceleration); + } else if (not std::isfinite(max_acceleration)) { + throwDetailedValidationError("maximum acceleration", max_acceleration); + } else if (not std::isfinite(min_acceleration)) { + throwDetailedValidationError("minimum acceleration", min_acceleration); + } +} + +auto ValidatedEntityStatus::buildValidatedCurrentVelocity( + const double speed, const geometry_msgs::msg::Quaternion & entity_orientation) const + noexcept(false) -> geometry_msgs::msg::Vector3 +{ + const auto euler_angles = math::geometry::convertQuaternionToEulerAngle(entity_orientation); + const double pitch = -euler_angles.y; + const double yaw = euler_angles.z; + const auto entity_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * speed) + .y(std::cos(pitch) * std::sin(yaw) * speed) + .z(std::sin(pitch) * speed); + if (not math::geometry::isFinite(entity_velocity)) { + throwDetailedValidationError("entity_velocity", entity_velocity); + } + return entity_velocity; +} + +auto ValidatedEntityStatus::validateBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> void +{ + 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); + } +} +} // 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 215ea627064..95349935605 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -145,16 +145,23 @@ 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 = - 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::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*status_), behavior_parameter_, + step_time), + hdmap_utils_ptr_, *polyline_trajectory_, + getDefaultMatchingDistanceForLaneletPoseCalculation(), getTargetSpeed(), step_time); + non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 98bac102b88..cc0cae7c54b 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 @@ -92,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; @@ -311,5 +312,29 @@ auto distanceToStopLine( return spline.getCollisionPointIn2D(polygon); } } + +auto distanceAlongLanelet( + const std::shared_ptr & hdmap_utils_ptr, + const geometry_msgs::msg::Point & from_position, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Point & to_position, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> double +{ + if (const auto from_lanelet_pose = + hdmap_utils_ptr->toLaneletPose(from_position, from_bounding_box, false, matching_distance); + from_lanelet_pose.has_value()) { + if (const auto to_lanelet_pose = + hdmap_utils_ptr->toLaneletPose(to_position, to_bounding_box, false, matching_distance); + to_lanelet_pose.has_value()) { + if (const auto distance = hdmap_utils_ptr->getLongitudinalDistance( + from_lanelet_pose.value(), to_lanelet_pose.value()); + distance.has_value()) { + return distance.value(); + } + } + } + return math::geometry::hypot(from_position, to_position); +} } // namespace distance } // namespace traffic_simulator