Updated BT client. Fixed some bugs

include/iri_joints_client_bt_alg_node.h

@@ -77,6 +77,27 @@ class IriJointsClientBtAlgNode : public algorithm_base::IriBaseAlgorithm<IriJoin
 
     bool send_ticks;
 
+    /**
+      * \brief Synchronized set move to angles parameters.
+      *
+      * This function sets join_names, angles and vel of each servo.
+      *
+      * It has the following output ports:
+      *
+      *   joint_names (std::vector<std::string>): Joint name of each servo.
+      *
+      *   angles (std::vecotr<double>): Each servo desired angle.
+      *
+      *   vel (std::vecotr<double>): Each servo desired velocity.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus set_move_angles_arguments(BT::TreeNode& self);
+
   public:
    /**
     * \brief Constructor

include/iri_joints_module/iri_joints_module.h

@@ -220,7 +220,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       *
       * \return True if action is called.
       */
-    bool update_target(double x, double y, double z);
+    bool target_update(double x, double y, double z);
 
     /**
       * \brief Function to start the tracker action.

include/iri_joints_module/iri_joints_module_bt.h

@@ -252,9 +252,9 @@ class CIriJointsModuleBT
 
     ///PointHeadTracker
     /**
-      * \brief Synchronized update target function function.
+      * \brief Synchronized target update function function.
       *
-      * This function calls update_target of iri_joints_module.
+      * This function calls target_update of iri_joints_module.
       *
       * It has the following input ports:
       *
@@ -271,7 +271,7 @@ class CIriJointsModuleBT
       * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
       *
       */
-    BT::NodeStatus update_target(BT::TreeNode& self);
+    BT::NodeStatus target_update(BT::TreeNode& self);
 
     /**
       * \brief Synchronized enable tracker function.

launch/joints_client_bt.launch

@@ -8,7 +8,7 @@
   <arg name="launch_prefix" default=""/>
 
   <arg name="xml_path"           default="$(find iri_joints_module)/src/xml"/>
-  <arg name="xml_file"           default="bt_point_to_test.xml"/>
+  <arg name="xml_file"           default="bt_move_to_angles_test.xml"/>
   <arg name="send_ticks"     default="False"/>
 
   <node name="$(arg node_name)"

src/iri_joints_client_alg_node.cpp

@@ -98,7 +98,7 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
   if (config.update_target)
   {
     config.update_target = false;
-    if (this->joints_module.update_target(config.point_x, config.point_y, config.point_z))
+    if (this->joints_module.target_update(config.point_x, config.point_y, config.point_z))
       ROS_DEBUG("Update target");
     else
       ROS_ERROR("Error while updating target");

src/iri_joints_client_bt_alg_node.cpp

@@ -28,6 +28,8 @@ IriJointsClientBtAlgNode::IriJointsClientBtAlgNode(void) :
     ROS_DEBUG_STREAM("IriJointsClientBtAlgNode: xml_file set to: " << xml_file);
 
   this->joints_module_bt.init(this->factory);
+  BT::PortsList set_move_param_ports = {BT::OutputPort<std::vector<std::string>>("joint_names"), BT::OutputPort<std::vector<double>>("angles"), BT::OutputPort<std::vector<double>>("vel")};
+  this->factory.registerSimpleAction("set_move_angles_arguments",  std::bind(&IriJointsClientBtAlgNode::set_move_angles_arguments, this, std::placeholders::_1), set_move_param_ports);
   std::cout << xml_path << "/" << xml_file << std::endl;
   this->tree = this->factory.createTreeFromFile(xml_path + "/" + xml_file);
 
@@ -86,6 +88,27 @@ void IriJointsClientBtAlgNode::mainNodeThread(void)
   }
 }
 
+BT::NodeStatus IriJointsClientBtAlgNode::set_move_angles_arguments(BT::TreeNode& self)
+{
+  std::vector<std::string> joint_names;
+  std::vector<double> angles, vel;
+
+  joint_names.push_back("pan");
+  joint_names.push_back("tilt");
+  joint_names.push_back("roll");
+  angles.push_back(0.0);
+  angles.push_back(0.0);
+  angles.push_back(0.0);
+  vel.push_back(0.5);
+  vel.push_back(0.5);
+  vel.push_back(0.5);
+
+  self.setOutput("joint_names", joint_names);
+  self.setOutput("angles", angles);
+  self.setOutput("vel", vel);
+  return BT::NodeStatus::SUCCESS;
+}
+
 /*  [subscriber callbacks] */
 
 /*  [service callbacks] */

