Following path

mep3_behavior/include/mep3_behavior/move_action.hpp

@@ -165,13 +165,15 @@ namespace mep3_behavior
           BT::InputPort<std::string>("frame_id"),
           BT::InputPort<bool>("ignore_obstacles"),
           BT::InputPort<int>("mode"),
+          BT::InputPort<int>("reversing"),
           BT::OutputPort<int>("error")};
     }
 
     bool setGoal(Goal &goal) override
     {
       double yaw_deg;
       int mode;
+      int reversing;
       std::string position;
       std::string token;
 
@@ -183,6 +185,7 @@ namespace mep3_behavior
       getInput<double>("angular_velocity", goal.angular_properties.max_velocity);
       getInput<double>("angular_velocity", goal.angular_properties.max_acceleration);
       getInput<int>("mode", mode);
+      getInput<int>("reversing", reversing);
 
       std::istringstream iss(position);
       std::getline(iss, token, ';');
@@ -194,6 +197,7 @@ namespace mep3_behavior
       std::getline(iss, token, ';');
       goal.target.theta = std::stod(token) * M_PI / 180.0;
       goal.mode = mode;
+      goal.reversing = reversing;
 
       std::cout << "RotateAction: setGoal" << std::endl;
       std::cout << "  x: " << goal.target.x << std::endl;
@@ -206,6 +210,7 @@ namespace mep3_behavior
       std::cout << "  anguar_velocity: " << goal.angular_properties.max_velocity << std::endl;
       std::cout << "  anguar_acceleration: " << goal.angular_properties.max_acceleration << std::endl;
       std::cout << "  ignore_obstacles: " << goal.ignore_obstacles << std::endl;
+      std::cout << "  reversing: " << goal.reversing << std::endl;
 
       return true;
     }

mep3_behavior/include/mep3_behavior/navigate_to_action.hpp

@@ -18,18 +18,21 @@
 #include <string>
 #include <cmath>
 #include <iostream>
