Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include <telekyb_vrpn/VRPNTrackerServer.hpp>
00009
00010 #include <telekyb_base/ROS/ROSModule.hpp>
00011
00012 #include <boost/filesystem.hpp>
00013
00014 namespace TELEKYB_NAMESPACE {
00015
00016 VRPNTrackerServer::VRPNTrackerServer(const std::string& topicName_, vrpn_Connection* c_)
00017 : c(c_), nodeHandle(ROSModule::Instance().getMainNodeHandle()), options(VRPNTrackerServerOptions::Instance())
00018 {
00019 std::string vrpnName = boost::filesystem::path(topicName_).filename().string();
00020
00021 vts = new vrpn_Tracker_Server(vrpnName.c_str(),c);
00022
00023 position[0] = 0.0;
00024 position[1] = 0.0;
00025 position[2] = 0.0;
00026
00027 rotation[0] = 0.0;
00028 rotation[1] = 0.0;
00029 rotation[2] = 0.0;
00030 rotation[3] = 0.0;
00031
00032 ROS_INFO("Set up VRPN Tracker Server with Service %s listening on port %d",
00033 vrpnName.c_str(), vrpn_DEFAULT_LISTEN_PORT_NO);
00034
00035
00036 sub = nodeHandle.subscribe(topicName_, 10, &VRPNTrackerServer::transformStampedCallback, this);
00037 }
00038
00039 VRPNTrackerServer::~VRPNTrackerServer() {
00040 delete vts;
00041 }
00042
00043 void VRPNTrackerServer::setPose(const Position3D& rosPosition, const Quaternion& rosRotation)
00044 {
00045 position[0] = rosPosition(0);
00046 position[1] = rosPosition(1);
00047 position[2] = rosPosition(2);
00048
00049 rotation[0] = rosRotation.x();
00050 rotation[1] = rosRotation.y();
00051 rotation[2] = rosRotation.z();
00052 rotation[3] = rosRotation.w();
00053 }
00054
00055
00056 void VRPNTrackerServer::sendPose()
00057 {
00058
00059
00060
00061
00062
00063
00064 gettimeofday(&packageTime, NULL);
00065 vts->report_pose(0,packageTime,position,rotation);
00066 }
00067
00068 void VRPNTrackerServer::transformStampedCallback(const geometry_msgs::TransformStamped::ConstPtr& msg)
00069 {
00070 Position3D rosPositon(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z);
00071 Quaternion rosRotation(msg->transform.rotation.w, msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z);
00072
00073
00074 rosPositon = options.tViconToNEDMatrix->getValue() * rosPositon;
00075 rosRotation.vec() = options.tViconToNEDMatrix->getValue() * rosRotation.vec();
00076
00077 setPose(rosPositon, rosRotation);
00078 sendPose();
00079 }
00080
00081 void VRPNTrackerServer::spin()
00082 {
00083 vts->mainloop();
00084 c->mainloop();
00085 }
00086
00087 }