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

remove velocity_factor #1768

Merged
merged 1 commit into from
Jan 15, 2025
Merged

remove velocity_factor #1768

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
2 changes: 0 additions & 2 deletions ...elocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner
{
namespace bg = boost::geometry;

using autoware::motion_utils::VelocityFactorInterface;

IntersectionModule::IntersectionModule(
const int64_t module_id, const int64_t lane_id,
[[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
Expand Down
19 changes: 12 additions & 7 deletions ...ehavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule(
debug_data_(),
state_(State::INIT)
{
velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE);
}

bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path)
Expand Down Expand Up @@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path)
// Get stop point and stop factor
{
const auto & stop_pose = op_stop_pose.value();
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");

const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
path->points, stop_pose.position, debug_data_.base_link2front);
Expand Down Expand Up @@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId *
// Get stop point and stop factor
{
const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0));
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");

const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
path->points, stop_pose.position, debug_data_.base_link2front);
Expand Down Expand Up @@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path)
// Get stop point and stop factor
{
const auto & stop_pose = ego_pos_on_path.pose;
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");

const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
path->points, stop_pose.position, debug_data_.base_link2front);
Expand Down
2 changes: 0 additions & 2 deletions ...ity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule(
: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface),
param_(planner_param)
{
velocity_factor_.init(PlanningBehavior::UNKNOWN);

if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
debug_data_.detection_type = "occupancy";
//! occupancy grid limitation( 100 * 100 )
Expand Down
24 changes: 0 additions & 24 deletions ...anner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
#include <autoware/behavior_velocity_planner_common/planner_data.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
Expand All @@ -28,8 +27,6 @@
#include <autoware/universe_utils/system/time_keeper.hpp>
#include <builtin_interfaces/msg/time.hpp>

#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
#include <autoware_planning_msgs/msg/path.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand All @@ -51,8 +48,6 @@
namespace autoware::behavior_velocity_planner
{

using autoware::motion_utils::PlanningBehavior;
using autoware::motion_utils::VelocityFactor;
using autoware::objects_of_interest_marker_interface::ColorName;
using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
using autoware::universe_utils::DebugPublisher;
Expand Down Expand Up @@ -97,8 +92,6 @@ class SceneModuleInterface
planner_data_ = planner_data;
}

void resetVelocityFactor() { velocity_factor_.reset(); }
VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); }
std::vector<ObjectOfInterest> getObjectsOfInterestData() const { return objects_of_interest_; }
void clearObjectsOfInterestData() { objects_of_interest_.clear(); }

Expand All @@ -107,7 +100,6 @@ class SceneModuleInterface
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;
std::shared_ptr<const PlannerData> planner_data_;
autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this
std::vector<ObjectOfInterest> objects_of_interest_;
mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface_;
Expand Down Expand Up @@ -143,8 +135,6 @@ class SceneModuleManagerInterface
}
pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
std::string("~/virtual_wall/") + module_name, 5);
pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
std::string("/planning/velocity_factors/") + module_name, 1);
planning_factor_interface_ =
std::make_shared<motion_utils::PlanningFactorInterface>(&node, module_name);

Expand Down Expand Up @@ -180,30 +170,18 @@ class SceneModuleManagerInterface
StopWatch<std::chrono::milliseconds> stop_watch;
stop_watch.tic("Total");
visualization_msgs::msg::MarkerArray debug_marker_array;
autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
velocity_factor_array.header.frame_id = "map";
velocity_factor_array.header.stamp = clock_->now();

for (const auto & scene_module : scene_modules_) {
scene_module->resetVelocityFactor();
scene_module->setPlannerData(planner_data_);
scene_module->modifyPathVelocity(path);

// The velocity factor must be called after modifyPathVelocity.
// TODO(soblin): remove this
const auto velocity_factor = scene_module->getVelocityFactor();
if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) {
velocity_factor_array.factors.emplace_back(velocity_factor);
}

for (const auto & marker : scene_module->createDebugMarkerArray().markers) {
debug_marker_array.markers.push_back(marker);
}

virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls());
}

pub_velocity_factor_->publish(velocity_factor_array);
planning_factor_interface_->publish();
pub_debug_->publish(debug_marker_array);
if (is_publish_debug_path_) {
Expand Down Expand Up @@ -273,8 +251,6 @@ class SceneModuleManagerInterface
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_virtual_wall_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_;
rclcpp::Publisher<tier4_planning_msgs::msg::PathWithLaneId>::SharedPtr pub_debug_path_;
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
pub_velocity_factor_;

std::shared_ptr<DebugPublisher> processing_time_publisher_;

Expand Down
1 change: 0 additions & 1 deletion ...behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
for (const auto & scene_module : scene_modules_) {
std::shared_ptr<TrafficLightModule> traffic_light_scene_module(
std::dynamic_pointer_cast<TrafficLightModule>(scene_module));
traffic_light_scene_module->resetVelocityFactor();
traffic_light_scene_module->setPlannerData(planner_data_);
traffic_light_scene_module->modifyPathVelocity(path);

Expand Down
Loading