From fd545872f032d27f85ccbc1851416f616ba4b59f Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 11 Nov 2021 10:57:43 +0900 Subject: [PATCH 1/5] Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura --- .../obstacle_collision_checker/CMakeLists.txt | 46 +++ control/obstacle_collision_checker/README.md | 86 +++++ .../obstacle_collision_checker.param.yaml | 11 + .../obstacle_collision_checker.hpp | 107 ++++++ .../obstacle_collision_checker_node.hpp | 110 ++++++ .../util/create_vehicle_footprint.hpp | 47 +++ .../obstacle_collision_checker.launch.xml | 17 + .../obstacle_collision_checker/package.xml | 35 ++ .../obstacle_collision_checker.cpp | 261 ++++++++++++++ .../obstacle_collision_checker_node.cpp | 341 ++++++++++++++++++ 10 files changed, 1061 insertions(+) create mode 100644 control/obstacle_collision_checker/CMakeLists.txt create mode 100644 control/obstacle_collision_checker/README.md create mode 100644 control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml create mode 100644 control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp create mode 100644 control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp create mode 100644 control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp create mode 100755 control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml create mode 100644 control/obstacle_collision_checker/package.xml create mode 100644 control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp create mode 100644 control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/obstacle_collision_checker/CMakeLists.txt new file mode 100644 index 0000000000000..9a329dfaedf3f --- /dev/null +++ b/control/obstacle_collision_checker/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(obstacle_collision_checker) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + + +include_directories( + include + ${PCL_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +# Target +## obstacle_collision_checker_node +ament_auto_add_library(obstacle_collision_checker SHARED + src/obstacle_collision_checker_node/obstacle_collision_checker.cpp + src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +) + +rclcpp_components_register_node(obstacle_collision_checker + PLUGIN "obstacle_collision_checker::ObstacleCollisionCheckerNode" + EXECUTABLE obstacle_collision_checker_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md new file mode 100644 index 0000000000000..27fb80cf34a64 --- /dev/null +++ b/control/obstacle_collision_checker/README.md @@ -0,0 +1,86 @@ +# obstacle_collision_checker + +## Purpose + +`obstacle_collision_checker` is a module to check obstacle collision for predicted trajectory and publish diagnostic errors if collision is found. + +## Inner-workings / Algorithms + +### Flow chart + +```plantuml +@startuml +skinparam monochrome true + +title obstacle collision checker : update +start + +:calculate braking distance; + +:resampling trajectory; +note right +to reduce calculation cost +end note +:filter point cloud by trajectory; + +:create vehicle foot prints; + +:create vehicle passing area; + +partition will_collide { + +while (has next ego vehicle foot print) is (yes) + :found collision with obstacle foot print; + if (has collision with obstacle) then (yes) + :set diag to ERROR; + stop + endif +end while (no) +:set diag to OK; +stop +} + +@enduml +``` + +### Algorithms + +### Check data + +Check that `obstacle_collision_checker` receives no ground pointcloud, predicted_trajectory, reference trajectory, and current velocity data. + +### Diagnostic update + +If any collision is found on predicted path, this module sets `ERROR` level as diagnostic status else sets `OK`. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------- | ---------------------------------------------- | -------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/sensing/lidar/no_ground/pointcloud` | `sensor_msgs::msg::PointCloud2` | No ground pointcloud | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | + +### Output + +| Name | Type | Description | +| ---------------- | -------------------------------------- | ------------------------ | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | + +## Parameters + +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------------------------- | :------------ | +| `delay_time` | `double` | Delay time of vehicle [s] | 0.3 | +| `footprint_margin` | `double` | Foot print margin [m] | 0.0 | +| `max_deceleration` | `double` | Max deceleration for ego vehicle to stop [m/s^2] | 2.0 | +| `resample_interval` | `double` | Interval for resampling trajectory [m] | 0.3 | +| `search_radius` | `double` | Search distance from trajectory to point cloud [m] | 5.0 | + +## Assumptions / Known limits + +To perform proper collision check, it is necessary to get probably predicted trajectory and obstacle pointclouds without noise. diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml new file mode 100644 index 0000000000000..883b4d8259f49 --- /dev/null +++ b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + + # Core + delay_time: 0.3 # delay time of vehicle [s] + footprint_margin: 0.0 # margin for footprint [m] + max_deceleration: 2.0 # max deceleration [m/ss] + resample_interval: 0.3 # interval distance to resample point cloud [m] + search_radius: 5.0 # search distance from trajectory to point cloud [m] diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp new file mode 100644 index 0000000000000..ba609407386d4 --- /dev/null +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -0,0 +1,107 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace obstacle_collision_checker +{ +using autoware_utils::LinearRing2d; + +struct Param +{ + double delay_time; + double footprint_margin; + double max_deceleration; + double resample_interval; + double search_radius; +}; + +struct Input +{ + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud; + geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; +}; + +struct Output +{ + std::map processing_time_map; + bool will_collide; + autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + std::vector vehicle_footprints; + std::vector vehicle_passing_areas; +}; + +class ObstacleCollisionChecker +{ +public: + explicit ObstacleCollisionChecker(rclcpp::Node & node); + Output update(const Input & input); + + void setParam(const Param & param) { param_ = param; } + +private: + Param param_; + vehicle_info_util::VehicleInfo vehicle_info_; + + //! This function assumes the input trajectory is sampled dense enough + static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval); + + static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length); + + static std::vector createVehicleFootprints( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const vehicle_info_util::VehicleInfo & vehicle_info); + + static std::vector createVehiclePassingAreas( + const std::vector & vehicle_footprints); + + static LinearRing2d createHullFromFootprints( + const LinearRing2d & area1, const LinearRing2d & area2); + + static bool willCollide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints); + + static bool hasCollision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint); +}; +} // namespace obstacle_collision_checker + +#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp new file mode 100644 index 0000000000000..591e1f1a0ea69 --- /dev/null +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -0,0 +1,110 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ + +#include "obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace obstacle_collision_checker +{ +struct NodeParam +{ + double update_rate; +}; + +class ObstacleCollisionCheckerNode : public rclcpp::Node +{ +public: + explicit ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; + rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::SharedPtr sub_odom_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_; + geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + + // Callback + void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + + // Publisher + std::shared_ptr debug_publisher_; + std::shared_ptr time_publisher_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + bool isDataReady(); + bool isDataTimeout(); + void onTimer(); + + // Parameter + NodeParam node_param_; + Param param_; + + // Dynamic Reconfigure + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // Core + Input input_; + Output output_; + std::unique_ptr obstacle_collision_checker_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; + + void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Visualization + visualization_msgs::msg::MarkerArray createMarkerArray() const; +}; +} // namespace obstacle_collision_checker + +#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp new file mode 100644 index 0000000000000..5d80924603974 --- /dev/null +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ + +#include +#include + +inline autoware_utils::LinearRing2d createVehicleFootprint( + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m + margin; + const double x_center = i.wheel_base_m / 2.0; + const double x_rear = -(i.rear_overhang_m + margin); + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; + const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +#endif // OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml new file mode 100755 index 0000000000000..4815c786cf967 --- /dev/null +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml new file mode 100644 index 0000000000000..71ff20b67d013 --- /dev/null +++ b/control/obstacle_collision_checker/package.xml @@ -0,0 +1,35 @@ + + + obstacle_collision_checker + 0.1.0 + The obstacle_collision_checker package + Satoshi Tanaka + Kenji Miyake + Apache License 2.0 + + ament_cmake + + autoware_auto_planning_msgs + autoware_utils + boost + diagnostic_updater + eigen + geometry_msgs + nav_msgs + pcl_ros + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp new file mode 100644 index 0000000000000..221bc5d6349cb --- /dev/null +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -0,0 +1,261 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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. + +#include "obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +namespace +{ +pcl::PointCloud getTransformedPointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, + const geometry_msgs::msg::Transform & transform) +{ + const Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast(); + + sensor_msgs::msg::PointCloud2 transformed_msg; + pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); + + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(transformed_msg, transformed_pointcloud); + + return transformed_pointcloud; +} + +pcl::PointCloud filterPointCloudByTrajectory( + const pcl::PointCloud & pointcloud, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) +{ + pcl::PointCloud filtered_pointcloud; + for (const auto & trajectory_point : trajectory.points) { + for (const auto & point : pointcloud.points) { + const double dx = trajectory_point.pose.position.x - point.x; + const double dy = trajectory_point.pose.position.y - point.y; + if (std::hypot(dx, dy) < radius) { + filtered_pointcloud.points.push_back(point); + } + } + } + return filtered_pointcloud; +} + +double calcBrakingDistance( + const double abs_velocity, const double max_deceleration, const double delay_time) +{ + const double idling_distance = abs_velocity * delay_time; + const double braking_distance = (abs_velocity * abs_velocity) / (2.0 * max_deceleration); + return idling_distance + braking_distance; +} + +} // namespace + +namespace obstacle_collision_checker +{ +ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) +: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +{ +} + +Output ObstacleCollisionChecker::update(const Input & input) +{ + Output output; + autoware_utils::StopWatch stop_watch; + + // resample trajectory by braking distance + constexpr double min_velocity = 0.01; + const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x); + const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; + const auto braking_distance = + calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); + output.resampled_trajectory = cutTrajectory( + resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); + output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); + + // resample pointcloud + const auto obstacle_pointcloud = + getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); + const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory( + obstacle_pointcloud, output.resampled_trajectory, param_.search_radius); + + output.vehicle_footprints = + createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_); + output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); + + output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); + + output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); + output.processing_time_map["willCollide"] = stop_watch.toc(true); + + return output; +} + +autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +{ + autoware_auto_planning_msgs::msg::Trajectory resampled; + resampled.header = trajectory.header; + + resampled.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = autoware_utils::fromMsg(point.pose.position).to_2d(); + + if (boost::geometry::distance(p1, p2) > interval) { + resampled.points.push_back(point); + } + } + resampled.points.push_back(trajectory.points.back()); + + return resampled; +} + +autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +{ + autoware_auto_planning_msgs::msg::Trajectory cut; + cut.header = trajectory.header; + + double total_length = 0.0; + cut.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + autoware_auto_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.points.push_back(p); + break; + } + + cut.points.push_back(point); + total_length += points_distance; + } + + return cut; +} + +std::vector ObstacleCollisionChecker::createVehicleFootprints( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info, param.footprint_margin); + + // Create vehicle footprint on each TrajectoryPoint + std::vector vehicle_footprints; + for (const auto & p : trajectory.points) { + vehicle_footprints.push_back(autoware_utils::transformVector( + local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); + } + + return vehicle_footprints; +} + +std::vector ObstacleCollisionChecker::createVehiclePassingAreas( + const std::vector & vehicle_footprints) +{ + // Create hull from two adjacent vehicle footprints + std::vector areas; + for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { + const auto & footprint1 = vehicle_footprints.at(i); + const auto & footprint2 = vehicle_footprints.at(i + 1); + areas.push_back(createHullFromFootprints(footprint1, footprint2)); + } + + return areas; +} + +LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( + const LinearRing2d & area1, const LinearRing2d & area2) +{ + autoware_utils::MultiPoint2d combined; + for (const auto & p : area1) { + combined.push_back(p); + } + for (const auto & p : area2) { + combined.push_back(p); + } + LinearRing2d hull; + boost::geometry::convex_hull(combined, hull); + return hull; +} + +bool ObstacleCollisionChecker::willCollide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints) +{ + for (const auto & vehicle_footprint : vehicle_footprints) { + if (hasCollision(obstacle_pointcloud, vehicle_footprint)) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide"); + return true; + } + } + + return false; +} + +bool ObstacleCollisionChecker::hasCollision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint) +{ + for (const auto & point : obstacle_pointcloud) { + if (boost::geometry::within(autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_collision_checker"), + "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); + return true; + } + } + + return false; +} +} // namespace obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp new file mode 100644 index 0000000000000..9817721d9c37a --- /dev/null +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -0,0 +1,341 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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. + +#include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" + +#include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace +{ +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = it->template get_value(); + } +} +} // namespace + +namespace obstacle_collision_checker +{ +ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_collision_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + + // Core Parameter + param_.delay_time = declare_parameter("delay_time", 0.3); + param_.footprint_margin = declare_parameter("footprint_margin", 0.0); + param_.max_deceleration = declare_parameter("max_deceleration", 2.0); + param_.resample_interval = declare_parameter("resample_interval", 0.5); + param_.search_radius = declare_parameter("search_radius", 5.0); + + // Dynamic Reconfigure + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ObstacleCollisionCheckerNode::paramCallback, this, _1)); + + // Core + obstacle_collision_checker_ = std::make_unique(*this); + obstacle_collision_checker_->setParam(param_); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); + + sub_obstacle_pointcloud_ = create_subscription( + "input/obstacle_pointcloud", 1, + std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); + sub_reference_trajectory_ = create_subscription( + "input/reference_trajectory", 1, + std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "input/predicted_trajectory", 1, + std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); + + // Publisher + debug_publisher_ = std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); + + // Diagnostic Updater + updater_.setHardwareID("obstacle_collision_checker"); + + updater_.add( + "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::checkLaneDeparture); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void ObstacleCollisionCheckerNode::onObstaclePointcloud( + const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + obstacle_pointcloud_ = msg; +} + +void ObstacleCollisionCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void ObstacleCollisionCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void ObstacleCollisionCheckerNode::initTimer(double period_s) +{ + auto timer_callback = std::bind(&ObstacleCollisionCheckerNode::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +bool ObstacleCollisionCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!obstacle_pointcloud_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for obstacle_pointcloud msg..."); + return false; + } + + if (!obstacle_transform_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for obstacle_transform..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for predicted_trajectory msg..."); + return false; + } + + if (!current_twist_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_twist msg..."); + return false; + } + + return true; +} + +bool ObstacleCollisionCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp).seconds() - now.seconds(); + if (pose_time_diff > th_pose_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void ObstacleCollisionCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + if (obstacle_pointcloud_) { + const auto & header = obstacle_pointcloud_->header; + obstacle_transform_ = transform_listener_->getTransform( + "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); + } + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + input_.current_pose = current_pose_; + input_.obstacle_pointcloud = obstacle_pointcloud_; + input_.obstacle_transform = obstacle_transform_; + input_.reference_trajectory = reference_trajectory_; + input_.predicted_trajectory = predicted_trajectory_; + input_.current_twist = current_twist_; + + output_ = obstacle_collision_checker_->update(input_); + + updater_.force_update(); + + debug_publisher_->publish("marker_array", createMarkerArray()); + + time_publisher_->publish(output_.processing_time_map); +} + +rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + Param param; + try { + update_param(parameters, "delay_time", param.delay_time); + update_param(parameters, "footprint_margin", param.footprint_margin); + update_param(parameters, "max_deceleration", param.max_deceleration); + update_param(parameters, "resample_interval", param.resample_interval); + update_param(parameters, "search_radius", param.search_radius); + param_ = param; + if (obstacle_collision_checker_) { + obstacle_collision_checker_->setParam(param_); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + return result; +} + +void ObstacleCollisionCheckerNode::checkLaneDeparture( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (output_.will_collide) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + + stat.summary(level, msg); +} + +visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const +{ + using autoware_utils::createDefaultMarker; + using autoware_utils::createMarkerColor; + using autoware_utils::createMarkerScale; + + visualization_msgs::msg::MarkerArray marker_array; + + const auto base_link_z = current_pose_->pose.position.z; + + if (output_.resampled_trajectory.points.size() >= 2) { + // Line of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_line", 0, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + + // Points of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_points", 0, + visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1), + createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + } + + // Vehicle Footprints + { + const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); + const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); + + auto color = color_ok; + if (output_.will_collide) { + color = color_will_collide; + } + + auto marker = createDefaultMarker( + "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0, 0), color); + + for (const auto & vehicle_footprint : output_.vehicle_footprints) { + for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { + const auto p1 = vehicle_footprint.at(i); + const auto p2 = vehicle_footprint.at(i + 1); + + marker.points.push_back(toMsg(p1.to_3d(base_link_z))); + marker.points.push_back(toMsg(p2.to_3d(base_link_z))); + } + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} +} // namespace obstacle_collision_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_collision_checker::ObstacleCollisionCheckerNode) From f66ee9bb886887da0fe9e4e4fc0ee52a5a4fd810 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 25 Nov 2021 17:27:15 +0900 Subject: [PATCH 2/5] Fix no ground pointcloud topic name (#733) Signed-off-by: j4tfwm6z Co-authored-by: j4tfwm6z --- control/obstacle_collision_checker/README.md | 14 +++++++------- .../launch/obstacle_collision_checker.launch.xml | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index 27fb80cf34a64..d2e810909e811 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| ------------------------------------- | ---------------------------------------------- | -------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/sensing/lidar/no_ground/pointcloud` | `sensor_msgs::msg::PointCloud2` | No ground pointcloud | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| -------------------------------------------- | ---------------------------------------------- | -------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/object_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | No ground pointcloud | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 4815c786cf967..2d7487ee07cac 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -1,6 +1,6 @@ - + From 252429269cf730717da2342e45ef8ebec7348fb3 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 26 Nov 2021 12:39:34 +0900 Subject: [PATCH 3/5] Update twist topic name (#736) Signed-off-by: wep21 --- .../launch/obstacle_collision_checker.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 2d7487ee07cac..848c67bad906c 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -3,7 +3,7 @@ - + From 5ce98cc1cd3448f3c682d33e4582f41f7bfff733 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 29 Nov 2021 15:09:08 +0900 Subject: [PATCH 4/5] fix/rename segmentation namespace (#742) * rename segmentation directory * fix namespace: system stack * fix namespace: planning * fix namespace: control stack * fix namespace: perception stack * fix readme --- control/obstacle_collision_checker/README.md | 14 +++++++------- .../launch/obstacle_collision_checker.launch.xml | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index d2e810909e811..5478db1f5a135 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| -------------------------------------------- | ---------------------------------------------- | -------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/perception/object_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | No ground pointcloud | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 848c67bad906c..1391692ad6b8a 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -1,6 +1,6 @@ - + From ca2ac266a93b5620b9ecfdd672815d88bdc79aa3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Nov 2021 10:09:49 +0000 Subject: [PATCH 5/5] ci(pre-commit): autofix --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index 70254bd0936f2..5ce2c268169c8 100644 --- a/.gitignore +++ b/.gitignore @@ -13,4 +13,3 @@ log/ # Python *.pyc -