$search
00001 #include "signal.h" 00002 00003 #include <cstring> 00004 #include <algorithm> 00005 #include <stdexcept> 00006 #include <boost/bind.hpp> 00007 #include <boost/foreach.hpp> 00008 #include <boost/format.hpp> 00009 #include <boost/make_shared.hpp> 00010 #include <boost/scope_exit.hpp> 00011 #include <ros/ros.h> 00012 #include <tf/transform_broadcaster.h> 00013 00014 #include <evart-client.h> 00015 00016 #include "evart.hh" 00017 00018 namespace evart 00019 { 00020 class Evart; 00021 00022 Evart* evart_instance = 0; 00023 00024 void sigintHandler(int) 00025 { 00026 if (evart_instance) 00027 { 00028 delete evart_instance; 00029 evart_instance = 0; 00030 } 00031 ros::requestShutdown(); 00032 } 00033 00034 Evart::Evart() 00035 : nodeHandle_("evart"), 00036 evartHost_(), 00037 evartPort_(), 00038 enableTfBroadcast_ (), 00039 tfReferenceFrameName_ (), 00040 trackSegmentsSrv_ (), 00041 listSegmentsSrv_ (), 00042 transformBroadcaster_ (), 00043 updateRate_ (), 00044 trackAllSegments_ () 00045 { 00046 ros::param::param<std::string>("~hostname", evartHost_, EVAS_STREAM_HOST); 00047 ros::param::param<int>("~port", evartPort_, EVAS_STREAM_PORT); 00048 00049 ros::param::param<bool> 00050 ("~enable_tf_broadcast", enableTfBroadcast_, true); 00051 00052 ros::param::param<std::string> 00053 ("~tf_ref_frame_id", tfReferenceFrameName_, "/mocap_world"); 00054 00055 ros::param::param<double> 00056 ("~update_rate", updateRate_, 100.); 00057 00058 ros::param::param<bool> 00059 ("~track_all_segments", trackAllSegments_, false); 00060 00061 evas_sethost(evartHost_.c_str()); 00062 evas_setport(evartPort_); 00063 00064 typedef boost::function<bool 00065 (evart_bridge::List::Request&, evart_bridge::List::Response&)> listCallback_t; 00066 00067 listCallback_t listCallback = 00068 boost::bind(&Evart::listSegments, this, _1, _2); 00069 listSegmentsSrv_ = nodeHandle_.advertiseService("list_segments", 00070 listCallback); 00071 00072 typedef boost::function< 00073 bool (evart_bridge::TrackSegment::Request&, 00074 evart_bridge::TrackSegment::Response&)> trackSegmentCallback_t; 00075 00076 trackSegmentCallback_t trackSegmentsCallback = 00077 boost::bind(&Evart::trackSegments, this, _1, _2); 00078 trackSegmentsSrv_ = nodeHandle_.advertiseService("track_segments", 00079 trackSegmentsCallback); 00080 } 00081 00082 Evart::~Evart() 00083 {} 00084 00085 bool 00086 Evart::trackSegments(evart_bridge::TrackSegment::Request& req, 00087 evart_bridge::TrackSegment::Response& res) 00088 { 00089 00090 00091 std::string topicName; 00092 { 00093 boost::format fmt("%1%/%2%"); 00094 fmt % req.body_name % req.segment_name; 00095 topicName = fmt.str(); 00096 std::replace (topicName.begin (), topicName.end (), '-', '_'); 00097 } 00098 00099 std::string childFrameName; 00100 { 00101 boost::format fmt("%1%/%2%/%3%"); 00102 fmt % tfReferenceFrameName_ % req.body_name % req.segment_name; 00103 childFrameName = fmt.str(); 00104 std::replace (childFrameName.begin (), childFrameName.end (), '-', '_'); 00105 } 00106 00107 BOOST_FOREACH(const TrackerShPtr& tracker, trackers_) 00108 if (tracker && tracker->childFrameName () == childFrameName) 00109 { 00110 ROS_ERROR_STREAM ("tracker for " 00111 << req.segment_name << ":" << req.body_name 00112 << " already exists."); 00113 res.succeed = false; 00114 return true; 00115 } 00116 boost::optional<tf::TransformBroadcaster&> broadcaster; 00117 if (enableTfBroadcast_) 00118 broadcaster = transformBroadcaster_; 00119 00120 try 00121 { 00122 TrackerShPtr ptr (new Tracker 00123 (nodeHandle_, 00124 req.body_name, 00125 req.segment_name, 00126 topicName, 00127 tfReferenceFrameName_, 00128 childFrameName, 00129 broadcaster)); 00130 trackers_.push_back(ptr); 00131 ROS_INFO_STREAM("start tracking segment " 00132 << req.body_name 00133 << ":" 00134 << req.segment_name); 00135 } 00136 catch (std::exception& e) 00137 { 00138 ROS_ERROR_STREAM (e.what ()); 00139 res.succeed = false; 00140 return true; 00141 } 00142 res.succeed = true; 00143 return true; 00144 } 00145 00146 bool 00147 Evart::listSegments(evart_bridge::List::Request& req, 00148 evart_bridge::List::Response& res) 00149 { 00150 const evas_body_list_t* bodies = evas_body_list(); 00151 if (!bodies) 00152 { 00153 ROS_ERROR("failed to retrieve bodies"); 00154 return false; 00155 } 00156 00157 evart_bridge::Body bodyMsg; 00158 evart_bridge::Segment segmentMsg; 00159 evart_bridge::Dof dofMsg; 00160 00161 for (uint32_t body = 0; body < bodies->nbodies; ++body) 00162 { 00163 const evas_body_segments_list_t* segments = 00164 evas_body_segments_list(body); 00165 const evas_body_markers_list_t* markers = 00166 evas_body_markers_list(body); 00167 const evas_body_dofs_list_t* dofs = 00168 evas_body_dofs_list(body); 00169 00170 if (!segments) 00171 { 00172 ROS_ERROR_STREAM("failed to retrieve segments for body " 00173 << bodies->bodies[body]); 00174 continue; 00175 } 00176 00177 if(!markers) 00178 { 00179 ROS_ERROR_STREAM("failed to retrieve markers for body " 00180 << bodies->bodies[body]); 00181 continue; 00182 } 00183 00184 if(!dofs) 00185 { 00186 ROS_ERROR_STREAM("failed to retrieve dofs for body " 00187 << bodies->bodies[body]); 00188 continue; 00189 } 00190 00191 bodyMsg.name = bodies->bodies[body]; 00192 00193 bodyMsg.segments.clear(); 00194 for(uint32_t segment = 0; segment < segments->nsegments; ++segment) 00195 { 00196 segmentMsg.name = segments->hier[segment].name; 00197 segmentMsg.parent = segments->hier[segment].parent; 00198 bodyMsg.segments.push_back(segmentMsg); 00199 } 00200 00201 bodyMsg.dofs.clear(); 00202 for(uint32_t dof = 0; dof < dofs->ndofs; ++dof) 00203 { 00204 dofMsg.name = dofs->dofs[dof]; 00205 bodyMsg.dofs.push_back(dofMsg); 00206 } 00207 res.bodies.push_back(bodyMsg); 00208 } 00209 return true; 00210 } 00211 00212 void 00213 Evart::trackAllSegments() 00214 { 00215 const evas_body_list_t* bodies = evas_body_list(); 00216 if (!bodies) 00217 { 00218 ROS_ERROR("failed to retrieve bodies"); 00219 return; 00220 } 00221 00222 evart_bridge::TrackSegment::Request req; 00223 evart_bridge::TrackSegment::Response res; 00224 00225 for (uint32_t body = 0; body < bodies->nbodies; ++body) 00226 { 00227 const evas_body_segments_list_t* segments = 00228 evas_body_segments_list(body); 00229 if (!segments) 00230 { 00231 ROS_ERROR_STREAM("failed to retrieve segments for body " 00232 << bodies->bodies[body]); 00233 continue; 00234 } 00235 00236 for(uint32_t segment = 0; segment < segments->nsegments; ++segment) 00237 { 00238 try 00239 { 00240 req.body_name = bodies->bodies[body]; 00241 req.segment_name = segments->hier[segment].name; 00242 trackSegments(req, res); 00243 } 00244 catch (std::exception& e) 00245 { 00246 ROS_WARN_STREAM("failed to track segment " 00247 << req.body_name 00248 << ":" 00249 << req.segment_name 00250 << "\n" 00251 << e.what()); 00252 } 00253 } 00254 } 00255 } 00256 00257 void 00258 Evart::spin() 00259 { 00260 // Unpoll as many messages as possible to avoid receiving 00261 // obsolete message kept in the buffer. 00262 evas_msg_t msg; 00263 memset (&msg, 0, sizeof (evas_msg_t)); 00264 evas_sethandler (0, 0); 00265 while (evas_recv (&msg, 0.001)) 00266 {} 00267 00268 if (trackAllSegments_) 00269 trackAllSegments(); 00270 00271 ros::Rate loopRateTracking(updateRate_); 00272 while (ros::ok ()) 00273 { 00274 while (evas_recv (&msg, 0.001) && ros::ok ()) 00275 { 00276 if (msg.type == EVAS_BODY_SEGMENTS) 00277 { 00278 BOOST_FOREACH (TrackerShPtr tracker, trackers_) 00279 for (unsigned i = 0; i < msg.body_segments.nsegments; ++i) 00280 if (tracker->bodyId () == msg.body_segments.index 00281 && tracker->segmentId () == i) 00282 tracker->callback (msg.body_segments); 00283 } 00284 ros::spinOnce(); 00285 } 00286 ros::spinOnce(); 00287 loopRateTracking.sleep(); 00288 } 00289 } 00290 00291 } // end of namespace evart. 00292 00293 00294 int main(int argc, char **argv) 00295 { 00296 BOOST_SCOPE_EXIT () 00297 { 00298 delete evart::evart_instance; 00299 evart::evart_instance = 0; 00300 } 00301 BOOST_SCOPE_EXIT_END; 00302 00303 ros::init(argc, argv, "evart", ros::init_options::NoSigintHandler); 00304 try 00305 { 00306 signal (SIGINT, evart::sigintHandler); 00307 evart::evart_instance = new evart::Evart (); 00308 if (ros::ok() && evart::evart_instance) 00309 evart::evart_instance->spin(); 00310 } 00311 catch(std::exception& e) 00312 { 00313 ROS_ERROR_STREAM("fatal error: " << e.what()); 00314 return 1; 00315 } 00316 catch(...) 00317 { 00318 ROS_ERROR_STREAM("unexpected error"); 00319 return 2; 00320 } 00321 return 0; 00322 }