-
Notifications
You must be signed in to change notification settings - Fork 640
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
add new package autoware_collision_detector
Signed-off-by: Go Sakayori <[email protected]>
- Loading branch information
1 parent
35ecc19
commit ccb9e99
Showing
7 changed files
with
602 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,33 @@ | ||
cmake_minimum_required(VERSION 3.14) | ||
project(autoware_collision_detector) | ||
|
||
find_package(autoware_cmake REQUIRED) | ||
autoware_package() | ||
|
||
find_package(eigen3_cmake_module REQUIRED) | ||
find_package(Eigen3 REQUIRED) | ||
find_package(PCL REQUIRED COMPONENTS common) | ||
|
||
include_directories( | ||
SYSTEM | ||
${EIGEN3_INCLUDE_DIR} | ||
) | ||
|
||
ament_auto_add_library(${PROJECT_NAME} SHARED | ||
src/node.cpp | ||
) | ||
|
||
target_link_libraries(${PROJECT_NAME} | ||
${PCL_LIBRARIES} | ||
) | ||
|
||
rclcpp_components_register_node(${PROJECT_NAME} | ||
PLUGIN "autoware::collision_detector::CollisionDetectorNode" | ||
EXECUTABLE ${PROJECT_NAME}_node | ||
) | ||
|
||
ament_auto_package( | ||
INSTALL_TO_SHARE | ||
config | ||
launch | ||
) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,49 @@ | ||
# Collision Checker | ||
|
||
## Purpose | ||
|
||
This module subscribes required data (ego-pose, obstacles, etc), and publishes diagnostics if an object is within a specific distance. | ||
|
||
## Inner-workings / Algorithms | ||
|
||
### Flow chart | ||
|
||
### Algorithms | ||
|
||
### Check data | ||
|
||
Check that `collision_detector` receives no ground pointcloud, dynamic objects. | ||
|
||
### Get distance to nearest object | ||
|
||
Calculate distance between ego vehicle and the nearest object. | ||
In this function, it calculates the minimum distance between the polygon of ego vehicle and all points in pointclouds and the polygons of dynamic objects. | ||
|
||
## Inputs / Outputs | ||
|
||
### Input | ||
|
||
| Name | Type | Description | | ||
| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | | ||
| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | | ||
| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | | ||
| `/tf` | `tf2_msgs::msg::TFMessage` | TF | | ||
| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | | ||
|
||
### Output | ||
|
||
| Name | Type | Description | | ||
| -------------- | --------------------------------------- | ----------- | | ||
| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics | | ||
|
||
## Parameters | ||
|
||
| Name | Type | Description | Default value | | ||
| :------------------- | :------- | :--------------------------------------------------------------- | :------------ | | ||
| `use_pointcloud` | `bool` | Use pointcloud as obstacle check | `false` | | ||
| `use_dynamic_object` | `bool` | Use dynamic object as obstacle check | `true` | | ||
| `collision_distance` | `double` | If objects exist in this distance, publish error diagnostics [m] | 0.1 | | ||
|
||
## Assumptions / Known limits | ||
|
||
- This module is based on `surround_obstacle_checker` |
5 changes: 5 additions & 0 deletions
5
control/autoware_collision_detector/config/collision_detector.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,5 @@ | ||
/**: | ||
ros__parameters: | ||
use_pointcloud: false # use pointcloud as obstacle check | ||
use_dynamic_object: true # use dynamic object as obstacle check | ||
collision_distance: 0.1 # Distance at which an object is determined to have collided with ego [m] |
102 changes: 102 additions & 0 deletions
102
control/autoware_collision_detector/include/autoware/collision_detector/node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,102 @@ | ||
// Copyright 2020 Tier IV, Inc. | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#ifndef AUTOWARE__COLLISION_DETECTOR__NODE_HPP_ | ||
#define AUTOWARE__COLLISION_DETECTOR__NODE_HPP_ | ||
|
||
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp> | ||
#include <diagnostic_updater/diagnostic_updater.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp> | ||
#include <autoware_perception_msgs/msg/predicted_objects.hpp> | ||
#include <autoware_planning_msgs/msg/trajectory.hpp> | ||
#include <nav_msgs/msg/odometry.hpp> | ||
#include <sensor_msgs/msg/point_cloud2.hpp> | ||
|
||
#include <boost/optional.hpp> | ||
|
||
#include <tf2/utils.h> | ||
#include <tf2_ros/buffer.h> | ||
#include <tf2_ros/transform_listener.h> | ||
|
||
#include <string> | ||
#include <utility> | ||
|
||
namespace autoware::collision_detector | ||
{ | ||
using autoware::vehicle_info_utils::VehicleInfo; | ||
using autoware_adapi_v1_msgs::msg::OperationModeState; | ||
using autoware_perception_msgs::msg::PredictedObjects; | ||
using autoware_perception_msgs::msg::Shape; | ||
|
||
using Obstacle = std::pair<double /* distance */, geometry_msgs::msg::Point>; | ||
|
||
class CollisionDetectorNode : public rclcpp::Node | ||
{ | ||
public: | ||
explicit CollisionDetectorNode(const rclcpp::NodeOptions & node_options); | ||
|
||
struct NodeParam | ||
{ | ||
bool use_pointcloud; | ||
bool use_dynamic_object; | ||
double collision_distance; | ||
}; | ||
|
||
private: | ||
void checkCollision(diagnostic_updater::DiagnosticStatusWrapper & stat); | ||
|
||
void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); | ||
|
||
void onDynamicObjects(const PredictedObjects::ConstSharedPtr msg); | ||
|
||
void onOperationMode(const OperationModeState::ConstSharedPtr msg); | ||
|
||
boost::optional<Obstacle> getNearestObstacle() const; | ||
|
||
boost::optional<Obstacle> getNearestObstacleByPointCloud() const; | ||
|
||
boost::optional<Obstacle> getNearestObstacleByDynamicObject() const; | ||
|
||
boost::optional<geometry_msgs::msg::TransformStamped> getTransform( | ||
const std::string & source, const std::string & target, const rclcpp::Time & stamp, | ||
double duration_sec) const; | ||
|
||
// ros | ||
mutable tf2_ros::Buffer tf_buffer_{get_clock()}; | ||
mutable tf2_ros::TransformListener tf_listener_{tf_buffer_}; | ||
rclcpp::TimerBase::SharedPtr timer_; | ||
|
||
// publisher and subscriber | ||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pointcloud_; | ||
rclcpp::Subscription<PredictedObjects>::SharedPtr sub_dynamic_objects_; | ||
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_; | ||
|
||
// parameter | ||
NodeParam node_param_; | ||
autoware::vehicle_info_utils::VehicleInfo vehicle_info_; | ||
|
||
// data | ||
sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_; | ||
PredictedObjects::ConstSharedPtr object_ptr_; | ||
OperationModeState::ConstSharedPtr operation_mode_ptr_; | ||
rclcpp::Time last_obstacle_found_stamp_; | ||
|
||
// Diagnostic Updater | ||
diagnostic_updater::Updater updater_; | ||
}; | ||
} // namespace autoware::collision_detector | ||
|
||
#endif // AUTOWARE__COLLISION_DETECTOR__NODE_HPP_ |
12 changes: 12 additions & 0 deletions
12
control/autoware_collision_detector/launch/collision_detector.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,12 @@ | ||
<launch> | ||
<arg name="param_path" default="$(find-pkg-share autoware_collision_detector)/config/collision_detector.param.yaml"/> | ||
|
||
<arg name="input_objects" default="/perception/object_recognition/objects"/> | ||
<arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/> | ||
|
||
<node pkg="autoware_collision_detector" exec="autoware_collision_detector_node" name="collision_detector" output="screen"> | ||
<param from="$(var param_path)"/> | ||
<remap from="~/input/pointcloud" to="$(var input_pointcloud)"/> | ||
<remap from="~/input/objects" to="$(var input_objects)"/> | ||
</node> | ||
</launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>autoware_collision_detector</name> | ||
<version>0.1.0</version> | ||
<description>The collision_detector package</description> | ||
<maintainer email="[email protected]">Shinnosuke Hirakawa</maintainer> | ||
<maintainer email="[email protected]">Kyoichi Sugahara</maintainer> | ||
<maintainer email="[email protected]">Go Sakayori</maintainer> | ||
|
||
<license>Apache License 2.0</license> | ||
|
||
<author email="[email protected]">Shinnosuke Hirakawa</author> | ||
|
||
<buildtool_depend>ament_cmake</buildtool_depend> | ||
<buildtool_depend>eigen3_cmake_module</buildtool_depend> | ||
|
||
<build_depend>autoware_cmake</build_depend> | ||
|
||
<depend>autoware_adapi_v1_msgs</depend> | ||
<depend>autoware_perception_msgs</depend> | ||
<depend>autoware_utils</depend> | ||
<depend>autoware_vehicle_info_utils</depend> | ||
<depend>diagnostic_msgs</depend> | ||
<depend>diagnostic_updater</depend> | ||
<depend>eigen</depend> | ||
<depend>libpcl-all-dev</depend> | ||
<depend>pcl_conversions</depend> | ||
<depend>rclcpp</depend> | ||
<depend>rclcpp_components</depend> | ||
<depend>sensor_msgs</depend> | ||
<depend>std_msgs</depend> | ||
<depend>tf2</depend> | ||
<depend>tf2_eigen</depend> | ||
<depend>tf2_geometry_msgs</depend> | ||
<depend>tf2_ros</depend> | ||
|
||
<test_depend>ament_lint_auto</test_depend> | ||
<test_depend>autoware_lint_common</test_depend> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
Oops, something went wrong.