src/iri_joints_module.cpp

@@ -433,7 +433,7 @@ bool CIriJointsModule::is_point_to_finished(void)
 }
 
 /*Tracking action*/
-bool CIriJointsModule::update_target(double x, double y, double z)
+bool CIriJointsModule::target_update(double x, double y, double z)
 {
   this->lock();
   if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)

src/iri_joints_module_bt.cpp

@@ -52,7 +52,7 @@ void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
   factory.registerSimpleAction("cancel_move_to_angles",  std::bind(&CIriJointsModuleBT::cancel_move_to_angles, this));
   factory.registerIriAsyncAction("async_is_move_to_angles_finished",  std::bind(&CIriJointsModuleBT::async_is_move_to_angles_finished, this));
 
-  factory.registerSimpleAction("update_target",  std::bind(&CIriJointsModuleBT::update_target, this, std::placeholders::_1), update_target_ports);
+  factory.registerSimpleAction("target_update",  std::bind(&CIriJointsModuleBT::target_update, this, std::placeholders::_1), update_target_ports);
   factory.registerSimpleAction("enable_tracker",  std::bind(&CIriJointsModuleBT::enable_tracker, this));
   factory.registerSimpleAction("cancel_tracker",  std::bind(&CIriJointsModuleBT::cancel_tracker, this));
   factory.registerIriAsyncAction("async_is_tracker_finished",  std::bind(&CIriJointsModuleBT::async_is_tracker_finished, this));
@@ -229,9 +229,9 @@ BT::NodeStatus CIriJointsModuleBT::async_is_move_to_angles_finished(void)
   return BT::NodeStatus::RUNNING;
 }
 
-BT::NodeStatus CIriJointsModuleBT::update_target(BT::TreeNode& self)
+BT::NodeStatus CIriJointsModuleBT::target_update(BT::TreeNode& self)
 {
-  ROS_INFO("update_target");
+  ROS_INFO("target_update");
   BT::Optional<double> x = self.getInput<double>("x");
   BT::Optional<double> y = self.getInput<double>("y");
   BT::Optional<double> z = self.getInput<double>("z");
@@ -247,7 +247,7 @@ BT::NodeStatus CIriJointsModuleBT::update_target(BT::TreeNode& self)
   y_goal = y.value();
   z_goal = z.value();
 
-  this->joints_module.update_target(x_goal, y_goal, z_goal);
+  this->joints_module.target_update(x_goal, y_goal, z_goal);
   return BT::NodeStatus::SUCCESS;
 }
 

src/xml/bt_move_to_angles_test.xml

@@ -3,116 +3,77 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <SequenceStar>
-            <SubTree ID="set_point_to_blackboard" x="x" y="y" z="z" max_vel="max_vel" new_goal="new_goal"/>
-            <Fallback>
-                <Action ID="async_point_to" max_vel="{max_vel}" new_goal="{new_goal}" x="{x}" y="{y}" z="{z}"/>
-                <Condition ID="is_point_to_finished"/>
-            </Fallback>
+            <Action ID="set_move_angles_arguments" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
             <SequenceStar>
