World.cpp
Go to the documentation of this file.
00001 
00011 // worldlib
00012 #include "worldlib/geometry/Pose.h"
00013 #include "worldlib/world/World.h"
00014 
00015 // ROS
00016 #include <ros/ros.h>
00017 #include <tf2_ros/transform_listener.h>
00018 
00019 using namespace std;
00020 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00021 using namespace rail::spatial_temporal_learning::worldlib::world;
00022 
00023 World::World(const string &fixed_frame_id) : fixed_frame_id_(fixed_frame_id)
00024 {
00025 }
00026 
00027 bool World::loadFromYaml(const std::string &file)
00028 {
00029 // check the YAML version
00030 #ifdef YAMLCPP_GT_0_5_0
00031   try
00032   {
00033     // TF buffer
00034     tf2_ros::Buffer tfs;
00035     const tf2_ros::TransformListener tf_listener(tfs);
00036     // give the buffer time to fill (half second)
00037     usleep(500000);
00038 
00039     // load the config file
00040     const YAML::Node world = YAML::LoadFile(file);
00041     // set the fixed frame
00042     fixed_frame_id_ = world["fixed_frame_id"].as<string>();
00043 
00044     // parse the rooms
00045     const YAML::Node &rooms = world["rooms"];
00046     for (size_t i = 0; i < rooms.size(); i++)
00047     {
00048       const YAML::Node &room = rooms[i];
00049       // get the name, frame ID, dimensions, and aliases
00050       const Object room_object = this->parseObject(room);
00051       // transform to get the Pose
00052       const geometry_msgs::TransformStamped room_tf = tfs.lookupTransform(fixed_frame_id_, room_object.getFrameID(),
00053                                                                           ros::Time(0));
00054       const Pose room_pose(room_tf.transform);
00055       // add the room
00056       rooms_.push_back(Room(room_object.getName(), room_object.getFrameID(), room_pose, room_object.getWidth(),
00057                             room_object.getDepth(), room_object.getHeight()));
00058       Room &cur_room = rooms_.back();
00059 
00060       // parse the surfaces
00061       const YAML::Node &surfaces = room["surfaces"];
00062       for (size_t j = 0; j < surfaces.size(); j++)
00063       {
00064         const YAML::Node &surface = surfaces[j];
00065         // get the name, frame ID, dimensions, and aliases
00066         const Object surface_object = this->parseObject(surface);
00067         // transform to get the Pose
00068         const geometry_msgs::TransformStamped surface_tf = tfs.lookupTransform(room_object.getFrameID(),
00069                                                                                surface_object.getFrameID(),
00070                                                                                ros::Time(0));
00071         const Pose surface_pose(surface_tf.transform);
00072         // add the surface
00073         cur_room.addSurface(Surface(surface_object.getName(), surface_object.getFrameID(), surface_pose,
00074                                     surface_object.getWidth(), surface_object.getDepth(),
00075                                     surface_object.getHeight()));
00076         Surface &cur_surface = cur_room.getSurfaces().back();
00077 
00078         // parse the placement surfaces
00079         const YAML::Node &placement_surfaces = surface["placement_surfaces"];
00080         for (size_t k = 0; k < placement_surfaces.size(); k++)
00081         {
00082           const YAML::Node &ps = placement_surfaces[k];
00083           // get the name, frame ID, dimensions, and aliases
00084           const Object ps_object = this->parseObject(ps);
00085           // parse the nav frame ID
00086           const string ps_nav_frame_id = ps["nav_frame_id"].as<string>();
00087           // transform to get the Pose
00088           const geometry_msgs::TransformStamped ps_tf = tfs.lookupTransform(surface_object.getFrameID(),
00089                                                                             ps_object.getFrameID(),
00090                                                                             ros::Time(0));
00091           const Pose ps_pose(ps_tf.transform);
00092           // add the placement surface
00093           cur_surface.addPlacementSurface(PlacementSurface(ps_object.getName(), ps_object.getFrameID(), ps_nav_frame_id,
00094                                                            ps_pose, ps_object.getWidth(), ps_object.getDepth(),
00095                                                            ps_object.getHeight()));
00096         }
00097         // parse the points of interest
00098         const YAML::Node &pois = surface["pois"];
00099         for (size_t k = 0; k < pois.size(); k++)
00100         {
00101           const YAML::Node &poi = pois[k];
00102           // get the name, frame ID, dimensions, and aliases
00103           const Object poi_object = this->parseObject(poi);
00104           // transform to get the Pose
00105           const geometry_msgs::TransformStamped poi_tf = tfs.lookupTransform(surface_object.getFrameID(),
00106                                                                              poi_object.getFrameID(),
00107                                                                              ros::Time(0));
00108           const Pose poi_pose(poi_tf.transform);
00109           // add the point of interest
00110           cur_surface.addPointOfInterest(PointOfInterest(poi_object.getName(), poi_object.getFrameID(), poi_pose,
00111                                                          poi_object.getWidth(), poi_object.getDepth(),
00112                                                          poi_object.getHeight()));
00113         }
00114       }
00115     }
00116     return true;
00117   } catch (YAML::Exception &e)
00118   {
00119     ROS_ERROR("World Configuration YAML Parser Error: %s", e.msg.c_str());
00120     return false;
00121   } catch (tf2::LookupException &e)
00122   {
00123     ROS_ERROR("World Configuration TF Error: %s", e.what());
00124     return false;
00125   }
00126 #else
00127   ROS_ERROR("Unsupported version of YAML. World configuration file could not be parsed.");
00128   return false;
00129 #endif
00130 }
00131 
00132 #ifdef YAMLCPP_GT_0_5_0
00133 Object World::parseObject(const YAML::Node &object) const
00134 {
00135   // get the name, frame ID, and dimensions
00136   const string name = object["name"].as<string>();
00137   const string frame_id = object["frame_id"].as<string>();
00138   const double width = object["width"].IsDefined() ? object["width"].as<double>() : 0.0;
00139   const double depth = object["depth"].IsDefined() ? object["depth"].as<double>() : 0.0;
00140   const double height = object["height"].IsDefined() ? object["height"].as<double>() : 0.0;
00141 
00142   // create the object
00143   Object o(name, frame_id, Pose(), width, depth, height);
00144 
00145   // check for aliases
00146   const YAML::Node &aliases = object["aliases"];
00147   if (aliases.IsDefined())
00148   {
00149     for (size_t i = 0; i < aliases.size(); i++)
00150     {
00151       o.addAlias(aliases[i].as<string>());
00152     }
00153   }
00154 
00155   return o;
00156 }
00157 #endif
00158 
00159 const string &World::getFixedFrameID() const
00160 {
00161   return fixed_frame_id_;
00162 }
00163 
00164 void World::setFixedFrameID(const string &fixed_frame_id)
00165 {
00166   fixed_frame_id_ = fixed_frame_id;
00167 }
00168 
00169 const vector<Room> &World::getRooms() const
00170 {
00171   return rooms_;
00172 }
00173 
00174 vector<Room> &World::getRooms()
00175 {
00176   return rooms_;
00177 }
00178 
00179 size_t World::getNumRooms() const
00180 {
00181   return rooms_.size();
00182 }
00183 
00184 const Room &World::getRoom(const size_t index) const
00185 {
00186   // check the index value first
00187   if (index < rooms_.size())
00188   {
00189     return rooms_[index];
00190   } else
00191   {
00192     throw out_of_range("World::getRoom : Room index does not exist.");
00193   }
00194 }
00195 
00196 Room &World::getRoom(const size_t index)
00197 {
00198   // check the index value first
00199   if (index < rooms_.size())
00200   {
00201     return rooms_[index];
00202   } else
00203   {
00204     throw out_of_range("World::getRoom : Room index does not exist.");
00205   }
00206 }
00207 
00208 void World::addRoom(const Room &room)
00209 {
00210   rooms_.push_back(room);
00211 }
00212 
00213 void World::removeRoom(const size_t index)
00214 {
00215   // check the index value first
00216   if (index < rooms_.size())
00217   {
00218     rooms_.erase(rooms_.begin() + index);
00219   } else
00220   {
00221     throw out_of_range("World::removeRoom : Room index does not exist.");
00222   }
00223 }
00224 
00225 bool World::roomExists(const string &name) const
00226 {
00227   // check if we can find it and catch any exceptions
00228   try
00229   {
00230     this->findRoom(name);
00231     return true;
00232   } catch (out_of_range &e)
00233   {
00234     return false;
00235   }
00236 }
00237 
00238 const Room &World::findRoom(const string &name) const
00239 {
00240   // check each surface
00241   for (size_t i = 0; i < rooms_.size(); i++)
00242   {
00243     // perform a check
00244     if (rooms_[i].checkName(name))
00245     {
00246       return rooms_[i];
00247     }
00248   }
00249   // no match found
00250   throw out_of_range("World::findRoom : Room name does not exist.");
00251 }
00252 
00253 
00254 Room &World::findRoom(const string &name)
00255 {
00256   // check each surface
00257   for (size_t i = 0; i < rooms_.size(); i++)
00258   {
00259     // perform a check
00260     if (rooms_[i].checkName(name))
00261     {
00262       return rooms_[i];
00263     }
00264   }
00265   // no match found
00266   throw out_of_range("World::findRoom : Room name does not exist.");
00267 }
00268 
00269 const vector<Item> &World::getItems() const
00270 {
00271   return items_;
00272 }
00273 
00274 vector<Item> &World::getItems()
00275 {
00276   return items_;
00277 }
00278 
00279 size_t World::getNumItems() const
00280 {
00281   return items_.size();
00282 }
00283 
00284 const Item &World::getItem(const size_t index) const
00285 {
00286   // check the index value first
00287   if (index < items_.size())
00288   {
00289     return items_[index];
00290   } else
00291   {
00292     throw out_of_range("World::getItem : Item index does not exist.");
00293   }
00294 }
00295 
00296 Item &World::getItem(const size_t index)
00297 {
00298   // check the index value first
00299   if (index < items_.size())
00300   {
00301     return items_[index];
00302   } else
00303   {
00304     throw out_of_range("World::getItem : Item index does not exist.");
00305   }
00306 }
00307 
00308 void World::addItem(const Item &item)
00309 {
00310   items_.push_back(item);
00311 }
00312 
00313 void World::removeItem(const size_t index)
00314 {
00315   // check the index value first
00316   if (index < items_.size())
00317   {
00318     items_.erase(items_.begin() + index);
00319   } else
00320   {
00321     throw out_of_range("World::removeItem : Item index does not exist.");
00322   }
00323 }
00324 
00325 bool World::itemExists(const string &name) const
00326 {
00327   // check if we can find it and catch any exceptions
00328   try
00329   {
00330     this->findItem(name);
00331     return true;
00332   } catch (out_of_range &e)
00333   {
00334     return false;
00335   }
00336 }
00337 
00338 const Item &World::findItem(const string &name) const
00339 {
00340   // check each surface
00341   for (size_t i = 0; i < items_.size(); i++)
00342   {
00343     // perform a check
00344     if (items_[i].checkName(name))
00345     {
00346       return items_[i];
00347     }
00348   }
00349   // no match found
00350   throw out_of_range("World::findItem : Item name does not exist.");
00351 }
00352 
00353 Item &World::findItem(const string &name)
00354 {
00355   // check each surface
00356   for (size_t i = 0; i < items_.size(); i++)
00357   {
00358     // perform a check
00359     if (items_[i].checkName(name))
00360     {
00361       return items_[i];
00362     }
00363   }
00364   // no match found
00365   throw out_of_range("World::findItem : Item name does not exist.");
00366 }
00367 
00368 void World::findClosestSurface(const Position &position, size_t &room_index, size_t &surface_index) const
00369 {
00370   bool found = false;
00371   // check each room
00372   double best = numeric_limits<double>::infinity();
00373   for (size_t i = 0; i < rooms_.size(); i++)
00374   {
00375     const Room &room = rooms_[i];
00376     const tf2::Transform t_room_world = room.getPose().toTF2Transform();
00377     if (room.getNumSurfaces() > 0)
00378     {
00379       // check for the closest surface
00380       for (size_t j = 0; j < room.getNumSurfaces(); j++)
00381       {
00382         // get the surface in the world frame
00383         const Surface &surface = room.getSurface(j);
00384         const tf2::Transform t_surface_room = surface.getPose().toTF2Transform();
00385         const Pose p_surface_world(t_room_world * t_surface_room);
00386         double distance = p_surface_world.getPosition().distance(position);
00387         if (distance < best)
00388         {
00389           best = distance;
00390           room_index = i;
00391           surface_index = j;
00392           found = true;
00393         }
00394       }
00395     }
00396   }
00397 
00398   // check if a valid match was found
00399   if (!found)
00400   {
00401     throw out_of_range("World::findClosestSurface : No surfaces exist.");
00402   }
00403 }
00404 
00405 bool World::findPlacementSurface(const Position &position, size_t &room_index, size_t &surface_index,
00406     size_t &placement_surface_index) const
00407 {
00408   const tf2::Transform t_pose_world = Pose(position).toTF2Transform();
00409   // check each room
00410   for (size_t i = 0; i < rooms_.size(); i++)
00411   {
00412     const Room &room = rooms_[i];
00413     const tf2::Transform t_room_world = room.getPose().toTF2Transform();
00414     if (room.getNumSurfaces() > 0)
00415     {
00416       // check each surface
00417       for (size_t j = 0; j < room.getNumSurfaces(); j++)
00418       {
00419         // get the surface in the world frame
00420         const Surface &surface = room.getSurface(j);
00421         const tf2::Transform t_surface_room = surface.getPose().toTF2Transform();
00422         const tf2::Transform t_surface_world = t_room_world * t_surface_room;
00423 
00424         // get the pose in relation to the surface
00425         const Pose p_pose_surface(t_surface_world.inverseTimes(t_pose_world));
00426 
00427         // first check if we are inside that surface's bounding box
00428         if ((surface.getWidth() / 2.0 >= abs(p_pose_surface.getPosition().getX()))
00429             && (surface.getDepth() / 2.0 >= abs(p_pose_surface.getPosition().getY())))
00430         {
00431           try
00432           {
00433             room_index = i;
00434             surface_index = j;
00435             placement_surface_index = surface.findClosestPlacementSurfaceIndex(p_pose_surface.getPosition());
00436             return true;
00437           } catch (std::out_of_range &e)
00438           {
00439             // means no PlacementSurface found, keep going
00440           }
00441         }
00442       }
00443     }
00444   }
00445 
00446   // nothing found
00447   return false;
00448 }


worldlib
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:36