Costmap_2d Static & Inflation plugin layer port & Removing Boost dependencies

src/libs/costmap_2d/CMakeLists.txt

@@ -10,7 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   #add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
@@ -27,34 +26,32 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(xmlrpcpp REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(message_filters REQUIRED)
 
-#	TODO(bpwilcox): port remaining packages
+# TODO(bpwilcox): port remaining packages
 #find_package(tf2_sensor_msgs REQUIRED)
-#find_package(dynamic_reconfigure REQUIRED)
-#find_package(message_filters REQUIRED)
+#find_package(dynamic_reconfigure REQUIRED) 
 #find_package(voxel_grid REQUIRED)
 
 remove_definitions(-DDISABLE_LIBUSB-1.0)
 find_package(Eigen3 REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system thread)
+#find_package(Boost REQUIRED COMPONENTS system thread)
 
 include_directories(
   include
   ${ament_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
 )
 
 add_definitions(${EIGEN3_DEFINITIONS})
 
-#	TODO(bpwilcox): Separate msgs generation into separate package
+# TODO(bpwilcox): Separate msgs generation into separate package
 #rosidl_generate_interfaces(${PROJECT_NAME}
 #  "msg/VoxelGrid.msg"
 #
 #	DEPENDENCIES std_msgs geometry_msgs map_msgs
 #)
 
-
 add_library(nav2_costmap_2d
   src/array_parser.cpp
   src/costmap_2d.cpp
@@ -64,10 +61,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
@@ -80,41 +76,37 @@ set(dependencies
   xmlrpcpp
   pluginlib
   tf2_geometry_msgs
+  message_filters
 )
 
 ament_target_dependencies(nav2_costmap_2d
   ${dependencies}
 )
 
-target_link_libraries(nav2_costmap_2d
-  ${Boost_LIBRARIES}
-)
-
 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
 )
 ament_target_dependencies(layers
   ${dependencies}
  )
-target_link_libraries(layers
+ target_link_libraries(layers
   nav2_costmap_2d
-  ${Boost_LIBRARIES}
 )
 
-#	TODO(bpwilcox): port costmap_2d_markers
+# TODO(bpwilcox): port costmap_2d_markers
 #add_executable(costmap_2d_markers src/costmap_2d_markers.cpp)
 #ament_target_dependencies(costmap_2d_markers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS})
 #target_link_libraries(costmap_2d_markers
   #costmap_2d
 #)
 
-#	TODO(bpwilcox): port costmap_2d_cloud
+# TODO(bpwilcox): port costmap_2d_cloud
 #add_executable(costmap_2d_cloud src/costmap_2d_cloud.cpp)
 #ament_target_dependencies(costmap_2d_cloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS})
 #target_link_libraries(costmap_2d_cloud

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

@@ -40,8 +40,11 @@
 
 #include <vector>
 #include <queue>
+#include <mutex>
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
 #include <geometry_msgs/msg/point.hpp>
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -296,7 +299,7 @@ class Costmap2D
   unsigned int cellDistance(double world_dist);
 
   // Provide a typedef to ease future code maintenance
-  typedef boost::recursive_mutex mutex_t;
+  typedef std::recursive_mutex mutex_t;
   mutex_t * getMutex()
   {
     return access_;

src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h

@@ -262,7 +262,7 @@ class Costmap2DROS
   void mapUpdateLoop(double frequency);
   bool map_update_thread_shutdown_;
   bool stop_updates_, initialized_, stopped_, robot_stopped_;
-  boost::thread * map_update_thread_;  ///< @brief A thread for updating the map
+  std::thread * map_update_thread_;  ///< @brief A thread for updating the map
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Time last_publish_;
   rclcpp::Duration publish_cycle_;
@@ -274,7 +274,7 @@ class Costmap2DROS
   //dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
   //costmap_2d::Costmap2DConfig old_config_;
 
-  boost::recursive_mutex configuration_mutex_;
+  std::recursive_mutex configuration_mutex_;
 
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
   rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;

src/libs/costmap_2d/include/costmap_2d/inflation_layer.h

@@ -44,7 +44,6 @@
 // TODO(bpwilcox): Resolve dynamic reconfigure dependencies
 //#include <costmap_2d/InflationPluginConfig.h>
 //#include <dynamic_reconfigure/server.h>
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -129,7 +128,7 @@ class InflationLayer : public Layer
 
 protected:
   virtual void onFootprintChanged();
-  boost::recursive_mutex * inflation_access_;
+  std::recursive_mutex * inflation_access_;
 
 private:
   /**

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,14 +40,12 @@
 #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>
 
-// Thread support
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -98,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
@@ -140,15 +138,16 @@ 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_;
   std::string topic_name_;
   double min_obstacle_height_, max_obstacle_height_;
-  boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
+  std::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
 };

src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h

@@ -86,31 +86,31 @@ class ObstacleLayer : public CostmapLayer
    * @param buffer A pointer to the observation buffer to update
    */
   void laserScanCallback(const sensor_msgs::LaserScanConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   /**
    * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
   void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr & message,
-      const boost::shared_ptr<ObservationBuffer> & buffer);
+      const std::shared_ptr<ObservationBuffer> & buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud messages
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloudCallback(const sensor_msgs::PointCloudConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud2 messages
    * @param message The message returned from a message notifier
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   // for testing purposes
   void addStaticObservation(costmap_2d::Observation & obs, bool marking, bool clearing);
@@ -163,11 +163,11 @@ class ObstacleLayer : public CostmapLayer
 
   laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
-  std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
-  std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
+  std::vector<std::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
+  std::vector<std::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
 
   // Used only for testing purposes
   std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;

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

@@ -38,13 +38,13 @@
 #ifndef COSTMAP_2D_STATIC_LAYER_H_
 #define COSTMAP_2D_STATIC_LAYER_H_
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <costmap_2d/costmap_layer.h>
 #include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/GenericPluginConfig.h>
-#include <dynamic_reconfigure/server.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <map_msgs/OccupancyGridUpdate.h>
+//#include <costmap_2d/GenericPluginConfig.h>
+//#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <map_msgs/msg/occupancy_grid_update.hpp>
 #include <message_filters/subscriber.h>
 
 namespace costmap_2d
@@ -76,9 +76,11 @@ class StaticLayer : public CostmapLayer
    * map along with its size will determine what parts of the costmap's
    * static map are overwritten.
    */
-  void incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map);
-  void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr & update);
-  void reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level);
+  void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map);
+  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,12 @@ 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_;
 
-  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> * dsrv_;
+  //TODO(bpwilcox): Replace dynamic_reconfigure functionality
+  //dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> * dsrv_;
 };
 
 }  // namespace costmap_2d

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);
 
@@ -60,7 +61,7 @@ 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(boost::shared_ptr<costmap_2d::Layer>(slayer));
+  layers.addPlugin(std::shared_ptr<costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf);
 }
 
@@ -69,7 +70,7 @@ costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap & layers
 {
   costmap_2d::ObstacleLayer * olayer = new costmap_2d::ObstacleLayer();
   olayer->initialize(&layers, "obstacles", &tf);
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
+  layers.addPlugin(std::shared_ptr<costmap_2d::Layer>(olayer));
   return olayer;
 }
 
@@ -101,7 +102,7 @@ costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap & laye
 {
   costmap_2d::InflationLayer * ilayer = new costmap_2d::InflationLayer();
   ilayer->initialize(&layers, "inflation", &tf);
-  boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
+  std::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
   layers.addPlugin(ipointer);
   return ilayer;
 }