Skip to content

Commit

Permalink
Refactored functions that involves with traffic lights
Browse files Browse the repository at this point in the history
  • Loading branch information
curious-jp committed Dec 18, 2024
1 parent 9a41f3c commit 3041e3d
Show file tree
Hide file tree
Showing 13 changed files with 121 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
#include <traffic_simulator/entity/entity_base.hpp>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator/helper/stop_watch.hpp>
#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
#include <traffic_simulator/traffic_lights/traffic_lights.hpp>
#include <traffic_simulator_msgs/msg/obstacle.hpp>
#include <traffic_simulator_msgs/msg/waypoints_array.hpp>
#include <unordered_map>
Expand All @@ -56,7 +56,8 @@ class ActionNodeBase : public BT::ActionNodeBase
*/
static BT::PortsList providedPorts()
{
return {// clang-format off
return {
// clang-format off
BT::InputPort<std::shared_ptr<constraints::ConstraintActivatorBase>>("activator"),
BT::InputPort<traffic_simulator::behavior::Request>("request"),
BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
Expand All @@ -73,7 +74,7 @@ class ActionNodeBase : public BT::ActionNodeBase
BT::OutputPort<traffic_simulator::behavior::Request>("request"),
BT::InputPort<entity_behavior::EntityStatusDict>("other_entity_status"),
BT::InputPort<std::vector<lanelet::Id>>("route_lanelets"),
BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>("traffic_light_manager"),
BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLights>>("traffic_lights"),
BT::OutputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>("obstacle"),
BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints")};
Expand All @@ -89,7 +90,7 @@ class ActionNodeBase : public BT::ActionNodeBase
std::shared_ptr<constraints::ConstraintActivatorBase> activator;
traffic_simulator::behavior::Request request;
std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager;
std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights;
std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> entity_status;
double current_time;
double step_time;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#include <geometry/spline/catmull_rom_subspline.hpp>
#include <memory>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
#include <traffic_simulator/traffic_lights/traffic_lights.hpp>

namespace context_gamma_planner
{
Expand Down Expand Up @@ -62,7 +62,7 @@ class ConstraintActivatorBase
public:
ConstraintActivatorBase(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr);
const std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights_ptr);
const auto calculateRVOObstacles() -> std::vector<std::vector<RVO::Vector2> >;
void deactivateAllConstraints();
void appendRoadEdgeConstraint(const lanelet::Id lanelet_id, const State & state = State::ACTIVE);
Expand Down Expand Up @@ -96,7 +96,7 @@ class ConstraintActivatorBase
std::vector<RoadEdgeConstraint> queryRoadEdgeConstraint(
const geometry_msgs::msg::Point & p, double distance_threashold, const char subtype[]);

Check warning on line 97 in simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp

View workflow job for this annotation

GitHub Actions / spell-check

Unknown word (threashold)
std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr_;
std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr_;
std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights_ptr_;
std::vector<std::string> lanelet_subtypes_;
std::vector<TrafficLightConstraint> traffic_light_constraints_;
std::vector<RoadEdgeConstraint> lane_constraints_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <geometry/spline/catmull_rom_subspline.hpp>
#include <memory>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
#include <traffic_simulator/traffic_lights/traffic_lights.hpp>

namespace context_gamma_planner
{
Expand All @@ -34,7 +34,7 @@ class ConstraintActivator : public context_gamma_planner::constraints::Constrain
public:
ConstraintActivator(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr);
const std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights_ptr);
};
} // namespace constraints
} // namespace pedestrian
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <geometry/spline/catmull_rom_subspline.hpp>
#include <memory>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
#include <traffic_simulator/traffic_lights/traffic_lights.hpp>

namespace context_gamma_planner
{
Expand All @@ -34,7 +34,7 @@ class ConstraintActivator : public context_gamma_planner::constraints::Constrain
public:
ConstraintActivator(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr);
const std::shared_ptr<traffic_simulator::TrafficLights> 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,
Expand Down
Original file line number Diff line number Diff line change
@@ -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 <string>
#include <traffic_simulator_msgs/msg/misc_object_parameters.hpp>
#include <traffic_simulator_msgs/msg/pedestrian_parameters.hpp>
#include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>

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_
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ class PedestrianPlugin : public entity_behavior::BehaviorPluginBase
DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr<math::geometry::CatmullRomSpline>)
DEFINE_GETTER_SETTER(StepTime, double)
DEFINE_GETTER_SETTER(TargetSpeed, std::optional<double>)
DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr<traffic_simulator::TrafficLightManager>)
DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr<traffic_simulator::TrafficLights>)
DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
DEFINE_GETTER_SETTER(GoalPoses, std::vector<geometry_msgs::msg::Pose>)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ class VehiclePlugin : public entity_behavior::BehaviorPluginBase
DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr<math::geometry::CatmullRomSpline>)
DEFINE_GETTER_SETTER(StepTime, double)
DEFINE_GETTER_SETTER(TargetSpeed, std::optional<double>)
DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr<traffic_simulator::TrafficLightManager>)
DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr<traffic_simulator::TrafficLights>)
DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
DEFINE_GETTER_SETTER(GoalPoses, std::vector<geometry_msgs::msg::Pose>)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@ namespace constraints
{
ConstraintActivatorBase::ConstraintActivatorBase(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr)
const std::shared_ptr<traffic_simulator::TrafficLights> 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()) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,9 @@ using State = context_gamma_planner::constraints::State;

ConstraintActivator::ConstraintActivator(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr)
const std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights_ptr)
: context_gamma_planner::constraints::ConstraintActivatorBase(
hd_map_utils_ptr, traffic_light_manager_ptr)
hd_map_utils_ptr, traffic_lights_ptr)
{
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,9 @@ using Side = context_gamma_planner::constraints::RoadEdgeConstraint::Side;

ConstraintActivator::ConstraintActivator(
const std::shared_ptr<hdmap_utils::HdMapUtils> hd_map_utils_ptr,
const std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager_ptr)
const std::shared_ptr<traffic_simulator::TrafficLights> 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);
Expand Down Expand Up @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ void PedestrianPlugin::tryInitializeConstraintActivator()
if (activator_ptr_ == nullptr) {
activator_ptr_ =
std::make_shared<context_gamma_planner::pedestrian::constraints::ConstraintActivator>(
getHdMapUtils(), getTrafficLightManager());
getHdMapUtils(), getTrafficLights());
setConstraintActivator(activator_ptr_);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ void VehiclePlugin::tryInitializeConstraintActivator()
if (activator_ptr_ == nullptr) {
activator_ptr_ =
std::make_shared<context_gamma_planner::vehicle::constraints::ConstraintActivator>(
getHdMapUtils(), getTrafficLightManager());
getHdMapUtils(), getTrafficLights());
setConstraintActivator(activator_ptr_);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ if(BUILD_TESTING)
endif()

install(
DIRECTORY config launch
DIRECTORY launch
DESTINATION share/${PROJECT_NAME})

ament_auto_package()

0 comments on commit 3041e3d

Please sign in to comment.