diff --git a/include/iri_joints_client_bt_alg_node.h b/include/iri_joints_client_bt_alg_node.h
index 1b9424f0968f106763328e3d1cddcd5d875a37a4..e28439d22391b868f342e21d8a95628fce072d01 100644
--- a/include/iri_joints_client_bt_alg_node.h
+++ b/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
diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 108a8e5672828e2fb43a0b608fa547c9c20a5e66..0c43985bb74beadd5df758c125831ebf95758663 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/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.
diff --git a/include/iri_joints_module/iri_joints_module_bt.h b/include/iri_joints_module/iri_joints_module_bt.h
index e3230c0629b726670f3cb8b850d938be9e32e178..caf9224f6cd025499bd8a8544558aefb88d92365 100644
--- a/include/iri_joints_module/iri_joints_module_bt.h
+++ b/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.
diff --git a/launch/joints_client_bt.launch b/launch/joints_client_bt.launch
index 34e12d32792ea73c63eba841786d0512806af310..56376cf1ae6a0e85649d6aa382182da5a2a6c06f 100644
--- a/launch/joints_client_bt.launch
+++ b/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)"
diff --git a/src/iri_joints_client_alg_node.cpp b/src/iri_joints_client_alg_node.cpp
index 81afc2a6bb80e9e3f9d0eb6cf8f16c132a152b74..572e52b5e600bc0fa0ec7e2dda06a729a3819b3a 100644
--- a/src/iri_joints_client_alg_node.cpp
+++ b/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");
diff --git a/src/iri_joints_client_bt_alg_node.cpp b/src/iri_joints_client_bt_alg_node.cpp
index 0d8cd754b2757bd2f8f70eb0ce83741dd22c9b4a..bd5289c4b685d0c2d6481b28cbd09f67421d97ea 100644
--- a/src/iri_joints_client_bt_alg_node.cpp
+++ b/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] */
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index 682ba4c9dcd33de2d201cbf249d06d99c41d8c3c..f761ec5c19b239c96510d6a9286ad4d2f3311d7e 100644
--- a/src/iri_joints_module.cpp
+++ b/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)
diff --git a/src/iri_joints_module_bt.cpp b/src/iri_joints_module_bt.cpp
index 127585fcd2efba259ec1d92e837266783b6c8059..5c0075304b66aa995a5218ac7322f5f862573482 100644
--- a/src/iri_joints_module_bt.cpp
+++ b/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;
}
diff --git a/src/xml/bt_move_to_angles_test.xml b/src/xml/bt_move_to_angles_test.xml
index a49c60325215c155825cb996cef5fc800412f300..f9a5178825dc299c26ea8fe6b8ab2c5e5367843b 100644
--- a/src/xml/bt_move_to_angles_test.xml
+++ b/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>
diff --git a/src/xml/bt_point_tracker_test.xml b/src/xml/bt_point_tracker_test.xml
index 65a2851aa8dfd7ca5a63a930c30234a78046ea8f..a10b165f5e5dfbc0790160548443de1b48dc5ec6 100644
--- a/src/xml/bt_point_tracker_test.xml
+++ b/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>
<!-- ////////// -->