-                <SubTree ID="set_base_frame_black_board"/>
-                <SubTree ID="set_tol_blackboard"/>
-                <SubTree ID="set_urdf_param_blackboard"/>
-            </SequenceStar>
-            <SequenceStar>
-                <Action ID="sync_point_to" max_vel="{max_vel}" x="{x}" y="{y}" z="{z}"/>
+                <Action ID="sync_move_to_angles" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
                 <Timeout msec="15000">
-                    <Condition ID="is_point_to_finished"/>
+                    <Action ID="async_is_move_to_angles_finished"/>
                 </Timeout>
             </SequenceStar>
-        </SequenceStar>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <BehaviorTree ID="set_base_frame_black_board">
-        <SequenceStar>
-            <SetBlackboard output_key="base_frame" value="servo2"/>
-            <Action ID="set_base_frame" base_frame="{base_frame}"/>
-        </SequenceStar>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <BehaviorTree ID="set_point_to_blackboard">
-        <SequenceStar>
-            <SetBlackboard output_key="x" value="0.8"/>
-            <SetBlackboard output_key="y" value="0.7"/>
-            <SetBlackboard output_key="z" value="0.3"/>
-            <SetBlackboard output_key="max_vel" value="0.5"/>
-            <SetBlackboard output_key="new_goal" value="True"/>
-        </SequenceStar>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <BehaviorTree ID="set_tol_blackboard">
-        <SequenceStar>
-            <SetBlackboard output_key="tol" value="0.01"/>
-            <Action ID="set_tolerance" tol="{tol}"/>
-        </SequenceStar>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <BehaviorTree ID="set_urdf_param_blackboard">
-        <SequenceStar>
-            <SetBlackboard output_key="urdf_param" value="r_desc"/>
-            <Action ID="set_urdf_param" urdf_param="{urdf_param}"/>
+            <Action ID="async_move_to_angles" new_goal="{new_goal}" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
         </SequenceStar>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
-        <Action ID="async_point_to">
-            <input_port default="0.5" name="max_vel"> The maximum velocity for the servos</input_port>
+        <Action ID="async_is_move_to_angles_finished"/>
+        <Action ID="async_is_point_to_finished"/>
+        <Action ID="async_is_tracker_finished"/>
+        <Action ID="async_move_to_angles">
+            <input_port name="angles"> Desired angle of each servo</input_port>
+            <input_port name="joint_names"> Name of the servos desired to move</input_port>
             <input_port default="True" name="new_goal"> The maximum velocity for the servos</input_port>
-            <input_port default="0.0" name="x"> Point X coordenate on its reference frame</input_port>
-            <input_port default="0.0" name="y"> Point Y coordenate on its reference frame</input_port>
-            <input_port default="0.0" name="z"> Point Z coordenate on its reference frame</input_port>
+            <input_port name="vel"> Desired velocity of each servo</input_port>
         </Action>
         <Action ID="async_move_to_angles">
+            <input_port name="angles"> Desired angle of each servo</input_port>
             <input_port name="joint_names"> Name of the servos desired to move</input_port>
             <input_port default="True" name="new_goal"> The maximum velocity for the servos</input_port>
-            <input_port name="angles"> Desired angle of each servo</input_port>
             <input_port name="vel"> Desired velocity of each servo</input_port>
         </Action>
-        <Action ID="cancel_point_to"/>
         <Action ID="cancel_move_to_angles"/>
+        <Action ID="cancel_point_to"/>
         <Action ID="cancel_tracker"/>
         <Action ID="enable_tracker"/>
-        <Action ID="async_is_point_to_finished"/>
-        <Action ID="async_is_move_to_angles_finished"/>
-        <Action ID="async_is_tracker_finished"/>
-        <Condition ID="is_joints_module_running"/>
-        <Condition ID="is_joints_module_success"/>
+        <Condition ID="is_joints_module_aborted"/>
         <Condition ID="is_joints_module_action_start_fail"/>
