refactor(obstacle_collision_checker): add some debugs, change QoS (au…

…towarefoundation#2191) Signed-off-by: Berkay Karaman <[email protected]> Signed-off-by: Berkay Karaman <[email protected]> Co-authored-by: Berkay Karaman <[email protected]>
Interplai · Nov 1, 2022 · ff1af93 · ff1af93
1 parent 3a69438
commit ff1af93
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/...acle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/...acle_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(

diff --git a/...collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/...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()) {