diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp index 3a241210c77..6f534dd4130 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp @@ -25,8 +25,6 @@ struct FollowPolylineTrajectoryAction : public PedestrianActionNode { std::shared_ptr polyline_trajectory; - std::optional target_speed; - using PedestrianActionNode::PedestrianActionNode; auto calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp index ac0b4e00c68..73a3f220ccd 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp @@ -25,8 +25,6 @@ struct FollowPolylineTrajectoryAction : public VehicleActionNode { std::shared_ptr polyline_trajectory; - std::optional target_speed; - using VehicleActionNode::VehicleActionNode; auto calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; 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 b3a38ec0d37..85dd8d843bc 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,16 +56,27 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus { + auto getTargetSpeed = [&]() -> double { + if (target_speed.has_value()) { + return target_speed.value(); + } else { + return entity_status->getTwist().linear.x; + } + }; + if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; + } else if (std::isnan(entity_status->getTime())) { + THROW_SIMULATION_ERROR( + "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); } else if ( const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, step_time, target_speed)) { + behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) { setOutput( "updated_status", std::make_shared( 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 6b164b7214f..6d0ee0babf8 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,16 +56,27 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus { + auto getTargetSpeed = [&]() -> double { + if (target_speed.has_value()) { + return target_speed.value(); + } else { + return entity_status->getTwist().linear.x; + } + }; + if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; + } else if (std::isnan(entity_status->getTime())) { + THROW_SIMULATION_ERROR( + "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); } else if ( const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, step_time, target_speed)) { + behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) { setOutput( "updated_status", std::make_shared( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index 8547427f2ff..f25eb8b7dcb 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -131,9 +131,6 @@ class ScenarioSimulator : public rclcpp::Node auto updateTrafficLights(const simulation_api_schema::UpdateTrafficLightsRequest &) -> simulation_api_schema::UpdateTrafficLightsResponse; - auto followPolylineTrajectory(const simulation_api_schema::FollowPolylineTrajectoryRequest &) - -> simulation_api_schema::FollowPolylineTrajectoryResponse; - auto attachPseudoTrafficLightDetector( const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest &) -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index bd0b234b291..34cd85d89a1 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -40,8 +40,6 @@ class EgoEntitySimulation public: const std::unique_ptr autoware; - traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory; - private: const VehicleModelType vehicle_model_type_; @@ -92,6 +90,10 @@ class EgoEntitySimulation const std::shared_ptr &, const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope); + auto overwrite( + const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, + double step_time, bool npc_logic_started) -> void; + auto update(double time, double step_time, bool npc_logic_started) -> void; auto requestSpeedChange(double value) -> void; diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 9a5253c5304..2a5a52b6cc6 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -44,7 +44,6 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options) [this](auto &&... xs) { return attachDetectionSensor(std::forward(xs)...); }, [this](auto &&... xs) { return attachOccupancyGridSensor(std::forward(xs)...); }, [this](auto &&... xs) { return updateTrafficLights(std::forward(xs)...); }, - [this](auto &&... xs) { return followPolylineTrajectory(std::forward(xs)...); }, [this](auto &&... xs) { return attachPseudoTrafficLightDetector(std::forward(xs)...); }, @@ -159,8 +158,16 @@ auto ScenarioSimulator::updateEntityStatus( try { if (isEgo(status.name())) { assert(ego_entity_simulation_ && "Ego is spawned but ego_entity_simulation_ is nullptr!"); - ego_entity_simulation_->update( - current_scenario_time_ + step_time_, step_time_, req.npc_logic_started()); + if (req.overwrite_ego_status()) { + traffic_simulator_msgs::msg::EntityStatus ego_status_msg; + simulation_interface::toMsg(status, ego_status_msg); + ego_entity_simulation_->overwrite( + ego_status_msg, current_scenario_time_ + step_time_, step_time_, + req.npc_logic_started()); + } else { + ego_entity_simulation_->update( + current_scenario_time_ + step_time_, step_time_, req.npc_logic_started()); + } simulation_api_schema::EntityStatus ego_status; simulation_interface::toProto(ego_entity_simulation_->getStatus(), ego_status); entity_status_.at(status.name()) = ego_status; @@ -329,21 +336,6 @@ auto ScenarioSimulator::updateTrafficLights( return res; } -auto ScenarioSimulator::followPolylineTrajectory( - const simulation_api_schema::FollowPolylineTrajectoryRequest & request) - -> simulation_api_schema::FollowPolylineTrajectoryResponse -{ - auto response = simulation_api_schema::FollowPolylineTrajectoryResponse(); - if (ego_entity_simulation_ and isEgo(request.name())) { - ego_entity_simulation_->polyline_trajectory = - simulation_interface::toROS2Message(request.trajectory()); - response.mutable_result()->set_success(true); - } else { - response.mutable_result()->set_success(false); - } - return response; -} - auto ScenarioSimulator::attachPseudoTrafficLightDetector( const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest & req) -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index c88290c5975..e279f08601f 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace vehicle_simulation @@ -203,110 +202,114 @@ void EgoEntitySimulation::requestSpeedChange(double value) vehicle_model_ptr_->setState(v); } +void EgoEntitySimulation::overwrite( + const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, + double step_time, bool npc_logic_started) +{ + autoware->rethrow(); + + if (npc_logic_started) { + using quaternion_operation::convertQuaternionToEulerAngle; + using quaternion_operation::getRotationMatrix; + + auto world_relative_position = [&]() -> Eigen::VectorXd { + auto v = Eigen::VectorXd(3); + v(0) = status.pose.position.x - initial_pose_.position.x; + v(1) = status.pose.position.y - initial_pose_.position.y; + v(2) = status.pose.position.z - initial_pose_.position.z; + return getRotationMatrix(initial_pose_.orientation).transpose() * v; + }(); + + const auto yaw = [&]() { + const auto q = Eigen::Quaterniond( + getRotationMatrix(initial_pose_.orientation).transpose() * + getRotationMatrix(status.pose.orientation)); + geometry_msgs::msg::Quaternion relative_orientation; + relative_orientation.x = q.x(); + relative_orientation.y = q.y(); + relative_orientation.z = q.z(); + relative_orientation.w = q.w(); + return convertQuaternionToEulerAngle(relative_orientation).z - + (previous_linear_velocity_ ? *previous_angular_velocity_ : 0) * step_time; + }(); + + switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) { + case VehicleModelType::DELAY_STEER_ACC: + case VehicleModelType::DELAY_STEER_ACC_GEARED: + case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: + state(5) = status.action_status.accel.linear.x; + [[fallthrough]]; + + case VehicleModelType::DELAY_STEER_VEL: + state(4) = 0; + [[fallthrough]]; + + case VehicleModelType::IDEAL_STEER_ACC: + case VehicleModelType::IDEAL_STEER_ACC_GEARED: + state(3) = status.action_status.twist.linear.x; + [[fallthrough]]; + + case VehicleModelType::IDEAL_STEER_VEL: + state(0) = world_relative_position(0); + state(1) = world_relative_position(1); + state(2) = yaw; + vehicle_model_ptr_->setState(state); + break; + + default: + THROW_SEMANTIC_ERROR( + "Unsupported simulation model ", toString(vehicle_model_type_), " specified"); + } + } + updateStatus(current_scenario_time, step_time); + updatePreviousValues(); +} + void EgoEntitySimulation::update( double current_scenario_time, double step_time, bool npc_logic_started) { autoware->rethrow(); if (npc_logic_started) { - if (auto status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - status_, polyline_trajectory, traffic_simulator_msgs::msg::BehaviorParameter(), - step_time); - status) { - using quaternion_operation::convertQuaternionToEulerAngle; - using quaternion_operation::getRotationMatrix; - - auto world_relative_position = [&]() -> Eigen::VectorXd { - auto v = Eigen::VectorXd(3); - v(0) = status->pose.position.x - initial_pose_.position.x; - v(1) = status->pose.position.y - initial_pose_.position.y; - v(2) = status->pose.position.z - initial_pose_.position.z; - return getRotationMatrix(initial_pose_.orientation).transpose() * v; - }(); - - const auto yaw = [&]() { - const auto q = Eigen::Quaterniond( - getRotationMatrix(initial_pose_.orientation).transpose() * - getRotationMatrix(status->pose.orientation)); - geometry_msgs::msg::Quaternion relative_orientation; - relative_orientation.x = q.x(); - relative_orientation.y = q.y(); - relative_orientation.z = q.z(); - relative_orientation.w = q.w(); - return convertQuaternionToEulerAngle(relative_orientation).z - - (previous_linear_velocity_ ? *previous_angular_velocity_ : 0) * step_time; - }(); - - switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) { - case VehicleModelType::DELAY_STEER_ACC: - case VehicleModelType::DELAY_STEER_ACC_GEARED: - case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: - state(5) = status->action_status.accel.linear.x; - [[fallthrough]]; - - case VehicleModelType::DELAY_STEER_VEL: - state(4) = 0; - [[fallthrough]]; - - case VehicleModelType::IDEAL_STEER_ACC: - case VehicleModelType::IDEAL_STEER_ACC_GEARED: - state(3) = status->action_status.twist.linear.x; - [[fallthrough]]; - - case VehicleModelType::IDEAL_STEER_VEL: - state(0) = world_relative_position(0); - state(1) = world_relative_position(1); - state(2) = yaw; - vehicle_model_ptr_->setState(state); - break; - - default: - THROW_SEMANTIC_ERROR( - "Unsupported simulation model ", toString(vehicle_model_type_), " specified"); - } - } else { - auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); - - auto acceleration_by_slope = [this]() { - if (consider_acceleration_by_road_slope_) { - // calculate longitudinal acceleration by slope - constexpr double gravity_acceleration = -9.81; - const double ego_pitch_angle = calculateEgoPitch(); - const double slope_angle = -ego_pitch_angle; - return gravity_acceleration * std::sin(slope_angle); - } else { - return 0.0; - } - }(); - - switch (vehicle_model_type_) { - case VehicleModelType::DELAY_STEER_ACC: - case VehicleModelType::DELAY_STEER_ACC_GEARED: - case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: - case VehicleModelType::IDEAL_STEER_ACC: - case VehicleModelType::IDEAL_STEER_ACC_GEARED: - input(0) = - autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope); - input(1) = autoware->getSteeringAngle(); - break; - - case VehicleModelType::DELAY_STEER_VEL: - case VehicleModelType::IDEAL_STEER_VEL: - input(0) = autoware->getVelocity(); - input(1) = autoware->getSteeringAngle(); - break; - - default: - THROW_SEMANTIC_ERROR( - "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified"); + auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); + + auto acceleration_by_slope = [this]() { + if (consider_acceleration_by_road_slope_) { + // calculate longitudinal acceleration by slope + constexpr double gravity_acceleration = -9.81; + const double ego_pitch_angle = calculateEgoPitch(); + const double slope_angle = -ego_pitch_angle; + return gravity_acceleration * std::sin(slope_angle); + } else { + return 0.0; } - - vehicle_model_ptr_->setGear(autoware->getGearCommand().command); - vehicle_model_ptr_->setInput(input); - vehicle_model_ptr_->update(step_time); + }(); + + switch (vehicle_model_type_) { + case VehicleModelType::DELAY_STEER_ACC: + case VehicleModelType::DELAY_STEER_ACC_GEARED: + case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: + case VehicleModelType::IDEAL_STEER_ACC: + case VehicleModelType::IDEAL_STEER_ACC_GEARED: + input(0) = autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope); + input(1) = autoware->getSteeringAngle(); + break; + + case VehicleModelType::DELAY_STEER_VEL: + case VehicleModelType::IDEAL_STEER_VEL: + input(0) = autoware->getVelocity(); + input(1) = autoware->getSteeringAngle(); + break; + + default: + THROW_SEMANTIC_ERROR( + "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified"); } - } + vehicle_model_ptr_->setGear(autoware->getGearCommand().command); + vehicle_model_ptr_->setInput(input); + vehicle_model_ptr_->update(step_time); + } updateStatus(current_scenario_time, step_time); updatePreviousValues(); } @@ -315,7 +318,8 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const -> std::optional { - // @note The lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose + // @note The lanelet matching algorithm should be equivalent to the one used in + // EgoEntity::setMapPose const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }(); diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp index 0d14b562aab..1ecd65017cc 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp @@ -79,9 +79,6 @@ class MultiClient auto call(const simulation_api_schema::UpdateTrafficLightsRequest &) -> simulation_api_schema::UpdateTrafficLightsResponse; - auto call(const simulation_api_schema::FollowPolylineTrajectoryRequest &) - -> simulation_api_schema::FollowPolylineTrajectoryResponse; - auto call(const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest &) -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse; diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp index 97e4f24f8c5..c9156cbed1e 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp @@ -71,7 +71,6 @@ class MultiServer DEFINE_FUNCTION_TYPE(AttachDetectionSensor); DEFINE_FUNCTION_TYPE(AttachOccupancyGridSensor); DEFINE_FUNCTION_TYPE(UpdateTrafficLights); - DEFINE_FUNCTION_TYPE(FollowPolylineTrajectory); DEFINE_FUNCTION_TYPE(AttachPseudoTrafficLightDetector); DEFINE_FUNCTION_TYPE(UpdateStepTime); @@ -80,8 +79,8 @@ class MultiServer std::tuple< Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity, DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor, - AttachOccupancyGridSensor, UpdateTrafficLights, FollowPolylineTrajectory, - AttachPseudoTrafficLightDetector, UpdateStepTime> + AttachOccupancyGridSensor, UpdateTrafficLights, AttachPseudoTrafficLightDetector, + UpdateStepTime> functions_; }; } // namespace zeromq diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index 7f1c3f80d87..b2b4b409d44 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -188,6 +188,7 @@ message DespawnEntityResponse { message UpdateEntityStatusRequest { repeated EntityStatus status = 1; // List of updated entity status in traffic simulator. bool npc_logic_started = 2; // Npc logic started flag + bool overwrite_ego_status = 3; } /** @@ -309,21 +310,6 @@ message UpdateTrafficLightsResponse { Result result = 1; // Result of [DespawnEntityRequest](#DespawnEntityRequest) } -/** - * Request to make an entity follow a trajectory. - **/ -message FollowPolylineTrajectoryRequest { - string name = 1; - traffic_simulator_msgs.PolylineTrajectory trajectory = 2; -} - -/** - * Response whether the entity's trajectory following request has been accepted. - **/ -message FollowPolylineTrajectoryResponse { - Result result = 1; -} - /** * Requests updating simulation step time. **/ @@ -354,7 +340,6 @@ message SimulationRequest { AttachDetectionSensorRequest attach_detection_sensor = 9; AttachOccupancyGridSensorRequest attach_occupancy_grid_sensor = 10; UpdateTrafficLightsRequest update_traffic_lights = 11; - FollowPolylineTrajectoryRequest follow_polyline_trajectory = 12; AttachPseudoTrafficLightDetectorRequest attach_pseudo_traffic_light_detector = 13; UpdateStepTimeRequest update_step_time = 14; } @@ -377,7 +362,6 @@ message SimulationResponse { AttachDetectionSensorResponse attach_detection_sensor = 9; AttachOccupancyGridSensorResponse attach_occupancy_grid_sensor = 10; UpdateTrafficLightsResponse update_traffic_lights = 11; - FollowPolylineTrajectoryResponse follow_polyline_trajectory = 12; AttachPseudoTrafficLightDetectorResponse attach_pseudo_traffic_light_detector = 13; UpdateStepTimeResponse update_step_time = 14; } diff --git a/simulation/simulation_interface/src/zmq_multi_client.cpp b/simulation/simulation_interface/src/zmq_multi_client.cpp index 32483feaa86..c8993841e3c 100644 --- a/simulation/simulation_interface/src/zmq_multi_client.cpp +++ b/simulation/simulation_interface/src/zmq_multi_client.cpp @@ -194,18 +194,6 @@ auto MultiClient::call(const simulation_api_schema::UpdateTrafficLightsRequest & } } -auto MultiClient::call(const simulation_api_schema::FollowPolylineTrajectoryRequest & request) - -> simulation_api_schema::FollowPolylineTrajectoryResponse -{ - if (is_running) { - auto simulation_request = simulation_api_schema::SimulationRequest(); - *simulation_request.mutable_follow_polyline_trajectory() = request; - return call(simulation_request).follow_polyline_trajectory(); - } else { - return {}; - } -} - auto MultiClient::call( const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest & request) -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse diff --git a/simulation/simulation_interface/src/zmq_multi_server.cpp b/simulation/simulation_interface/src/zmq_multi_server.cpp index eb4c8211857..d5c0ee95298 100644 --- a/simulation/simulation_interface/src/zmq_multi_server.cpp +++ b/simulation/simulation_interface/src/zmq_multi_server.cpp @@ -73,10 +73,6 @@ void MultiServer::poll() *sim_response.mutable_update_traffic_lights() = std::get(functions_)(proto.update_traffic_lights()); break; - case simulation_api_schema::SimulationRequest::RequestCase::kFollowPolylineTrajectory: - *sim_response.mutable_follow_polyline_trajectory() = - std::get(functions_)(proto.follow_polyline_trajectory()); - break; case simulation_api_schema::SimulationRequest::RequestCase::kAttachPseudoTrafficLightDetector: *sim_response.mutable_attach_pseudo_traffic_light_detector() = std::get(functions_)( diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 86843200f1d..275e078f894 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -266,10 +266,6 @@ class API void startNpcLogic(); - auto requestFollowTrajectory( - const std::string &, const std::shared_ptr &) - -> bool; - void requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id); void requestLaneChange(const std::string & name, const lane_change::Direction & direction); @@ -344,6 +340,7 @@ class API FORWARD_TO_ENTITY_MANAGER(reachPosition); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); + FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); FORWARD_TO_ENTITY_MANAGER(requestSpeedChange); FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index e979358d28f..97d49013c6b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ #include +#include #include #include #include @@ -27,7 +28,8 @@ namespace follow_trajectory auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus &, traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, double step_time, + const traffic_simulator_msgs::msg::BehaviorParameter &, + const std::shared_ptr &, double, std::optional target_speed = std::nullopt) -> std::optional; } // namespace follow_trajectory diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp index 1a4154d13c5..89fa6610e33 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp @@ -144,7 +144,7 @@ class FollowWaypointController << ", max_deceleration: " << c.max_deceleration << ", max_acceleration_rate: " << c.max_acceleration_rate << ", max_deceleration_rate: " << c.max_deceleration_rate - << ", target_speed: " << c.target_speed << "."; + << ", target_speed: " << c.target_speed << ". "; return stream; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index d29a5a1b330..a72ec7dcadf 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -39,6 +39,11 @@ class EgoEntity : public VehicleEntity static auto makeFieldOperatorApplication(const Configuration &) -> std::unique_ptr; + bool is_controlled_by_simulator_{false}; + std::optional target_speed_; + traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; + std::shared_ptr polyline_trajectory_; + public: explicit EgoEntity() = delete; @@ -90,6 +95,9 @@ class EgoEntity : public VehicleEntity void requestAssignRoute(const std::vector &) override; + auto requestFollowTrajectory( + const std::shared_ptr &) -> void override; + void requestLaneChange(const lanelet::Id) override; auto requestLaneChange(const traffic_simulator::lane_change::Parameter &) -> void override; @@ -102,6 +110,8 @@ class EgoEntity : public VehicleEntity const speed_change::RelativeTargetSpeed &, const speed_change::Transition, const speed_change::Constraint, const bool continuous) -> void override; + auto isControlledBySimulator() const -> bool override; + auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override; 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 ad9547569dd..990766f7938 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -184,6 +184,8 @@ class EntityBase virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool); + virtual auto isControlledBySimulator() const -> bool; + virtual auto requestFollowTrajectory( const std::shared_ptr &) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 44fb81c32ee..ba42a8e9e05 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -309,6 +309,7 @@ class EntityManager FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); + FORWARD_TO_ENTITY(isControlledBySimulator, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 9f530e7007b..31e87cd8354 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -226,6 +226,9 @@ bool API::updateEntitiesStatusInSim() for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { auto entity_status = entity_manager_ptr_->getEntityStatus(entity_name); simulation_interface::toProto(static_cast(entity_status), *req.add_status()); + if (entity_manager_ptr_->isEgo(entity_name)) { + req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); + } } simulation_api_schema::UpdateEntityStatusResponse res; @@ -284,21 +287,6 @@ void API::startNpcLogic() clock_.start(); } -auto API::requestFollowTrajectory( - const std::string & name, - const std::shared_ptr & trajectory) -> bool -{ - if (entity_manager_ptr_->isEgo(name)) { - auto request = simulation_api_schema::FollowPolylineTrajectoryRequest(); - *request.mutable_name() = name; - *request.mutable_trajectory() = simulation_interface::toProtobufMessage(*trajectory); - return zeromq_client_.call(request).result().success(); - } else { - entity_manager_ptr_->requestFollowTrajectory(name, trajectory); - return true; - } -} - void API::requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id) { entity_manager_ptr_->requestLaneChange(name, lanelet_id); diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index ed4b4e6c985..7670a2d95f4 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -44,7 +45,8 @@ auto any(F f, T && x, Ts &&... xs) 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, double step_time, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const std::shared_ptr & hdmap_utils, double step_time, std::optional target_speed) -> std::optional { using math::arithmetic::isApproximatelyEqualTo; @@ -57,6 +59,7 @@ auto makeUpdatedStatus( using math::geometry::operator+=; using math::geometry::hypot; + using math::geometry::innerProduct; using math::geometry::norm; using math::geometry::normalize; using math::geometry::truncate; @@ -94,7 +97,7 @@ auto makeUpdatedStatus( } return makeUpdatedStatus( - entity_status, polyline_trajectory, behavior_parameter, step_time, target_speed); + entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, target_speed); }; auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; @@ -227,41 +230,41 @@ auto makeUpdatedStatus( 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] - isinf(acceleration) or isnan(acceleration)) { + 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, "."); + 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] */); - isinf(max_acceleration) or isnan(max_acceleration)) { + 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, "."); + "'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] */); - isinf(min_acceleration) or isnan(min_acceleration)) { + 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, "."); + "'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] - isinf(speed) or isnan(speed)) { + 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 @@ -314,14 +317,14 @@ auto makeUpdatedStatus( "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, "."); + desired_speed, ". "); } else if (const auto desired_velocity = [&]() { /* @@ -351,9 +354,22 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); + } else if (const auto current_velocity = + [&]() { + const auto yaw = quaternion_operation::convertQuaternionToEulerAngle( + entity_status.pose.orientation) + .z; + geometry_msgs::msg::Vector3 direction; + direction.x = std::cos(yaw) * speed; + direction.y = std::sin(yaw) * speed; + direction.z = 0.0; + return direction; + }(); + (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_to_front_waypoint, distance, acceleration, - speed); + 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 " @@ -361,7 +377,8 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, - ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", speed, "."); + ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", speed, ". ", + follow_waypoint_controller); } else { auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; if constexpr (false) { @@ -492,9 +509,6 @@ auto makeUpdatedStatus( " from that waypoint which is greater than the accepted accuracy."); } } - const auto current_velocity = - quaternion_operation::convertQuaternionToEulerAngle(entity_status.pose.orientation) * - entity_status.action_status.twist.linear.x; /* Note: If obstacle avoidance is to be implemented, the steering behavior @@ -543,7 +557,15 @@ auto makeUpdatedStatus( updated_status.time = entity_status.time + step_time; - updated_status.lanelet_pose_valid = false; + // matching distance has been set to 3.0 due to matching problems during lane changes + if (const auto lanelet_pose = + hdmap_utils->toLaneletPose(updated_status.pose, entity_status.bounding_box, false, 3.0); + lanelet_pose) { + updated_status.lanelet_pose = lanelet_pose.value(); + updated_status.lanelet_pose_valid = true; + } else { + updated_status.lanelet_pose_valid = false; + } return updated_status; } diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp index 85b2a894fad..71126289ef0 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp @@ -65,7 +65,11 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( auto FollowWaypointController::roundTimeToFullStepsWithTolerance( const double remaining_time_source, const double time_tolerance) const -> double { - if (remaining_time_source >= std::numeric_limits::max() * step_time) { + if (std::isnan(remaining_time_source) || std::isinf(remaining_time_source)) { + throw ControllerError( + "Value remaining_time_source (", remaining_time_source, + ") is NaN or inf - it cannot be rounded."); + } else if (remaining_time_source >= std::numeric_limits::max() * step_time) { throw ControllerError( "Exceeded the range of the variable type (", remaining_time_source, "/", step_time, ") - the value is too large. Probably the distance to the waypoint is extremely", @@ -154,8 +158,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 > max_speed || speed_min < -local_epsilon || - speed_min > max_speed) { + 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) { throw ControllerError( "Incorrect acceleration limits [", local_min_acceleration, ", ", local_max_acceleration, "] for acceleration: ", acceleration, " and speed: ", speed, " -> speed_min: ", speed_min, @@ -296,7 +300,7 @@ auto FollowWaypointController::getAcceleration( const double step_acceleration = (local_max_acceleration - local_min_acceleration) / number_of_acceleration_candidates; - auto min_distance_diff = std::numeric_limits::max(); + auto min_distance_diff = std::numeric_limits::lowest(); std::optional best_acceleration = std::nullopt; @@ -307,8 +311,8 @@ auto FollowWaypointController::getAcceleration( candidate_acceleration, remaining_distance, acceleration, speed); predicted_state_opt) { if (const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance; - distance_diff >= 0 && - (std::abs(distance_diff) < std::abs(min_distance_diff) || min_distance_diff < 0)) { + (distance_diff >= 0 || min_distance_diff < 0) && + (std::abs(distance_diff) < std::abs(min_distance_diff))) { min_distance_diff = distance_diff; best_acceleration = candidate_acceleration; } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 30cbaa0a083..e3a056c2dc8 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -100,14 +100,10 @@ auto EgoEntity::getCurrentAction() const -> std::string auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter { - traffic_simulator_msgs::msg::BehaviorParameter parameter; /** * @brief TODO, Input values get from autoware. */ - parameter.see_around = true; - parameter.dynamic_constraints.max_acceleration = 0; - parameter.dynamic_constraints.max_deceleration = 0; - return parameter; + return behavior_parameter_; } auto EgoEntity::getEntityTypename() const -> const std::string & @@ -155,6 +151,18 @@ void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); + if (is_controlled_by_simulator_ && npc_logic_started_) { + if ( + const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(status_), *polyline_trajectory_, + behavior_parameter_, hdmap_utils_ptr_, step_time, + target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { + setStatus(CanonicalizedEntityStatus(*updated_status, hdmap_utils_ptr_)); + } else { + is_controlled_by_simulator_ = false; + } + } + updateStandStillDuration(step_time); updateTraveledDistance(step_time); @@ -209,6 +217,16 @@ void EgoEntity::requestAssignRoute(const std::vector & } } +auto EgoEntity::isControlledBySimulator() const -> bool { return is_controlled_by_simulator_; } + +auto EgoEntity::requestFollowTrajectory( + const std::shared_ptr & parameter) -> void +{ + polyline_trajectory_ = parameter; + VehicleEntity::requestFollowTrajectory(parameter); + is_controlled_by_simulator_ = true; +} + void EgoEntity::requestLaneChange(const lanelet::Id) { THROW_SEMANTIC_ERROR( @@ -247,12 +265,15 @@ auto EgoEntity::getDefaultDynamicConstraints() const THROW_SEMANTIC_ERROR("getDefaultDynamicConstraints function does not support EgoEntity"); } -auto EgoEntity::setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void +auto EgoEntity::setBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) -> void { + behavior_parameter_ = behavior_parameter; } void EgoEntity::requestSpeedChange(double value, bool) { + target_speed_ = value; field_operator_application->restrictTargetSpeed(value); } @@ -263,6 +284,7 @@ void EgoEntity::requestSpeedChange( auto EgoEntity::setVelocityLimit(double value) -> void // { + behavior_parameter_.dynamic_constraints.max_speed = value; field_operator_application->setVelocityLimit(value); } diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 016f1aec130..6bbdf68167b 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -654,6 +654,8 @@ void EntityBase::requestSpeedChange( } } +auto EntityBase::isControlledBySimulator() const -> bool { return true; } + auto EntityBase::requestFollowTrajectory( const std::shared_ptr &) -> void { diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 2bd9a67537e..18568ecb05b 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -227,7 +227,7 @@ void VehicleEntity::requestAssignRoute(const std::vectorsetPolylineTrajectory(parameter); behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_POLYLINE_TRAJECTORY); + std::vector waypoints; + for (const auto & vertex : parameter->shape.vertices) { + if (const auto lanelet_waypoint = + hdmap_utils_ptr_->toLaneletPose(vertex.position, getBoundingBox(), false); + lanelet_waypoint) { + waypoints.emplace_back(CanonicalizedLaneletPose(lanelet_waypoint.value(), hdmap_utils_ptr_)); + } else { + /// @todo such a protection most likely makes sense, but test scenario + /// RoutingAction.FollowTrajectoryAction-star has waypoints outside lanelet2 + // THROW_SEMANTIC_ERROR("FollowTrajectory waypoint should be on lane."); + } + } + route_planner_.setWaypoints(waypoints); } void VehicleEntity::requestLaneChange(const lanelet::Id to_lanelet_id)