adding test for collision checker

adding test cases

move collision checker to nav2_costmap_2d

nav2_costmap_2d/CMakeLists.txt

@@ -49,8 +49,6 @@ add_library(nav2_costmap_2d_core SHARED
   src/costmap_layer.cpp
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
-  src/footprint_subscriber.cpp
-  src/costmap_subscriber.cpp
 )
 
 set(dependencies
@@ -92,6 +90,20 @@ target_link_libraries(layers
   nav2_costmap_2d_core
 )
 
+add_library(nav2_costmap_2d_client SHARED
+  src/footprint_subscriber.cpp
+  src/costmap_subscriber.cpp
+  src/collision_checker.cpp
+)
+
+ament_target_dependencies(nav2_costmap_2d_client
+  ${dependencies}
+)
+
+target_link_libraries(nav2_costmap_2d_client
+  nav2_costmap_2d_core
+)
+
 add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp)
 target_link_libraries(nav2_costmap_2d_markers
   nav2_costmap_2d_core

nav2_dwb_controller/dwb_critics/include/dwb_critics/collision_checker.hpp → nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef DWB_CRITICS__COLLISION_CHECKER_HPP_
-#define DWB_CRITICS__COLLISION_CHECKER_HPP_
+#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
 
 #include <string>
 
@@ -33,7 +33,7 @@
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
 
-namespace dwb_critics
+namespace nav2_costmap_2d
 {
 typedef std::vector<geometry_msgs::msg::Point> Footprint;
 
@@ -42,8 +42,8 @@ class CollisionChecker
 public:
   CollisionChecker(
     rclcpp::Node::SharedPtr ros_node,
-    std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
-    std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub,
+    std::shared_ptr<CostmapSubscriber> costmap_sub,
+    std::shared_ptr<FootprintSubscriber> footprint_sub,
     tf2_ros::Buffer & tf_buffer,
     std::string name = "collision_checker");
   
@@ -69,9 +69,9 @@ protected:
 
   rclcpp::Node::SharedPtr node_;
   std::string name_;
-  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
-  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;  
+  std::shared_ptr<CostmapSubscriber> costmap_sub_;
+  std::shared_ptr<FootprintSubscriber> footprint_sub_;  
 };
-}  // namespace dwb_critics
+}  // namespace nav2_costmap_2d
 
-#endif  // DWB_CRITICS__COLLISION_CHECKER_HPP_
+#endif  // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
+#define NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+#include <memory>
+
+namespace nav2_costmap_2d
+{
+
+class CollisionCheckerException : public std::runtime_error
+{
+public:
+  explicit CollisionCheckerException(const std::string description)
+  : std::runtime_error(description) {}
+  typedef std::shared_ptr<CollisionCheckerException> Ptr;
+};
+
+/**
+ * @class IllegalPoseException
+ * @brief Thrown when CollisionChecker encounters a fatal error
+ */
+class IllegalPoseException : public CollisionCheckerException
+{
+public:
+  IllegalPoseException(const std::string name, const std::string description)
+  : CollisionCheckerException(description), name_(name) {}
+  std::string getCriticName() const {return name_;}
+
+protected:
+  std::string name_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__EXCEPTIONS_HPP_

nav2_dwb_controller/dwb_critics/src/collision_checker.cpp → nav2_costmap_2d/src/collision_checker.cpp

@@ -12,27 +12,29 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "dwb_critics/collision_checker.hpp"
+#include "nav2_costmap_2d/collision_checker.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
-#include "dwb_critics/line_iterator.hpp"
-#include "dwb_core/exceptions.hpp"
+#include "nav2_util/line_iterator.hpp"
+#include "nav2_costmap_2d/exceptions.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
 
-namespace dwb_critics
+namespace nav2_costmap_2d
 {
 
 CollisionChecker::CollisionChecker(
   rclcpp::Node::SharedPtr ros_node,
-  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
-  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub,
+  std::shared_ptr<CostmapSubscriber> costmap_sub,
+  std::shared_ptr<FootprintSubscriber> footprint_sub,
   tf2_ros::Buffer & tf_buffer,
   std::string name)
 : tf_buffer_(tf_buffer),
   node_(ros_node), name_(name),
   costmap_sub_(costmap_sub), footprint_sub_(footprint_sub)
 {
-  node_->get_parameter_or<std::string>("global_frame", global_frame_, std::string("map"));
-  node_->get_parameter_or<std::string>("robot_base_frame", robot_base_frame_, std::string("base_link"));
+  node_->get_parameter_or<std::string>(
+    "global_frame", global_frame_, std::string("map"));
+  node_->get_parameter_or<std::string>(
+    "robot_base_frame", robot_base_frame_, std::string("base_link"));
 }
 
 CollisionChecker::~CollisionChecker() {}
@@ -45,10 +47,10 @@ const geometry_msgs::msg::Pose2D & pose)
       return false;
     }
     return true;
-  } catch (const nav_core2::IllegalTrajectoryException & e) {
+  } catch (const IllegalPoseException & e) {
     RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
     return false;
-  } catch (const nav_core2::PlannerException & e) {
+  } catch (const CollisionCheckerException & e) {
     RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
     return false;
   }
