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(obstacle_stop_planner): fix the issues when use_predicted_objects true #3556

Merged
merged 1 commit into from
May 9, 2023
Merged

fix(obstacle_stop_planner): fix the issues when use_predicted_objects true #3556

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
170 changes: 110 additions & 60 deletions planning/obstacle_stop_planner/README.md
View file
Open in desktop

Large diffs are not rendered by default.

12 changes: 12 additions & 0 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
Expand All @@ -32,6 +33,7 @@ namespace motion_planning
{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using TrajectoryPoints = std::vector<TrajectoryPoint>;
using autoware_auto_perception_msgs::msg::PredictedObject;
class AdaptiveCruiseController
{
public:
Expand All @@ -47,6 +49,14 @@ class AdaptiveCruiseController
const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop,
TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header);

void insertAdaptiveCruiseVelocity(
const TrajectoryPoints & trajectory, const int nearest_collision_point_idx,
const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point,
const rclcpp::Time nearest_collision_point_time,
const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop,
TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header,
const PredictedObject & target_object);

private:
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;

Expand Down Expand Up @@ -187,6 +197,8 @@ class AdaptiveCruiseController
bool estimatePointVelocityFromPcl(
const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
const rclcpp::Time & nearest_collision_point_time, double * velocity);
void calculateProjectedVelocityFromObject(
const PredictedObject & object, const double traj_yaw, double * velocity);
double estimateRoughPointVelocity(double current_vel);
bool isObstacleVelocityHigh(const double obj_vel);
double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel);
Expand Down
15 changes: 9 additions & 6 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ using vehicle_info_util::VehicleInfo;

using TrajectoryPoints = std::vector<TrajectoryPoint>;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;

using autoware_auto_perception_msgs::msg::PredictedObject;
struct ObstacleWithDetectionTime
{
explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p)
Expand All @@ -102,13 +102,15 @@ struct ObstacleWithDetectionTime

struct PredictedObjectWithDetectionTime
{
explicit PredictedObjectWithDetectionTime(const rclcpp::Time & t, Pose & p)
: detection_time(t), point(p)
explicit PredictedObjectWithDetectionTime(
const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
: detection_time(t), point(p), object(std::move(obj))
{
}

rclcpp::Time detection_time;
Pose point;
geometry_msgs::msg::Point point;
PredictedObject object;
};

class ObstacleStopPlannerNode : public rclcpp::Node
Expand Down Expand Up @@ -173,8 +175,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node
const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr);

void searchPredictedObject(
const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data,
const VehicleInfo & vehicle_info, const StopParam & stop_param);
const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output,
PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info,
const StopParam & stop_param);

void insertVelocity(
TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header,
Expand Down
3 changes: 3 additions & 0 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/pose.hpp>
Expand Down Expand Up @@ -249,6 +250,8 @@ struct PlannerData

pcl::PointXYZ lateral_nearest_slow_down_point;

autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{};

Pose nearest_collision_point_pose{};

Pose nearest_slow_down_point_pose{};
Expand Down
20 changes: 16 additions & 4 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,15 @@
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
#include <vehicle_info_util/vehicle_info_util.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <boost/optional.hpp>
#include <boost/variant.hpp>

#include <map>
#include <string>
Expand All @@ -54,6 +56,9 @@ using vehicle_info_util::VehicleInfo;

using TrajectoryPoints = std::vector<TrajectoryPoint>;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using PointVariant = boost::variant<float, double>;

boost::optional<std::pair<double, double>> calcFeasibleMarginAndVelocity(
const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle,
Expand Down Expand Up @@ -101,12 +106,12 @@ void getLateralNearestPoint(
const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point,
double * deviation);

void getNearestPointForPredictedObject(
const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point,
rclcpp::Time * nearest_collision_point_time);
double getNearestPointAndDistanceForPredictedObject(
const geometry_msgs::msg::PoseArray & points, const Pose & base_pose,
geometry_msgs::msg::Point * nearest_collision_point);

void getLateralNearestPointForPredictedObject(
const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point,
const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point,
double * deviation);

Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info);
Expand Down Expand Up @@ -137,6 +142,13 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e
TrajectoryPoint getExtendTrajectoryPoint(
double extend_distance, const TrajectoryPoint & goal_point);

bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max);

pcl::PointXYZ pointToPcl(const double x, const double y, const double z);

boost::optional<PredictedObject> getObstacleFromUuid(
const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id);

rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr);

} // namespace motion_planning
Expand Down
73 changes: 73 additions & 0 deletions planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -273,6 +273,68 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
*need_to_stop = false;
}

void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
const TrajectoryPoints & trajectory, const int nearest_collision_point_idx,
const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point,
const rclcpp::Time nearest_collision_point_time,
const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop,
TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header,
const PredictedObject & target_object)
{
debug_values_.data.clear();
debug_values_.data.resize(num_debug_values_, 0.0);

const double current_velocity = current_odometry_ptr->twist.twist.linear.x;
double col_point_distance;
double point_velocity;
/*
* calc distance to collision point
*/
calcDistanceToNearestPointOnPath(
trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point,
nearest_collision_point_time, &col_point_distance, trajectory_header);

/*
* calc yaw of trajectory at collision point
*/
const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx);

/*
* estimate velocity of collision point
*/
calculateProjectedVelocityFromObject(target_object, traj_yaw, &point_velocity);

// calculate max(target) velocity of self
const double upper_velocity =
calcUpperVelocity(col_point_distance, point_velocity, current_velocity);
pub_debug_->publish(debug_values_);

if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
// if the target object is obstacle return stop true
RCLCPP_DEBUG_THROTTLE(
node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
"Target object is pedestrian. Insert stop line.");
*need_to_stop = true;
return;
}

if (upper_velocity <= param_.thresh_vel_to_stop) {
// if upper velocity is too low, need to stop
RCLCPP_DEBUG_THROTTLE(
node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
"Upper velocity is too low. Insert stop line.");
*need_to_stop = true;
return;
}

/*
* insert max velocity
*/
insertMaxVelocityToPath(
self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory);
*need_to_stop = false;
}

void AdaptiveCruiseController::calcDistanceToNearestPointOnPath(
const TrajectoryPoints & trajectory, const int nearest_point_idx,
const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point,
Expand Down Expand Up @@ -376,6 +438,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject(
}
}

void AdaptiveCruiseController::calculateProjectedVelocityFromObject(
const PredictedObject & object, const double traj_yaw, double * velocity)
{
/* get object velocity, and current yaw */
double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);

*velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw));
debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
}

bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
const rclcpp::Time & nearest_collision_point_time, double * velocity)
Expand Down
Loading