diff --git a/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Arm.h b/ambf_ros_modules/dvrk_arm/include/dvrk_arm/Arm.h index 9e2e23787..afcbe4c8c 100644 --- 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 index 45856a323..589355255 100644 --- 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 poseFcnHandle; + FcnHandle poseFcnHandle; FcnHandle jointFcnHandle; FcnHandle wrenchFcnHandle; FcnHandle gripperFcnHandle; @@ -122,7 +122,7 @@ class DVRK_Bridge: public States, public DVRK_FootPedals{ std::vector 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 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 index 06f848ddc..36c758a30 100644 --- 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 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 index 8be12a70a..af7080f77 100644 --- 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(_namespace + "/servo_jp", 10); - servo_cp_pub = n->advertise(_namespace + "/servo_cp", 10); + servo_cp_pub = n->advertise(_namespace + "/servo_cp", 10); state_pub = n->advertise(_namespace + "/set_robot_state", 10); servo_cf_pub = n->advertise(_namespace + "/body/servo_cf", 10); force_orientation_lock_pub = n->advertise(_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;