diff --git a/CMakeLists.txt b/CMakeLists.txt
index 245db1733626ef042cbfd484a1cc73b250def55f..8bb624f55efbf2d02dd2a6115ac5a8b69dffa265 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs sensor_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -106,7 +106,7 @@ set(module_bt_name tiago_head_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs geometry_msgs sensor_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
# DEPENDS system_lib
)
diff --git a/cfg/TiagoHeadClient.cfg b/cfg/TiagoHeadClient.cfg
index 04a0db976081ce682a2da2c6757c219bd0f044fc..c01e8a59cba889aeb28f60760580cdaf70961317 100755
--- a/cfg/TiagoHeadClient.cfg
+++ b/cfg/TiagoHeadClient.cfg
@@ -42,6 +42,8 @@ point_action = gen.add_group("Point head action")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("print_current_joint_state", bool_t, 0, "Flag to print current Head jointstate info", False)
+
move_action.add("pan_angle", double_t, 0, "Pan angle for the regulat motion", 0.0, -3.14159, 3.14159)
move_action.add("tilt_angle", double_t, 0, "Tilt angle for the regulat motion",0.0, -3.14159, 3.14159)
move_action.add("move_duration", double_t, 0, "Maximum time allowed for the motion",1.0, 0.1, 10)
diff --git a/cfg/TiagoHeadModule.cfg b/cfg/TiagoHeadModule.cfg
index 754a4a909ebf9dc3ef85aa3f18006a6ffd0a05b6..ec0338b1f2b19c47de6278c324c7fe6e1a896b0c 100755
--- a/cfg/TiagoHeadModule.cfg
+++ b/cfg/TiagoHeadModule.cfg
@@ -38,11 +38,15 @@ from iri_ros_tools.dyn_params import add_module_service_params,add_module_action
gen = ParameterGenerator()
+joints_subs = gen.add_group("Joint states subscriber")
move_action = gen.add_group("Move head action")
point_action = gen.add_group("Point head action")
# Name Type Reconfiguration level Description Default Min Max
add_module_params(gen,"head_module")
+
+joints_subs.add("js_watchdog_time_s",double_t,0, "Maximum time between joint state messages",1,0.01,50)
+
move_action=add_module_action_params(gen,"move")
move_action.add("move_default_duration", double_t, 0, "Default move time", 1.0, 1.0, 10.0)
point_action=add_module_action_params(gen,"point")
diff --git a/config/ivo_head_module_default.yaml b/config/ivo_head_module_default.yaml
index 26748ef2c7a1ee9480aad9422313956db90e09c5..76931bfa050f4584e8c7f8ea97836d56281f9a6b 100644
--- a/config/ivo_head_module_default.yaml
+++ b/config/ivo_head_module_default.yaml
@@ -1,5 +1,8 @@
head_module_rate_hz: 1.0
+#joint states watchdog time
+js_watchdog_time_s: 1.0
+
move_num_retries: 1
move_feedback_watchdog_time_s: 1.0
move_enable_watchdog: True
diff --git a/config/tiago_head_module_default.yaml b/config/tiago_head_module_default.yaml
index da88a071ac8c2b065118e83daf2962be93d6b1ca..9487361717c00f623383801602c633b86f85ec7f 100644
--- a/config/tiago_head_module_default.yaml
+++ b/config/tiago_head_module_default.yaml
@@ -1,5 +1,8 @@
head_module_rate_hz: 1.0
+#joint states watchdog time
+js_watchdog_time_s: 1.0
+
move_num_retries: 1
move_feedback_watchdog_time_s: 1.0
move_enable_watchdog: True
diff --git a/include/tiago_head_module/tiago_head_bt_module.h b/include/tiago_head_module/tiago_head_bt_module.h
index cc0eeb77693648d3d83c456f50c832e8831a3cd4..833d5dceaeda48864a93bfefdc131ead150072f1 100644
--- a/include/tiago_head_module/tiago_head_bt_module.h
+++ b/include/tiago_head_module/tiago_head_bt_module.h
@@ -358,6 +358,24 @@ class CTiagoHeadModuleBT
*/
BT::NodeStatus set_tiago_head_max_velocity(BT::TreeNode& self);
+ /**
+ * \brief Synchronized get_current_joint_state TIAGo head function.
+ *
+ * This function calls get_current_joint_state of tiago_head_module.
+ *
+ * It has the following output ports:
+ *
+ * joint_state (sensor_msgs::JointState): Current head joint states.
+ *
+ * \param self 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_tiago_head_current_joint_state(BT::TreeNode& self);
+
/**
* \brief Synchronized stop TIAGo head function.
*
diff --git a/include/tiago_head_module/tiago_head_module.h b/include/tiago_head_module/tiago_head_module.h
index 5f936326e2c5334294c3bbee1aa38ebe1205e4f6..5d2276e667e5848e381012ca77c2781c4bd20132 100644
--- a/include/tiago_head_module/tiago_head_module.h
+++ b/include/tiago_head_module/tiago_head_module.h
@@ -9,6 +9,7 @@
//Action
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/PointHeadAction.h>
+#include <sensor_msgs/JointState.h>
//States
typedef enum {TIAGO_HEAD_MODULE_IDLE,TIAGO_HEAD_MOVE_START,TIAGO_HEAD_MOVE_WAIT,TIAGO_HEAD_POINT_START,TIAGO_HEAD_POINT_WAIT} tiago_head_module_state_t;
@@ -32,6 +33,7 @@ typedef enum {TIAGO_HEAD_MODULE_IDLE,TIAGO_HEAD_MOVE_START,TIAGO_HEAD_MOVE_WAIT,
* canceled by another goal.
* * TIAGO_HEAD_MODULE_REJECTED: indicates that the goal informatio is not valid and the
* goal will not be executed.
+ * * TIAGO_HEAD_MODULE_NO_JOINT_STATES: indicates that no joint_state has benn received on a while.
*/
typedef enum {TIAGO_HEAD_MODULE_RUNNING,
@@ -41,7 +43,8 @@ typedef enum {TIAGO_HEAD_MODULE_RUNNING,
TIAGO_HEAD_MODULE_FB_WATCHDOG,
TIAGO_HEAD_MODULE_ABORTED,
TIAGO_HEAD_MODULE_PREEMPTED,
- TIAGO_HEAD_MODULE_REJECTED} tiago_head_module_status_t;
+ TIAGO_HEAD_MODULE_REJECTED,
+ TIAGO_HEAD_MODULE_NO_JOINT_STATES} tiago_head_module_status_t;
/**
* \brief Head module
@@ -94,6 +97,12 @@ class CTiagoHeadModule : public CModule<tiago_head_module::TiagoHeadModuleConfig
*/
bool new_point;
+ // joint states
+ CROSWatchdog joint_states_watchdog;
+ ros::Subscriber joint_states_subscriber;
+ void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
+ sensor_msgs::JointState current_js;
+
//Variables
/**
* \brief
@@ -256,6 +265,15 @@ class CTiagoHeadModule : public CModule<tiago_head_module::TiagoHeadModuleConfig
*/
bool move_to(std::vector<double> &pan, std::vector<double> &tilt, std::vector<double> &duration);
+ /**
+ * \brief Function to get current head joint_states
+ *
+ * \param joint_state The current head jointState.
+ *
+ * \return If succeded.
+ */
+ bool get_current_joint_state(sensor_msgs::JointState &joint_state);
+
/**
* \brief Destructor
*
diff --git a/launch/ivo_head_bt_client.launch b/launch/ivo_head_bt_client.launch
index 45107349cb744b3644093e86375c8779e313e4a8..b71e36736b3542c4fde3d85c8eb125c44d675b94 100644
--- a/launch/ivo_head_bt_client.launch
+++ b/launch/ivo_head_bt_client.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/ivo_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
<arg name="tree_file" default="move_to_bt" />
@@ -18,6 +19,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
<arg name="tree_path" value="$(arg tree_path)"/>
<arg name="tree_file" value="$(arg tree_file)"/>
<arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
diff --git a/launch/ivo_head_bt_client_sim.launch b/launch/ivo_head_bt_client_sim.launch
index 1b9cb3684bc43673a9caaad182d5d784a1f4b02a..7d8a98c5339acff11269c4b1bbeb9baa14dde4dc 100644
--- a/launch/ivo_head_bt_client_sim.launch
+++ b/launch/ivo_head_bt_client_sim.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/ivo_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
<arg name="tree_file" default="move_to_bt" />
@@ -21,6 +22,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
<arg name="tree_path" value="$(arg tree_path)"/>
<arg name="tree_file" value="$(arg tree_file)"/>
<arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
diff --git a/launch/ivo_head_client.launch b/launch/ivo_head_client.launch
index e149215bd1aefe2b5d60ec2ed9b202697ad1d869..b93f08f8685c41db60ef1929b52f65edd8b25140 100644
--- a/launch/ivo_head_client.launch
+++ b/launch/ivo_head_client.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/ivo_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<include file="$(find tiago_head_module)/launch/tiago_head_client.launch">
<arg name="node_name" value="$(arg node_name)" />
@@ -14,6 +15,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
</include>
</launch>
diff --git a/launch/ivo_head_client_sim.launch b/launch/ivo_head_client_sim.launch
index b30a722108ec9f3dbeab27746af2aae575991dfc..f40d3b443b3f1f10ec5a989f8366a2cd2150a8f2 100644
--- a/launch/ivo_head_client_sim.launch
+++ b/launch/ivo_head_client_sim.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/ivo_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<include file="$(find ivo_1_gazebo)/launch/ivo_gazebo.launch">
</include>
@@ -17,6 +18,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
</include>
</launch>
diff --git a/launch/tiago_head_bt_client.launch b/launch/tiago_head_bt_client.launch
index a1276c060138eb19c9bd54fa9704499b936582ef..023f4ebeded691c47a3b11b914635fce551b711f 100644
--- a/launch/tiago_head_bt_client.launch
+++ b/launch/tiago_head_bt_client.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
<arg name="tree_file" default="move_to_bt" />
@@ -20,6 +21,8 @@
to="$(arg move_action)"/>
<remap from="~/tiago_head_module/point_head"
to="$(arg point_action)"/>
+ <remap from="~/head_module/joint_states"
+ to="$(arg joint_states_topic)"/>
<rosparam file="$(arg config_file)" command="load" ns="tiago_head_module" />
<param name="path" value="$(arg tree_path)"/>
diff --git a/launch/tiago_head_bt_client_sim.launch b/launch/tiago_head_bt_client_sim.launch
index cb92fdcc2badceb3ea0fa695e0c01046e359b48a..82fbf1039e58e43a943df3f395ad37417684becc 100644
--- a/launch/tiago_head_bt_client_sim.launch
+++ b/launch/tiago_head_bt_client_sim.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<arg name="tree_path" default="$(find tiago_head_module)/src/xml" />
<arg name="tree_file" default="move_to_bt" />
@@ -23,6 +24,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
<arg name="tree_path" value="$(arg tree_path)"/>
<arg name="tree_file" value="$(arg tree_file)"/>
<arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
diff --git a/launch/tiago_head_client.launch b/launch/tiago_head_client.launch
index afff4c8ce19f07b4af79173c4720ec212e1bd329..1af8082a596d83b88bac5430bf993d6474cddcda 100644
--- a/launch/tiago_head_client.launch
+++ b/launch/tiago_head_client.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<!-- launch the play motion client node -->
<node name="$(arg node_name)"
@@ -17,6 +18,8 @@
to="$(arg move_action)"/>
<remap from="~/head_module/point_head"
to="$(arg point_action)"/>
+ <remap from="~/head_module/joint_states"
+ to="$(arg joint_states_topic)"/>
<rosparam file="$(arg config_file)" command="load" ns="head_module" />
</node>
diff --git a/launch/tiago_head_client_sim.launch b/launch/tiago_head_client_sim.launch
index fbefae7f7eb70504b7e4f5ae07f8067b1f485cf4..83ff08d187520ac6f251a6488c0b448ab4d56606 100644
--- a/launch/tiago_head_client_sim.launch
+++ b/launch/tiago_head_client_sim.launch
@@ -6,6 +6,7 @@
<arg name="config_file" default="$(find tiago_head_module)/config/tiago_head_module_default.yaml" />
<arg name="move_action" default="/head_controller/follow_joint_trajectory"/>
<arg name="point_action" default="/head_controller/point_head_action"/>
+ <arg name="joint_states_topic" default="/joint_states"/>
<include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
<arg name="public_sim" value="true" />
@@ -19,6 +20,7 @@
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_action" value="$(arg move_action)"/>
<arg name="point_action" value="$(arg point_action)"/>
+ <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
</include>
</launch>
diff --git a/package.xml b/package.xml
index 62490930cfd11d5f6848150cb456ea05af12ca7d..c42e94837de6f7c30efaf1b8eda3597193789ebe 100644
--- a/package.xml
+++ b/package.xml
@@ -43,6 +43,7 @@
<build_depend>actionlib</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
+ <build_depend>sensor_msgs</build_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>iri_base_bt_client</build_depend>
<build_depend>iri_behaviortree</build_depend>
@@ -54,6 +55,7 @@
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
+ <run_depend>sensor_msgs</run_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>iri_base_bt_client</run_depend>
<run_depend>iri_behaviortree</run_depend>
diff --git a/src/tiago_head_bt_module.cpp b/src/tiago_head_bt_module.cpp
index 8b8b54d1046704ce26683f8579d9cf96bfbe1f88..641704b700ac6cec013a8deee3f1b696fc272fa7 100644
--- a/src/tiago_head_bt_module.cpp
+++ b/src/tiago_head_bt_module.cpp
@@ -27,6 +27,7 @@ void CTiagoHeadModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList set_max_velocity_ports = {BT::InputPort<double>("vel")};
+ BT::PortsList get_joint_states = {BT::OutputPort<sensor_msgs::JointState>("joint_states")};
//Registration of conditions
factory.registerSimpleCondition("is_tiago_head_finished", std::bind(&CTiagoHeadModuleBT::is_tiago_head_finished, this));
@@ -58,6 +59,7 @@ void CTiagoHeadModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerIriAsyncAction("async_tiago_head_move_to_trajectory", std::bind(&CTiagoHeadModuleBT::async_tiago_head_move_to_trajectory, this, std::placeholders::_1), async_head_move_to_trajectory_ports);
factory.registerSimpleAction("set_tiago_head_max_velocity", std::bind(&CTiagoHeadModuleBT::set_tiago_head_max_velocity, this, std::placeholders::_1), set_max_velocity_ports);
+ factory.registerSimpleAction("get_tiago_head_current_joint_state", std::bind(&CTiagoHeadModuleBT::get_tiago_head_current_joint_state, this, std::placeholders::_1), get_joint_states);
}
CTiagoHeadModuleBT::~CTiagoHeadModuleBT(void)
@@ -457,6 +459,18 @@ BT::NodeStatus CTiagoHeadModuleBT::set_tiago_head_max_velocity(BT::TreeNode& sel
return BT::NodeStatus::SUCCESS;
}
+BT::NodeStatus CTiagoHeadModuleBT::get_tiago_head_current_joint_state(BT::TreeNode& self)
+{
+ sensor_msgs::JointState js;
+ if (this->tiago_head_module.get_current_joint_state(js))
+ {
+ self.setOutput("joint_states", js);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+}
+
BT::NodeStatus CTiagoHeadModuleBT::sync_cancel_tiago_head_action(void)
{
ROS_DEBUG("CTiagoHeadModuleBT::sync_cancel_tiago_head_action-> sync_cancel_tiago_head_action");
diff --git a/src/tiago_head_client_alg_node.cpp b/src/tiago_head_client_alg_node.cpp
index 00ea5fd988e0fd717cc52420662958e22f17d464..12e1fad7644b290141f6a63426a6fff3b22623ea 100644
--- a/src/tiago_head_client_alg_node.cpp
+++ b/src/tiago_head_client_alg_node.cpp
@@ -83,6 +83,23 @@ void TiagoHeadClientAlgNode::node_config_update(Config &config, uint32_t level)
this->head.stop();
config.stop_point=false;
}
+ if (config.print_current_joint_state)
+ {
+ config.print_current_joint_state = false;
+ sensor_msgs::JointState js;
+ if (this->head.get_current_joint_state(js))
+ {
+ for (unsigned int i = 0; i < js.name.size(); i++)
+ {
+ ROS_INFO_STREAM("Joint " << js.name[i]);
+ ROS_INFO_STREAM(" position " << js.position[i]);
+ ROS_INFO_STREAM(" velocity " << js.velocity[i]);
+ ROS_INFO_STREAM(" effort " << js.effort[i]);
+ }
+ }
+ else
+ ROS_ERROR("No joint_state received");
+ }
this->alg_.unlock();
}
diff --git a/src/tiago_head_module.cpp b/src/tiago_head_module.cpp
index d4436485e93a5ee09bda33341e61c7f31e65b65f..54ce9f594686ce27a98ca0382a9398d219c287fa 100644
--- a/src/tiago_head_module.cpp
+++ b/src/tiago_head_module.cpp
@@ -27,6 +27,23 @@ CTiagoHeadModule::CTiagoHeadModule(const std::string &name,const std::string &na
this->point_goal.pointing_axis.y=0.0;
this->point_goal.pointing_axis.z=1.0;
this->point_goal.max_velocity=DEFAULT_MAX_VEL;
+
+ // initialize jointStates subscriber
+ if(!this->module_nh.getParam("js_watchdog_time_s", this->config.js_watchdog_time_s))
+ {
+ ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'js_watchdog_time_s' not found. Configuring wathcdog with 1.0 sec");
+ this->joint_states_watchdog.reset(ros::Duration(1.0));
+ }
+ else
+ this->joint_states_watchdog.reset(ros::Duration(this->config.js_watchdog_time_s));
+ this->joint_states_subscriber = this->module_nh.subscribe("joint_states",1,&CTiagoHeadModule::joint_states_callback,this);
+
+ this->current_js.name.resize(2);
+ this->current_js.position.resize(2);
+ this->current_js.velocity.resize(2);
+ this->current_js.effort.resize(2);
+ this->current_js.name[0]="head_1_joint";
+ this->current_js.name[1]="head_2_joint";
}
/* State machine */
@@ -195,6 +212,29 @@ void CTiagoHeadModule::reconfigure_callback(tiago_head_module::TiagoHeadModuleCo
this->unlock();
}
+void CTiagoHeadModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ ROS_DEBUG("CTiagoHeadModule: joint states callback");
+
+ this->lock();
+ // reset the odometry callback
+ this->joint_states_watchdog.reset(ros::Duration(this->config.js_watchdog_time_s));
+ this->current_js.header=msg->header;
+ for(unsigned int i=0;i<msg->name.size();i++)
+ {
+ for(unsigned int j=0; j<this->current_js.name.size(); j++)
+ {
+ if(msg->name[i]==this->current_js.name[j])
+ {
+ this->current_js.position[j] = msg->position[i];
+ this->current_js.velocity[j] = msg->velocity[i];
+ this->current_js.effort[j] = msg->effort[i];
+ }
+ }
+ }
+ this->unlock();
+}
+
void CTiagoHeadModule::stop(void)
{
if(this->state!=TIAGO_HEAD_MODULE_IDLE)
@@ -328,6 +368,21 @@ bool CTiagoHeadModule::move_to(std::vector<double> &pan, std::vector<double> &ti
}
}
+bool CTiagoHeadModule::get_current_joint_state(sensor_msgs::JointState &joint_state)
+{
+ this->lock();
+ if(this->joint_states_watchdog.is_active())
+ {
+ this->status = TIAGO_HEAD_MODULE_NO_JOINT_STATES;
+ ROS_ERROR("CTiagoHeadModule::get_current_joint_state -> joint_state whatchdog active");
+ this->unlock();
+ return false;
+ }
+ joint_state = this->current_js;
+ this->unlock();
+ return true;
+}
+
CTiagoHeadModule::~CTiagoHeadModule()
{
if(!this->move_action.is_finished())
diff --git a/src/xml/move_to_bt.xml b/src/xml/move_to_bt.xml
index dd6dc71f3aeacdb4a00b461f7b8295cc6db301e6..cd5651fd41c0311cd92343b9c4408741f86de5c8 100644
--- a/src/xml/move_to_bt.xml
+++ b/src/xml/move_to_bt.xml
@@ -97,6 +97,9 @@
<Action ID="set_tiago_head_max_velocity">
<input_port name="vel"> Head maximum velocity</input_port>
</Action>
+ <Action ID="get_tiago_head_current_joint_state">
+ <output_port name="joint_states">Current head joint_states</output_port>
+ </Action>
<Condition ID="is_tiago_head_finished"/>
<Condition ID="is_tiago_head_module_running"/>
<Condition ID="is_tiago_head_module_success"/>
diff --git a/src/xml/point_to_bt.xml b/src/xml/point_to_bt.xml
index 1b462706731a3b2e6c739406a82a7dbbd17b94a2..12cb7e40ac30d99f084f2df6287f284bb4e08a80 100644
--- a/src/xml/point_to_bt.xml
+++ b/src/xml/point_to_bt.xml
@@ -97,6 +97,9 @@
<Action ID="set_tiago_head_max_velocity">
<input_port name="vel"> Head maximum velocity</input_port>
</Action>
+ <Action ID="get_tiago_head_current_joint_state">
+ <output_port name="joint_states">Current head joint_states</output_port>
+ </Action>
<Condition ID="is_tiago_head_finished"/>
<Condition ID="is_tiago_head_module_running"/>
<Condition ID="is_tiago_head_module_success"/>