Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(behavior_path_planner): remove velocity factor #1746

Merged
merged 1 commit into from
Dec 27, 2024
Merged

fix(behavior_path_planner): remove velocity factor #1746

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions ...havior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
#include "planner_manager.hpp"

#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
#include <autoware/motion_utils/factor/steering_factor_interface.hpp>
#include <autoware/universe_utils/ros/polling_subscriber.hpp>
#include <autoware/universe_utils/ros/published_time_publisher.hpp>

Expand Down Expand Up @@ -53,7 +52,6 @@
namespace autoware::behavior_path_planner
{
using autoware::motion_utils::PlanningFactorInterface;
using autoware::motion_utils::SteeringFactorInterface;
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_map_msgs::msg::LaneletMapBin;
using autoware_perception_msgs::msg::PredictedObject;
Expand Down Expand Up @@ -123,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;
rclcpp::TimerBase::SharedPtr timer_;

std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
Expand All @@ -138,7 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node

std::shared_ptr<PlannerManager> planner_manager_;

SteeringFactorInterface steering_factor_interface_;
std::unique_ptr<PlanningFactorInterface> planning_factor_interface_;

std::mutex mutex_pd_; // mutex for planner_data_
Expand Down
28 changes: 0 additions & 28 deletions ...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
const auto durable_qos = rclcpp::QoS(1).transient_local();
modified_goal_publisher_ =
create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
pub_steering_factors_ =
create_publisher<SteeringFactorArray>("/planning/steering_factor/intersection", 1);
reroute_availability_publisher_ =
create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);
debug_avoidance_msg_array_publisher_ =
Expand Down Expand Up @@ -117,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
turn_signal_search_time, turn_signal_intersection_angle_threshold_deg);
}

steering_factor_interface_.init(PlanningBehavior::INTERSECTION);

// Start timer
{
const auto planning_hz = declare_parameter<double>("planning_hz");
Expand Down Expand Up @@ -465,20 +461,9 @@ void BehaviorPathPlannerNode::publish_steering_factor(
const auto [intersection_flag, approaching_intersection_flag] =
planner_data->turn_signal_decider.getIntersectionTurnSignalFlag();
if (intersection_flag || approaching_intersection_flag) {
const uint16_t steering_factor_direction = std::invoke([&turn_signal]() {
if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
return SteeringFactor::LEFT;
}
return SteeringFactor::RIGHT;
});

const auto [intersection_pose, intersection_distance] =
planner_data->turn_signal_decider.getIntersectionPoseAndDistance();

steering_factor_interface_.set(
{intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
steering_factor_direction, SteeringFactor::TURNING, "");

const uint16_t planning_factor_direction = std::invoke([&turn_signal]() {
if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
return PlanningFactor::TURN_LEFT;
Expand All @@ -489,21 +474,8 @@ void BehaviorPathPlannerNode::publish_steering_factor(
planning_factor_interface_->add(
intersection_distance, intersection_distance, intersection_pose, intersection_pose,
planning_factor_direction, SafetyFactorArray{});
} else {
steering_factor_interface_.reset();
}

autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array;
steering_factor_array.header.frame_id = "map";
steering_factor_array.header.stamp = this->now();

const auto steering_factor = steering_factor_interface_.get();
if (steering_factor.behavior != PlanningBehavior::UNKNOWN) {
steering_factor_array.factors.emplace_back(steering_factor);
}

pub_steering_factors_->publish(steering_factor_array);

planning_factor_interface_->publish();
}

Expand Down
2 changes: 0 additions & 2 deletions planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,8 +182,6 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) {
m->updateObserver();
m->publishRTCStatus();
m->publishSteeringFactor();
m->publishVelocityFactor();
m->publish_planning_factors();
});

Expand Down
60 changes: 0 additions & 60 deletions ...common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@

#include <autoware/behavior_path_planner_common/turn_signal_decider.hpp>
#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
#include <autoware/motion_utils/factor/steering_factor_interface.hpp>
#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
#include <autoware/motion_utils/marker/marker_helper.hpp>
#include <autoware/motion_utils/trajectory/path_with_lane_id.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
Expand All @@ -37,7 +35,6 @@
#include <magic_enum.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/steering_factor.hpp>
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_rtc_msgs/msg/state.hpp>
Expand All @@ -56,16 +53,11 @@
namespace autoware::behavior_path_planner
{
using autoware::motion_utils::PlanningFactorInterface;
using autoware::motion_utils::SteeringFactorInterface;
using autoware::motion_utils::VelocityFactorInterface;
using autoware::objects_of_interest_marker_interface::ColorName;
using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
using autoware::rtc_interface::RTCInterface;
using autoware::universe_utils::calcOffsetPose;
using autoware::universe_utils::generateUUID;
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::SteeringFactor;
using autoware_adapi_v1_msgs::msg::VelocityFactor;
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
using tier4_planning_msgs::msg::PathWithLaneId;
using tier4_planning_msgs::msg::PlanningFactor;
Expand Down Expand Up @@ -106,26 +98,6 @@ class SceneModuleInterface
}
}

// TODO(satoshi-ota): remove this constructor after all planning factors have been migrated.
SceneModuleInterface(
const std::string & name, rclcpp::Node & node,
std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map,
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
objects_of_interest_marker_interface_ptr_map)
: name_{name},
logger_{node.get_logger().get_child(name)},
clock_{node.get_clock()},
rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
objects_of_interest_marker_interface_ptr_map_(
std::move(objects_of_interest_marker_interface_ptr_map)),
planning_factor_interface_{std::make_shared<PlanningFactorInterface>(&node, "tmp")},
time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
{
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
uuid_map_.emplace(module_name, generateUUID());
}
}

