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(motion_velocity_planner)!: remove velocity factor #1738

Merged
merged 4 commits into from
Dec 26, 2024
Merged

fix(motion_velocity_planner)!: remove velocity factor #1738

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2306e06
fix(dynamic_obstacle_stop): remove velocity factor
satoshi-ota Dec 26, 2024
682ec00
fix(obstacle_velocity_limiter): remove velocity factor
satoshi-ota Dec 26, 2024
0845563
fix(out_of_lane): remove velocity factor
satoshi-ota Dec 26, 2024
307d698
fix(motion_velocity_planner): remove velocity factor
satoshi-ota Dec 26, 2024
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
10 changes: 0 additions & 10 deletions ...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
module_name_ = module_name;
logger_ = node.get_logger().get_child(ns_);
clock_ = node.get_clock();
velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE);

planning_factor_interface_ = std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
&node, "dynamic_obstacle_stop");
Expand Down Expand Up @@ -162,18 +161,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
debug_data_.stop_pose = stop_pose;
if (stop_pose) {
result.stop_points.push_back(stop_pose->position);
const auto stop_pose_reached =
planner_data->current_odometry.twist.twist.linear.x < 1e-3 &&
autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3;
planning_factor_interface_->add(
ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
SafetyFactorArray{});
velocity_factor_interface_.set(
ego_trajectory_points, ego_data.pose, *stop_pose,
stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED
: autoware::motion_utils::VelocityFactor::APPROACHING,
"dynamic_obstacle_stop");
result.velocity_factor = velocity_factor_interface_.get();
create_virtual_walls();
}
}
Expand Down
1 change: 0 additions & 1 deletion ...motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node);
obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node);
velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node);
velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE);

debug_publisher_ =
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
Expand Down
10 changes: 0 additions & 10 deletions ...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,6 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
logger_ = node.get_logger();
clock_ = node.get_clock();
init_parameters(node);
velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE);

planning_factor_interface_ =
std::make_unique<autoware::motion_utils::PlanningFactorInterface>(&node, "out_of_lane");
Expand Down Expand Up @@ -312,18 +311,9 @@ VelocityPlanningResult OutOfLaneModule::plan(
slowdown_pose->position, slowdown_pose->position, slowdown_velocity);
}

const auto is_approaching =
motion_utils::calcSignedArcLength(
ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 &&
planner_data->current_odometry.twist.twist.linear.x > 0.1;
const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING
: motion_utils::VelocityFactor::STOPPED;
planning_factor_interface_->add(
ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
SafetyFactorArray{});
velocity_factor_interface_.set(
ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane");
result.velocity_factor = velocity_factor_interface_.get();
virtual_wall_marker_creator.add_virtual_walls(
out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_));
virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
Expand Down
2 changes: 0 additions & 2 deletions ...lanner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
#include "velocity_planning_result.hpp"

#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
#include <rclcpp/rclcpp.hpp>

Expand Down Expand Up @@ -47,7 +46,6 @@ class PluginModuleInterface
const std::shared_ptr<const PlannerData> planner_data) = 0;
virtual std::string get_module_name() const = 0;
virtual void publish_planning_factor() {}
autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_;
rclcpp::Logger logger_ = rclcpp::get_logger("");
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
Expand Down
2 changes: 0 additions & 2 deletions ...anner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@

#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>

#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand All @@ -43,7 +42,6 @@ struct VelocityPlanningResult
{
std::vector<geometry_msgs::msg::Point> stop_points{};
std::vector<SlowdownInterval> slowdown_intervals{};
std::optional<autoware_adapi_v1_msgs::msg::VelocityFactor> velocity_factor{};
};
} // namespace autoware::motion_velocity_planner

Expand Down
8 changes: 4 additions & 4 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ This means that to stop before a wall, a stop point is inserted in the trajector

## Output topics

| Name | Type | Description |
| --------------------------- | ------------------------------------------------- | -------------------------------------------------- |
| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile |
| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile |
| Name | Type | Description |
| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- |
| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile |
| `~/output/planning_factors/<MODULE_NAME>` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile |

## Services

Expand Down
10 changes: 0 additions & 10 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,9 +82,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
// Publishers
trajectory_pub_ =
this->create_publisher<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
velocity_factor_publisher_ =
this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
"~/output/velocity_factors", 1);
processing_time_publisher_ =
this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
debug_viz_pub_ =
Expand Down Expand Up @@ -425,20 +422,13 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj
resampled_trajectory, std::make_shared<const PlannerData>(planner_data_));
processing_times["plan_velocities"] = stop_watch.toc("plan_velocities");

autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors;
velocity_factors.header.frame_id = "map";
velocity_factors.header.stamp = get_clock()->now();

for (const auto & planning_result : planning_results) {
for (const auto & stop_point : planning_result.stop_points)
insert_stop(output_trajectory_msg, stop_point);
for (const auto & slowdown_interval : planning_result.slowdown_intervals)
insert_slowdown(output_trajectory_msg, slowdown_interval);
if (planning_result.velocity_factor)
velocity_factors.factors.push_back(*planning_result.velocity_factor);
}

velocity_factor_publisher_->publish(velocity_factors);
return output_trajectory_msg;
}

Expand Down
2 changes: 0 additions & 2 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,8 +93,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
// publishers
rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr trajectory_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
velocity_factor_publisher_;
autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
this, "~/debug/processing_time_ms_diag"};
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
Expand Down
Loading