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 }