Check for collisions in motion primitives

nav2_bringup/launch/nav2_params.yaml

@@ -100,7 +100,7 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.17
+      robot_radius: 0.22
       inflation_layer.cost_scaling_factor: 3.0
       obstacle_layer:
         enabled: True
@@ -122,7 +122,7 @@ global_costmap:
   global_costmap:
     ros__parameters:
       use_sim_time: True
-      robot_radius: 0.17
+      robot_radius: 0.22
       obstacle_layer:
         enabled: True
       always_send_full_costmap: True

nav2_costmap_2d/CMakeLists.txt

@@ -90,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
@@ -110,7 +124,7 @@ target_link_libraries(nav2_costmap_2d
   layers
 )
 
-install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers
+install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers nav2_costmap_2d_client
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -137,7 +151,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(layers nav2_costmap_2d_core)
+ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client)
 ament_export_dependencies(${dependencies})
 pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
 ament_package()

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

@@ -0,0 +1,70 @@
+// 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.
+
+#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_costmap_2d/footprint_subscriber.hpp"
+#include "nav2_util/get_robot_pose_client.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+
+namespace nav2_costmap_2d
+{
+typedef std::vector<geometry_msgs::msg::Point> Footprint;
+
+class CollisionChecker
+{
+public:
+  CollisionChecker(
+    CostmapSubscriber & costmap_sub,
+    FootprintSubscriber & footprint_sub,
+    nav2_util::GetRobotPoseClient & get_robot_pose_client,
+    std::string name = "collision_checker");
+
+  ~CollisionChecker();
+
+  // Returns the obstacle footprint score for a particular pose
+  double scorePose(const geometry_msgs::msg::Pose2D & pose);
+  bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose);
+
+protected:
+  double lineCost(int x0, int x1, int y0, int y1) const;
+  double pointCost(int x, int y) const;
+  bool getRobotPose(geometry_msgs::msg::Pose & current_pose);
+  void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
+  void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+  Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
+  double footprintCost(const Footprint footprint);
+
+  std::shared_ptr<Costmap2D> costmap_;
+
+  // Name used for logging
+  std::string name_;
+  nav2_util::GetRobotPoseClient & get_robot_pose_client_;
+  CostmapSubscriber & costmap_sub_;
+  FootprintSubscriber & footprint_sub_;
+};
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -45,6 +45,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
 #include "tf2/transform_datatypes.h"
 #include "nav2_util/lifecycle_node.hpp"
 
@@ -77,11 +78,13 @@ class Costmap2DPublisher
   {
     costmap_pub_->on_activate();
     costmap_update_pub_->on_activate();
+    costmap_raw_pub_->on_activate();
   }
   void on_deactivate()
   {
     costmap_pub_->on_deactivate();
     costmap_update_pub_->on_deactivate();
+    costmap_raw_pub_->on_deactivate();
   }
   void on_cleanup() {}
 
@@ -111,6 +114,7 @@ class Costmap2DPublisher
 private:
   /** @brief Prepare grid_ message for publication. */
   void prepareGrid();
+  void prepareCostmap();
 
   /** @brief Publish the latest full costmap to the new subscriber. */
   // void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
@@ -125,13 +129,19 @@ class Costmap2DPublisher
   bool active_;
   bool always_send_full_costmap_;
 
+  // Publisher for translated costmap values as msg::OccupancyGrid used in visualization
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
   rclcpp_lifecycle::LifecyclePublisher<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
     costmap_update_pub_;
+  // Publisher for raw costmap values as msg::Costmap from layered costmap
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
 
   nav_msgs::msg::OccupancyGrid grid_;
+  nav2_msgs::msg::Costmap costmap_raw_;
   // Translate from 0-255 values in costmap to -1 to 100 values in message.
   static char * cost_translation_table_;
 };
+
 }  // namespace nav2_costmap_2d
+
 #endif  // NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -0,0 +1,67 @@
+// 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.
+
+#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_
+#define NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class CostmapSubscriber
+{
+public:
+  CostmapSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name);
+
+  CostmapSubscriber(
+    rclcpp::Node::SharedPtr node,
+    std::string & topic_name);
+
+  CostmapSubscriber(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    std::string & topic_name);
+
+  ~CostmapSubscriber() {}
+
+  std::shared_ptr<Costmap2D> getCostmap();
+
+protected:
+  // Interfaces used for logging and creating publishers and subscribers
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
+  void toCostmap2D();
+  void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg);
+
+  std::shared_ptr<Costmap2D> costmap_;
+  nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
+  std::string topic_name_;
+  bool costmap_received_{false};
+  rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__COSTMAP_2D_SUBSCRIBER_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

@@ -0,0 +1,68 @@
+/*
+ * 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) {}
+};
+
+/**
+ * @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_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

@@ -0,0 +1,64 @@
+// 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.
+
+#ifndef NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
+#define NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class FootprintSubscriber
+{
+public:
+  FootprintSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name);
+
+  FootprintSubscriber(
+    rclcpp::Node::SharedPtr node,
+    std::string & topic_name);
+
+  FootprintSubscriber(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    std::string & topic_name);
+
+  ~FootprintSubscriber() {}
+
+  bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
+
+protected:
+  // Interfaces used for logging and creating publishers and subscribers
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
+  void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
+
+  std::string topic_name_;
+  bool footprint_received_{false};
+  geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
+  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_