Fix #234 (#235)

WPI-AIM · Jul 23, 2024 · accb6ba · accb6ba
1 parent 8690849
commit accb6ba
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 19 deletions.
diff --git a/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Arm.h b/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Arm.h
@@ -158,7 +158,7 @@ class DVRK_Arm: public States{
 
     void init();
     void handle_frames();
-    void pose_fcn_cb(const geometry_msgs::TransformStamped &pose);
+    void pose_fcn_cb(const geometry_msgs::PoseStamped &pose);
     void gripper_state_fcn_cb(const sensor_msgs::JointState &state);
     void joint_state_fcn_cb(const sensor_msgs::JointState &jnt);
     void wrench_fcn_cb(const geometry_msgs::WrenchStamped &wrench);

diff --git a/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Bridge.h b/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Bridge.h
@@ -45,7 +45,7 @@
 #define CDVRK_BRIDGEH
 
 #include "ros/ros.h"
-#include "geometry_msgs/TransformStamped.h"
+#include "geometry_msgs/PoseStamped.h"
 #include "sensor_msgs/JointState.h"
 #include "sensor_msgs/Joy.h"
 #include "std_msgs/String.h"
@@ -71,7 +71,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     DVRK_Bridge(const std::string &arm_name, int bridge_frequnce = 1000);
     ~DVRK_Bridge();
 
-    void servo_cp(const geometry_msgs::TransformStamped &pose);
+    void servo_cp(const geometry_msgs::PoseStamped &pose);
     void servo_cf(const geometry_msgs::Wrench &wrench);
     void servo_jp(const sensor_msgs::JointState &jnt_state);
     void set_cur_mode(const std::string &state, bool lock_ori);
@@ -92,7 +92,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
 
     bool shutDown();
 
-    FcnHandle<const geometry_msgs::TransformStamped&> poseFcnHandle;
+    FcnHandle<const geometry_msgs::PoseStamped&> poseFcnHandle;
     FcnHandle<const sensor_msgs::JointState&> jointFcnHandle;
     FcnHandle<const geometry_msgs::WrenchStamped&> wrenchFcnHandle;
     FcnHandle<const sensor_msgs::JointState&> gripperFcnHandle;
@@ -122,7 +122,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     std::vector<std::string> valid_arms;
     void init();
     void state_cb(const std_msgs::StringConstPtr &msg);
-    void measured_cp_cb(const geometry_msgs::TransformStampedConstPtr &msg);
+    void measured_cp_cb(const geometry_msgs::PoseStampedConstPtr &msg);
     void measured_js_cb(const sensor_msgs::JointStateConstPtr &msg);
     void measured_cf_cb(const geometry_msgs::WrenchStampedConstPtr &wrench);
     void gripper_sub_cb(const std_msgs::BoolConstPtr &gripper);
@@ -132,7 +132,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{
     void run();
     std::shared_ptr<boost::thread> loop_thread;
 
-    geometry_msgs::TransformStamped cur_pose, pre_pose, cmd_pose;
+    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::WrenchStamped cur_wrench, cmd_wrench;

diff --git a/ambf_ros_modules/dvrk_arm/src/Arm.cpp b/ambf_ros_modules/dvrk_arm/src/Arm.cpp
@@ -63,12 +63,12 @@ DVRK_Arm::DVRK_Arm(const std::string &arm_name){
 void DVRK_Arm::init(){
 }
 
-void DVRK_Arm::pose_fcn_cb(const geometry_msgs::TransformStamped &pose){
+void DVRK_Arm::pose_fcn_cb(const geometry_msgs::PoseStamped &pose){
     std::lock_guard<std::mutex> lock(m_mutex);
-    m_freeFramePtr->pos.setX(pose.transform.translation.x);
-    m_freeFramePtr->pos.setY(pose.transform.translation.y);
-    m_freeFramePtr->pos.setZ(pose.transform.translation.z);
-    tf::quaternionMsgToTF(pose.transform.rotation, m_freeFramePtr->rot_quat);
+    m_freeFramePtr->pos.setX(pose.pose.position.x);
+    m_freeFramePtr->pos.setY(pose.pose.position.y);
+    m_freeFramePtr->pos.setZ(pose.pose.position.z);
+    tf::quaternionMsgToTF(pose.pose.orientation, m_freeFramePtr->rot_quat);
 
     m_freeFramePtr->trans.setOrigin(m_freeFramePtr->pos);
     m_freeFramePtr->trans.setRotation(m_freeFramePtr->rot_quat);
@@ -424,12 +424,12 @@ void DVRK_Arm::set_mode(const std::string &state, bool lock_wrench_ori){
 }
 
 void DVRK_Arm::move_arm_cartesian(tf::Transform trans){
-    geometry_msgs::TransformStamped servo_cf_cmd;
+    geometry_msgs::PoseStamped servo_cf_cmd;
     trans = m_originFramePtr->trans * trans * m_afxdTipFramePtr->trans.inverse();
-    servo_cf_cmd.transform.translation.x = trans.getOrigin().getX();
-    servo_cf_cmd.transform.translation.y = trans.getOrigin().getY();
-    servo_cf_cmd.transform.translation.z = trans.getOrigin().getZ();
-    tf::quaternionTFToMsg(trans.getRotation().normalized(), servo_cf_cmd.transform.rotation);
+    servo_cf_cmd.pose.position.x = trans.getOrigin().getX();
+    servo_cf_cmd.pose.position.y = trans.getOrigin().getY();
+    servo_cf_cmd.pose.position.z = trans.getOrigin().getZ();
+    tf::quaternionTFToMsg(trans.getRotation().normalized(), servo_cf_cmd.pose.orientation);
 
     m_bridge->servo_cp(servo_cf_cmd);
 }

diff --git a/ambf_ros_modules/dvrk_arm/src/Bridge.cpp b/ambf_ros_modules/dvrk_arm/src/Bridge.cpp
@@ -89,7 +89,7 @@ void DVRK_Bridge::init(){
     gripper_measured_js_sub = n->subscribe(_namespace + "/gripper/measured_js", 10, &DVRK_Bridge::gripper_measured_js_cb, this);
 
     servo_jp_pub = n->advertise<sensor_msgs::JointState>(_namespace + "/servo_jp", 10);
-    servo_cp_pub  = n->advertise<geometry_msgs::TransformStamped>(_namespace + "/servo_cp", 10);
+    servo_cp_pub  = n->advertise<geometry_msgs::PoseStamped>(_namespace + "/servo_cp", 10);
     state_pub = n->advertise<std_msgs::String>(_namespace + "/set_robot_state", 10);
     servo_cf_pub = n->advertise<geometry_msgs::WrenchStamped>(_namespace + "/body/servo_cf", 10);
     force_orientation_lock_pub = n->advertise<std_msgs::Bool>(_namespace + "/body/set_cf_orientation_absolute", 10);
@@ -123,7 +123,7 @@ void DVRK_Bridge::measured_js_cb(const sensor_msgs::JointStateConstPtr &msg){
     }
 }
 
-void DVRK_Bridge::measured_cp_cb(const geometry_msgs::TransformStampedConstPtr &msg){
+void DVRK_Bridge::measured_cp_cb(const geometry_msgs::PoseStampedConstPtr &msg){
     pre_pose = cur_pose;
     cur_pose = *msg;
     if(poseFcnHandle._is_set){
@@ -195,7 +195,7 @@ void DVRK_Bridge::set_cur_mode(const std::string &state, bool lock_ori){
     _start_pubs = false;
 }
 
-void DVRK_Bridge::servo_cp(const geometry_msgs::TransformStamped &pose){
+void DVRK_Bridge::servo_cp(const geometry_msgs::PoseStamped &pose){
     cmd_pose = pose;
     activeState = DVRK_POSITION_CARTESIAN;
     _start_pubs = true;