+#include <vector>
 
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
 #include "mep3_behavior/bt_action_node.hpp"
 #include "mep3_behavior/pose_2d.hpp"
 #include "mep3_behavior/blackboard.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace mep3_behavior
 {
   class NavigateToAction
-    : public BT::RosActionNode<nav2_msgs::action::NavigateToPose>
+      : public BT::RosActionNode<nav2_msgs::action::NavigateToPose>
   {
   public:
     NavigateToAction(const std::string &name,
@@ -40,46 +43,45 @@ namespace mep3_behavior
     {
       if (!getInput("goal", target_pose_))
         throw BT::RuntimeError(
-          "Navigate action requires 'goal' argument"
-        );
+            "Navigate action requires 'goal' argument");
       getInput("behavior_tree", behavior_tree_);
 
       std::string table = this->config().blackboard->get<std::string>("table");
       BT::Pose2D goal_offset;
-      if (table.length() > 0 && getInput("goal_" + table, goal_offset)) {
+      if (table.length() > 0 && getInput("goal_" + table, goal_offset))
+      {
         target_pose_ += goal_offset;
       }
 
       BT::TeamColor color = this->config().blackboard->get<BT::TeamColor>("color");
-      if (color == BT::TeamColor::GREEN) {
+      if (color == BT::TeamColor::GREEN)
+      {
         target_pose_ = BT::mirrorPose(target_pose_);
       }
-
     }
 
     static BT::PortsList providedPorts()
     {
       // Static parameters
       BT::PortsList port_list = {
-        BT::InputPort<BT::Pose2D>("goal"),
-        BT::InputPort<std::string>("behavior_tree")
-      };
+          BT::InputPort<BT::Pose2D>("goal"),
+          BT::InputPort<std::string>("behavior_tree")};
 
       // Dynamic parameters
-      for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables")) {
+      for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables"))
+      {
         port_list.insert(
-          BT::InputPort<BT::Pose2D>("goal_" + table)
-        );
+            BT::InputPort<BT::Pose2D>("goal_" + table));
       }
 
       return port_list;
     }
 
     bool setGoal(Goal &goal) override
     {
-      std::cout << "Navigating to x=" << target_pose_.x \
-              << " y=" << target_pose_.y \
-              << " θ=" << target_pose_.theta << "°" << std::endl;
+      std::cout << "Navigating to x=" << target_pose_.x
+                << " y=" << target_pose_.y
+                << " θ=" << target_pose_.theta << "°" << std::endl;
 
       goal.pose.header.frame_id = "map";
       goal.pose.header.stamp = node_->get_clock()->now();
@@ -98,7 +100,7 @@ namespace mep3_behavior
       return true;
     }
 
-    BT::NodeStatus onResultReceived(const WrappedResult &/*wr*/) override
+    BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override
     {
       return BT::NodeStatus::SUCCESS;
     }
@@ -108,6 +110,87 @@ namespace mep3_behavior
     std::string behavior_tree_;
   };
 
+  class FollowPathAction
+      : public BT::RosActionNode<nav2_msgs::action::FollowPath>
+  {
+  public:
+    FollowPathAction(const std::string &name,
+                     const BT::NodeConfiguration &conf,
+                     const BT::ActionNodeParams &params,
+                     typename std::shared_ptr<ActionClient> action_client)
+        : BT::RosActionNode<nav2_msgs::action::FollowPath>(name, conf, params, action_client)
+    {
+      if (!getInput("path", target_pose_))
+        throw BT::RuntimeError(
+            "Follow path action requires 'path' argument");
+    }
+
+    static BT::PortsList providedPorts()
+    {
+      // Static parameters
+      BT::PortsList port_list = {
+          BT::InputPort<std::string>("path")};
+
+      return port_list;
+    }
+
+    bool setGoal(Goal &goal) override
+    {
+      // std::cout << "Foll to x=" << target_pose_.x
+      //           << " y=" << target_pose_.y
+      //           << " θ=" << target_pose_.theta << "°" << std::endl;
+
+      // // Position
+
+      std::istringstream iss(target_pose_);
+      std::string entry;
+      std::vector<geometry_msgs::msg::PoseStamped> poses;
+      goal.path.header.frame_id = "map";
+      goal.path.header.stamp = node_->get_clock()->now();
+
+      while (std::getline(iss, entry, '|'))
+      {
+        std::istringstream entryStream(entry);
+        std::string x, y, angle;
+
+        std::getline(entryStream, x, ';');
+        std::getline(entryStream, y, ';');
+        std::getline(entryStream, angle, ';');
+        double theta = std::stod(angle) * M_PI / 180.0;
+        auto pose_stamped = geometry_msgs::msg::PoseStamped();
+
+        pose_stamped.header.frame_id = "map";
+        pose_stamped.header.stamp = node_->get_clock()->now();
+
+        pose_stamped.pose.position.x = std::stod(x);
+        pose_stamped.pose.position.y = std::stod(y);
+        pose_stamped.pose.position.z = 0.0;
+
+        pose_stamped.pose.orientation.x = 0.0;
+        pose_stamped.pose.orientation.y = 0.0;
+
+        // https://math.stackexchange.com/a/1972382
+        pose_stamped.pose.orientation.z = std::sin(theta / 2.0);
+        pose_stamped.pose.orientation.w = std::cos(theta / 2.0);
+
+        poses.push_back(pose_stamped);
+
+        std::cout
+            << "x: " << x << " y: " << y << " theta: " << theta << std::endl;
+      }
+      goal.path.poses = poses;
+      return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override
+    {
+      return BT::NodeStatus::SUCCESS;
+    }
+
+  private:
+    std::string target_pose_;
+  };
+
 } // namespace mep3_behavior
 
 #endif // MEP3_BEHAVIOR_TREE__NAVIGATE_TO_ACTION_HPP_

mep3_behavior/src/mep3_behavior_tree.cpp

@@ -135,6 +135,7 @@ int main(int argc, char **argv)
 
   BT::RegisterRosAction<mep3_behavior::JointPositionCommandAction>(factory, "JointPosition", {node, "joint_position_command", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::NavigateToAction>(factory, "Navigate", {node, "navigate_to_pose", std::chrono::seconds(30)});
+  BT::RegisterRosAction<mep3_behavior::FollowPathAction>(factory, "FollowPath", {node, "follow_path", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::PumpAction>(factory, "Pump", {node, "pump", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::TranslateAction>(factory, "Translate", {node, "move/move", std::chrono::seconds(30)});
   BT::RegisterRosAction<mep3_behavior::RotateAction>(factory, "Rotate", {node, "move/move", std::chrono::seconds(30)});

mep3_behavior/strategies/project.btproj

@@ -29,6 +29,9 @@
             <input_port name="can_id">For example, 0x12345</input_port>
             <input_port name="message">For example, 0x32;0x34</input_port>
         </Action>
+        <Action ID="FollowPath" editable="true">
+            <input_port name="path"/>
+        </Action>
         <Action ID="JointPosition" editable="true">
             <input_port name="instance"/>
             <input_port name="max_effort" default="0"/>
@@ -48,6 +51,7 @@
             <input_port name="mode" default="7"/>
             <input_port name="linear_acceleration" default="0.8"/>
             <input_port name="angular_acceleration" default="1.8"/>
+            <input_port name="reversing" default="0"/>
         </Action>
         <Action ID="Navigate" editable="true">
             <input_port name="behavior_tree"/>