Ported to ROS 2 Humble - Build working

include/social_force_window_planner/sfw_planner_node.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 #include "nav2_core/controller.hpp"
+#include "nav2_core/goal_checker.hpp"
 #include "nav2_core/exceptions.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -71,9 +72,9 @@ class SFWPlannerNode : public nav2_core::Controller {
    * @param costmap_ros Costmap2DROS object of environment
    */
   void
-  configure(const rclcpp_lifecycle::LifecycleNode::SharedPtr &parent,
-            const std::string name, const std::shared_ptr<tf2_ros::Buffer> &tf,
-            const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &costmap_ros)
+  configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
+            const std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
+            const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
       override;
 
   /**
@@ -107,14 +108,22 @@ class SFWPlannerNode : public nav2_core::Controller {
    */
   geometry_msgs::msg::TwistStamped
   computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
-                          const geometry_msgs::msg::Twist &velocity) override;
+                          const geometry_msgs::msg::Twist &velocity,
+                          nav2_core::GoalChecker * goal_checker) override;
 
   /**
    * @brief nav2_core setPlan - Sets the global plan
    * @param path The global plan
    */
   void setPlan(const nav_msgs::msg::Path &path) override;
 
+ /**
+    * @brief Set new speed limit from callback
+    * @param speed_limit Speed limit to use
+    * @param percentage Bool if the speed limit is absolute or relative
+    */
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
+
 protected:
   /**
    * @brief Transforms global plan into same frame as pose (robot_pose), clips
@@ -151,7 +160,7 @@ class SFWPlannerNode : public nav2_core::Controller {
 
   bool isGoalReached();
 
-  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   std::string name_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;

src/sensor_interface.cpp

@@ -319,7 +319,7 @@ void SFMSensorInterface::publish_obstacle_points(
   m.color.b = 0.0;
   m.color.a = 1.0;
   m.id = 1000;
-  m.lifetime = rclcpp::Duration(0.3);
+  m.lifetime = rclcpp::Duration::from_seconds(0.3);
   // printf("Published Obstacles: ");
   for (utils::Vector2d p : points) {
     geometry_msgs::msg::Point pt;

src/sfw_planner.cpp

@@ -100,7 +100,7 @@ void SFWPlanner::initializeMarkers() {
       markers_.markers[counter].id = counter;
       markers_.markers[counter].type = 4;
       markers_.markers[counter].action = 0; // 0 - add/modify an object
-      markers_.markers[counter].lifetime = rclcpp::Duration(0.3);
+      markers_.markers[counter].lifetime = rclcpp::Duration::from_seconds(0.3);
       markers_.markers[counter].scale.x = 0.01;
       markers_.markers[counter].color.a = 1.0;
       markers_.markers[counter].pose.orientation.w = 1.0;

src/sfw_planner_node.cpp

@@ -45,12 +45,13 @@ Iter min_by(Iter begin, Iter end, Getter getCompareVal) {
 }
 
 void SFWPlannerNode::configure(
-    const rclcpp_lifecycle::LifecycleNode::SharedPtr &parent,
-    const std::string name, const std::shared_ptr<tf2_ros::Buffer> &tf,
-    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &costmap_ros) {
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
+    const std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
 
-  logger_ = parent->get_logger();
   node_ = parent;
+  auto node = node_.lock();
+
   name_ = name;
 
   RCLCPP_INFO(logger_,
@@ -63,19 +64,19 @@ void SFWPlannerNode::configure(
   tf_ = tf;
 
   // sensor interface
-  sensor_iface_ = std::make_shared<SFMSensorInterface>(parent, tf, name);
+  sensor_iface_ = std::make_shared<SFMSensorInterface>(node, tf, name);
 
   global_path_pub_ =
-      parent->create_publisher<nav_msgs::msg::Path>("robot_global_plan", 1);
+      node->create_publisher<nav_msgs::msg::Path>("robot_global_plan", 1);
 
   local_path_pub_ =
-      parent->create_publisher<nav_msgs::msg::Path>("robot_local_plan", 1);
+      node->create_publisher<nav_msgs::msg::Path>("robot_local_plan", 1);
 
-  traj_pub_ = parent->create_publisher<visualization_msgs::msg::MarkerArray>(
+  traj_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>(
       "robot_local_trajectories", 1);
 
   sfw_planner_ =
-      std::make_shared<SFWPlanner>(parent, name, sensor_iface_, *costmap_,
+      std::make_shared<SFWPlanner>(node, name, sensor_iface_, *costmap_,
                                    costmap_ros_->getRobotFootprint());
 }
 
@@ -219,8 +220,10 @@ bool SFWPlannerNode::transformPoint(
 
 geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands(
     const geometry_msgs::msg::PoseStamped &pose,
-    const geometry_msgs::msg::Twist &speed) {
+    const geometry_msgs::msg::Twist &speed,
+    nav2_core::GoalChecker * goal_checker) {
 
+  auto node = node_.lock();
   // RCLCPP_INFO(logger_, "ComputeVelocityCommands called!!!!");
   geometry_msgs::msg::TwistStamped velStamp;
   sensor_iface_->start();
@@ -237,6 +240,13 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands(
         "Unable to transform robot pose into costmap's frame");
   }
 
+  // Update for the current goal checker's state
+  geometry_msgs::msg::Pose pose_tolerance;
+  geometry_msgs::msg::Twist vel_tolerance;
+  if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) {
+    RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
+  }
+
   // TODO: Check here if we have already reached the goal
 
   // std::vector<geometry_msgs::msg::PoseStamped> transformed_plan;
@@ -293,7 +303,7 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands(
   // pass along drive commands
 
   if (!ok) {
-    RCLCPP_DEBUG(node_->get_logger(),
+    RCLCPP_DEBUG(node->get_logger(),
                  "The sfw planner failed to find a valid plan. This "
                  "means that the footprint of the robot was in collision "
                  "for all simulated trajectories.");
@@ -308,7 +318,7 @@ geometry_msgs::msg::TwistStamped SFWPlannerNode::computeVelocityCommands(
   global_path_pub_->publish(transformed_plan);
   traj_pub_->publish(markers);
 
-  velStamp.header.stamp = node_->get_clock()->now();
+  velStamp.header.stamp = node->get_clock()->now();
   velStamp.header.frame_id = costmap_ros_->getGlobalFrameID();
   velStamp.twist = drive_cmds;
   return velStamp;