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 #include <ros/ros.h>
00022 #include <ros/callback_queue.h>
00023
00024 #include <blackboard/remote.h>
00025 #include <blackboard/interface_listener.h>
00026
00027 #include <interfaces/ZoneInterface.h>
00028 #include <interfaces/Position3DInterface.h>
00029 #include <interfaces/TagVisionInterface.h>
00030 #include <interfaces/RobotinoLightInterface.h>
00031 #include <interfaces/NavGraphWithMPSGeneratorInterface.h>
00032
00033 #include <utils/time/time.h>
00034
00035 #include <rcll_fawkes_sim_msgs/ExplorationZoneInfo.h>
00036 #include <rcll_fawkes_sim_msgs/MPSMarkerArray.h>
00037 #include <rcll_fawkes_sim_msgs/MPSLightState.h>
00038 #include <rcll_fawkes_sim_msgs/NavgraphWithMPSGenerate.h>
00039 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00040
00041 #include <memory>
00042
00043 #define GET_PRIV_PARAM(P) \
00044 { \
00045 if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
00046 ROS_ERROR("%s: Failed to retrieve parameter " #P ", aborting", ros::this_node::getName().c_str()); \
00047 exit(-1); \
00048 } \
00049 }
00050
00051 int
00052 fawkes_to_ros_light_state(const fawkes::RobotinoLightInterface::LightState &state)
00053 {
00054 switch (state) {
00055 case fawkes::RobotinoLightInterface::ON:
00056 return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_ON;
00057 case fawkes::RobotinoLightInterface::OFF:
00058 return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_OFF;
00059 case fawkes::RobotinoLightInterface::BLINKING:
00060 return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_BLINKING;
00061 }
00062 return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_UNKNOWN;
00063 }
00064
00065 fawkes::NavGraphWithMPSGeneratorInterface::Side
00066 ros_to_fawkes_mps_side(int side)
00067 {
00068 if (side == rcll_fawkes_sim_msgs::NavgraphMPSStation::SIDE_INPUT) {
00069 return fawkes::NavGraphWithMPSGeneratorInterface::INPUT;
00070 } else {
00071 return fawkes::NavGraphWithMPSGeneratorInterface::OUTPUT;
00072 }
00073 }
00074
00075 class RcllFawkesSimNode : public fawkes::BlackBoardInterfaceListener
00076 {
00077 public:
00078 RcllFawkesSimNode(ros::NodeHandle rn, std::shared_ptr<fawkes::BlackBoard> bb)
00079 : BlackBoardInterfaceListener("RcllFawkesSimNode"),
00080 rosnode(rn), blackboard_(bb)
00081 {}
00082
00083 void
00084 init()
00085 {
00086 ifc_zone_ = blackboard_->open_for_reading<fawkes::ZoneInterface>("/explore-zone/info");
00087 ifc_tag_vision_ = blackboard_->open_for_reading<fawkes::TagVisionInterface>("/tag-vision/info");
00088 for (size_t i = 0; i < std::min(ifc_tag_vision_->maxlenof_tag_id(), ifcs_tag_pos_.size()); ++i) {
00089 std::string ifc_id = "/tag-vision/" + std::to_string(i);
00090 ifcs_tag_pos_[i] = blackboard_->open_for_reading<fawkes::Position3DInterface>(ifc_id.c_str());
00091
00092
00093 }
00094 ifc_machine_signal_ = blackboard_->open_for_reading<fawkes::RobotinoLightInterface>("/machine-signal/best");
00095 ifc_navgraph_gen_ =
00096 blackboard_->open_for_reading<fawkes::NavGraphWithMPSGeneratorInterface>("/navgraph-generator-mps");
00097 ifc_pose_ = blackboard_->open_for_reading<fawkes::Position3DInterface>("Pose");
00098
00099
00100 bbil_add_data_interface(ifc_zone_);
00101 bbil_add_data_interface(ifc_tag_vision_);
00102 bbil_add_data_interface(ifc_machine_signal_);
00103 bbil_add_data_interface(ifc_pose_);
00104
00105
00106 pub_expl_zone_info_ =
00107 rosnode.advertise<rcll_fawkes_sim_msgs::ExplorationZoneInfo>("rcll_sim/explore_zone_info", 10);
00108 pub_mps_marker_array_ =
00109 rosnode.advertise<rcll_fawkes_sim_msgs::MPSMarkerArray>("rcll_sim/mps_marker_array", 10);
00110 pub_mps_light_state_ =
00111 rosnode.advertise<rcll_fawkes_sim_msgs::MPSLightState>("rcll_sim/mps_light_state", 10);
00112 pub_pose_ =
00113 rosnode.advertise<geometry_msgs::PoseWithCovarianceStamped>("rcll_sim/amcl_pose", 10);
00114
00115 blackboard_->register_listener(this, fawkes::BlackBoard::BBIL_FLAG_DATA);
00116
00117
00118 srv_navgraph_gen_ = rosnode.advertiseService("rcll_sim/navgraph_generate",
00119 &RcllFawkesSimNode::srv_cb_navgraph_gen, this);
00120 }
00121
00122 void
00123 finalize()
00124 {
00125 blackboard_->unregister_listener(this);
00126
00127 blackboard_->close(ifc_zone_);
00128 blackboard_->close(ifc_tag_vision_);
00129 for (const auto &i : ifcs_tag_pos_) {
00130 blackboard_->close(i);
00131 }
00132 blackboard_->close(ifc_machine_signal_);
00133 blackboard_->close(ifc_navgraph_gen_);
00134 blackboard_->close(ifc_pose_);
00135
00136 pub_expl_zone_info_.shutdown();
00137 pub_mps_marker_array_.shutdown();
00138 pub_mps_light_state_.shutdown();
00139 pub_pose_.shutdown();
00140 srv_navgraph_gen_.shutdown();
00141 }
00142
00143 virtual void bb_interface_data_changed(fawkes::Interface *interface) throw()
00144 {
00145 interface->read();
00146 if (strcmp(interface->uid(), ifc_pose_->uid()) == 0) {
00147 const fawkes::Time *ts = ifc_pose_->timestamp();
00148 geometry_msgs::PoseWithCovarianceStamped p;
00149 p.header.frame_id = ifc_pose_->frame();
00150 p.header.stamp.sec = ts->get_sec();
00151 p.header.stamp.nsec = ts->get_nsec();
00152 p.pose.pose.position.x = ifc_pose_->translation(0);
00153 p.pose.pose.position.y = ifc_pose_->translation(1);
00154 p.pose.pose.orientation.x = ifc_pose_->rotation(0);
00155 p.pose.pose.orientation.y = ifc_pose_->rotation(1);
00156 p.pose.pose.orientation.z = ifc_pose_->rotation(2);
00157 p.pose.pose.orientation.w = ifc_pose_->rotation(3);
00158
00159 for (int i = 0; i < ifc_pose_->maxlenof_covariance(); ++i) {
00160 p.pose.covariance[i] = ifc_pose_->covariance(i);
00161 }
00162 pub_pose_.publish(p);
00163
00164 } else if (strcmp(interface->uid(), ifc_zone_->uid()) == 0) {
00165 rcll_fawkes_sim_msgs::ExplorationZoneInfo rezi;
00166 rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_UNKNOWN;
00167 switch (ifc_zone_->search_state()) {
00168 case fawkes::ZoneInterface::NO:
00169 rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_NO;
00170 break;
00171 case fawkes::ZoneInterface::YES:
00172 rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_YES;
00173 break;
00174 case fawkes::ZoneInterface::MAYBE:
00175 rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_MAYBE;
00176 break;
00177 }
00178 rezi.marker_id = ifc_zone_->tag_id();
00179 ROS_DEBUG("%s: Publishing zone info, marker ID %i", ros::this_node::getName().c_str(), ifc_zone_->tag_id());
00180 pub_expl_zone_info_.publish(rezi);
00181 } else if (strcmp(interface->uid(), ifc_tag_vision_->uid()) == 0) {
00182 rcll_fawkes_sim_msgs::MPSMarkerArray rma;
00183 for (size_t i = 0; i < std::min(ifc_tag_vision_->maxlenof_tag_id(), ifcs_tag_pos_.size()); ++i) {
00184 if (ifc_tag_vision_->tag_id(i) > 0) {
00185 ifcs_tag_pos_[i]->read();
00186 const fawkes::Time *ts = ifcs_tag_pos_[i]->timestamp();
00187 rcll_fawkes_sim_msgs::MPSMarker rm;
00188 rm.id = ifc_tag_vision_->tag_id(i);
00189 rm.pose.header.stamp.sec = ts->get_sec();
00190 rm.pose.header.stamp.nsec = ts->get_nsec();
00191 rm.pose.header.frame_id = ifcs_tag_pos_[i]->frame();
00192 rm.pose.name = ifcs_tag_pos_[i]->id();
00193 rm.pose.visibility_history = ifcs_tag_pos_[i]->visibility_history();
00194 rm.pose.pose.position.x = ifcs_tag_pos_[i]->translation(0);
00195 rm.pose.pose.position.y = ifcs_tag_pos_[i]->translation(1);
00196 rm.pose.pose.position.z = ifcs_tag_pos_[i]->translation(2);
00197 rm.pose.pose.orientation.x = ifcs_tag_pos_[i]->rotation(0);
00198 rm.pose.pose.orientation.y = ifcs_tag_pos_[i]->rotation(1);
00199 rm.pose.pose.orientation.z = ifcs_tag_pos_[i]->rotation(2);
00200 rm.pose.pose.orientation.w = ifcs_tag_pos_[i]->rotation(3);
00201 rma.markers.push_back(rm);
00202 }
00203 }
00204 pub_mps_marker_array_.publish(rma);
00205 } else if (strcmp(interface->uid(), ifc_machine_signal_->uid()) == 0) {
00206 rcll_fawkes_sim_msgs::MPSLightState rls;
00207 rls.ready = ifc_machine_signal_->is_ready();
00208 rls.light_state_red = fawkes_to_ros_light_state(ifc_machine_signal_->red());
00209 rls.light_state_yellow = fawkes_to_ros_light_state(ifc_machine_signal_->yellow());
00210 rls.light_state_green = fawkes_to_ros_light_state(ifc_machine_signal_->green());
00211 rls.visibility_history = ifc_machine_signal_->visibility_history();
00212 pub_mps_light_state_.publish(rls);
00213 }
00214
00215 }
00216
00217 bool
00218 srv_cb_navgraph_gen(rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Request &req,
00219 rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Response &res)
00220 {
00221 if (! blackboard_->is_alive()) {
00222 res.ok = false;
00223 res.error_msg = "Blackboard is not connected";
00224 return true;
00225 }
00226
00227 if (! ifc_navgraph_gen_->has_writer()) {
00228 res.ok = false;
00229 res.error_msg = "No writer for navgraph generator with MPS interface";
00230 return true;
00231 }
00232
00233 std::queue<fawkes::Message *> msgs;
00234
00235 ifc_navgraph_gen_->msgq_enqueue(new fawkes::NavGraphWithMPSGeneratorInterface::ClearMessage());
00236
00237 for (size_t i = 0; i < req.mps_stations.size(); ++i) {
00238 const rcll_fawkes_sim_msgs::NavgraphMPSStation &mps = req.mps_stations[i];
00239 fawkes::NavGraphWithMPSGeneratorInterface::UpdateStationByTagMessage *upm =
00240 new fawkes::NavGraphWithMPSGeneratorInterface::UpdateStationByTagMessage();
00241 #ifdef HAVE_OLD_NAVGRAPH_GENMPS_INTERFACE
00242 upm->set_id(mps.name.c_str());
00243 #else
00244 upm->set_name(mps.name.c_str());
00245 #endif
00246 upm->set_side(ros_to_fawkes_mps_side(mps.marker_side));
00247 upm->set_frame(mps.marker_frame.c_str());
00248 upm->set_tag_translation(0, mps.marker_pose.position.x);
00249 upm->set_tag_translation(1, mps.marker_pose.position.y);
00250 upm->set_tag_translation(2, mps.marker_pose.position.z);
00251 upm->set_tag_rotation(0, mps.marker_pose.orientation.x);
00252 upm->set_tag_rotation(1, mps.marker_pose.orientation.y);
00253 upm->set_tag_rotation(2, mps.marker_pose.orientation.z);
00254 upm->set_tag_rotation(3, mps.marker_pose.orientation.w);
00255 msgs.push(upm);
00256 }
00257
00258 {
00259 fawkes::NavGraphWithMPSGeneratorInterface::SetExplorationZonesMessage *sezm =
00260 new fawkes::NavGraphWithMPSGeneratorInterface::SetExplorationZonesMessage();
00261 bool zones[sezm->maxlenof_zones()];
00262 for (int i = 0; i < sezm->maxlenof_zones(); ++i) zones[i] = false;
00263 for (size_t i = 0; i < req.exploration_zones.size(); ++i) {
00264 int zone_idx = req.exploration_zones[i] - 1;
00265 if (zone_idx < 0 || zone_idx > sezm->maxlenof_zones()) {
00266 delete sezm;
00267 res.ok = false;
00268 res.error_msg = "Exploration zone index out of bounds";
00269 return true;
00270 }
00271 zones[zone_idx] = true;
00272 }
00273 sezm->set_zones(zones);
00274 msgs.push(sezm);
00275 }
00276
00277 {
00278 fawkes::NavGraphWithMPSGeneratorInterface::SetWaitZonesMessage *swzm =
00279 new fawkes::NavGraphWithMPSGeneratorInterface::SetWaitZonesMessage();
00280 bool zones[swzm->maxlenof_zones()];
00281 for (int i = 0; i < swzm->maxlenof_zones(); ++i) zones[i] = false;
00282 for (size_t i = 0; i < req.wait_zones.size(); ++i) {
00283 int zone_idx = req.wait_zones[i] - 1;
00284 if (zone_idx < 0 || zone_idx > swzm->maxlenof_zones()) {
00285 delete swzm;
00286 res.ok = false;
00287 res.error_msg = "Wait zone index out of bounds";
00288 return true;
00289 }
00290 zones[zone_idx] = true;
00291 }
00292 swzm->set_zones(zones);
00293 msgs.push(swzm);
00294 }
00295
00296 while (! msgs.empty()) {
00297 fawkes::Message *m = msgs.front();
00298 ifc_navgraph_gen_->msgq_enqueue(m);
00299 msgs.pop();
00300 }
00301
00302 fawkes::NavGraphWithMPSGeneratorInterface::ComputeMessage *compmsg =
00303 new fawkes::NavGraphWithMPSGeneratorInterface::ComputeMessage();
00304 compmsg->ref();
00305 ifc_navgraph_gen_->msgq_enqueue(compmsg);
00306 unsigned int msgid = compmsg->id();
00307 compmsg->unref();
00308 bool finished = false;
00309 while (! finished && ifc_navgraph_gen_->has_writer()) {
00310 ifc_navgraph_gen_->read();
00311 if (ifc_navgraph_gen_->msgid() == msgid && ifc_navgraph_gen_->is_final()) {
00312 res.ok = true;
00313 finished = true;
00314 }
00315 ros::Duration(0.1 ).sleep();
00316 }
00317
00318 if (finished) {
00319 res.ok = true;
00320 } else {
00321 res.ok = false;
00322 res.error_msg = "Navgraph generator with MPS interface writer disappeared while waiting";
00323 }
00324
00325 return true;
00326 }
00327
00328 private:
00329 ros::NodeHandle rosnode;
00330
00331 std::shared_ptr<fawkes::BlackBoard> blackboard_;
00332
00333 fawkes::ZoneInterface *ifc_zone_;
00334 fawkes::TagVisionInterface *ifc_tag_vision_;
00335 std::array<fawkes::Position3DInterface *, 16> ifcs_tag_pos_;
00336 fawkes::RobotinoLightInterface *ifc_machine_signal_;
00337 fawkes::NavGraphWithMPSGeneratorInterface *ifc_navgraph_gen_;
00338 fawkes::Position3DInterface *ifc_pose_;
00339
00340 ros::Publisher pub_expl_zone_info_;
00341 ros::Publisher pub_mps_marker_array_;
00342 ros::Publisher pub_mps_light_state_;
00343 ros::Publisher pub_pose_;
00344 ros::ServiceServer srv_navgraph_gen_;
00345 };
00346
00347
00348 int
00349 main(int argc, char **argv)
00350 {
00351 ros::init(argc, argv, "rcll_fawkes_sim");
00352
00353 ros::NodeHandle n;
00354
00355 std::string cfg_fawkes_host_;
00356 int cfg_fawkes_port_;
00357 std::shared_ptr<fawkes::BlackBoard> blackboard_;
00358 std::shared_ptr<RcllFawkesSimNode> node;
00359
00360 GET_PRIV_PARAM(fawkes_host);
00361 GET_PRIV_PARAM(fawkes_port);
00362
00363 while (ros::ok()) {
00364 if (!blackboard_) {
00365 try {
00366 blackboard_ =
00367 std::make_shared<fawkes::RemoteBlackBoard>(cfg_fawkes_host_.c_str(), cfg_fawkes_port_);
00368 node = std::make_shared<RcllFawkesSimNode>(n, blackboard_);
00369 node->init();
00370 ROS_INFO("%s: Blackboard connected and initialized", ros::this_node::getName().c_str());
00371 } catch (fawkes::Exception &e) {
00372 ROS_WARN_THROTTLE(10, "%s: Initialization failed, retrying", ros::this_node::getName().c_str());
00373 if (node) {
00374 node->finalize();
00375 node.reset();
00376 }
00377 blackboard_.reset();
00378 }
00379 } else if (! blackboard_->is_alive()) {
00380 ROS_WARN_THROTTLE(30, "%s: blackboard connection lost, retrying", ros::this_node::getName().c_str());
00381 if (blackboard_->try_aliveness_restore()) {
00382 ROS_INFO("%s: Blackboard re-connected", ros::this_node::getName().c_str());
00383 }
00384 }
00385
00386 ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
00387 }
00388
00389 if (node) node->finalize();
00390
00391 return 0;
00392 }