From: Aakash Rohra <aakash.r@barrett.com>
Date: Wed, 21 Aug 2019 20:40:54 +0000 (-0400)
Subject: Added new messages for FtTorques and tactile data
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1538ccec068626879011bb911e583a91aa2fdb0a;p=barrett-ros-pkg.git

Added new messages for FtTorques and tactile data
---

diff --git a/wam_common/wam_msgs/msg/FtTorques.msg b/wam_common/wam_msgs/msg/FtTorques.msg
new file mode 100644
index 0000000..2380642
--- /dev/null
+++ b/wam_common/wam_msgs/msg/FtTorques.msg
@@ -0,0 +1 @@
+int32[] torque
diff --git a/wam_common/wam_msgs/msg/tactilePressure.msg b/wam_common/wam_msgs/msg/tactilePressure.msg
new file mode 100644
index 0000000..4fb6941
--- /dev/null
+++ b/wam_common/wam_msgs/msg/tactilePressure.msg
@@ -0,0 +1 @@
+float64[] pressure
diff --git a/wam_common/wam_msgs/msg/tactilePressureArray.msg b/wam_common/wam_msgs/msg/tactilePressureArray.msg
new file mode 100644
index 0000000..b176cdb
--- /dev/null
+++ b/wam_common/wam_msgs/msg/tactilePressureArray.msg
@@ -0,0 +1 @@
+wam_msgs/tactilePressure[] tactilePressures
diff --git a/wam_robot/wam_node/src/wam_node.cpp b/wam_robot/wam_node/src/wam_node.cpp
index fdce1c9..2105975 100644
--- a/wam_robot/wam_node/src/wam_node.cpp
+++ b/wam_robot/wam_node/src/wam_node.cpp
@@ -42,6 +42,9 @@
 #include "wam_msgs/RTOrtnPos.h"
 #include "wam_msgs/RTOrtnVel.h"
 #include "wam_srvs/GravityComp.h"
+#include "wam_msgs/FtTorques.h"
+#include "wam_msgs/tactilePressure.h"
+#include "wam_msgs/tactilePressureArray.h"
 #include "wam_srvs/Hold.h"
 #include "wam_srvs/JointMove.h"
 #include "wam_srvs/PoseMove.h"
@@ -206,11 +209,14 @@ template<size_t DOF>
 
     //Published Topics
     sensor_msgs::JointState wam_joint_state, bhand_joint_state;
+    wam_msgs::FtTorques ftTorque_state;
+    wam_msgs::tactilePressureArray tactileStates;
+    wam_msgs::tactilePressure tactileState;
     geometry_msgs::PoseStamped wam_pose;
     geometry_msgs::Wrench fts_state;
 
     //Publishers
-    ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub, fts_pub;
+    ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub, fts_pub, tps_pub, fingerTs_pub;
 
     //Services
     ros::ServiceServer gravity_srv, go_home_srv, hold_jpos_srv, hold_cpos_srv;
@@ -319,7 +325,7 @@ template<size_t DOF>
 	  
 	  //Publishing the following topics only if there is a BarrettHand present
       fts_pub = fts_.advertise < geometry_msgs::Wrench > ("fts_states", 1); // fts/states
-	  
+	    
     }
 
     if (pm.foundHand()) //Does the following only if a BarrettHand is present
@@ -342,7 +348,8 @@ template<size_t DOF>
 
       //Publishing the following topics only if there is a BarrettHand present
       bhand_joint_state_pub = nh_.advertise < sensor_msgs::JointState > ("joint_states", 1); // bhand/joint_states
-
+      tps_pub = nh_.advertise < wam_msgs::tactilePressureArray > ("tactile_states", 1); // tactile  sensors
+      fingerTs_pub = nh_.advertise < wam_msgs::FtTorques > ("finger_tip_states", 1); // finger tip torques
       //Advertise the following services only if there is a BarrettHand present
       hand_open_grsp_srv = nh_.advertiseService("open_grasp", &WamNode<DOF>::handOpenGrasp, this); // bhand/open_grasp
       hand_close_grsp_srv = nh_.advertiseService("close_grasp", &WamNode::handCloseGrasp, this); // bhand/close_grasp
@@ -358,6 +365,9 @@ template<size_t DOF>
       //Set up the BarrettHand joint state publisher
       const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
       std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7);
+      tactileState.pressure.resize(24);
+      tactileStates.tactilePressures.resize(4);
+      ftTorque_state.torque.resize(4);
       bhand_joint_state.name.resize(7);
       bhand_joint_state.name = bhand_joints;
       bhand_joint_state.position.resize(7);
@@ -753,14 +763,32 @@ template<size_t DOF>
     while (ros::ok())
     {
       hand->update(); // Update the hand sensors
+      std::vector<TactilePuck*> tps = hand->getTactilePucks();
+      std::vector<int> fingerTip = hand->getFingertipTorque();
       Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information
       Hand::jp_type ho = hand->getOuterLinkPosition();
+      for (int i = 0; i < tps.size(); i++)
+      {
+        
+        TactilePuck::v_type pressures(tps[i]->getFullData());
+        for (int j = 0; j < pressures.size(); j++)
+        {
+          tactileState.pressure[j] = pressures[j];
+        }
+        tactileStates.tactilePressures[i] = tactileState;
+      }
+      for (int i = 0; i < fingerTip.size(); i++)
+      {
+        ftTorque_state.torque[i] = fingerTip[i];
+      }
       for (size_t i = 0; i < 4; i++) // Save finger positions
         bhand_joint_state.position[i] = hi[i];
       for (size_t j = 0; j < 3; j++)
         bhand_joint_state.position[j + 4] = ho[j];
       bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp
       bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states
+      tps_pub.publish(tactileStates);
+      fingerTs_pub.publish(ftTorque_state);
       btsleep(1.0 / BHAND_PUBLISH_FREQ); // Sleep according to the specified publishing frequency
     }
   }