refactor(obstacle_collision_checker): move to autoware namespace (#9047)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

.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

control/obstacle_collision_checker/CMakeLists.txt 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
 )
 

control/obstacle_collision_checker/README.md control/autoware_obstacle_collision_checker/README.md

@@ -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 <visualization_msgs/msg/marker_array.hpp>
+
+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_

control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml control/autoware_obstacle_collision_checker/config/obstacle_collision_checker.param.yaml

@@ -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 <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
@@ -30,7 +30,7 @@
 #include <string>
 #include <vector>
 
-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<pcl::PointXYZ> & 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_

control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.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 <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
@@ -30,12 +30,11 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <memory>
 #include <vector>
 
-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_

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp

@@ -4,11 +4,11 @@
   <arg name="input/reference_trajectory" default="/planning/scenario_planning/trajectory"/>
   <arg name="input/predicted_trajectory" default="/control/trajectory_follower/predicted_trajectory"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
-  <arg name="config_file" default="$(find-pkg-share obstacle_collision_checker)/config/obstacle_collision_checker.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_obstacle_collision_checker)/config/obstacle_collision_checker.param.yaml"/>
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share autoware_vehicle_info_utils)/config/vehicle_info.param.yaml"/>
 
-  <node pkg="obstacle_collision_checker" exec="obstacle_collision_checker_node" name="obstacle_collision_checker_node" output="screen">
+  <node pkg="autoware_obstacle_collision_checker" exec="obstacle_collision_checker_node" name="obstacle_collision_checker_node" output="screen">
     <param from="$(var config_file)"/>
     <param from="$(var vehicle_info_param_file)"/>
     <remap from="input/lanelet_map_bin" to="$(var input/lanelet_map_bin)"/>

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp

@@ -1,7 +1,7 @@
 <?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>obstacle_collision_checker</name>
+  <name>autoware_obstacle_collision_checker</name>
   <version>0.1.0</version>
   <description>The obstacle_collision_checker package</description>
   <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>

control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml

@@ -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 <autoware/universe_utils/ros/marker_helper.hpp>
+
+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

control/obstacle_collision_checker/package.xml control/autoware_obstacle_collision_checker/package.xml

@@ -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 <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/normalization.hpp>
@@ -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

control/obstacle_collision_checker/schema/obstacle_collision_checker.json control/autoware_obstacle_collision_checker/schema/autoware_obstacle_collision_checker.json

@@ -12,19 +12,20 @@
 // 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 <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
-#include <autoware/universe_utils/ros/marker_helper.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
 #include <memory>
 #include <string>
 #include <vector>
 
-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<autoware::universe_utils::SelfPoseListener>(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_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_collision_checker::ObstacleCollisionCheckerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode)