Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <string>
00031
00032 #include <ros/ros.h>
00033
00034 #include <geometry_msgs/PoseStamped.h>
00035 #include <std_srvs/Empty.h>
00036 #include <tf/transform_broadcaster.h>
00037 #include <tf/transform_listener.h>
00038
00039 std::string master_root, master_tip;
00040 std::string slave_root, slave_tip;
00041 double scaling = 1.0;
00042
00043 bool tracking = false;
00044 tf::Transform offset;
00045 tf::Transform root_offset;
00046 tf::Transform tip_offset;
00047 tf::Vector3 trans_offset;
00048 tf::Quaternion rot_offset;
00049 tf::TransformListener *g_tf = NULL;
00050
00051 bool engage(std_srvs::Empty::Request &req,
00052 std_srvs::Empty::Response &resp)
00053 {
00054 assert(g_tf);
00055 tracking = false;
00056
00057 tf::StampedTransform master_pose, slave_pose;
00058 g_tf->lookupTransform(master_root, master_tip, ros::Time(0), master_pose);
00059 g_tf->lookupTransform(slave_root, slave_tip, ros::Time(0), slave_pose);
00060
00061 root_offset = slave_pose * master_pose.inverse();
00062 root_offset.setRotation(tf::Quaternion(0,0,0,1));
00063
00064 tip_offset = slave_pose * master_pose.inverse();
00065 tip_offset.setOrigin(tf::Vector3(0,0,0));
00066
00067 offset = slave_pose * master_pose.inverse();
00068
00069 trans_offset = slave_pose.getOrigin() - scaling * master_pose.getOrigin();
00070 rot_offset = slave_pose.getRotation() * master_pose.getRotation().inverse();
00071 rot_offset = master_pose.getRotation().inverse() * slave_pose.getRotation();
00072 rot_offset.setEuler(0,0,0);
00073
00074 ROS_INFO("Engaged");
00075
00076 tracking = true;
00077 return true;
00078 }
00079
00080 bool disengage(std_srvs::Empty::Request &req,
00081 std_srvs::Empty::Response &resp)
00082 {
00083 tracking = false;
00084 ROS_INFO("Disengaged");
00085 return true;
00086 }
00087
00088 int main(int argc, char** argv)
00089 {
00090 ros::init(argc, argv, "teleop_microscribe");
00091
00092 ros::NodeHandle pn("~");
00093 ros::NodeHandle node;
00094
00095 pn.param("master_root", master_root, std::string("XX/r_ms/base_link"));
00096 pn.param("master_tip", master_tip, std::string("XX/r_ms/stylus_tip_link"));
00097 pn.param("slave_root", slave_root, std::string("XX/base_link"));
00098 pn.param("slave_tip", slave_tip, std::string("XX/r_gripper_tool_frame"));
00099 pn.param("scaling", scaling, 1.0);
00100
00101 tf::TransformListener tf;
00102 g_tf = &tf;
00103 tf::TransformBroadcaster tf_broadcaster;
00104
00105 ros::Publisher pub_command = node.advertise<geometry_msgs::PoseStamped>("command", 3);
00106
00107
00108 ros::ServiceServer srv_engage = pn.advertiseService("engage", engage);
00109 ros::ServiceServer srv_disengage = pn.advertiseService("disengage", disengage);
00110
00111
00112 ros::Rate rate(100);
00113 while (ros::ok())
00114 {
00115 ros::spinOnce();
00116 if (tracking)
00117 {
00118 try
00119 {
00120
00121 tf::Stamped<tf::Pose> master_pose;
00122 master_pose.frame_id_ = master_tip;
00123 master_pose.setIdentity();
00124 tf.transformPose(master_root, master_pose, master_pose);
00125
00126
00127
00128
00129
00130 tf::Stamped<tf::Pose> slave_desi;
00131
00132
00133 slave_desi.setOrigin(master_pose.getOrigin() * scaling + trans_offset);
00134
00135
00136 slave_desi.setRotation(master_pose.getRotation() * rot_offset);
00137 slave_desi.frame_id_ = slave_root;
00138 slave_desi.stamp_ = ros::Time::now();
00139
00140
00141 geometry_msgs::PoseStamped slave_desi_msg;
00142 tf::poseStampedTFToMsg(slave_desi, slave_desi_msg);
00143 pub_command.publish(slave_desi_msg);
00144
00145 }
00146 catch(const tf::TransformException &ex)
00147 {
00148 printf("Transform exception: %s\n", ex.what());
00149 }
00150 }
00151
00152
00153 tf::StampedTransform master_in_slave;
00154
00155 master_in_slave.setOrigin(trans_offset);
00156 master_in_slave.setRotation(tf::Quaternion(0,0,0,1));
00157 master_in_slave.stamp_ = ros::Time::now();
00158 master_in_slave.frame_id_ = slave_root;
00159 master_in_slave.child_frame_id_ = master_root;
00160 tf_broadcaster.sendTransform(master_in_slave);
00161
00162 rate.sleep();
00163 }
00164
00165 return 0;
00166 }