Skip to content

Commit

Permalink
refactor(behavior_velocity_planner_common): add behavior_velocity_rtc…
Browse files Browse the repository at this point in the history
…_interface and move RTC-related implementation (autowarefoundation#9799)

* split into planer_common and rtc_interface

Signed-off-by: Takayuki Murooka <[email protected]>

* Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

Co-authored-by: Mamoru Sobue <[email protected]>

* Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp

Co-authored-by: Mamoru Sobue <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
Co-authored-by: Mamoru Sobue <[email protected]>
  • Loading branch information
2 people authored and kyoichi-sugahara committed Jan 6, 2025
1 parent 76cbad7 commit ebe9ffe
Show file tree
Hide file tree
Showing 47 changed files with 562 additions and 381 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand All @@ -42,8 +42,8 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC

void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path) override;
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
};

class BlindSpotModulePlugin : public PluginWrapper<BlindSpotModuleManager>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_

#include <autoware/behavior_velocity_blind_spot_module/util.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/predicted_objects.hpp>
Expand Down Expand Up @@ -59,7 +59,7 @@ struct Safe

using BlindSpotDecision = std::variant<InternalError, OverPassJudge, Unsafe, Safe>;

class BlindSpotModule : public SceneModuleInterface
class BlindSpotModule : public SceneModuleInterfaceWithRTC
{
public:
struct DebugData
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_behavior_velocity_rtc_interface</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_perception_msgs</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
BlindSpotModuleManager::getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ BlindSpotModule::BlindSpotModule(
const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterfaceWithRTC(module_id, logger, clock),
lane_id_(lane_id),
planner_param_{planner_param},
turn_direction_(turn_direction),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_behavior_velocity_rtc_interface</depend>
<depend>autoware_grid_map_utils</depend>
<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_interpolation</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
Expand All @@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id());
}

return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
return crosswalk_id_set.count(scene_module->getModuleId()) == 0;
};
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
#include <rclcpp/rclcpp.hpp>

Expand Down Expand Up @@ -48,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC

void launchNewModules(const PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const PathWithLaneId & path) override;
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
getModuleExpiredFunction(const PathWithLaneId & path) override;
};

class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ CrosswalkModule::CrosswalkModule(
const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterfaceWithRTC(module_id, logger, clock),
module_id_(module_id),
planner_param_(planner_param),
use_regulatory_element_(reg_elem_id)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include "autoware/behavior_velocity_crosswalk_module/util.hpp"

#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
Expand Down Expand Up @@ -112,7 +112,7 @@ double InterpolateMap(
}
} // namespace

class CrosswalkModule : public SceneModuleInterface
class CrosswalkModule : public SceneModuleInterfaceWithRTC
{
public:
struct PlannerParam
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_behavior_velocity_rtc_interface</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_planning_msgs</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -74,16 +74,17 @@ void DetectionAreaModuleManager::launchNewModules(
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
DetectionAreaModuleManager::getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path)
{
const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath<DetectionArea>(
path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);

return [detection_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return detection_area_id_set.count(scene_module->getModuleId()) == 0;
};
return
[detection_area_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
return detection_area_id_set.count(scene_module->getModuleId()) == 0;
};
}

} // namespace autoware::behavior_velocity_planner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand All @@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC

void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path) override;
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
};

class DetectionAreaModulePlugin : public PluginWrapper<DetectionAreaModuleManager>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule(
const lanelet::autoware::DetectionArea & detection_area_reg_elem,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterfaceWithRTC(module_id, logger, clock),
lane_id_(lane_id),
detection_area_reg_elem_(detection_area_reg_elem),
state_(State::GO),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <autoware_lanelet2_extension/regulatory_elements/detection_area.hpp>
#include <rclcpp/rclcpp.hpp>

Expand All @@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair<size_t, Point2d>; // front
using PathIndexWithOffset = std::pair<size_t, double>; // front index, offset
using tier4_planning_msgs::msg::PathWithLaneId;

class DetectionAreaModule : public SceneModuleInterface
class DetectionAreaModule : public SceneModuleInterfaceWithRTC
{
public:
enum class State { GO, STOP };
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_behavior_velocity_rtc_interface</depend>
<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_interpolation</depend>
<depend>autoware_lanelet2_extension</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -353,14 +353,14 @@ void IntersectionModuleManager::launchNewModules(
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
IntersectionModuleManager::getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path)
{
const auto lane_set = planning_utils::getLaneletsOnPath(
path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);

return [lane_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return [lane_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
const auto & associative_ids = intersection_module->getAssociativeIds();
for (const auto & lane : lane_set) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <rclcpp/rclcpp.hpp>

#include <std_msgs/msg/string.hpp>
Expand All @@ -47,8 +48,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC

void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path) override;
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;

Expand All @@ -63,7 +64,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
tl_observation_pub_;
};

class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<>
{
public:
explicit MergeFromPrivateModuleManager(rclcpp::Node & node);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ IntersectionModule::IntersectionModule(
const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterfaceWithRTC(module_id, logger, clock),
planner_param_(planner_param),
lane_id_(lane_id),
associative_ids_(associative_ids),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@
#include "object_manager.hpp"
#include "result.hpp"

#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
#include <rclcpp/rclcpp.hpp>

Expand All @@ -46,7 +46,7 @@
namespace autoware::behavior_velocity_planner
{

class IntersectionModule : public SceneModuleInterface
class IntersectionModule : public SceneModuleInterfaceWithRTC
{
public:
struct PlannerParam
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@

namespace autoware::behavior_velocity_planner
{
class NoDrivableLaneModuleManager : public SceneModuleManagerInterface
class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<>
{
public:
explicit NoDrivableLaneModuleManager(rclcpp::Node & node);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_behavior_velocity_planner_common</depend>
<depend>autoware_behavior_velocity_rtc_interface</depend>
<depend>autoware_interpolation</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_motion_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,16 +69,17 @@ void NoStoppingAreaModuleManager::launchNewModules(
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
NoStoppingAreaModuleManager::getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path)
{
const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath<NoStoppingArea>(
path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);

return [no_stopping_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0;
};
return
[no_stopping_area_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0;
};
}

} // namespace autoware::behavior_velocity_planner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand All @@ -41,8 +41,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC

void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path) override;
std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
};

class NoStoppingAreaModulePlugin : public PluginWrapper<NoStoppingAreaModuleManager>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ NoStoppingAreaModule::NoStoppingAreaModule(
const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterfaceWithRTC(module_id, logger, clock),
lane_id_(lane_id),
no_stopping_area_reg_elem_(no_stopping_area_reg_elem),
planner_param_(planner_param),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@

#include "utils.hpp"

#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/state_machine.hpp>
#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
#include <autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp>
#include <rclcpp/rclcpp.hpp>

Expand All @@ -31,7 +31,7 @@

namespace autoware::behavior_velocity_planner
{
class NoStoppingAreaModule : public SceneModuleInterface
class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC
{
public:
struct PlannerParam
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

namespace autoware::behavior_velocity_planner
{
class OcclusionSpotModuleManager : public SceneModuleManagerInterface
class OcclusionSpotModuleManager : public SceneModuleManagerInterface<>
{
public:
explicit OcclusionSpotModuleManager(rclcpp::Node & node);
Expand Down
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
# Behavior Velocity Planner Common

This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules.
This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules.
Loading

0 comments on commit ebe9ffe

Please sign in to comment.