Fixes #7 for retrieving wrench and joint data

dvrk_arm/include/dvrk_arm/Arm.h

@@ -87,6 +87,8 @@ class DVRK_Arm: public DVRK_Bridge{
     void init();
     void handle_frames();
     void cisstPose_to_userTransform(const geometry_msgs::PoseStamped &pose);
+    void cisstJoint_to_userJoint(const sensor_msgs::JointState &jnt);
+    void cisstWrench_to_userWrench(const geometry_msgs::WrenchStamped &wrench);
     void userPose_to_cisstPose(geometry_msgs::PoseStamped &pose);
     void move_arm_cartesian(tf::Transform trans);
     void set_arm_wrench(tf::Vector3 &force, tf::Vector3 &wrench);

dvrk_arm/include/dvrk_arm/Bridge.h

@@ -7,14 +7,15 @@
 #include "sensor_msgs/Joy.h"
 #include "std_msgs/String.h"
 #include "std_msgs/Bool.h"
-#include "geometry_msgs/Wrench.h"
+#include "geometry_msgs/WrenchStamped.h"
 #include "FootPedals.h"
 #include "Console.h"
 #include "string.h"
 #include "boost/bind.hpp"
 #include "boost/function.hpp"
 #include "ros/callback_queue.h"
 #include "dvrk_arm/States.h"
+#include "boost/type_traits/is_same.hpp"
 
 class DVRK_Bridge: public States, public DVRK_FootPedals{
 public:
@@ -24,8 +25,13 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     DVRK_Bridge(const std::string &arm_name, int bridge_frequnce = 1000);
     ~DVRK_Bridge();
 
-    template <class T, class U>
-    void assign_conversion_fcn(void (T::*conversion_fcn)(U), T *obj);
+    template <class T>
+    void assign_conversion_fcn(void (T::*conversion_fcn)(const geometry_msgs::PoseStamped&), T *obj);
+    template <class T>
+    void assign_conversion_fcn(void (T::*conversion_fcn)(const sensor_msgs::JointState&), T *obj);
+    template <class T>
+    void assign_conversion_fcn(void (T::*conversion_fcn)(const geometry_msgs::WrenchStamped&), T *obj);
+
     void set_cur_pose(const geometry_msgs::PoseStamped &pose);
     void set_cur_wrench(const geometry_msgs::Wrench &wrench);
     void set_cur_joint(const sensor_msgs::JointState &jnt_state);
@@ -57,34 +63,52 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     ros::Subscriber pose_sub;
     ros::Subscriber joint_sub;
     ros::Subscriber state_sub;
+    ros::Subscriber wrench_sub;
     ros::CallbackQueue cb_queue, cb_queue_timer;
     ros::Timer timer;
     AspinPtr aspin;
     RatePtr rate;
     int _freq;
 
     double scale;
-    bool _is_cnvFcn_set;
+    bool _is_pose_cnvFcn_set, _is_joint_cnvFcn_set, _is_wrench_cnvFcn_set;
     std::vector<std::string> valid_arms;
     void init();
     void state_sub_cb(const std_msgs::StringConstPtr &msg);
     void pose_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg);
     void joint_sub_cb(const sensor_msgs::JointStateConstPtr &msg);
+    void wrench_sub_cb(const geometry_msgs::WrenchStampedConstPtr &wrench);
     void timer_cb(const ros::TimerEvent&);
     void _rate_sleep();
 
     geometry_msgs::PoseStamped cur_pose, pre_pose, cmd_pose;
     sensor_msgs::JointState cur_joint, pre_joint, cmd_joint;
     std_msgs::String cur_state, state_cmd;
-    geometry_msgs::Wrench cur_wrench, cmd_wrench;
+    geometry_msgs::WrenchStamped cur_wrench, cmd_wrench;
+
+
+    boost::function<void (const geometry_msgs::PoseStamped&)> conversion_function_pose;
+    boost::function<void (const sensor_msgs::JointState&)> conversion_function_joint;
+    boost::function<void (const geometry_msgs::WrenchStamped&)> conversion_function_wrench;
 
-    boost::function<void (const geometry_msgs::PoseStamped &pose)> conversion_function;
 };
 