@@ -57,27 +59,27 @@ const geometry_msgs::msg::Pose2D & pose)
 double CollisionChecker::scorePose(
 const geometry_msgs::msg::Pose2D & pose)
 {
-  nav2_costmap_2d::Costmap2D * costmap_;
+  Costmap2D * costmap_;
   try {
     costmap_ = costmap_sub_->getCostmap();
   } catch (const std::runtime_error & e) {
-    throw nav_core2::PlannerException(e.what());
+    throw CollisionCheckerException(e.what());
   }
 
   unsigned int cell_x, cell_y;
   if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
-    RCLCPP_ERROR(node_->get_logger(), "Map Cell: [%d, %d]", cell_x, cell_y);
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
+    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", cell_x, cell_y);
+    throw IllegalPoseException(name_, "Pose Goes Off Grid.");
   }
 
   Footprint footprint;
   if (!footprint_sub_->getFootprint(footprint)) {
-    throw nav_core2::PlannerException("Footprint not available.");
+    throw CollisionCheckerException("Footprint not available.");
   }
 
   Footprint footprint_spec;
   unorientFootprint(footprint, footprint_spec);
-  nav2_costmap_2d::transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
+  transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
 
   // now we really have to lay down the footprint in the costmap grid
   unsigned int x0, x1, y0, y1;
@@ -89,13 +91,13 @@ const geometry_msgs::msg::Pose2D & pose)
     // get the cell coord of the first point
     if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
       RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0);
-      throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid at map.");
+      throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
     }
 
     // get the cell coord of the second point
     if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
       RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1);
-      throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+      throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
     }
 
     line_cost = lineCost(x0, x1, y0, y1);
@@ -106,13 +108,13 @@ const geometry_msgs::msg::Pose2D & pose)
   // get the cell coord of the last point
   if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
       RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0);
-    throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+    throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
   }
 
   // get the cell coord of the first point
   if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
       RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1);
-    throw nav_core2::IllegalTrajectoryException(name_, "Footprint Goes Off Grid.");
+    throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
   }
 
   line_cost = lineCost(x0, x1, y0, y1);
@@ -127,7 +129,7 @@ double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)
   double line_cost = 0.0;
   double point_cost = -1.0;
 
-  for (LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
+  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
     point_cost = pointCost(line.getX(), line.getY());   // Score the current point
 
     if (line_cost < point_cost) {
@@ -140,16 +142,16 @@ double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)
 
 double CollisionChecker::pointCost(int x, int y)
 {
-  nav2_costmap_2d::Costmap2D * costmap_ = costmap_sub_->getCostmap();
+  Costmap2D * costmap_ = costmap_sub_->getCostmap();
 
   unsigned char cost = costmap_->getCost(x, y);
   // if the cell is in an obstacle the path is invalid or unknown
-  if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
+  if (cost == LETHAL_OBSTACLE) {
     RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x, y);
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
-  } else if (cost == nav2_costmap_2d::NO_INFORMATION) {
+    throw IllegalPoseException(name_, "Footprint Hits Obstacle.");
+  } else if (cost == NO_INFORMATION) {
     RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x, y);
-    throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
+    throw IllegalPoseException(name_, "Footprint Hits Unknown Region.");
   }
 
   return cost;
