Object.cpp
Go to the documentation of this file.
00001 
00013 // worldlib
00014 #include "worldlib/world/Object.h"
00015 
00016 // Boost
00017 #include <boost/algorithm/string.hpp>
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 Object::Object(const string &name, const string &frame_id, const Pose &pose, const double width, const double depth,
00024     const double height) : name_(name), frame_id_(frame_id), pose_(pose)
00025 {
00026   width_ = width;
00027   depth_ = depth;
00028   height_ = height;
00029 }
00030 
00031 const string &Object::getName() const
00032 {
00033   return name_;
00034 }
00035 
00036 void Object::setName(const string &name)
00037 {
00038   name_ = name;
00039 }
00040 
00041 const string &Object::getFrameID() const
00042 {
00043   return frame_id_;
00044 }
00045 
00046 void Object::setFrameID(const string &frame_id)
00047 {
00048   frame_id_ = frame_id;
00049 }
00050 
00051 const Pose &Object::getPose() const
00052 {
00053   return pose_;
00054 }
00055 
00056 Pose &Object::getPose()
00057 {
00058   return pose_;
00059 }
00060 
00061 void Object::setPose(Pose &pose)
00062 {
00063   pose_ = pose;
00064 }
00065 
00066 double Object::getWidth() const
00067 {
00068   return width_;
00069 }
00070 
00071 void Object::setWidth(const double width)
00072 {
00073   width_ = width;
00074 }
00075 
00076 double Object::getDepth() const
00077 {
00078   return depth_;
00079 }
00080 
00081 void Object::setDepth(const double depth)
00082 {
00083   depth_ = depth;
00084 }
00085 
00086 double Object::getHeight() const
00087 {
00088   return height_;
00089 }
00090 
00091 void Object::setHeight(const double height)
00092 {
00093   height_ = height;
00094 }
00095 
00096 const vector<string> &Object::getAliases() const
00097 {
00098   return aliases_;
00099 }
00100 
00101 size_t Object::getNumAliases() const
00102 {
00103   return aliases_.size();
00104 }
00105 
00106 const string &Object::getAlias(const size_t index) const
00107 {
00108   // check the index value first
00109   if (index < aliases_.size())
00110   {
00111     return aliases_[index];
00112   } else
00113   {
00114     throw out_of_range("Object::getAlias : Alias index does not exist.");
00115   }
00116 }
00117 
00118 void Object::addAlias(const string &alias)
00119 {
00120   aliases_.push_back(alias);
00121 }
00122 
00123 void Object::removeAlias(const size_t index)
00124 {
00125   // check the index value first
00126   if (index < aliases_.size())
00127   {
00128     aliases_.erase(aliases_.begin() + index);
00129   } else
00130   {
00131     throw out_of_range("Object::removeAlias : Alias index does not exist.");
00132   }
00133 }
00134 
00135 bool Object::checkName(const string &name) const
00136 {
00137   string name_uc = boost::to_upper_copy(name);
00138 
00139   // check the basic name
00140   if (boost::to_upper_copy(name_) == name_uc)
00141   {
00142     return true;
00143   } else
00144   {
00145     // check the aliases
00146     for (size_t i = 0; i < aliases_.size(); i++)
00147     {
00148       if (boost::to_upper_copy(aliases_[i]) == name_uc)
00149       {
00150         return true;
00151       }
00152     }
00153   }
00154 
00155   // no match found
00156   return false;
00157 }
00158 
00159 Position Object::fromParentFrame(const Position &position) const
00160 {
00161   // create a Pose and use the Pose method
00162   Pose p1(position);
00163   Pose p2 = this->fromParentFrame(p1);
00164   return p2.getPosition();
00165 }
00166 
00167 Pose Object::fromParentFrame(const Pose &pose) const
00168 {
00169   // use TF2
00170   tf2::Transform t_pose_parent = pose.toTF2Transform();
00171   tf2::Transform t_object_parent = pose_.toTF2Transform();
00172   tf2::Transform t_pose_object = t_object_parent.inverseTimes(t_pose_parent);
00173   return Pose(t_pose_object);
00174 }


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