-template <class T, class U>
-void DVRK_Bridge::assign_conversion_fcn(void (T::*conversion_fcn)(U), T *obj){
-    conversion_function = boost::bind(conversion_fcn, obj, _1);
-    _is_cnvFcn_set = true;
+template <class T>
+void DVRK_Bridge::assign_conversion_fcn(void (T::*conversion_fcn)(const geometry_msgs::PoseStamped&), T *obj){
+    conversion_function_pose = boost::bind(conversion_fcn, obj, _1);
+    _is_pose_cnvFcn_set = true;
+}
+
+template <class T>
+void DVRK_Bridge::assign_conversion_fcn(void (T::*conversion_fcn)(const sensor_msgs::JointState&), T *obj){
+    conversion_function_joint = boost::bind(conversion_fcn, obj, _1);
+    _is_joint_cnvFcn_set = true;
+}
+
+template <class T>
+void DVRK_Bridge::assign_conversion_fcn(void (T::*conversion_fcn)(const geometry_msgs::WrenchStamped&), T *obj){
+    conversion_function_wrench = boost::bind(conversion_fcn, obj, _1);
+    _is_wrench_cnvFcn_set = true;
 }
 
 #endif

dvrk_arm/src/Arm.cpp

@@ -12,6 +12,8 @@ DVRK_Arm::DVRK_Arm(const std::string &arm_name): DVRK_Bridge(arm_name){
 
     init();
     assign_conversion_fcn(&DVRK_Arm::cisstPose_to_userTransform, this);
+    assign_conversion_fcn(&DVRK_Arm::cisstJoint_to_userJoint, this);
+    assign_conversion_fcn(&DVRK_Arm::cisstWrench_to_userWrench, this);
 
 }
 
@@ -32,6 +34,14 @@ void DVRK_Arm::cisstPose_to_userTransform(const geometry_msgs::PoseStamped &pose
     handle_frames();
 }
 
+void DVRK_Arm::cisstJoint_to_userJoint(const sensor_msgs::JointState &jnt){
+
+}
+
+void DVRK_Arm::cisstWrench_to_userWrench(const geometry_msgs::WrenchStamped &wrench){
+
+}
+
 void DVRK_Arm::set_origin_frame(const tf::Vector3 &pos, const tf::Matrix3x3 &tf_mat){
     tf_mat.getRotation(originFramePtr->rot_quat);
     set_origin_frame(pos, originFramePtr->rot_quat);

dvrk_arm/src/Bridge.cpp

@@ -41,6 +41,7 @@ void DVRK_Bridge::init(){
     pose_sub = n->subscribe("/dvrk/" + arm_name + "/position_cartesian_current", 10, &DVRK_Bridge::pose_sub_cb, this);
     state_sub = n->subscribe("/dvrk/" + arm_name + "/robot_state", 10, &DVRK_Bridge::state_sub_cb, this);
     joint_sub = n->subscribe("/dvrk/" + arm_name + "/position_joint_current", 10, &DVRK_Bridge::joint_sub_cb, this);
+    wrench_sub = n->subscribe("/dvrk/" + arm_name + "/wrench_body_current", 10, &DVRK_Bridge::wrench_sub_cb, this);
 
     joint_pub = n->advertise<sensor_msgs::JointState>("/dvrk/" + arm_name + "/set_position_joint", 10);
     pose_pub  = n->advertise<geometry_msgs::Pose>("/dvrk/" + arm_name + "/set_position_cartesian", 10);
@@ -52,11 +53,11 @@ void DVRK_Bridge::init(){
 
     cmd_pose.pose.position.x = 0; cmd_pose.pose.position.y = 0; cmd_pose.pose.position.z = 0;
     cmd_pose.pose.orientation.x = 0; cmd_pose.pose.orientation.y = 0; cmd_pose.pose.orientation.z = 0; cmd_pose.pose.orientation.w = 1;
-    cmd_wrench.force.x = 0; cmd_wrench.force.y = 0; cmd_wrench.force.z = 0;
-    cmd_wrench.torque.x = 0; cmd_wrench.torque.y = 0; cmd_wrench.torque.z = 0;
+    cmd_wrench.wrench.force.x = 0; cmd_wrench.wrench.force.y = 0; cmd_wrench.wrench.force.z = 0;
+    cmd_wrench.wrench.torque.x = 0; cmd_wrench.wrench.torque.y = 0; cmd_wrench.wrench.torque.z = 0;
 
     DVRK_FootPedals::init(n);
-    _is_cnvFcn_set = false;
+    _is_pose_cnvFcn_set = false; _is_joint_cnvFcn_set = false; _is_wrench_cnvFcn_set = false;
     _start_pubs = false;
     sleep(1);
     aspin->start();
@@ -66,12 +67,23 @@ void DVRK_Bridge::init(){
 void DVRK_Bridge::joint_sub_cb(const sensor_msgs::JointStateConstPtr &msg){
     pre_joint = cur_joint;
     cur_joint = *msg;
+    if(_is_joint_cnvFcn_set){
+        conversion_function_joint(cur_joint);
+    }
 }
+
 void DVRK_Bridge::pose_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg){
     pre_pose = cur_pose;
     cur_pose = *msg;
-    if(_is_cnvFcn_set){
-        conversion_function(cur_pose);
+    if(_is_pose_cnvFcn_set){
+        conversion_function_pose(cur_pose);
+    }
+}
+
+void DVRK_Bridge::wrench_sub_cb(const geometry_msgs::WrenchStampedConstPtr &msg){
+    cur_wrench = *msg;
+    if(_is_wrench_cnvFcn_set){
+        conversion_function_wrench(cur_wrench);
     }
 }
 
@@ -95,7 +107,7 @@ void DVRK_Bridge::timer_cb(const ros::TimerEvent& event){
             pose_pub.publish(cmd_pose.pose);
             break;
         case DVRK_EFFORT_CARTESIAN:
-            force_pub.publish(cmd_wrench);
+            force_pub.publish(cmd_wrench.wrench);
             break;
         default:
             break;
@@ -125,7 +137,7 @@ void DVRK_Bridge::set_cur_pose(const geometry_msgs::PoseStamped &pose){
 }
 
 void DVRK_Bridge::set_cur_wrench(const geometry_msgs::Wrench &wrench){
-    cmd_wrench = wrench;
+    cmd_wrench.wrench = wrench;
     _start_pubs = true;
 }