00001
00032 #include "vrpn_client_ros/vrpn_client_ros.h"
00033
00034 #include "tf2/LinearMath/Quaternion.h"
00035 #include "tf2/LinearMath/Matrix3x3.h"
00036 #include "tf2_ros/transform_broadcaster.h"
00037
00038 #include <vector>
00039 #include <unordered_set>
00040 #include <algorithm>
00041
00042 namespace
00043 {
00044 std::unordered_set<std::string> name_blacklist_({"VRPN Control"});
00045 }
00046
00047 namespace vrpn_client_ros
00048 {
00049
00053 bool isInvalidFirstCharInName(const char c)
00054 {
00055 return ! ( isalpha(c) || c == '/' || c == '~' );
00056 }
00057
00058 bool isInvalidSubsequentCharInName(const char c)
00059 {
00060 return ! ( isalnum(c) || c == '/' || c == '_' );
00061 }
00062
00063 VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, ros::NodeHandle nh)
00064 {
00065 tracker_remote_ = std::make_shared<vrpn_Tracker_Remote>(tracker_name.c_str(), connection.get());
00066
00067 std::string clean_name = tracker_name;
00068
00069 if (clean_name.size() > 0)
00070 {
00071 int start_subsequent = 1;
00072 if (isInvalidFirstCharInName(clean_name[0]))
00073 {
00074 clean_name = clean_name.substr(1);
00075 start_subsequent = 0;
00076 }
00077
00078 clean_name.erase( std::remove_if( clean_name.begin() + start_subsequent, clean_name.end(), isInvalidSubsequentCharInName ), clean_name.end() );
00079 }
00080
00081 init(clean_name, nh, false);
00082 }
00083
00084 VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, std::string host, ros::NodeHandle nh)
00085 {
00086 std::string tracker_address;
00087 tracker_address = tracker_name + "@" + host;
00088 tracker_remote_ = std::make_shared<vrpn_Tracker_Remote>(tracker_address.c_str());
00089 init(tracker_name, nh, true);
00090 }
00091
00092 void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer)
00093 {
00094 ROS_INFO_STREAM("Creating new tracker " << tracker_name);
00095
00096 tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose);
00097 tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist);
00098 tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel);
00099 tracker_remote_->shutup = true;
00100
00101 std::string error;
00102 if (!ros::names::validate(tracker_name, error))
00103 {
00104 ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error);
00105 return;
00106 }
00107
00108 this->tracker_name = tracker_name;
00109
00110 output_nh_ = ros::NodeHandle(nh, tracker_name);
00111
00112 std::string frame_id;
00113 nh.param<std::string>("frame_id", frame_id, "world");
00114 nh.param<bool>("use_server_time", use_server_time_, false);
00115 nh.param<bool>("broadcast_tf", broadcast_tf_, false);
00116 nh.param<bool>("process_sensor_id", process_sensor_id_, false);
00117
00118 pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id;
00119
00120 if (create_mainloop_timer)
00121 {
00122 double update_frequency;
00123 nh.param<double>("update_frequency", update_frequency, 100.0);
00124 mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency),
00125 boost::bind(&VrpnTrackerRos::mainloop, this));
00126 }
00127 }
00128
00129 VrpnTrackerRos::~VrpnTrackerRos()
00130 {
00131 ROS_INFO_STREAM("Destroying tracker " << transform_stamped_.child_frame_id);
00132 tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_pose);
00133 tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_twist);
00134 tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_accel);
00135 }
00136
00137 void VrpnTrackerRos::mainloop()
00138 {
00139 tracker_remote_->mainloop();
00140 }
00141
00142 void VRPN_CALLBACK VrpnTrackerRos::handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose)
00143 {
00144 VrpnTrackerRos *tracker = static_cast<VrpnTrackerRos *>(userData);
00145
00146 ros::Publisher *pose_pub;
00147 std::size_t sensor_index(0);
00148 ros::NodeHandle nh = tracker->output_nh_;
00149
00150 if (tracker->process_sensor_id_)
00151 {
00152 sensor_index = static_cast<std::size_t>(tracker_pose.sensor);
00153 nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_pose.sensor));
00154 }
00155
00156 if (tracker->pose_pubs_.size() <= sensor_index)
00157 {
00158 tracker->pose_pubs_.resize(sensor_index + 1);
00159 }
00160 pose_pub = &(tracker->pose_pubs_[sensor_index]);
00161
00162 if (pose_pub->getTopic().empty())
00163 {
00164 *pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 1);
00165 }
00166
00167 if (pose_pub->getNumSubscribers() > 0)
00168 {
00169 if (tracker->use_server_time_)
00170 {
00171 tracker->pose_msg_.header.stamp.sec = tracker_pose.msg_time.tv_sec;
00172 tracker->pose_msg_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000;
00173 }
00174 else
00175 {
00176 tracker->pose_msg_.header.stamp = ros::Time::now();
00177 }
00178
00179 tracker->pose_msg_.pose.position.x = tracker_pose.pos[0];
00180 tracker->pose_msg_.pose.position.y = tracker_pose.pos[1];
00181 tracker->pose_msg_.pose.position.z = tracker_pose.pos[2];
00182
00183 tracker->pose_msg_.pose.orientation.x = tracker_pose.quat[0];
00184 tracker->pose_msg_.pose.orientation.y = tracker_pose.quat[1];
00185 tracker->pose_msg_.pose.orientation.z = tracker_pose.quat[2];
00186 tracker->pose_msg_.pose.orientation.w = tracker_pose.quat[3];
00187
00188 pose_pub->publish(tracker->pose_msg_);
00189 }
00190
00191 if (tracker->broadcast_tf_)
00192 {
00193 static tf2_ros::TransformBroadcaster tf_broadcaster;
00194
00195 if (tracker->use_server_time_)
00196 {
00197 tracker->transform_stamped_.header.stamp.sec = tracker_pose.msg_time.tv_sec;
00198 tracker->transform_stamped_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000;
00199 }
00200 else
00201 {
00202 tracker->transform_stamped_.header.stamp = ros::Time::now();
00203 }
00204
00205 if (tracker->process_sensor_id_)
00206 {
00207 tracker->transform_stamped_.child_frame_id = tracker->tracker_name + "/" + std::to_string(tracker_pose.sensor);
00208 }
00209 else
00210 {
00211 tracker->transform_stamped_.child_frame_id = tracker->tracker_name;
00212 }
00213
00214 tracker->transform_stamped_.transform.translation.x = tracker_pose.pos[0];
00215 tracker->transform_stamped_.transform.translation.y = tracker_pose.pos[1];
00216 tracker->transform_stamped_.transform.translation.z = tracker_pose.pos[2];
00217
00218 tracker->transform_stamped_.transform.rotation.x = tracker_pose.quat[0];
00219 tracker->transform_stamped_.transform.rotation.y = tracker_pose.quat[1];
00220 tracker->transform_stamped_.transform.rotation.z = tracker_pose.quat[2];
00221 tracker->transform_stamped_.transform.rotation.w = tracker_pose.quat[3];
00222
00223 tf_broadcaster.sendTransform(tracker->transform_stamped_);
00224 }
00225 }
00226
00227 void VRPN_CALLBACK VrpnTrackerRos::handle_twist(void *userData, const vrpn_TRACKERVELCB tracker_twist)
00228 {
00229 VrpnTrackerRos *tracker = static_cast<VrpnTrackerRos *>(userData);
00230
00231 ros::Publisher *twist_pub;
00232 std::size_t sensor_index(0);
00233 ros::NodeHandle nh = tracker->output_nh_;
00234
00235 if (tracker->process_sensor_id_)
00236 {
00237 sensor_index = static_cast<std::size_t>(tracker_twist.sensor);
00238 nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_twist.sensor));
00239 }
00240
00241 if (tracker->twist_pubs_.size() <= sensor_index)
00242 {
00243 tracker->twist_pubs_.resize(sensor_index + 1);
00244 }
00245 twist_pub = &(tracker->twist_pubs_[sensor_index]);
00246
00247 if (twist_pub->getTopic().empty())
00248 {
00249 *twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist", 1);
00250 }
00251
00252 if (twist_pub->getNumSubscribers() > 0)
00253 {
00254 if (tracker->use_server_time_)
00255 {
00256 tracker->twist_msg_.header.stamp.sec = tracker_twist.msg_time.tv_sec;
00257 tracker->twist_msg_.header.stamp.nsec = tracker_twist.msg_time.tv_usec * 1000;
00258 }
00259 else
00260 {
00261 tracker->twist_msg_.header.stamp = ros::Time::now();
00262 }
00263
00264 tracker->twist_msg_.twist.linear.x = tracker_twist.vel[0];
00265 tracker->twist_msg_.twist.linear.y = tracker_twist.vel[1];
00266 tracker->twist_msg_.twist.linear.z = tracker_twist.vel[2];
00267
00268 double roll, pitch, yaw;
00269 tf2::Matrix3x3 rot_mat(
00270 tf2::Quaternion(tracker_twist.vel_quat[0], tracker_twist.vel_quat[1], tracker_twist.vel_quat[2],
00271 tracker_twist.vel_quat[3]));
00272 rot_mat.getRPY(roll, pitch, yaw);
00273
00274 tracker->twist_msg_.twist.angular.x = roll;
00275 tracker->twist_msg_.twist.angular.y = pitch;
00276 tracker->twist_msg_.twist.angular.z = yaw;
00277
00278 twist_pub->publish(tracker->twist_msg_);
00279 }
00280 }
00281
00282 void VRPN_CALLBACK VrpnTrackerRos::handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel)
00283 {
00284 VrpnTrackerRos *tracker = static_cast<VrpnTrackerRos *>(userData);
00285
00286 ros::Publisher *accel_pub;
00287 std::size_t sensor_index(0);
00288 ros::NodeHandle nh = tracker->output_nh_;
00289
00290 if (tracker->process_sensor_id_)
00291 {
00292 sensor_index = static_cast<std::size_t>(tracker_accel.sensor);
00293 nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_accel.sensor));
00294 }
00295
00296 if (tracker->accel_pubs_.size() <= sensor_index)
00297 {
00298 tracker->accel_pubs_.resize(sensor_index + 1);
00299 }
00300 accel_pub = &(tracker->accel_pubs_[sensor_index]);
00301
00302 if (accel_pub->getTopic().empty())
00303 {
00304 *accel_pub = nh.advertise<geometry_msgs::TwistStamped>("accel", 1);
00305 }
00306
00307 if (accel_pub->getNumSubscribers() > 0)
00308 {
00309 if (tracker->use_server_time_)
00310 {
00311 tracker->accel_msg_.header.stamp.sec = tracker_accel.msg_time.tv_sec;
00312 tracker->accel_msg_.header.stamp.nsec = tracker_accel.msg_time.tv_usec * 1000;
00313 }
00314 else
00315 {
00316 tracker->accel_msg_.header.stamp = ros::Time::now();
00317 }
00318
00319 tracker->accel_msg_.accel.linear.x = tracker_accel.acc[0];
00320 tracker->accel_msg_.accel.linear.y = tracker_accel.acc[1];
00321 tracker->accel_msg_.accel.linear.z = tracker_accel.acc[2];
00322
00323 double roll, pitch, yaw;
00324 tf2::Matrix3x3 rot_mat(
00325 tf2::Quaternion(tracker_accel.acc_quat[0], tracker_accel.acc_quat[1], tracker_accel.acc_quat[2],
00326 tracker_accel.acc_quat[3]));
00327 rot_mat.getRPY(roll, pitch, yaw);
00328
00329 tracker->accel_msg_.accel.angular.x = roll;
00330 tracker->accel_msg_.accel.angular.y = pitch;
00331 tracker->accel_msg_.accel.angular.z = yaw;
00332
00333 accel_pub->publish(tracker->accel_msg_);
00334 }
00335 }
00336
00337 VrpnClientRos::VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh)
00338 {
00339 output_nh_ = private_nh;
00340
00341 host_ = getHostStringFromParams(private_nh);
00342
00343 ROS_INFO_STREAM("Connecting to VRPN server at " << host_);
00344 connection_ = std::shared_ptr<vrpn_Connection>(vrpn_get_connection_by_name(host_.c_str()));
00345 ROS_INFO("Connection established");
00346
00347 double update_frequency;
00348 private_nh.param<double>("update_frequency", update_frequency, 100.0);
00349 mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), boost::bind(&VrpnClientRos::mainloop, this));
00350
00351 double refresh_tracker_frequency;
00352 private_nh.param<double>("refresh_tracker_frequency", refresh_tracker_frequency, 0.0);
00353
00354 if (refresh_tracker_frequency > 0.0)
00355 {
00356 refresh_tracker_timer_ = nh.createTimer(ros::Duration(1 / refresh_tracker_frequency),
00357 boost::bind(&VrpnClientRos::updateTrackers, this));
00358 }
00359
00360 std::vector<std::string> param_tracker_names_;
00361 if (private_nh.getParam("trackers", param_tracker_names_))
00362 {
00363 for (std::vector<std::string>::iterator it = param_tracker_names_.begin();
00364 it != param_tracker_names_.end(); ++it)
00365 {
00366 trackers_.insert(std::make_pair(*it, std::make_shared<VrpnTrackerRos>(*it, connection_, output_nh_)));
00367 }
00368 }
00369 }
00370
00371 std::string VrpnClientRos::getHostStringFromParams(ros::NodeHandle host_nh)
00372 {
00373 std::stringstream host_stream;
00374 std::string server;
00375 int port;
00376
00377 host_nh.param<std::string>("server", server, "localhost");
00378 host_stream << server;
00379
00380 if (host_nh.getParam("port", port))
00381 {
00382 host_stream << ":" << port;
00383 }
00384 return host_stream.str();
00385 }
00386
00387 void VrpnClientRos::mainloop()
00388 {
00389 connection_->mainloop();
00390 if (!connection_->doing_okay())
00391 {
00392 ROS_WARN("VRPN connection is not 'doing okay'");
00393 }
00394 for (TrackerMap::iterator it = trackers_.begin(); it != trackers_.end(); ++it)
00395 {
00396 it->second->mainloop();
00397 }
00398 }
00399
00400 void VrpnClientRos::updateTrackers()
00401 {
00402 int i = 0;
00403 while (connection_->sender_name(i) != NULL)
00404 {
00405 if (trackers_.count(connection_->sender_name(i)) == 0 && name_blacklist_.count(connection_->sender_name(i)) == 0)
00406 {
00407 ROS_INFO_STREAM("Found new sender: " << connection_->sender_name(i));
00408 trackers_.insert(std::make_pair(connection_->sender_name(i),
00409 std::make_shared<VrpnTrackerRos>(connection_->sender_name(i), connection_,
00410 output_nh_)));
00411 }
00412 i++;
00413 }
00414 }
00415 }