Added get_current_joint_states

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
 )

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)

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")

config/ivo_head_module_default.yaml

 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

config/tiago_head_module_default.yaml

 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

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.
       *

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
       *

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)"/>

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)"/>

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>

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>

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)"/>

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)"/>

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>

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>

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>

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");

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();
 }
 

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())

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"/>