-        <Condition ID="is_point_to_finished"/>
-        <Condition ID="is_move_to_angles_finished"/>
-        <Condition ID="is_tracker_finished"/>
-        <Condition ID="is_joints_module_timeout"/>
         <Condition ID="is_joints_module_fb_watchdog"/>
-        <Condition ID="is_joints_module_aborted"/>
+        <Condition ID="is_joints_module_no_joint_states"/>
+        <Condition ID="is_joints_module_param_not_present"/>
         <Condition ID="is_joints_module_preempted"/>
         <Condition ID="is_joints_module_rejected"/>
+        <Condition ID="is_joints_module_running"/>
         <Condition ID="is_joints_module_set_param_fail"/>
-        <Condition ID="is_joints_module_param_not_present"/>
-        <Condition ID="is_joints_module_no_joint_states"/>
+        <Condition ID="is_joints_module_success"/>
         <Condition ID="is_joints_module_target_lost"/>
+        <Condition ID="is_joints_module_timeout"/>
+        <Condition ID="is_move_to_angles_finished"/>
+        <Condition ID="is_point_to_finished"/>
+        <Condition ID="is_tracker_finished"/>
         <Action ID="set_base_frame">
             <input_port default="servo1" name="base_frame"> Servo kinematic chain base frame</input_port>
         </Action>
-        <SubTree ID="set_base_frame_black_board"/>
-        <SubTree ID="set_point_to_blackboard"/>
-        <SubTree ID="set_tol_blackboard"/>
         <Action ID="set_tolerance">
             <input_port default="0.001" name="tol"> Inverse kinematic solver tolerance</input_port>
         </Action>
         <Action ID="set_urdf_param">
             <input_port default="robot_description" name="urdf_param"> Ros param where is stored the urdf model</input_port>
         </Action>
-        <SubTree ID="set_urdf_param_blackboard"/>
+        <Action ID="sync_move_to_angles">
+            <input_port name="angles"> Desired angle of each servo</input_port>
+            <input_port name="joint_names"> Name of the servos desired to move</input_port>
+            <input_port name="vel"> Desired velocity of each servo</input_port>
+        </Action>
+        <Action ID="set_move_angles_arguments">
+            <output_port name="angles"> Desired angle of each servo</output_port>
+            <output_port name="joint_names"> Name of the servos desired to move</output_port>
+            <output_port name="vel"> Desired velocity of each servo</output_port>
+        </Action>
         <Action ID="sync_point_to">
             <input_port default="0.5" name="max_vel"> The maximum velocity for the servos</input_port>
             <input_port default="0.0" name="x"> Point X coordenate on its reference frame</input_port>
             <input_port default="0.0" name="y"> Point Y coordenate on its reference frame</input_port>
             <input_port default="0.0" name="z"> Point Z coordenate on its reference frame</input_port>
         </Action>
-        <Action ID="sync_move_to_angles">
-            <input_port name="joint_names"> Name of the servos desired to move</input_port>
-            <input_port name="angles"> Desired angle of each servo</input_port>
-            <input_port name="vel"> Desired velocity of each servo</input_port>
-        </Action>
         <Action ID="update_target">
             <input_port default="0.0" name="x"> Point X coordenate on its reference frame</input_port>
             <input_port default="0.0" name="y"> Point Y coordenate on its reference frame</input_port>

src/xml/bt_point_tracker_test.xml

@@ -4,11 +4,11 @@
     <BehaviorTree ID="BehaviorTree">
         <SequenceStar>
             <Action ID="enable_tracker"/>
-            <ReactiveFallback>
-                <SubTree ID="set_target_blackboard"/>
+            <ReactiveSequence>
+                <SubTree ID="set_target_blackboard" x="x" y="y" z="z"/>
                 <Action ID="target_update" x="{x}" y="{y}" z="{z}"/>
                 <Action ID="async_is_tracker_finished"/>
-            </ReactiveFallback>
+            </ReactiveSequence>
         </SequenceStar>
     </BehaviorTree>
     <!-- ////////// -->