diff --git a/dvrk_arm/include/dvrk_arm/Arm.h b/dvrk_arm/include/dvrk_arm/Arm.h index 8c8804b..5dd0c86 100644 --- a/dvrk_arm/include/dvrk_arm/Arm.h +++ b/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); diff --git a/dvrk_arm/include/dvrk_arm/Bridge.h b/dvrk_arm/include/dvrk_arm/Bridge.h index e45d086..f5b7488 100644 --- a/dvrk_arm/include/dvrk_arm/Bridge.h +++ b/dvrk_arm/include/dvrk_arm/Bridge.h @@ -7,7 +7,7 @@ #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" @@ -15,6 +15,7 @@ #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 - void assign_conversion_fcn(void (T::*conversion_fcn)(U), T *obj); + template + void assign_conversion_fcn(void (T::*conversion_fcn)(const geometry_msgs::PoseStamped&), T *obj); + template + void assign_conversion_fcn(void (T::*conversion_fcn)(const sensor_msgs::JointState&), T *obj); + template + 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,6 +63,7 @@ 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; @@ -64,27 +71,44 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{ int _freq; double scale; - bool _is_cnvFcn_set; + bool _is_pose_cnvFcn_set, _is_joint_cnvFcn_set, _is_wrench_cnvFcn_set; std::vector 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 conversion_function_pose; + boost::function conversion_function_joint; + boost::function conversion_function_wrench; - boost::function conversion_function; }; -template -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 +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 +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 +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 diff --git a/dvrk_arm/src/Arm.cpp b/dvrk_arm/src/Arm.cpp index 726a387..0908217 100644 --- a/dvrk_arm/src/Arm.cpp +++ b/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); diff --git a/dvrk_arm/src/Bridge.cpp b/dvrk_arm/src/Bridge.cpp index bb2da44..03b2cd0 100644 --- a/dvrk_arm/src/Bridge.cpp +++ b/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("/dvrk/" + arm_name + "/set_position_joint", 10); pose_pub = n->advertise("/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; }