Skip to content

Commit

Permalink
feat(costmap_generator): activate by location (#692)
Browse files Browse the repository at this point in the history 
* Activate by location

Signed-off-by: Hirokazu Ishida <[email protected]>

* Fix markdown

Signed-off-by: Hirokazu Ishida <[email protected]>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@HiroIshida @pre-commit-ci
HiroIshida and pre-commit-ci[bot] authored Apr 28, 2022
1 parent 5541f1d commit 5ab1962
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 30 deletions.
41 changes: 21 additions & 20 deletions planning/costmap_generator/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,26 +31,27 @@ None

### Parameters

| Name | Type | Description |
| ---------------------------- | ------ | ------------------------------------------------------- |
| `update_rate` | double | timer's update rate |
| `use_objects` | bool | whether using `~input/objects` or not |
| `use_points` | bool | whether using `~input/points_no_ground` or not |
| `use_wayarea` | bool | whether using `wayarea` from `~input/vector_map` or not |
| `costmap_frame` | string | created costmap's coordinate |
| `vehicle_frame` | string | vehicle's coordinate |
| `map_frame` | string | map's coordinate |
| `grid_min_value` | double | minimum cost for gridmap |
| `grid_max_value` | double | maximum cost for gridmap |
| `grid_resolution` | double | resolution for gridmap |
| `grid_length_x` | int | size of gridmap for x direction |
| `grid_length_y` | int | size of gridmap for y direction |
| `grid_position_x` | int | offset from coordinate in x direction |
| `grid_position_y` | int | offset from coordinate in y direction |
| `maximum_lidar_height_thres` | double | maximum height threshold for pointcloud data |
| `minimum_lidar_height_thres` | double | minimum height threshold for pointcloud data |
| `expand_rectangle_size` | double | expand object's rectangle with this value |
| `size_of_expansion_kernel` | int | kernel size for blurring effect on object's costmap |
| Name | Type | Description |
| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
| `update_rate` | double | timer's update rate |
| `activate_by_scenario` | bool | if true, activate by scenario = parking. Otherwise, activate if vehicle is inside parking lot. |
| `use_objects` | bool | whether using `~input/objects` or not |
| `use_points` | bool | whether using `~input/points_no_ground` or not |
| `use_wayarea` | bool | whether using `wayarea` from `~input/vector_map` or not |
| `costmap_frame` | string | created costmap's coordinate |
| `vehicle_frame` | string | vehicle's coordinate |
| `map_frame` | string | map's coordinate |
| `grid_min_value` | double | minimum cost for gridmap |
| `grid_max_value` | double | maximum cost for gridmap |
| `grid_resolution` | double | resolution for gridmap |
| `grid_length_x` | int | size of gridmap for x direction |
| `grid_length_y` | int | size of gridmap for y direction |
| `grid_position_x` | int | offset from coordinate in x direction |
| `grid_position_y` | int | offset from coordinate in y direction |
| `maximum_lidar_height_thres` | double | maximum height threshold for pointcloud data |
| `minimum_lidar_height_thres` | double | minimum height threshold for pointcloud data |
| `expand_rectangle_size` | double | expand object's rectangle with this value |
| `size_of_expansion_kernel` | int | kernel size for blurring effect on object's costmap |

### Flowchart

Expand Down
3 changes: 3 additions & 0 deletions planning/costmap_generator/include/costmap_generator/costmap_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ class CostmapGenerator : public rclcpp::Node
std::string map_frame_;

double update_rate_;
bool activate_by_scenario_;

double grid_min_value_;
double grid_max_value_;
Expand Down Expand Up @@ -153,6 +154,8 @@ class CostmapGenerator : public rclcpp::Node

void onTimer();

bool isActive();

/// \brief initialize gridmap parameters based on rosparam
void initGridmap();

Expand Down
92 changes: 82 additions & 10 deletions planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include <lanelet2_extension/visualization/visualization.hpp>
#include <pcl_ros/transforms.hpp>

#include <lanelet2_core/geometry/Polygon.h>
#include <tf2/utils.h>
#include <tf2_eigen/tf2_eigen.h>

Expand All @@ -61,20 +62,72 @@

namespace
{
bool isActive(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario)

// Copied from scenario selector
geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose(
const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger)
{
if (!scenario) {
return false;
geometry_msgs::msg::TransformStamped tf_current_pose;

try {
tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(logger, "%s", ex.what());
return nullptr;
}

const auto & s = scenario->activating_scenarios;
if (
std::find(std::begin(s), std::end(s), tier4_planning_msgs::msg::Scenario::PARKING) !=
std::end(s)) {
return true;
geometry_msgs::msg::PoseStamped::SharedPtr p(new geometry_msgs::msg::PoseStamped());
p->header = tf_current_pose.header;
p->pose.orientation = tf_current_pose.transform.rotation;
p->pose.position.x = tf_current_pose.transform.translation.x;
p->pose.position.y = tf_current_pose.transform.translation.y;
p->pose.position.z = tf_current_pose.transform.translation.z;

return geometry_msgs::msg::PoseStamped::ConstSharedPtr(p);
}

// copied from scenario selector
std::shared_ptr<lanelet::ConstPolygon3d> findNearestParkinglot(
const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
const lanelet::BasicPoint2d & search_point)
{
std::vector<std::pair<double, lanelet::Lanelet>> nearest_lanelets =
lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 1);

if (nearest_lanelets.empty()) {
return {};
}

return false;
const auto nearest_lanelet = nearest_lanelets.front().second;
const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr);

const auto linked_parking_lot = std::make_shared<lanelet::ConstPolygon3d>();
const auto result = lanelet::utils::query::getLinkedParkingLot(
nearest_lanelet, all_parking_lots, linked_parking_lot.get());

if (result) {
return linked_parking_lot;
} else {
return {};
}
}

// copied from scenario selector
bool isInParkingLot(
const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
const geometry_msgs::msg::Pose & current_pose)
{
const auto & p = current_pose.position;
const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z);

const auto nearest_parking_lot =
findNearestParkinglot(lanelet_map_ptr, search_point.basicPoint2d());

if (!nearest_parking_lot) {
return false;
}

return lanelet::geometry::within(search_point, nearest_parking_lot->basicPolygon());
}

// Convert from Point32 to Point
Expand Down Expand Up @@ -116,6 +169,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
vehicle_frame_ = this->declare_parameter<std::string>("vehicle_frame", "base_link");
map_frame_ = this->declare_parameter<std::string>("map_frame", "map");
update_rate_ = this->declare_parameter<double>("update_rate", 10.0);
activate_by_scenario_ = this->declare_parameter<bool>("activate_by_scenario", true);
grid_min_value_ = this->declare_parameter<double>("grid_min_value", 0.0);
grid_max_value_ = this->declare_parameter<double>("grid_max_value", 1.0);
grid_resolution_ = this->declare_parameter<double>("grid_resolution", 0.2);
Expand Down Expand Up @@ -241,7 +295,7 @@ void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::Cons

void CostmapGenerator::onTimer()
{
if (!isActive(scenario_)) {
if (!isActive()) {
return;
}

Expand Down Expand Up @@ -278,6 +332,24 @@ void CostmapGenerator::onTimer()
publishCostmap(costmap_);
}

bool CostmapGenerator::isActive()
{
if (activate_by_scenario_) {
if (scenario_) {
const auto & s = scenario_->activating_scenarios;
if (
std::find(std::begin(s), std::end(s), tier4_planning_msgs::msg::Scenario::PARKING) !=
std::end(s)) {
return true;
}
}
return false;
} else {
const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger());
return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose);
}
}

void CostmapGenerator::initGridmap()
{
costmap_.setFrameId(costmap_frame_);
Expand Down

0 comments on commit 5ab1962

Please sign in to comment.