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
00031
00032
00033
00034
00035
00036 #include <ros/ros.h>
00037 #include <tf/transform_broadcaster.h>
00038 #include <geometry_msgs/TransformStamped.h>
00039 #include <stdio.h>
00040 #include <math.h>
00041
00042 #include <vrpn_Connection.h>
00043 #include <vrpn_Tracker.h>
00044
00045 #include <LinearMath/btQuaternion.h>
00046
00047 void VRPN_CALLBACK track_target (void *, const vrpn_TRACKERCB t);
00048
00049 class TargetState{
00050 public:
00051 geometry_msgs::TransformStamped target;
00052 };
00053
00054
00055 TargetState *target_state;
00056 std::string frame_id;
00057
00058
00059 bool fresh_data = false;
00060 vrpn_TRACKERCB prev_vrpn_data;
00061
00062
00063 class Rigid_Body {
00064 private:
00065 ros::Publisher target_pub;
00066 tf::TransformBroadcaster br;
00067 vrpn_Connection *connection;
00068 vrpn_Tracker_Remote *tracker;
00069
00070 public:
00071 Rigid_Body(ros::NodeHandle& nh, std::string server_ip,
00072 int port)
00073 {
00074 target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
00075 std::string connec_nm = server_ip + ":" + boost::lexical_cast<std::string>(port);
00076 connection = vrpn_get_connection_by_name(connec_nm.c_str());
00077 std::string target_name = nh.getNamespace().substr(1);
00078 tracker = new vrpn_Tracker_Remote(target_name.c_str(), connection);
00079 this->tracker->register_change_handler(NULL, track_target);
00080 }
00081
00082 void publish_target_state(TargetState *target_state)
00083 {
00084 br.sendTransform(target_state->target);
00085 target_pub.publish(target_state->target);
00086 }
00087
00088 void step_vrpn()
00089 {
00090 this->tracker->mainloop();
00091 this->connection->mainloop();
00092 }
00093 };
00094
00095
00096 void VRPN_CALLBACK track_target (void *, const vrpn_TRACKERCB t)
00097 {
00098 btQuaternion q_orig(t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
00099 btQuaternion q_fix(0.70710678, 0., 0., 0.70710678);
00100
00101
00102
00103
00104
00105 btQuaternion q_rot = q_fix * q_orig * q_fix.inverse();
00106
00107
00108 btVector3 axis = q_rot.getAxis();
00109 btVector3 pos(t.pos[0], -t.pos[2], t.pos[1]);
00110
00111
00112
00113 if ( prev_vrpn_data.quat[0] == t.quat[0] and \
00114 prev_vrpn_data.quat[1] == t.quat[1] and \
00115 prev_vrpn_data.quat[2] == t.quat[2] and \
00116 prev_vrpn_data.quat[3] == t.quat[3] and \
00117 prev_vrpn_data.pos[0] == t.pos[0] and \
00118 prev_vrpn_data.pos[1] == t.pos[1] and \
00119 prev_vrpn_data.pos[2] == t.pos[2] )
00120 ROS_WARN("Repeated Values");
00121
00122 prev_vrpn_data = t;
00123
00124 target_state->target.transform.translation.x = pos.x();
00125 target_state->target.transform.translation.y = pos.y();
00126 target_state->target.transform.translation.z = pos.z();
00127
00128 target_state->target.transform.rotation.x = q_rot.x();
00129 target_state->target.transform.rotation.y = q_rot.y();
00130 target_state->target.transform.rotation.z = q_rot.z();
00131 target_state->target.transform.rotation.w = q_rot.w();
00132
00133 target_state->target.header.frame_id = "optitrak";
00134 target_state->target.child_frame_id = frame_id;
00135 target_state->target.header.stamp = ros::Time::now();
00136
00137 fresh_data = true;
00138 }
00139
00140
00141
00142 int main(int argc, char* argv[])
00143 {
00144 ros::init(argc, argv, "vrpn_tracked_object_1");
00145 ros::NodeHandle nh("~");
00146
00147 target_state = new TargetState;
00148
00149 frame_id = nh.getNamespace();
00150
00151 std::string vrpn_server_ip;
00152 int vrpn_port;
00153 std::string tracked_object_name;
00154
00155 nh.param<std::string>("vrpn_server_ip", vrpn_server_ip, std::string());
00156 nh.param<int>("vrpn_port", vrpn_port, 3883);
00157
00158 std::cout<<"vrpn_server_ip:"<<vrpn_server_ip<<std::endl;
00159 std::cout<<"vrpn_port:"<<vrpn_port<<std::endl;
00160
00161 Rigid_Body tool(nh, vrpn_server_ip, vrpn_port);
00162
00163 ros::Rate loop_rate(1000);
00164
00165 while(ros::ok())
00166 {
00167 tool.step_vrpn();
00168
00169 if (fresh_data == true)
00170 {
00171 tool.publish_target_state(target_state);
00172 fresh_data = false;
00173 }
00174
00175 loop_rate.sleep();
00176 }
00177 return 0;
00178 }
00179
00180
00181
ros_vrpn_client
Author(s): Advait Jain, Chi-Hung King, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:32:10