diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp index c1d000c4d17..a12733f2339 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,7 +56,8 @@ class ActionNodeBase : public BT::ActionNodeBase */ static BT::PortsList providedPorts() { - return {// clang-format off + return { + // clang-format off BT::InputPort>("activator"), BT::InputPort("request"), BT::InputPort>("hdmap_utils"), @@ -73,7 +74,7 @@ class ActionNodeBase : public BT::ActionNodeBase BT::OutputPort("request"), BT::InputPort("other_entity_status"), BT::InputPort>("route_lanelets"), - BT::InputPort>("traffic_light_manager"), + BT::InputPort>("traffic_lights"), BT::OutputPort>("hdmap_utils"), BT::OutputPort>("obstacle"), BT::OutputPort("waypoints")}; @@ -89,7 +90,7 @@ class ActionNodeBase : public BT::ActionNodeBase std::shared_ptr activator; traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; - std::shared_ptr traffic_light_manager; + std::shared_ptr traffic_lights; std::shared_ptr entity_status; double current_time; double step_time; diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp index 9511e824648..22c29382e33 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include namespace context_gamma_planner { @@ -62,7 +62,7 @@ class ConstraintActivatorBase public: ConstraintActivatorBase( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr); + const std::shared_ptr traffic_lights_ptr); const auto calculateRVOObstacles() -> std::vector >; void deactivateAllConstraints(); void appendRoadEdgeConstraint(const lanelet::Id lanelet_id, const State & state = State::ACTIVE); @@ -96,7 +96,7 @@ class ConstraintActivatorBase std::vector queryRoadEdgeConstraint( const geometry_msgs::msg::Point & p, double distance_threashold, const char subtype[]); std::shared_ptr hd_map_utils_ptr_; - std::shared_ptr traffic_light_manager_ptr_; + std::shared_ptr traffic_lights_ptr_; std::vector lanelet_subtypes_; std::vector traffic_light_constraints_; std::vector lane_constraints_; diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp index edc9e0c1af1..c32d7102fd5 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include namespace context_gamma_planner { @@ -34,7 +34,7 @@ class ConstraintActivator : public context_gamma_planner::constraints::Constrain public: ConstraintActivator( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr); + const std::shared_ptr traffic_lights_ptr); }; } // namespace constraints } // namespace pedestrian diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp index de840cb8a33..15d985516ce 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include namespace context_gamma_planner { @@ -34,7 +34,7 @@ class ConstraintActivator : public context_gamma_planner::constraints::Constrain public: ConstraintActivator( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr); + const std::shared_ptr traffic_lights_ptr); /// @param stop_velocity_threshold [m/s] Set a small value since a complete stop is not possible. void appendStopLineConstraint( const lanelet::Ids & route_ids, diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp new file mode 100644 index 00000000000..ad27141c7f6 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp @@ -0,0 +1,97 @@ +// 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 CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ +#define CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace mock +{ +class Catalogs +{ +public: + /** + * @brief Get the default vehicle parameters. + * @return traffic_simulator_msgs::msg::VehicleParameters The vehicle parameters. + */ + auto getVehicleParameters() const -> traffic_simulator_msgs::msg::VehicleParameters + { + traffic_simulator_msgs::msg::VehicleParameters parameters; + parameters.name = "vehicle.volkswagen.t"; + parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + parameters.performance.max_speed = 69.444; + parameters.performance.max_acceleration = 200; + parameters.performance.max_deceleration = 10.0; + parameters.bounding_box.center.x = 1.5; + parameters.bounding_box.center.y = 0.0; + parameters.bounding_box.center.z = 0.9; + parameters.bounding_box.dimensions.x = 4.5; + parameters.bounding_box.dimensions.y = 1.7; + parameters.bounding_box.dimensions.z = 1.8; + parameters.axles.front_axle.max_steering = 0.5; + parameters.axles.front_axle.wheel_diameter = 0.6; + parameters.axles.front_axle.track_width = 1.8; + parameters.axles.front_axle.position_x = 3.1; + parameters.axles.front_axle.position_z = 0.3; + parameters.axles.rear_axle.max_steering = 0.0; + parameters.axles.rear_axle.wheel_diameter = 0.6; + parameters.axles.rear_axle.track_width = 1.8; + parameters.axles.rear_axle.position_x = 0.0; + parameters.axles.rear_axle.position_z = 0.3; + return parameters; + } + + /** + * @brief Get the default pedestrian parameters. + * @return traffic_simulator_msgs::msg::PedestrianParameters The pedestrian parameters. + */ + auto getPedestrianParameters() const -> traffic_simulator_msgs::msg::PedestrianParameters + { + traffic_simulator_msgs::msg::PedestrianParameters parameters; + parameters.name = "pedestrian"; + parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; + parameters.bounding_box.center.x = 0.0; + parameters.bounding_box.center.y = 0.0; + parameters.bounding_box.center.z = 0.5; + parameters.bounding_box.dimensions.x = 1.0; + parameters.bounding_box.dimensions.y = 1.0; + parameters.bounding_box.dimensions.z = 2.0; + return parameters; + } + + /** + * @brief Get the default parameters of a miscellaneous object. + * @return traffic_simulator_msgs::msg::MiscObjectParameters The parameters of the miscellaneous object. + */ + auto getMiscObjectParameters() const -> traffic_simulator_msgs::msg::MiscObjectParameters + { + traffic_simulator_msgs::msg::MiscObjectParameters misc_object_param; + misc_object_param.bounding_box.dimensions.x = 1.0; + misc_object_param.bounding_box.dimensions.y = 1.0; + misc_object_param.bounding_box.dimensions.z = 1.0; + misc_object_param.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; + misc_object_param.name = "obstacle"; + return misc_object_param; + } +}; +} // namespace mock +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp index ec408c4d9cf..d40cefc98e3 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp @@ -66,7 +66,7 @@ class PedestrianPlugin : public entity_behavior::BehaviorPluginBase DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) DEFINE_GETTER_SETTER(GoalPoses, std::vector) diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp index cf3a82e2acf..82c71dc5afc 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp @@ -65,7 +65,7 @@ class VehiclePlugin : public entity_behavior::BehaviorPluginBase DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) DEFINE_GETTER_SETTER(GoalPoses, std::vector) diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp index 8e1a55b83f6..5ba89c4fa70 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp @@ -21,9 +21,9 @@ namespace constraints { ConstraintActivatorBase::ConstraintActivatorBase( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr) + const std::shared_ptr traffic_lights_ptr) : hd_map_utils_ptr_(hd_map_utils_ptr), - traffic_light_manager_ptr_(traffic_light_manager_ptr), + traffic_lights_ptr_(traffic_lights_ptr), is_stoped_(false) { for (const auto & id : hd_map_utils_ptr_->getLaneletIds()) { diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp index 08a7bf227c1..419cf5492f8 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp @@ -25,9 +25,9 @@ using State = context_gamma_planner::constraints::State; ConstraintActivator::ConstraintActivator( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr) + const std::shared_ptr traffic_lights_ptr) : context_gamma_planner::constraints::ConstraintActivatorBase( - hd_map_utils_ptr, traffic_light_manager_ptr) + hd_map_utils_ptr, traffic_lights_ptr) { } diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp index 81f8bce6858..77314da821c 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp @@ -26,9 +26,9 @@ using Side = context_gamma_planner::constraints::RoadEdgeConstraint::Side; ConstraintActivator::ConstraintActivator( const std::shared_ptr hd_map_utils_ptr, - const std::shared_ptr traffic_light_manager_ptr) + const std::shared_ptr traffic_lights_ptr) : context_gamma_planner::constraints::ConstraintActivatorBase( - hd_map_utils_ptr, traffic_light_manager_ptr) + hd_map_utils_ptr, traffic_lights_ptr) { for (const auto & id : hd_map_utils_ptr_->getTrafficLightIds()) { const auto stop_line_ids = hd_map_utils_ptr_->getTrafficLightStopLineIds(id); @@ -80,7 +80,7 @@ void ConstraintActivator::appendStopLineConstraint( void ConstraintActivator::appendTrafficLightConstraint(const lanelet::Ids & route_ids) { for (const auto & traffic_light_id : hd_map_utils_ptr_->getTrafficLightIdsOnPath(route_ids)) { - if (traffic_light_manager_ptr_->getTrafficLight(traffic_light_id) + if (traffic_lights_ptr_->getTrafficLight(traffic_light_id) .contains( traffic_simulator::TrafficLight::Color::green, traffic_simulator::TrafficLight::Status::solid_on, diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp index b078665b043..b5136d6a7fd 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp +++ b/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp @@ -135,7 +135,7 @@ void PedestrianPlugin::tryInitializeConstraintActivator() if (activator_ptr_ == nullptr) { activator_ptr_ = std::make_shared( - getHdMapUtils(), getTrafficLightManager()); + getHdMapUtils(), getTrafficLights()); setConstraintActivator(activator_ptr_); } } diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp index f177d86238a..75be91974ad 100644 --- a/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp +++ b/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp @@ -138,7 +138,7 @@ void VehiclePlugin::tryInitializeConstraintActivator() if (activator_ptr_ == nullptr) { activator_ptr_ = std::make_shared( - getHdMapUtils(), getTrafficLightManager()); + getHdMapUtils(), getTrafficLights()); setConstraintActivator(activator_ptr_); } } diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt b/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt index 076e655d91a..5ccaf71b52c 100644 --- a/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt +++ b/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt @@ -75,7 +75,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY config launch + DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_auto_package()