diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b00fe3371d11f..e03ca0b8ad30f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -58,7 +58,7 @@ control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.m control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/autoware_obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/autoware_obstacle_collision_checker/CMakeLists.txt similarity index 72% rename from control/obstacle_collision_checker/CMakeLists.txt rename to control/autoware_obstacle_collision_checker/CMakeLists.txt index a57aeb093332b..cfe2546617140 100644 --- a/control/obstacle_collision_checker/CMakeLists.txt +++ b/control/autoware_obstacle_collision_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(obstacle_collision_checker) +project(autoware_obstacle_collision_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,12 +12,11 @@ include_directories( ) 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 + DIRECTORY src ) rclcpp_components_register_node(obstacle_collision_checker - PLUGIN "obstacle_collision_checker::ObstacleCollisionCheckerNode" + PLUGIN "autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode" EXECUTABLE obstacle_collision_checker_node ) diff --git a/control/obstacle_collision_checker/README.md b/control/autoware_obstacle_collision_checker/README.md similarity index 100% rename from control/obstacle_collision_checker/README.md rename to control/autoware_obstacle_collision_checker/README.md diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/autoware_obstacle_collision_checker/config/obstacle_collision_checker.param.yaml similarity index 100% rename from control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml rename to control/autoware_obstacle_collision_checker/config/obstacle_collision_checker.param.yaml diff --git a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp new file mode 100644 index 0000000000000..017e64ab15e02 --- /dev/null +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 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 AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ + +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include + +namespace autoware::obstacle_collision_checker +{ +/// @brief create debug markers of the given output +/// @param output structure with output data calculated by the obstacle_collision_checker module +/// @param base_link_z current z value of the base_link in map frame +/// @param now current time +/// @return debug markers +visualization_msgs::msg::MarkerArray create_marker_array( + const Output & output, const double base_link_z, const rclcpp::Time & now); +} // namespace autoware::obstacle_collision_checker +#endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp similarity index 90% rename from control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp rename to control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp index dd09c8ee34ce8..2a3d8cf8778b2 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #include @@ -30,7 +30,7 @@ #include #include -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { using autoware::universe_utils::LinearRing2d; @@ -89,6 +89,6 @@ bool will_collide( bool has_collision( const pcl::PointCloud & obstacle_pointcloud, const LinearRing2d & vehicle_footprint); -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker -#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#endif // AUTOWARE__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/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp similarity index 87% rename from control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp rename to control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 94f08136e064d..86fe8e06872b9 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -12,10 +12,10 @@ // 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_ +#ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ -#include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" #include #include @@ -30,12 +30,11 @@ #include #include #include -#include #include #include -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { struct NodeParam { @@ -101,10 +100,7 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node diagnostic_updater::Updater updater_; void check_lane_departure(diagnostic_updater::DiagnosticStatusWrapper & stat); - - // Visualization - visualization_msgs::msg::MarkerArray create_marker_array() const; }; -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker -#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml similarity index 79% rename from control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml rename to control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index f82384534e040..c2cb75415a0f3 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/control/obstacle_collision_checker/package.xml b/control/autoware_obstacle_collision_checker/package.xml similarity index 97% rename from control/obstacle_collision_checker/package.xml rename to control/autoware_obstacle_collision_checker/package.xml index 45b142fe601fe..9dc29ce6fd387 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/autoware_obstacle_collision_checker/package.xml @@ -1,7 +1,7 @@ - obstacle_collision_checker + autoware_obstacle_collision_checker 0.1.0 The obstacle_collision_checker package Taiki Tanaka diff --git a/control/obstacle_collision_checker/schema/obstacle_collision_checker.json b/control/autoware_obstacle_collision_checker/schema/autoware_obstacle_collision_checker.json similarity index 100% rename from control/obstacle_collision_checker/schema/obstacle_collision_checker.json rename to control/autoware_obstacle_collision_checker/schema/autoware_obstacle_collision_checker.json diff --git a/control/autoware_obstacle_collision_checker/src/debug.cpp b/control/autoware_obstacle_collision_checker/src/debug.cpp new file mode 100644 index 0000000000000..ac847264d3b66 --- /dev/null +++ b/control/autoware_obstacle_collision_checker/src/debug.cpp @@ -0,0 +1,89 @@ +// Copyright 2024 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 "autoware/obstacle_collision_checker/debug.hpp" + +#include + +namespace autoware::obstacle_collision_checker +{ +visualization_msgs::msg::MarkerArray create_marker_array( + const Output & output, const double base_link_z, const rclcpp::Time & now) +{ + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; + + visualization_msgs::msg::MarkerArray marker_array; + + if (output.resampled_trajectory.points.size() >= 2) { + // Line of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", 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", 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", 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 autoware::obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp similarity index 98% rename from control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp rename to control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp index b5a2859cfbd54..bc8cb373a60df 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" #include #include @@ -74,7 +74,7 @@ double calc_braking_distance( } // namespace -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { Output check_for_collisions(const Input & input) { @@ -253,4 +253,4 @@ bool has_collision( return false; } -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp similarity index 74% rename from control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp rename to control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp index 0b0ac42589a72..598ae592f4c8d 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp" + +#include "autoware/obstacle_collision_checker/debug.hpp" #include #include -#include #include #include @@ -24,7 +25,7 @@ #include #include -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_collision_checker_node", node_options), @@ -45,7 +46,7 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt // Dynamic Reconfigure set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &::obstacle_collision_checker::ObstacleCollisionCheckerNode::param_callback, this, _1)); + &autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode::param_callback, this, _1)); // Subscriber self_pose_listener_ = std::make_shared(this); @@ -207,7 +208,8 @@ void ObstacleCollisionCheckerNode::on_timer() updater_.force_update(); - debug_publisher_->publish("marker_array", create_marker_array()); + debug_publisher_->publish( + "marker_array", create_marker_array(output_, current_pose_->pose.position.z, this->now())); time_publisher_->publish(output_.processing_time_map); } @@ -256,79 +258,7 @@ void ObstacleCollisionCheckerNode::check_lane_departure( stat.summary(level, msg); } - -visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::create_marker_array() const -{ - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_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 +} // namespace autoware::obstacle_collision_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_collision_checker::ObstacleCollisionCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode) diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp similarity index 97% rename from control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp rename to control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index 581b75ca463ab..97b26162812ef 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. All rights reserved. +// Copyright 2022-2024 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. @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/obstacle_collision_checker_node/obstacle_collision_checker.cpp" // NOLINT +#include "../src/obstacle_collision_checker.cpp" // NOLINT #include "gtest/gtest.h" -#include "obstacle_collision_checker/obstacle_collision_checker.hpp" #include @@ -159,7 +158,7 @@ TEST(test_obstacle_collision_checker, calcBrakingDistance) TEST(test_obstacle_collision_checker, check_for_collisions) { - obstacle_collision_checker::Input input; + autoware::obstacle_collision_checker::Input input; // call with empty input causes a segfault // const auto output = check_for_collisions(input); input.param.delay_time = 1.0; diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index cc24f339ed404..9b2efd2da8961 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -174,7 +174,7 @@ - +