Skip to content

Commit

Permalink
feat(surround_obstacle_checker): make parameters more tunable (autowa…
Browse files Browse the repository at this point in the history 
…refoundation#4925)

* feat(surround_obstacle_checker): make parameters more tunable

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* update param

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 committed Sep 12, 2023
1 parent 6aa1834 commit 3d2ad62
Show file tree
Hide file tree
Showing 5 changed files with 282 additions and 92 deletions.
59 changes: 54 additions & 5 deletions planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,61 @@
/**:
ros__parameters:

# obstacle check
use_pointcloud: true # use pointcloud as obstacle check
use_dynamic_object: true # use dynamic object as obstacle check
surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m]
# surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
# surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m]
pointcloud:
enable_check: false
surround_check_front_distance: 0.5
surround_check_side_distance: 0.5
surround_check_back_distance: 0.5
unknown:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.5
surround_check_back_distance: 0.5
car:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.0
surround_check_back_distance: 0.5
truck:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.0
surround_check_back_distance: 0.5
bus:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.0
surround_check_back_distance: 0.5
trailer:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.0
surround_check_back_distance: 0.5
motorcycle:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.0
surround_check_back_distance: 0.5
bicycle:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.5
surround_check_back_distance: 0.5
pedestrian:
enable_check: true
surround_check_front_distance: 0.5
surround_check_side_distance: 0.5
surround_check_back_distance: 0.5

surround_check_hysteresis_distance: 0.3

state_clear_time: 2.0

# ego stop state
stop_state_ego_speed: 0.1 #[m/s]

# debug
publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets
debug_footprint_label: "car"
24 changes: 16 additions & 8 deletions planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_

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

#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
#include <geometry_msgs/msg/polygon_stamped.hpp>
Expand All @@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped;
using tier4_planning_msgs::msg::StopFactor;
using tier4_planning_msgs::msg::StopReason;
using tier4_planning_msgs::msg::StopReasonArray;
using vehicle_info_util::VehicleInfo;
using visualization_msgs::msg::Marker;
using visualization_msgs::msg::MarkerArray;

Expand All @@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode
{
public:
explicit SurroundObstacleCheckerDebugNode(
const Polygon2d & ego_polygon, const double base_link2front,
const double & surround_check_distance, const double & surround_check_recover_distance,
const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
rclcpp::Node & node);
const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
const std::string & object_label, const double & surround_check_front_distance,
const double & surround_check_side_distance, const double & surround_check_back_distance,
const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node);

bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
void publish();
void publishFootprints();
void updateFootprintMargin(
const std::string & object_label, const double front_distance, const double side_distance,
const double back_distance);

private:
rclcpp::Publisher<MarkerArray>::SharedPtr debug_virtual_wall_pub_;
Expand All @@ -72,18 +78,20 @@ class SurroundObstacleCheckerDebugNode
rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_offset_pub_;
rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_recover_offset_pub_;

Polygon2d ego_polygon_;
vehicle_info_util::VehicleInfo vehicle_info_;
double base_link2front_;
double surround_check_distance_;
double surround_check_recover_distance_;
std::string object_label_;
double surround_check_front_distance_;
double surround_check_side_distance_;
double surround_check_back_distance_;
double surround_check_hysteresis_distance_;
geometry_msgs::msg::Pose self_pose_;

MarkerArray makeVirtualWallMarker();
MarkerArray makeVisualizationMarker();
StopReasonArray makeStopReasonArray();
VelocityFactorArray makeVelocityFactorArray();

Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset);
PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);

std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
Expand Down
25 changes: 19 additions & 6 deletions planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@

#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

Expand All @@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node

struct NodeParam
{
bool use_pointcloud;
bool use_dynamic_object;
bool is_surround_obstacle;
// For preventing chattering,
// surround_check_recover_distance_ must be bigger than surround_check_distance_
double surround_check_recover_distance;
double surround_check_distance;
std::unordered_map<int, bool> enable_check_map;
std::unordered_map<int, double> surround_check_front_distance_map;
std::unordered_map<int, double> surround_check_side_distance_map;
std::unordered_map<int, double> surround_check_back_distance_map;
bool pointcloud_enable_check;
double pointcloud_surround_check_front_distance;
double pointcloud_surround_check_side_distance;
double pointcloud_surround_check_back_distance;
double surround_check_hysteresis_distance;
double state_clear_time;
bool publish_debug_footprints;
std::string debug_footprint_label;
};

private:
rcl_interfaces::msg::SetParametersResult onParam(
const std::vector<rclcpp::Parameter> & parameters);

std::array<double, 3> getCheckDistances(const std::string & str_label) const;

void onTimer();

void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
Expand Down Expand Up @@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;

// parameter callback result
OnSetParametersCallbackHandle::SharedPtr set_param_res_;

// stop checker
std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;

Expand Down
86 changes: 54 additions & 32 deletions planning/surround_obstacle_checker/src/debug_marker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,29 @@

namespace surround_obstacle_checker
{
namespace
{
Polygon2d createSelfPolygon(
const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0,
const double rear_margin = 0.0)
{
const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin;
const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin;
const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin;
const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin;

Polygon2d ego_polygon;

ego_polygon.outer().push_back(Point2d(front_m, width_left_m));
ego_polygon.outer().push_back(Point2d(front_m, width_right_m));
ego_polygon.outer().push_back(Point2d(rear_m, width_right_m));
ego_polygon.outer().push_back(Point2d(rear_m, width_left_m));

bg::correct(ego_polygon);

return ego_polygon;
}
} // namespace

using motion_utils::createStopVirtualWallMarker;
using tier4_autoware_utils::appendMarkerArray;
Expand All @@ -36,14 +59,18 @@ using tier4_autoware_utils::createMarkerScale;
using tier4_autoware_utils::createPoint;

SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
const Polygon2d & ego_polygon, const double base_link2front,
const double & surround_check_distance, const double & surround_check_recover_distance,
const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
rclcpp::Node & node)
: ego_polygon_(ego_polygon),
const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
const std::string & object_label, const double & surround_check_front_distance,
const double & surround_check_side_distance, const double & surround_check_back_distance,
const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node)
: vehicle_info_(vehicle_info),
base_link2front_(base_link2front),
surround_check_distance_(surround_check_distance),
surround_check_recover_distance_(surround_check_recover_distance),
object_label_(object_label),
surround_check_front_distance_(surround_check_front_distance),
surround_check_side_distance_(surround_check_side_distance),
surround_check_back_distance_(surround_check_back_distance),
surround_check_hysteresis_distance_(surround_check_hysteresis_distance),
self_pose_(self_pose),
clock_(clock)
{
Expand Down Expand Up @@ -86,20 +113,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint(

void SurroundObstacleCheckerDebugNode::publishFootprints()
{
const auto ego_polygon = createSelfPolygon(vehicle_info_);

/* publish vehicle footprint polygon */
const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z);
const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z);
vehicle_footprint_pub_->publish(footprint);

