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

refactor(safety_checker): remove lane change dependency #4387

Merged
merged 2 commits into from
Jul 25, 2023
Merged

refactor(safety_checker): remove lane change dependency #4387

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
019b492
refactor(safety_checker): remove lane change dependency
satoshi-ota Jul 25, 2023
ee16dc1
refactor(safety_checker): init primitive member variables
satoshi-ota Jul 25, 2023
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: 4 additions & 0 deletions ...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@
namespace behavior_path_planner
{
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
Expand Down
52 changes: 4 additions & 48 deletions ..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_

#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
#include "behavior_path_planner/utils/safety_check.hpp"
#include "lanelet2_core/geometry/Lanelet.h"

#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
Expand All @@ -28,51 +29,6 @@
namespace behavior_path_planner
{

struct PoseWithVelocity
{
Pose pose;
double velocity;

PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {}
};

struct PoseWithVelocityStamped : public PoseWithVelocity
{
double time;

PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity)
: PoseWithVelocity(pose, velocity), time(time)
{
}
};

struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped
{
Polygon2d poly;

PoseWithVelocityAndPolygonStamped(
const double time, const Pose & pose, const double velocity, const Polygon2d & poly)
: PoseWithVelocityStamped(time, pose, velocity), poly(poly)
{
}
};

struct PredictedPathWithPolygon
{
float confidence;
std::vector<PoseWithVelocityAndPolygonStamped> path;
};

struct ExtendedPredictedObject
{
unique_identifier_msgs::msg::UUID uuid;
geometry_msgs::msg::PoseWithCovariance initial_pose;
geometry_msgs::msg::TwistWithCovariance initial_twist;
geometry_msgs::msg::AccelWithCovariance initial_acceleration;
autoware_auto_perception_msgs::msg::Shape shape;
std::vector<PredictedPathWithPolygon> predicted_paths;
};

struct LaneChangeCancelParameters
{
bool enable_on_prepare_phase{true};
Expand Down Expand Up @@ -176,9 +132,9 @@ struct LaneChangeTargetObjectIndices

struct LaneChangeTargetObjects
{
std::vector<ExtendedPredictedObject> current_lane{};
std::vector<ExtendedPredictedObject> target_lane{};
std::vector<ExtendedPredictedObject> other_lane{};
std::vector<utils::safety_check::ExtendedPredictedObject> current_lane{};
std::vector<utils::safety_check::ExtendedPredictedObject> target_lane{};
std::vector<utils::safety_check::ExtendedPredictedObject> other_lane{};
};

enum class LaneChangeModuleType {
Expand Down
7 changes: 4 additions & 3 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,10 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_perception_msgs::msg::PredictedPath;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using behavior_path_planner::ExtendedPredictedObject;
using behavior_path_planner::PoseWithVelocityAndPolygonStamped;
using behavior_path_planner::PredictedPathWithPolygon;
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
using data::lane_change::PathSafetyStatus;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
Expand Down
46 changes: 45 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"

#include <tier4_autoware_utils/geometry/boost_geometry.hpp>

Expand Down Expand Up @@ -50,6 +49,51 @@ using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;
using vehicle_info_util::VehicleInfo;

struct PoseWithVelocity
{
Pose pose;
double velocity{0.0};

PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {}
};

struct PoseWithVelocityStamped : public PoseWithVelocity
{
double time{0.0};

PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity)
: PoseWithVelocity(pose, velocity), time(time)
{
}
};

struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped
{
Polygon2d poly;

PoseWithVelocityAndPolygonStamped(
const double time, const Pose & pose, const double velocity, const Polygon2d & poly)
: PoseWithVelocityStamped(time, pose, velocity), poly(poly)
{
}
};

struct PredictedPathWithPolygon
{
float confidence{0.0};
std::vector<PoseWithVelocityAndPolygonStamped> path;
};

struct ExtendedPredictedObject
{
unique_identifier_msgs::msg::UUID uuid;
geometry_msgs::msg::PoseWithCovariance initial_pose;
geometry_msgs::msg::TwistWithCovariance initial_twist;
geometry_msgs::msg::AccelWithCovariance initial_acceleration;
autoware_auto_perception_msgs::msg::Shape shape;
std::vector<PredictedPathWithPolygon> predicted_paths;
};

namespace bg = boost::geometry;

bool isTargetObjectFront(
Expand Down
5 changes: 5 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
#include "behavior_path_planner/utils/lane_following/module_data.hpp"
#include "behavior_path_planner/utils/safety_check.hpp"
#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
#include "motion_utils/motion_utils.hpp"
#include "object_recognition_utils/predicted_path_utils.hpp"
Expand Down Expand Up @@ -65,6 +66,10 @@ using autoware_auto_perception_msgs::msg::Shape;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseArray;
Expand Down