@@ -182,7 +184,6 @@ CollisionChecker::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) co
       "Extrapolation Error looking up robot pose: %s\n", ex.what());
     return false;
   }
-  // check global_pose timeout
 
   return true;
 }
@@ -193,7 +194,7 @@ void CollisionChecker::unorientFootprint(
 {
   geometry_msgs::msg::PoseStamped current_pose;
    if (!getRobotPose(current_pose)) {
-    throw nav_core2::PlannerException("Robot pose unavailable.");
+    throw CollisionCheckerException("Robot pose unavailable.");
   }
 
   double x = current_pose.pose.position.x;
@@ -201,9 +202,9 @@ void CollisionChecker::unorientFootprint(
   double theta = tf2::getYaw(current_pose.pose.orientation);
 
   Footprint temp;
-  nav2_costmap_2d::transformFootprint(-x, -y, 0, oriented_footprint, temp);
-  nav2_costmap_2d::transformFootprint(0, 0, -theta, temp, reset_footprint);
+  transformFootprint(-x, -y, 0, oriented_footprint, temp);
+  transformFootprint(0, 0, -theta, temp, reset_footprint);
 }
 
-}  // namespace dwb_critics
+}  // namespace nav2_costmap_2d
 

nav2_costmap_2d/test/integration/CMakeLists.txt

@@ -9,6 +9,18 @@ target_link_libraries(footprint_tests_exec
   layers
 )
 
+ament_add_gtest_executable(test_collision_checker_exec
+  test_collision_checker.cpp
+)
+ament_target_dependencies(test_collision_checker_exec
+  ${dependencies}
+)
+target_link_libraries(test_collision_checker_exec
+  nav2_costmap_2d_core
+  nav2_costmap_2d_client
+  layers
+)
+
 ament_add_gtest_executable(inflation_tests_exec
   inflation_tests.cpp
 )
@@ -31,6 +43,16 @@ target_link_libraries(obstacle_tests_exec
   layers
 )
 