/* publish vehicle footprint polygon with offset */
const auto polygon_with_offset =
createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_);
const auto polygon_with_offset = createSelfPolygon(
vehicle_info_, surround_check_front_distance_, surround_check_side_distance_,
surround_check_back_distance_);
const auto footprint_with_offset =
boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z);
vehicle_footprint_offset_pub_->publish(footprint_with_offset);

/* publish vehicle footprint polygon with recover offset */
const auto polygon_with_recover_offset =
createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_);
const auto polygon_with_recover_offset = createSelfPolygon(
vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_,
surround_check_side_distance_ + surround_check_hysteresis_distance_,
surround_check_back_distance_ + surround_check_hysteresis_distance_);
const auto footprint_with_recover_offset =
boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z);
vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset);
Expand Down Expand Up @@ -206,26 +238,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
return velocity_factor_array;
}

Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset(
const Polygon2d & base_polygon, const double & offset)
{
typedef double coordinate_type;
const double buffer_distance = offset;
const int points_per_circle = 36;
boost::geometry::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy(
buffer_distance);
boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle);
boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle);
boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle);
boost::geometry::strategy::buffer::side_straight side_strategy;
boost::geometry::model::multi_polygon<Polygon2d> result;
// Create the buffer of a multi polygon
boost::geometry::buffer(
base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy,
circle_strategy);
return result.front();
}

PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
const Polygon2d & boost_polygon, const double & z)
{
Expand All @@ -244,4 +256,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
return polygon_stamped;
}

void SurroundObstacleCheckerDebugNode::updateFootprintMargin(
const std::string & object_label, const double front_distance, const double side_distance,
const double back_distance)
{
object_label_ = object_label;
surround_check_front_distance_ = front_distance;
surround_check_side_distance_ = side_distance;
surround_check_back_distance_ = back_distance;
}

} // namespace surround_obstacle_checker
Loading

0 comments on commit 3d2ad62

Please sign in to comment.