SceneModuleInterface(const SceneModuleInterface &) = delete;
SceneModuleInterface(SceneModuleInterface &&) = delete;
SceneModuleInterface & operator=(const SceneModuleInterface &) = delete;
Expand Down Expand Up @@ -210,8 +182,6 @@ class SceneModuleInterface
unlockNewModuleLaunch();
unlockOutputPath();

reset_factor();

processOnExit();
}

Expand Down Expand Up @@ -284,16 +254,6 @@ class SceneModuleInterface

ModuleStatus getCurrentStatus() const { return current_state_; }

void reset_factor()
{
steering_factor_interface_.reset();
velocity_factor_interface_.reset();
}

auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); }

auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); }

std::string name() const { return name_; }

PoseWithDetailOpt getStopPose() const
Expand Down Expand Up @@ -584,22 +544,6 @@ class SceneModuleInterface
}
}

void setVelocityFactor(const PathWithLaneId & path)
{
if (stop_pose_.has_value()) {
velocity_factor_interface_.set(
path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop");
return;
}

if (!slow_pose_.has_value()) {
return;
}

velocity_factor_interface_.set(
path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down");
}

void set_longitudinal_planning_factor(const PathWithLaneId & path)
{
if (stop_pose_.has_value()) {
Expand Down Expand Up @@ -673,10 +617,6 @@ class SceneModuleInterface

mutable std::shared_ptr<PlanningFactorInterface> planning_factor_interface_;

mutable SteeringFactorInterface steering_factor_interface_;

mutable VelocityFactorInterface velocity_factor_interface_;

mutable PoseWithDetailOpt stop_pose_{std::nullopt};

mutable PoseWithDetailOpt slow_pose_{std::nullopt};
Expand Down
48 changes: 0 additions & 48 deletions ...nclude/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,6 @@
#include <rclcpp/parameter.hpp>
#include <rclcpp/publisher.hpp>

#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
#include <unique_identifier_msgs/msg/uuid.hpp>

#include <cstddef>
Expand All @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker;
using autoware::motion_utils::createSlowDownVirtualWallMarker;
using autoware::motion_utils::createStopVirtualWallMarker;
using autoware::universe_utils::toHexString;
using autoware_adapi_v1_msgs::msg::SteeringFactorArray;
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
using unique_identifier_msgs::msg::UUID;
using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
using SceneModuleObserver = std::weak_ptr<SceneModuleInterface>;
Expand Down Expand Up @@ -106,46 +102,6 @@ class SceneModuleManagerInterface
}
}

void publishSteeringFactor()
{
SteeringFactorArray steering_factor_array;
steering_factor_array.header.frame_id = "map";
steering_factor_array.header.stamp = node_->now();

for (const auto & m : observers_) {
if (m.expired()) {
continue;
}

const auto steering_factor = m.lock()->get_steering_factor();
if (steering_factor.behavior != PlanningBehavior::UNKNOWN) {
steering_factor_array.factors.emplace_back(steering_factor);
}
}

pub_steering_factors_->publish(steering_factor_array);
}

void publishVelocityFactor()
{
VelocityFactorArray velocity_factor_array;
velocity_factor_array.header.frame_id = "map";
velocity_factor_array.header.stamp = node_->now();

for (const auto & m : observers_) {
if (m.expired()) {
continue;
}

const auto velocity_factor = m.lock()->get_velocity_factor();
if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) {
velocity_factor_array.factors.emplace_back(velocity_factor);
}
}

pub_velocity_factors_->publish(velocity_factor_array);
}

void publish_planning_factors() { planning_factor_interface_->publish(); }

void publishVirtualWall() const
Expand Down Expand Up @@ -306,10 +262,6 @@ class SceneModuleManagerInterface

rclcpp::Publisher<MarkerArray>::SharedPtr pub_drivable_lanes_;

rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;

rclcpp::Publisher<VelocityFactorArray>::SharedPtr pub_velocity_factors_;

rclcpp::Publisher<universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_;

std::string name_;
Expand Down
4 changes: 0 additions & 4 deletions ...er/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,6 @@ void SceneModuleManagerInterface::initInterface(
pub_drivable_lanes_ = node->create_publisher<MarkerArray>("~/drivable_lanes/" + name_, 20);
pub_processing_time_ = node->create_publisher<universe_utils::ProcessingTimeDetail>(
"~/processing_time/" + name_, 20);
pub_steering_factors_ =
node->create_publisher<SteeringFactorArray>("/planning/steering_factor/" + name_, 1);
pub_velocity_factors_ =
node->create_publisher<VelocityFactorArray>("/planning/velocity_factors/" + name_, 1);
}

// planning factor
Expand Down
Loading