adding test for collision checker

adding test cases

move collision checker to nav2_costmap_2d

nav2_costmap_2d/CMakeLists.txt

@@ -50,8 +50,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
@@ -93,6 +91,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 @@ class CollisionChecker
 
   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

@@ -0,0 +1,69 @@
+/*
+ * 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,17 +194,17 @@ 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;
   double y = current_pose.pose.position.y;
   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/src/costmap_2d_publisher.cpp

@@ -160,7 +160,9 @@ void Costmap2DPublisher::prepareCostmap()
 
 void Costmap2DPublisher::publishCostmap()
 {
-  if (node_->count_subscribers(topic_name_) == 0) {
+  if (node_->count_subscribers(topic_name_) == 0 &&
+    node_->count_subscribers(topic_name_ + "_raw") == 0) 
+  {
     // No subscribers, so why do any work?
     return;
   }

nav2_costmap_2d/test/integration/CMakeLists.txt

@@ -1,3 +1,15 @@
+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(footprint_tests_exec
 #   footprint_tests.cpp
 # )
@@ -42,6 +54,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"