+ament_add_test(test_collision_checker
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_collision_checker_exec>
+)
+
 ament_add_test(footprint_tests
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"

nav2_costmap_2d/test/integration/test_collision_checker.cpp

+// Copyright (c) 2019 Intel Corporation
+//
+// 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 <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/static_layer.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/transform_datatypes.h"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+
+using namespace std::chrono_literals;
+
+class TestCollisionChecker : public nav2_costmap_2d::CollisionChecker
+{
+public:
+  TestCollisionChecker(
+    rclcpp::Node::SharedPtr node,
+    std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
+    std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub,
+    tf2_ros::Buffer & tf_buffer)
+  : CollisionChecker(node, costmap_sub, footprint_sub, tf_buffer),
+    node_(node),
+    layers_("frame", false, false),
+    costmap_received_(false),
+    tf_(tf_buffer)
+  {
+    node_->get_parameter_or<std::string>("global_frame", global_frame_, std::string("map"));
+
+    base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
+    base_rel_map.child_frame_id = "base_link";
+    base_rel_map.header.frame_id = "map";
+    base_rel_map.header.stamp = node_->now();
+    tf_.setTransform(base_rel_map, "collision_checker_test");
+
+    // Add Static Layer
+    nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer();
+    layers_.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
+    slayer->initialize(&layers_, "static", &tf_, node_);
+
+    // Add Inflation Layer
+    nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer();
+    ilayer->initialize(&layers_, "inflation", &tf_, node_);
+    std::shared_ptr<nav2_costmap_2d::Layer> ipointer(ilayer);
+    layers_.addPlugin(ipointer);
+
+    footprint_pub_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>(
+      "footprint", rmw_qos_profile_default);
+
+    costmap_pub_ = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(node_,
+        layers_.getCostmap(), global_frame_, "costmap", true);
+    
+    publish();
+  }
+
+  ~TestCollisionChecker() {}
+
+  bool testPose(double x,double y, double theta)
+  {
+    geometry_msgs::msg::Pose2D pose;
+    pose.x = x;
+    pose.y = y;
+    pose.theta = theta;
+
+    setPose(x,y,theta);
+    costmap_received_ = false;
+
+    while(!costmap_received_)
+    {
+      rclcpp::spin_some(node_);
+    }
+
+    return isCollisionFree(pose);
+  }
+
+  void setFootprint(double footprint_padding, double robot_radius)
+  {
+    std::vector<geometry_msgs::msg::Point> new_footprint;
+    new_footprint = nav2_costmap_2d::makeFootprintFromRadius(robot_radius);
+    nav2_costmap_2d::padFootprint(new_footprint, footprint_padding);
+    footprint_ = new_footprint;
+    layers_.setFootprint(footprint_);
+  }
+
+protected:
+  void setPose(double x, double y, double theta)
+  {
+    x_ = x;
+    y_ = y;
+    yaw_ = theta;
+
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = x_;
+    pose.position.y = y_;
+    pose.position.z = 0;
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw_);
+    pose.orientation = tf2::toMsg(q);
+
+    tf2::Transform transform;
+    tf2::fromMsg(pose, transform);
+    base_rel_map.transform = tf2::toMsg(transform);
+    base_rel_map.header.stamp = node_->now();
+    tf_.setTransform(base_rel_map, "collision_checker_test");
+  }
+
+  void publish()
+  {
+    auto timer_callback = [this]() -> void 
+    {
+      try {
+        costmap_sub_->getCostmap();
+        costmap_received_ = true;
+      } catch (const std::runtime_error & e) {
+        costmap_received_ = false;
+      }
+        publishFootprint();
+        publishCostmap();
+    };
+
+    timer_ = node_->create_wall_timer(0.1s, timer_callback);     
+  }
+
+  void publishFootprint()
+  {
+    geometry_msgs::msg::PolygonStamped oriented_footprint;
+    oriented_footprint.header.frame_id = global_frame_;
+    oriented_footprint.header.stamp = node_->now();
+    nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint);
+    footprint_pub_->publish(oriented_footprint);
+  }
+
+  void publishCostmap()
+  {
+    layers_.updateMap(x_, y_, yaw_);
+    costmap_pub_->publishCostmap();
+  }
+
+  double x_, y_, yaw_;
+  rclcpp::Node::SharedPtr node_;
+  nav2_costmap_2d::LayeredCostmap layers_;
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
+  std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> costmap_pub_;
+  std::vector<geometry_msgs::msg::Point> footprint_;
+  std::string global_frame_;
+  bool costmap_received_;
+  geometry_msgs::msg::TransformStamped base_rel_map;
+  tf2_ros::Buffer & tf_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  {
+
+    node_ = rclcpp::Node::make_shared(
+      "test_collision_checker", nav2_util::get_node_options_default());
+    
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+    std::string costmap_topic = "costmap_raw";
+    std::string footprint_topic = "footprint";
+    costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(node_, costmap_topic);
+    footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(node_, footprint_topic);
+
+    collision_checker_ = std::make_unique<TestCollisionChecker>(node_, costmap_sub_, footprint_sub_, *tf_buffer_);
+  }
+
+  ~TestNode() {}
+
+protected:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::unique_ptr<TestCollisionChecker> collision_checker_;
+};
+
+TEST_F(TestNode, uknownSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // Completely off map
+  ASSERT_EQ(collision_checker_->testPose(5, 13, 0), false);
+
+  // Partially off map
+  ASSERT_EQ(collision_checker_->testPose(5, 9.5, 0), false);
+
+  // In unknown region inside map
+  ASSERT_EQ(collision_checker_->testPose(2, 4, 0), false);
+
+}
+
+TEST_F(TestNode, FreeSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // In complete free space
+  ASSERT_EQ(collision_checker_->testPose(2, 8.5, 0), true);
+
+  // Partially in inscribed space
+  ASSERT_EQ(collision_checker_->testPose(2.5, 7, 0), true);
+}
+
+TEST_F(TestNode, CollisionSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // Completely in obstacle
+  ASSERT_EQ(collision_checker_->testPose(8.5, 6.5, 0), false);
+
+  // Partially in obstacle
+  ASSERT_EQ(collision_checker_->testPose(4.5, 4.5, 0), false);
+
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(0, nullptr);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_dwb_controller/dwb_critics/CMakeLists.txt

@@ -34,7 +34,6 @@ add_library(${PROJECT_NAME} SHARED
     src/prefer_forward.cpp
     src/rotate_to_goal.cpp
     src/twirling.cpp
-    src/collision_checker.cpp
 )
 
 set(dependencies
@@ -72,10 +71,14 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
 endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
+
+
 ament_export_dependencies(
   ${dependencies}
 )

nav2_util/include/nav2_util/line_iterator.hpp

+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef NAV2_UTIL__LINE_ITERATOR_HPP_
+#define NAV2_UTIL__LINE_ITERATOR_HPP_
+
+#include <stdlib.h>
+
+namespace nav2_util
+{
+
+/** An iterator implementing Bresenham Ray-Tracing. */
+class LineIterator
+{
+public:
+  LineIterator(int x0, int y0, int x1, int y1)
+  : x0_(x0),
+    y0_(y0),
+    x1_(x1),
+    y1_(y1),
+    x_(x0),    // X and Y start of at first endpoint.
+    y_(y0),
+    deltax_(abs(x1 - x0)),
+    deltay_(abs(y1 - y0)),
+    curpixel_(0)
+  {
+    if (x1_ >= x0_) {                // The x-values are increasing
+      xinc1_ = 1;
+      xinc2_ = 1;
+    } else {                      // The x-values are decreasing
+      xinc1_ = -1;
+      xinc2_ = -1;
+    }
+
+    if (y1_ >= y0_) {               // The y-values are increasing
+      yinc1_ = 1;
+      yinc2_ = 1;
+    } else {                      // The y-values are decreasing
+      yinc1_ = -1;
+      yinc2_ = -1;
+    }
+
+    if (deltax_ >= deltay_) {        // There is at least one x-value for every y-value
+      xinc1_ = 0;                  // Don't change the x when numerator >= denominator
+      yinc2_ = 0;                  // Don't change the y for every iteration
+      den_ = deltax_;
+      num_ = deltax_ / 2;
+      numadd_ = deltay_;
+      numpixels_ = deltax_;         // There are more x-values than y-values
+    } else {                      // There is at least one y-value for every x-value
+      xinc2_ = 0;                  // Don't change the x for every iteration
+      yinc1_ = 0;                  // Don't change the y when numerator >= denominator
+      den_ = deltay_;
+      num_ = deltay_ / 2;
+      numadd_ = deltax_;
+      numpixels_ = deltay_;         // There are more y-values than x-values
+    }
+  }
+
+  bool isValid() const
+  {
+    return curpixel_ <= numpixels_;
+  }
+
+  void advance()
+  {
+    num_ += numadd_;              // Increase the numerator by the top of the fraction
+    if (num_ >= den_) {           // Check if numerator >= denominator
+      num_ -= den_;               // Calculate the new numerator value
+      x_ += xinc1_;               // Change the x as appropriate
+      y_ += yinc1_;               // Change the y as appropriate
+    }
+    x_ += xinc2_;                 // Change the x as appropriate
+    y_ += yinc2_;                 // Change the y as appropriate
+
+    curpixel_++;
+  }
+
+  int getX() const
+  {
+    return x_;
+  }
+  int getY() const
+  {
+    return y_;
+  }
+
+  int getX0() const
+  {
+    return x0_;
+  }
+  int getY0() const
+  {
+    return y0_;
+  }
+
+  int getX1() const
+  {
+    return x1_;
+  }
+  int getY1() const
+  {
+    return y1_;
+  }
+
+private:
+  int x0_;  ///< X coordinate of first end point.
+  int y0_;  ///< Y coordinate of first end point.
+  int x1_;  ///< X coordinate of second end point.
+  int y1_;  ///< Y coordinate of second end point.
+
+  int x_;  ///< X coordinate of current point.
+  int y_;  ///< Y coordinate of current point.
+
+  int deltax_;  ///< Difference between Xs of endpoints.
+  int deltay_;  ///< Difference between Ys of endpoints.
+
+  int curpixel_;  ///< index of current point in line loop.
+
+  int xinc1_, xinc2_, yinc1_, yinc2_;
+  int den_, num_, numadd_, numpixels_;
+};
+
+}  // end namespace nav2_util
+
+#endif  // NAV2_UTIL__LINE_ITERATOR_HPP_