added example of move_base action

Author: facontidavide

CMakeLists.txt

@@ -12,7 +12,8 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 ######################################################
 
-set(ROS_DEPENDENCIES roscpp std_msgs message_generation behaviortree_cpp)
+set(ROS_DEPENDENCIES
+    roscpp std_msgs behaviortree_cpp_v3 actionlib_msgs actionlib message_generation)
 
 find_package(catkin REQUIRED COMPONENTS ${ROS_DEPENDENCIES} )
 find_package(GTest)
@@ -45,10 +46,13 @@ include_directories( include ${catkin_INCLUDE_DIRS})
 # LIBRARIES
 
 add_library(behaviortree_ros
-    src/loggers/rosout_logger.cpp )
+    src/loggers/rosout_logger.cpp
+    src/actions/movebase_client.cpp
+    )
 
 target_link_libraries(behaviortree_ros ${catkin_LIBRARIES})
 
+
 ######################################################
 # TESTS
 

package.xml

@@ -13,11 +13,15 @@
 
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>behaviortree_cpp</build_depend>
+  <build_depend>behaviortree_cpp_v3</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
-  <run_depend>behaviortree_cpp</run_depend>
+  <run_depend>behaviortree_cpp_v3</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
 
   <build_depend>message_generation</build_depend>
   <run_depend>message_generation</run_depend>

src/actions/movebase_client.cpp

@@ -0,0 +1,52 @@
+#include "movebase_client.h"
+
+BT::NodeStatus MoveBase::tick() {
+  // if no server is present, fail after 2 seconds
+  if (!_client.waitForServer(ros::Duration(2.0))) {
+    ROS_ERROR("Can't contact move_base server");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Take the goal from the InputPort of the Node
+  Pose2D goal;
+  if (!getInput<Pose2D>("goal", goal)) {
+    // if I can't get this, there is something wrong with your BT.
+    // For this reason throw an exception instead of returning FAILURE
+    throw BT::RuntimeError("missing required input [goal]");
+  }
+
+  // Reset this flag
+  _aborted = false;
+
+  ROS_INFO("Sending goal %f %f", goal.x, goal.y);
+
+  // Build the message from Pose2D
+  move_base_msgs::MoveBaseGoal msg;
+  msg.target_pose.header.frame_id = "map";
+  msg.target_pose.header.stamp = ros::Time::now();
+  msg.target_pose.pose.position.x = goal.x;
+  msg.target_pose.pose.position.y = goal.y;
+  tf::Quaternion rot = tf::createQuaternionFromYaw(goal.theta);
+  tf::quaternionTFToMsg(rot, msg.target_pose.pose.orientation);
+
+  _client.sendGoal(msg);
+
+  while (!_aborted && !_client.waitForResult(ros::Duration(0.02))) {
+    // polling at 50 Hz. No big deal in terms of CPU
+  }
+
+  if (_aborted) {
+    // this happens only if method halt() was invoked
+    _client.cancelAllGoals();
+    ROS_ERROR("MoveBase aborted");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) {
+    ROS_ERROR("MoveBase failed");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  ROS_INFO("Target reached");
+  return BT::NodeStatus::SUCCESS;
+}

src/actions/movebase_client.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+#include <behaviortree_cpp/action_node.h>
+#include <tf/transform_datatypes.h>
+
+
+// Custom type
+struct Pose2D
+{
+    double x, y, theta;
+};
+
+
+namespace BT
+{
+template <> inline
+Pose2D convertFromString(StringView key)
+{
+    // three real numbers separated by semicolons
+    auto parts = BT::splitString(key, ';');
+    if (parts.size() != 3)
+    {
+        throw BT::RuntimeError("invalid input)");
+    }
+    else
+    {
+        Pose2D output;
+        output.x     = convertFromString<double>(parts[0]);
+        output.y     = convertFromString<double>(parts[1]);
+        output.theta = convertFromString<double>(parts[2]);
+        return output;
+    }
+}
+} // end namespace BT
+
+//----------------------------------------------------------------
+
+class MoveBase : public BT::AsyncActionNode
+{
+public:
+
+    MoveBase(const std::string& name, const BT::NodeConfiguration& config)
+        : BT::AsyncActionNode(name, config),
+          _client("move_base", true)
+    {
+    }
+
+    // It is mandatory to define this static method.
+    static BT::PortsList providedPorts()
+    {
+        return{ BT::InputPort<Pose2D>("goal") };
+    }
+
+    virtual BT::NodeStatus tick() override;
+
+    virtual void halt() override
+    {
+        _aborted = true;
+    }
+
+private:
+    typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
+    MoveBaseClient _client;
+    bool _aborted;
+};
+
+