refactor(obstacle_collision_checker): add some debugs, change QoS (#2191)

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
Co-authored-by: Berkay Karaman <berkay@leodrive.ai>

Author: brkay54

control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp

@@ -163,7 +163,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec
     const auto remain_distance = length - total_length;
 
     // Over length
-    if (remain_distance <= 0) {
+    if (remain_distance <= 0.0) {
       break;
     }
 
@@ -239,7 +239,9 @@ bool ObstacleCollisionChecker::willCollide(
   const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
   const std::vector<LinearRing2d> & vehicle_footprints)
 {
-  for (const auto & vehicle_footprint : vehicle_footprints) {
+  for (size_t i = 1; i < vehicle_footprints.size(); i++) {
+    // skip first footprint because surround obstacle checker handle it
+    const auto & vehicle_footprint = vehicle_footprints.at(i);
     if (hasCollision(obstacle_pointcloud, vehicle_footprint)) {
       RCLCPP_WARN(
         rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide");
@@ -254,7 +256,7 @@ bool ObstacleCollisionChecker::hasCollision(
   const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
   const LinearRing2d & vehicle_footprint)
 {
-  for (const auto & point : obstacle_pointcloud) {
+  for (const auto & point : obstacle_pointcloud.points) {
     if (boost::geometry::within(
           tier4_autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) {
       RCLCPP_WARN(

control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp

@@ -76,7 +76,7 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt
   transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
 
   sub_obstacle_pointcloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
-    "input/obstacle_pointcloud", 1,
+    "input/obstacle_pointcloud", rclcpp::SensorDataQoS(),
     std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1));
   sub_reference_trajectory_ = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
     "input/reference_trajectory", 1,
@@ -199,8 +199,15 @@ 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));
+    try {
+      obstacle_transform_ = transform_listener_->getTransform(
+        "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_INFO(
+        this->get_logger(), "Could not transform map to %s: %s", header.frame_id.c_str(),
+        ex.what());
+      return;
+    }
   }
 
   if (!isDataReady()) {