Merge branch 'master' of https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_base_bt_client

CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   geometry_msgs
   sensor_msgs
+  iri_ros_tools
 )
 
 ## System dependencies are found with CMake's conventions
@@ -98,7 +99,7 @@ set(head_helpers_name head_helpers)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME} ${head_helpers_name}
-  CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs
+  CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs iri_ros_tools
 #  DEPENDS system_lib
   DEPENDS Boost
 )

include/iri_bt_basic_nodes.h

@@ -4,6 +4,7 @@
 #include "iri_bt_factory.h"
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
+#include <iri_ros_tools/timeout.h>
 
 class IriBTBasicNodes
 {
@@ -25,6 +26,7 @@ class IriBTBasicNodes
     // variables to know whether start or stop have been demanded by the user
     tf2_ros::Buffer tf2_buffer;
     tf2_ros::TransformListener tf2_listener;
+    std::map<std::string, CROSTimeout> timeouts;
 
   protected:
     /**
@@ -151,6 +153,76 @@ class IriBTBasicNodes
       *
       */
     BT::NodeStatus print_msg(BT::TreeNode& _self);
+
+    /**
+      * \brief Action to create (if necessary) and to start a timeout
+      *
+      * It has the following input ports:
+      *
+      *   tout_name (std::string): Timeout name.
+      * 
+      *   duration (double): Duration time in seconds.
+      *
+      * \param _self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus start_timeout(BT::TreeNode& _self);
+
+    /**
+      * \brief Action to stop a timeout
+      *
+      * It has the following input ports:
+      *
+      *   tout_name (std::string): Timeout name.
+      *
+      * \param _self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus stop_timeout(BT::TreeNode& _self);
+
+    /**
+      * \brief Action to stop a timeout
+      *
+      * It has the following input ports:
+      *
+      *   tout_name (std::string): Timeout name.
+      *
+      * It also has the following output ports:
+      * 
+      *   timed_out (bool): If is timed out.
+      * 
+      * \param _self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus get_timed_out(BT::TreeNode& _self);
+
+    /**
+      * \brief Action to erase a timeout
+      *
+      * It has the following input ports:
+      *
+      *   tout_name (std::string): Timeout name.
+      * 
+      * \param _self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus erase_timeout(BT::TreeNode& _self);
     };
 
 #endif

package.xml

@@ -21,7 +21,7 @@
   <depend>tf2_ros</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
-  
+  <depend>iri_ros_tools</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/iri_bt_basic_nodes.cpp

@@ -13,6 +13,9 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
   BT::PortsList compute_distance_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose1"),BT::InputPort<geometry_msgs::PoseStamped>("pose2"),BT::OutputPort<double>("distance")};
   BT::PortsList compare_ports = {BT::InputPort<double>("value1"),BT::InputPort<double>("value2")};
   BT::PortsList print_msg_ports = {BT::InputPort<std::string>("msg"), BT::InputPort<std::string>("type")};
+  BT::PortsList start_timeout_ports = {BT::InputPort<std::string>("tout_name"), BT::InputPort<double>("duration")};
+  BT::PortsList stop_timeout_ports = {BT::InputPort<std::string>("tout_name")};
+  BT::PortsList get_timed_out_ports = {BT::InputPort<std::string>("tout_name"), BT::OutputPort<bool>("timed_out")};
 
   //Registration of conditions
   factory.registerSimpleCondition("is_variable_true", std::bind(&IriBTBasicNodes::is_variable_true, this, std::placeholders::_1),check_variable_ports);
@@ -25,6 +28,10 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
   factory.registerSimpleAction("compare_bigger", std::bind(&IriBTBasicNodes::compare_bigger, this, std::placeholders::_1),compare_ports);
   factory.registerSimpleAction("compare_smaller", std::bind(&IriBTBasicNodes::compare_smaller, this, std::placeholders::_1),compare_ports);
   factory.registerSimpleAction("print_msg",  std::bind(&IriBTBasicNodes::print_msg, this, std::placeholders::_1), print_msg_ports);
+  factory.registerSimpleAction("start_timeout",  std::bind(&IriBTBasicNodes::start_timeout, this, std::placeholders::_1), start_timeout_ports);
+  factory.registerSimpleAction("stop_timeout",  std::bind(&IriBTBasicNodes::stop_timeout, this, std::placeholders::_1), stop_timeout_ports);
+  factory.registerSimpleAction("get_timed_out",  std::bind(&IriBTBasicNodes::get_timed_out, this, std::placeholders::_1), get_timed_out_ports);
+  factory.registerSimpleAction("erase_timeout",  std::bind(&IriBTBasicNodes::erase_timeout, this, std::placeholders::_1), stop_timeout_ports);
 }
 
 BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
@@ -34,7 +41,7 @@ BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
 
   if(!variable)
   {
-    ROS_ERROR("IriBTBasicNodes::is_variable_true-> Incorrect or missing input. It needs the following input ports: variable (bool)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::is_variable_true (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: variable (bool)");
     return BT::NodeStatus::FAILURE;
   }
   bool variable_value=variable.value();
@@ -51,7 +58,7 @@ BT::NodeStatus IriBTBasicNodes::is_variable_false(BT::TreeNode& self)
 
   if(!variable)
   {
-    ROS_ERROR("IriBTBasicNodes::is_variable_false-> Incorrect or missing input. It needs the following input ports: variable (bool)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::is_variable_false (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: variable (bool)");
     return BT::NodeStatus::FAILURE;
   }
   bool variable_value=variable.value();
@@ -73,7 +80,7 @@ BT::NodeStatus IriBTBasicNodes::transform_pose(BT::TreeNode& self)
 
   if((!pose_in) || (!target_frame))
   {
-    ROS_ERROR("IriBTBasicNodes::transform_pose-> Incorrect or missing input. It needs the following input ports: pose_in (geometry_msgs::PoseStamped) target_frame (std::string) pose_out (geometry_msgs::PoseStamped)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::transform_pose (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose_in (geometry_msgs::PoseStamped) target_frame (std::string) pose_out (geometry_msgs::PoseStamped)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -115,7 +122,7 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
 
   if((!pose1) || (!pose2))
   {
-    ROS_ERROR("IriBTBasicNodes::compute_distance-> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -124,12 +131,11 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
 
   if (pose1_value.header.frame_id != pose1_value.header.frame_id)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance-> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
     return BT::NodeStatus::FAILURE;
   }
 
   distance=sqrt(pow(pose1_value.pose.position.x-pose2_value.pose.position.x,2.0)+pow(pose1_value.pose.position.y-pose2_value.pose.position.y,2.0)+pow(pose1_value.pose.position.z-pose2_value.pose.position.z,2.0));
-  std::cout << "distance: " << distance << std::endl;
   self.setOutput("distance",distance);
   return BT::NodeStatus::SUCCESS;
 }
@@ -145,7 +151,7 @@ BT::NodeStatus IriBTBasicNodes::compute_y_distance(BT::TreeNode& self)
 
   if((!pose1) || (!pose2))
   {
-    ROS_ERROR("IriBTBasicNodes::compute_y_distance-> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -154,7 +160,7 @@ BT::NodeStatus IriBTBasicNodes::compute_y_distance(BT::TreeNode& self)
 
   if (pose1_value.header.frame_id != pose1_value.header.frame_id)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance-> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
     return BT::NodeStatus::FAILURE;
   }
 
@@ -172,7 +178,7 @@ BT::NodeStatus IriBTBasicNodes::compare_bigger(BT::TreeNode& self)
 
   if((!value1) || (!value2))
   {
-    ROS_ERROR("IriBTBasicNodes::compare_bigger-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -193,7 +199,7 @@ BT::NodeStatus IriBTBasicNodes::compare_smaller(BT::TreeNode& self)
 
   if((!value1) || (!value2))
   {
-    ROS_ERROR("IriBTBasicNodes::compare_smaller-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -220,7 +226,7 @@ BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
 
   if (!msg_p || !type_p)
   {
-    ROS_ERROR_STREAM("common_helpers::print_msg-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("common_helpers::print_msg (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " msg (sdt::string) and type (std::string)");
     return BT::NodeStatus::FAILURE;
   }
@@ -237,3 +243,109 @@ BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
     ROS_INFO_STREAM(msg);
   return BT::NodeStatus::SUCCESS;
 }
+
+BT::NodeStatus IriBTBasicNodes::start_timeout(BT::TreeNode& _self)
+{
+  BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+  BT::Optional<double> duration_p = _self.getInput<double>("duration");
+  std::string name;
+  double duration;
+  std::map<std::string, CROSTimeout>::iterator it;
+
+  if (!name_p || !duration_p)
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::start_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
+                      << " tout_name (sdt::string) and duration (double)");
+    return BT::NodeStatus::FAILURE;
+  }
+  name = name_p.value();
+  duration = duration_p.value();
+
+  it = this->timeouts.find(name);
+  if (it == this->timeouts.end())
+  {
+    CROSTimeout tout;
+    this->timeouts[name] = tout;
+    it = this->timeouts.find(name);
+  }
+  it->second.start(ros::Duration(duration));
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::stop_timeout(BT::TreeNode& _self)
+{
+  BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+  std::string name;
+  std::map<std::string, CROSTimeout>::iterator it;
+
+  if (!name_p)
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
+                      << " tout_name (sdt::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+  name = name_p.value();
+
+  it = this->timeouts.find(name);
+  if (it == this->timeouts.end())
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout (" << _self.name() << ") -> No timeout found with name " << name);
+    return BT::NodeStatus::FAILURE;
+  }
+  it->second.stop();
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::get_timed_out(BT::TreeNode& _self)
+{
+  BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+  std::string name;
+  std::map<std::string, CROSTimeout>::iterator it;
+  bool timed_out = false;
+
+  _self.setOutput("timed_out", timed_out);
+  if (!name_p)
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
+                      << " tout_name (sdt::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+  name = name_p.value();
+
+  it = this->timeouts.find(name);
+  if (it == this->timeouts.end())
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out (" << _self.name() << ") -> No timeout found with name " << name);
+    return BT::NodeStatus::FAILURE;
+  }
+  timed_out = it->second.timed_out();
+  _self.setOutput("timed_out", timed_out);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::erase_timeout(BT::TreeNode& _self)
+{
+  BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+  std::string name;
+  std::map<std::string, CROSTimeout>::iterator it;
+
+  if (!name_p)
+  {
+    ROS_ERROR_STREAM("IriBTBasicNodes::erase_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
+                      << " tout_name (sdt::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+  name = name_p.value();
+
+  it = this->timeouts.find(name);
+  if (it == this->timeouts.end())
+  {
+    ROS_WARN_STREAM("IriBTBasicNodes::erase_timeout (" << _self.name() << ") -> No timeout found with name " << name);
+    return BT::NodeStatus::SUCCESS;
+  }
+  this->timeouts.erase(it);
+
+  return BT::NodeStatus::SUCCESS;
+}
\ No newline at end of file

src/xml/bt_definitions.xml

@@ -8,12 +8,12 @@
     <TreeNodesModel>
         <!-- basic_nodes -->
         <Action ID="RUNNING"/>
-        <Action ID="is_variable_true">
+        <Condition ID="is_variable_true">
             <input_port name="variable"> (bool): Bool variable to check.</input_port>
-        </Action>
-        <Action ID="is_variable_false">
+        </Condition>
+        <Condition ID="is_variable_false">
             <input_port name="variable"> (bool): Bool variable to check.</input_port>
-        </Action>
+        </Condition>
         <Action ID="transform_pose">
             <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
             <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
@@ -24,6 +24,11 @@
             <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
             <output_port name="distance"> (double): Distance.</output_port>
         </Action>
+        <Action ID="compute_y_distance">
+            <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+            <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+            <output_port name="distance"> (double): Distance.</output_port>
+        </Action>
         <Action ID="compare_bigger">
             <input_port name="value1"> (double): Value 1.</input_port>
             <input_port name="value2"> (double): Value 2.</input_port>
@@ -69,6 +74,20 @@
             <input_port name="param_name"> (std::string): Parameter name.</input_port>
             <output_port name="param_value"> (bool): Parameter value.</output_port>
         </Action>
+        <Action ID="start_timeout">
+            <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+            <input_port name="duration"> (double): Duration time in second.</input_port>
+        </Action>
+        <Action ID="stop_timeout">
+            <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+        </Action>
+        <Action ID="get_timed_out">
+            <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+            <output_port name="timed_out"> (bool): If is timed out.</output_port>
+        </Action>
+        <Action ID="erase_timeout">
+            <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+        </Action>
         <!-- Head helepers -->
         <Action ID="from_img_point_to_pan_tilt">
             <input_port name="x"> Image x coordinate</input_port>