Costmap_2d Static & Inflation plugin layer port & Removing Boost dependencies

src/libs/costmap_2d/CMakeLists.txt

@@ -63,10 +63,9 @@ add_library(nav2_costmap_2d
   src/costmap_2d_publisher.cpp
   src/costmap_math.cpp
   src/footprint.cpp
-  src/costmap_layer.cpp
-  # TODO(bpwilcox): need ROS PCL dependencies to port
-  #src/observation_buffer.cpp
-
+  src/costmap_layer.cpp	
+  #	TODO(bpwilcox): need tf2_sensor_msgs ported
+  # src/observation_buffer.cpp  
 )
 
 set(dependencies
@@ -90,7 +89,7 @@ add_library(layers
   plugins/inflation_layer.cpp
 
 	#	TODO(bpwilcox): port additional plugin layers
-  #plugins/static_layer.cpp
+  plugins/static_layer.cpp
   #plugins/obstacle_layer.cpp
   #plugins/voxel_layer.cpp
   #src/observation_buffer.cpp

src/libs/costmap_2d/include/costmap_2d/observation.h

@@ -32,8 +32,8 @@
 #ifndef COSTMAP_2D_OBSERVATION_H_
 #define COSTMAP_2D_OBSERVATION_H_
 
-#include <geometry_msgs/Point.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 namespace costmap_2d
 {
@@ -66,7 +66,7 @@ class Observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    * @param raytrace_range The range out to which an observation should be able to clear via raytracing
    */
-  Observation(geometry_msgs::Point & origin, const sensor_msgs::PointCloud2 & cloud,
+  Observation(geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
       double obstacle_range, double raytrace_range)
     : origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
     obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
@@ -88,14 +88,14 @@ class Observation
    * @param cloud The point cloud of the observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    */
-  Observation(const sensor_msgs::PointCloud2 & cloud, double obstacle_range)
+  Observation(const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_range)
     : cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range),
     raytrace_range_(0.0)
   {
   }
 
-  geometry_msgs::Point origin_;
-  sensor_msgs::PointCloud2 * cloud_;
+  geometry_msgs::msg::Point origin_;
+  sensor_msgs::msg::PointCloud2 * cloud_;
   double obstacle_range_, raytrace_range_;
 };
 

src/libs/costmap_2d/include/costmap_2d/observation_buffer.h

@@ -40,11 +40,11 @@
 #include <vector>
 #include <list>
 #include <string>
-#include <ros/time.h>
+#include "rclcpp/time.hpp"
 #include <costmap_2d/observation.h>
 #include <tf2_ros/buffer.h>
 
-#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 
 namespace costmap_2d
@@ -96,7 +96,7 @@ class ObservationBuffer
    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
    * @param  cloud The cloud to be buffered
    */
-  void bufferCloud(const sensor_msgs::PointCloud2 & cloud);
+  void bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud);
 
   /**
    * @brief  Pushes copies of all current observations onto the end of the vector passed in
@@ -138,9 +138,10 @@ class ObservationBuffer
   void purgeStaleObservations();
 
   tf2_ros::Buffer & tf2_buffer_;
-  const ros::Duration observation_keep_time_;
-  const ros::Duration expected_update_rate_;
-  ros::Time last_updated_;
+  const rclcpp::Duration observation_keep_time_;
+  const rclcpp::Duration expected_update_rate_;
+  rclcpp::Clock clock_;
+  rclcpp::Time last_updated_;
   std::string global_frame_;
   std::string sensor_frame_;
   std::list<Observation> observation_list_;

src/libs/costmap_2d/include/costmap_2d/static_layer.h

@@ -43,8 +43,8 @@
 #include <costmap_2d/layered_costmap.h>
 //#include <costmap_2d/GenericPluginConfig.h>
 //#include <dynamic_reconfigure/server.h>
-#include <nav_msgs/msg/occupancy_grid.h>
-#include <map_msgs/msg/occupancy_grid_update.h>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <map_msgs/msg/occupancy_grid_update.hpp>
 #include <message_filters/subscriber.h>
 
 namespace costmap_2d
@@ -77,7 +77,9 @@ class StaticLayer : public CostmapLayer
    * static map are overwritten.
    */
   void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map);
-  void incomingUpdate(const map_msgs::msg::OccupancyGridUpdateConstPtr & update);
+  void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update);
+  //TODO(bpwilcox): Replace dynamic_reconfigure functionality
+
   //void reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level);
 
   unsigned char interpretValue(unsigned char value);
@@ -92,11 +94,11 @@ class StaticLayer : public CostmapLayer
   bool use_maximum_;
   bool first_map_only_;      ///< @brief Store the first static map and reuse it on reinitializing
   bool trinary_costmap_;
-  ros::Subscriber map_sub_, map_update_sub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
   rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
   unsigned char lethal_threshold_, unknown_cost_value_;
 
+  //TODO(bpwilcox): Replace dynamic_reconfigure functionality
   //dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> * dsrv_;
 };
 

src/libs/costmap_2d/include/costmap_2d/testing_helper.h

@@ -1,13 +1,14 @@
 #ifndef COSTMAP_2D_TESTING_HELPER_H
 #define COSTMAP_2D_TESTING_HELPER_H
 
+#include "rclcpp/rclcpp.hpp"
 #include <costmap_2d/cost_values.h>
 #include <costmap_2d/costmap_2d.h>
 #include <costmap_2d/static_layer.h>
 #include <costmap_2d/obstacle_layer.h>
 #include <costmap_2d/inflation_layer.h>
 
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 const double MAX_Z(1.0);
 
@@ -34,6 +35,7 @@ char printableCost(unsigned char cost)
 
 void printMap(costmap_2d::Costmap2D & costmap)
 {
+
   printf("map:\n");
   for (int i = 0; i < costmap.getSizeInCellsY(); i++) {
     for (int j = 0; j < costmap.getSizeInCellsX(); j++) {
@@ -59,9 +61,13 @@ unsigned int countValues(costmap_2d::Costmap2D & costmap, unsigned char value, b
 
 void addStaticLayer(costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf)
 {
+
   costmap_2d::StaticLayer * slayer = new costmap_2d::StaticLayer();
+
   layers.addPlugin(std::shared_ptr<costmap_2d::Layer>(slayer));
+
   slayer->initialize(&layers, "static", &tf);
+
 }
 
 costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap & layers,

src/libs/costmap_2d/plugins/inflation_layer.cpp

@@ -66,13 +66,13 @@ InflationLayer::InflationLayer()
   last_max_y_(std::numeric_limits<float>::max())
 {
   inflation_access_ = new std::recursive_mutex();
+  enabled_ = true;
 }
 
 void InflationLayer::onInitialize()
 {
   {
     std::unique_lock<std::recursive_mutex> lock(*inflation_access_);
-    //ros::NodeHandle nh("~/" + name_), g_nh;
 
     auto private_nh = rclcpp::Node::make_shared(name_);
     rclcpp::Node::SharedPtr g_nh;
@@ -102,6 +102,9 @@ void InflationLayer::onInitialize()
   }
 
   matchSize();
+  //TODO(bpwilcox): Values hard-coded from config file default, replace with dynamic approach
+  setInflationParameters(0.55, 10);
+
 }
 // TODO(bpwilcox): Resolve dynamic reconfigure dependencies
 /*