Additional ROS2 humble changes

Author: nakai-omer

Diff for: nav_layer_from_points/CMakeLists.txt

@@ -12,6 +12,13 @@ find_package(nav2_costmap_2d REQUIRED)
 
 ament_auto_find_build_dependencies()
 
+set(dep_pkgs
+  rclcpp
+  tf2_ros
+  geometry_msgs
+  nav2_costmap_2d
+)
+
 ### Build
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/costmap_layer.cpp
@@ -24,9 +31,13 @@ target_include_directories(${PROJECT_NAME}
 )
 
 # Export package
-ament_export_dependencies(${PROJECT_NAME} PUBLIC tf2_ros geometry_msgs nav2_costmap_2d)
+ament_export_dependencies(${dep_pkgs})
+# Export package nav2_costmap_2d
+pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
+
 
 ### Install
+
 ament_auto_package(INSTALL_TO_SHARE
   config
 )

Diff for: nav_layer_from_points/costmap_plugins.xml

@@ -1,7 +1,5 @@
-<class_libraries>
-  <library path="libcostmap_layers">
-    <class type="nav_layer_from_points::NavLayerFromPoints"  base_class_type="nav2_costmap_2d::Layer">
-      <description>Represensts obstacles based on points list.</description>
-    </class>
-  </library>
-</class_libraries>
+<library path="nav_layer_from_points">
+  <class type="nav_layer_from_points::NavLayerFromPoints" base_class_type="nav2_costmap_2d::Layer">
+    <description>Represensts obstacles based on points list.</description>
+  </class>
+</library>

Diff for: nav_layer_from_points/include/nav_layer_from_points/costmap_layer.h

@@ -25,8 +25,8 @@
 
 #include <pluginlib/class_list_macros.hpp>
 
-#include <geometry_msgs/msg/polygon_stamped.hpp>
-#include <geometry_msgs/msg/point_stamped.hpp>
+#include "geometry_msgs/msg/polygon_stamped.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
 
 #include <tf2_ros/buffer.h>
 
@@ -70,8 +70,6 @@ class NavLayerFromPoints : public nav2_costmap_2d::CostmapLayer
 
   geometry_msgs::msg::PolygonStamped points_list_;  ///< List of received points
 
-  tf2_ros::Buffer tf_buffer_;
-
   std::list<geometry_msgs::msg::PointStamped> transformed_points_;
 
   // After this time points will be delete
@@ -80,6 +78,7 @@ class NavLayerFromPoints : public nav2_costmap_2d::CostmapLayer
   std::recursive_mutex lock_;
   bool first_time_;
   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+  std::string topic_;
 
 private:
   double point_radius_;

Diff for: nav_layer_from_points/package.xml

@@ -18,9 +18,10 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 
-<export>
-  <build_type>ament_cmake</build_type>
-</export>
+  <export>
+    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
 
 

Diff for: nav_layer_from_points/src/costmap_layer.cpp

@@ -22,7 +22,6 @@ namespace nav_layer_from_points
 {
 
 NavLayerFromPoints::NavLayerFromPoints()
-: tf_buffer_(clock_)
 {
   layered_costmap_ = nullptr;
 }
@@ -37,13 +36,16 @@ void NavLayerFromPoints::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  node->declare_parameter(name_ + "." + "enabled", true);
-  node->declare_parameter(name_ + "." + "keep_time", 0.75);
-  node->declare_parameter(name_ + "." + "point_radius", 0.2);
-  node->declare_parameter(name_ + "." + "robot_radius", 0.6);
+  node->declare_parameter(getFullName("keep_time"), 0.75);
+  enabled_ = node->declare_parameter(getFullName("enabled"), true);
+  point_radius_ = node->declare_parameter(getFullName("point_radius"), 0.2);
+  robot_radius_ = node->declare_parameter(getFullName("robot_radius"), 0.6);
+  topic_ = node->declare_parameter(getFullName("topic"), "points");
+
+  points_keep_time_ = rclcpp::Duration::from_seconds(node->get_parameter(getFullName("keep_time")).as_double());
 
   sub_points_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
-    "points", 1, std::bind(&NavLayerFromPoints::pointsCallback, this, std::placeholders::_1));
+    topic_, 1, std::bind(&NavLayerFromPoints::pointsCallback, this, std::placeholders::_1));
 }
 
 void NavLayerFromPoints::pointsCallback(
@@ -96,7 +98,7 @@ void NavLayerFromPoints::updateBounds(
       pt.point.z =  tpt.point.z;
       pt.header.frame_id = points_list_.header.frame_id;
 
-      tf_buffer_.transform(pt, out_pt, global_frame);
+      tf_->transform(pt, out_pt, global_frame);
       tpt.point.x = out_pt.point.x;
       tpt.point.y = out_pt.point.y;
       tpt.point.z = out_pt.point.z;