add footprint_subscriber

nav2_costmap_2d/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/costmap_layer.cpp
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
+  src/footprint_subscriber.cpp
 )
 
 set(dependencies

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

+// 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"
+
+namespace nav2_costmap_2d
+{
+
+class FootprintSubscriber
+{
+public:
+  FootprintSubscriber(
+    rclcpp::Node::SharedPtr ros_node,
+    std::string & topic_name);
+  ~FootprintSubscriber() {}
+
+bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
+
+protected:
+  void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
+
+  rclcpp::Node::SharedPtr node_;
+  std::string topic_name_;
+  bool footprint_received_;
+  std::vector<geometry_msgs::msg::Point> footprint_;
+  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_

nav2_costmap_2d/src/footprint_subscriber.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 "nav2_costmap_2d/footprint_subscriber.hpp"
+
+namespace nav2_costmap_2d
+{
+
+FootprintSubscriber::FootprintSubscriber(
+  rclcpp::Node::SharedPtr ros_node,
+  std::string & topic_name)
+: node_(ros_node), topic_name_(topic_name), footprint_received_(false)
+{
+  footprint_sub_ = node_->create_subscription<geometry_msgs::msg::PolygonStamped>(topic_name,
+      std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
+}
+
+bool
+FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point> & footprint)
+{
+  if (!footprint_received_) {
+    return false;
+  }
+  footprint = footprint_;
+  return true;
+}
+
+void
+FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg)
+{
+  if (!footprint_received_) {
+    footprint_received_ = true;
+  }
+
+  footprint_ = toPointVector(
+    std::make_shared<geometry_msgs::msg::Polygon>(msg->polygon));
+}
+
+}  // namespace nav2_costmap_2d
+