Skip to content
Snippets Groups Projects
Commit 1e255c40 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Updated BT client. Fixed some bugs

parent c4d1d878
No related branches found
No related tags found
No related merge requests found
...@@ -77,6 +77,27 @@ class IriJointsClientBtAlgNode : public algorithm_base::IriBaseAlgorithm<IriJoin ...@@ -77,6 +77,27 @@ class IriJointsClientBtAlgNode : public algorithm_base::IriBaseAlgorithm<IriJoin
bool send_ticks; 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: public:
/** /**
* \brief Constructor * \brief Constructor
......
...@@ -220,7 +220,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig ...@@ -220,7 +220,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
* *
* \return True if action is called. * \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. * \brief Function to start the tracker action.
......
...@@ -252,9 +252,9 @@ class CIriJointsModuleBT ...@@ -252,9 +252,9 @@ class CIriJointsModuleBT
///PointHeadTracker ///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: * It has the following input ports:
* *
...@@ -271,7 +271,7 @@ class CIriJointsModuleBT ...@@ -271,7 +271,7 @@ class CIriJointsModuleBT
* If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE. * 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. * \brief Synchronized enable tracker function.
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
<arg name="launch_prefix" default=""/> <arg name="launch_prefix" default=""/>
<arg name="xml_path" default="$(find iri_joints_module)/src/xml"/> <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"/> <arg name="send_ticks" default="False"/>
<node name="$(arg node_name)" <node name="$(arg node_name)"
......
...@@ -98,7 +98,7 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level) ...@@ -98,7 +98,7 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
if (config.update_target) if (config.update_target)
{ {
config.update_target = false; 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"); ROS_DEBUG("Update target");
else else
ROS_ERROR("Error while updating target"); ROS_ERROR("Error while updating target");
......
...@@ -28,6 +28,8 @@ IriJointsClientBtAlgNode::IriJointsClientBtAlgNode(void) : ...@@ -28,6 +28,8 @@ IriJointsClientBtAlgNode::IriJointsClientBtAlgNode(void) :
ROS_DEBUG_STREAM("IriJointsClientBtAlgNode: xml_file set to: " << xml_file); ROS_DEBUG_STREAM("IriJointsClientBtAlgNode: xml_file set to: " << xml_file);
this->joints_module_bt.init(this->factory); 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; std::cout << xml_path << "/" << xml_file << std::endl;
this->tree = this->factory.createTreeFromFile(xml_path + "/" + xml_file); this->tree = this->factory.createTreeFromFile(xml_path + "/" + xml_file);
...@@ -86,6 +88,27 @@ void IriJointsClientBtAlgNode::mainNodeThread(void) ...@@ -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] */ /* [subscriber callbacks] */
/* [service callbacks] */ /* [service callbacks] */
......
...@@ -433,7 +433,7 @@ bool CIriJointsModule::is_point_to_finished(void) ...@@ -433,7 +433,7 @@ bool CIriJointsModule::is_point_to_finished(void)
} }
/*Tracking action*/ /*Tracking action*/
bool CIriJointsModule::update_target(double x, double y, double z) bool CIriJointsModule::target_update(double x, double y, double z)
{ {
this->lock(); this->lock();
if (this->state == IRI_JOINTS_MODULE_POINT_WAIT) if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)
......
...@@ -52,7 +52,7 @@ void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory) ...@@ -52,7 +52,7 @@ void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("cancel_move_to_angles", std::bind(&CIriJointsModuleBT::cancel_move_to_angles, this)); 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.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("enable_tracker", std::bind(&CIriJointsModuleBT::enable_tracker, this));
factory.registerSimpleAction("cancel_tracker", std::bind(&CIriJointsModuleBT::cancel_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)); 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) ...@@ -229,9 +229,9 @@ BT::NodeStatus CIriJointsModuleBT::async_is_move_to_angles_finished(void)
return BT::NodeStatus::RUNNING; 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> x = self.getInput<double>("x");
BT::Optional<double> y = self.getInput<double>("y"); BT::Optional<double> y = self.getInput<double>("y");
BT::Optional<double> z = self.getInput<double>("z"); BT::Optional<double> z = self.getInput<double>("z");
...@@ -247,7 +247,7 @@ BT::NodeStatus CIriJointsModuleBT::update_target(BT::TreeNode& self) ...@@ -247,7 +247,7 @@ BT::NodeStatus CIriJointsModuleBT::update_target(BT::TreeNode& self)
y_goal = y.value(); y_goal = y.value();
z_goal = z.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; return BT::NodeStatus::SUCCESS;
} }
......
...@@ -3,116 +3,77 @@ ...@@ -3,116 +3,77 @@
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="BehaviorTree"> <BehaviorTree ID="BehaviorTree">
<SequenceStar> <SequenceStar>
<SubTree ID="set_point_to_blackboard" x="x" y="y" z="z" max_vel="max_vel" new_goal="new_goal"/> <Action ID="set_move_angles_arguments" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
<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>
<SequenceStar> <SequenceStar>
<SubTree ID="set_base_frame_black_board"/> <Action ID="sync_move_to_angles" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
<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}"/>
<Timeout msec="15000"> <Timeout msec="15000">
<Condition ID="is_point_to_finished"/> <Action ID="async_is_move_to_angles_finished"/>
</Timeout> </Timeout>
</SequenceStar> </SequenceStar>
</SequenceStar> <Action ID="async_move_to_angles" new_goal="{new_goal}" joint_names="{joint_names}" angles="{angles}" vel="{vel}"/>
</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}"/>
</SequenceStar> </SequenceStar>
</BehaviorTree> </BehaviorTree>
<!-- ////////// --> <!-- ////////// -->
<TreeNodesModel> <TreeNodesModel>
<Action ID="async_point_to"> <Action ID="async_is_move_to_angles_finished"/>
<input_port default="0.5" name="max_vel"> The maximum velocity for the servos</input_port> <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="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 name="vel"> Desired velocity of each servo</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>
<Action ID="async_move_to_angles"> <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 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="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> <input_port name="vel"> Desired velocity of each servo</input_port>
</Action> </Action>
<Action ID="cancel_point_to"/>
<Action ID="cancel_move_to_angles"/> <Action ID="cancel_move_to_angles"/>
<Action ID="cancel_point_to"/>
<Action ID="cancel_tracker"/> <Action ID="cancel_tracker"/>
<Action ID="enable_tracker"/> <Action ID="enable_tracker"/>
<Action ID="async_is_point_to_finished"/> <Condition ID="is_joints_module_aborted"/>
<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_action_start_fail"/> <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_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_preempted"/>
<Condition ID="is_joints_module_rejected"/> <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_set_param_fail"/>
<Condition ID="is_joints_module_param_not_present"/> <Condition ID="is_joints_module_success"/>
<Condition ID="is_joints_module_no_joint_states"/>
<Condition ID="is_joints_module_target_lost"/> <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"> <Action ID="set_base_frame">
<input_port default="servo1" name="base_frame"> Servo kinematic chain base frame</input_port> <input_port default="servo1" name="base_frame"> Servo kinematic chain base frame</input_port>
</Action> </Action>
<SubTree ID="set_base_frame_black_board"/>
<SubTree ID="set_point_to_blackboard"/>
<SubTree ID="set_tol_blackboard"/>
<Action ID="set_tolerance"> <Action ID="set_tolerance">
<input_port default="0.001" name="tol"> Inverse kinematic solver tolerance</input_port> <input_port default="0.001" name="tol"> Inverse kinematic solver tolerance</input_port>
</Action> </Action>
<Action ID="set_urdf_param"> <Action ID="set_urdf_param">
<input_port default="robot_description" name="urdf_param"> Ros param where is stored the urdf model</input_port> <input_port default="robot_description" name="urdf_param"> Ros param where is stored the urdf model</input_port>
</Action> </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"> <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.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="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="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 default="0.0" name="z"> Point Z coordenate on its reference frame</input_port>
</Action> </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"> <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="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="y"> Point Y coordenate on its reference frame</input_port>
......
...@@ -4,11 +4,11 @@ ...@@ -4,11 +4,11 @@
<BehaviorTree ID="BehaviorTree"> <BehaviorTree ID="BehaviorTree">
<SequenceStar> <SequenceStar>
<Action ID="enable_tracker"/> <Action ID="enable_tracker"/>
<ReactiveFallback> <ReactiveSequence>
<SubTree ID="set_target_blackboard"/> <SubTree ID="set_target_blackboard" x="x" y="y" z="z"/>
<Action ID="target_update" x="{x}" y="{y}" z="{z}"/> <Action ID="target_update" x="{x}" y="{y}" z="{z}"/>
<Action ID="async_is_tracker_finished"/> <Action ID="async_is_tracker_finished"/>
</ReactiveFallback> </ReactiveSequence>
</SequenceStar> </SequenceStar>
</BehaviorTree> </BehaviorTree>
<!-- ////////// --> <!-- ////////// -->
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment