00001 00011 // worldlib 00012 #include "worldlib/geometry/Pose.h" 00013 #include "worldlib/model/TaskModel.h" 00014 00015 using namespace std; 00016 using namespace rail::spatial_temporal_learning::worldlib::geometry; 00017 using namespace rail::spatial_temporal_learning::worldlib::model; 00018 using namespace rail::spatial_temporal_learning::worldlib::world; 00019 00020 TaskModel::TaskModel(const uint32_t task_id, const string &name) : name_(name) 00021 { 00022 task_id_ = task_id; 00023 } 00024 00025 uint32_t TaskModel::getTaskID() 00026 { 00027 return task_id_; 00028 } 00029 00030 void TaskModel::setTaskID(const uint32_t task_id) 00031 { 00032 task_id_ = task_id; 00033 } 00034 00035 const string &TaskModel::getName() const 00036 { 00037 return name_; 00038 } 00039 00040 void TaskModel::setName(const std::string &name) 00041 { 00042 name_ = name; 00043 } 00044 00045 const vector<PlacementModel> &TaskModel::getPlacementModels() const 00046 { 00047 return placement_models_; 00048 } 00049 00050 vector<PlacementModel> &TaskModel::getPlacementModels() 00051 { 00052 return placement_models_; 00053 } 00054 00055 size_t TaskModel::getNumPlacementModels() const 00056 { 00057 return placement_models_.size(); 00058 } 00059 00060 const PlacementModel &TaskModel::getPlacementModel(const size_t index) const 00061 { 00062 // check the index value first 00063 if (index < placement_models_.size()) 00064 { 00065 return placement_models_[index]; 00066 } else 00067 { 00068 throw out_of_range("TaskModel::getPlacementModel : PlacementModel index does not exist."); 00069 } 00070 } 00071 00072 PlacementModel &TaskModel::getPlacementModel(const size_t index) 00073 { 00074 // check the index value first 00075 if (index < placement_models_.size()) 00076 { 00077 return placement_models_[index]; 00078 } else 00079 { 00080 throw out_of_range("TaskModel::getPlacementModel : PlacementModel index does not exist."); 00081 } 00082 } 00083 00084 void TaskModel::addPlacementModel(const PlacementModel &room) 00085 { 00086 placement_models_.push_back(room); 00087 } 00088 00089 void TaskModel::removePlacementModel(const size_t index) 00090 { 00091 // check the index value first 00092 if (index < placement_models_.size()) 00093 { 00094 placement_models_.erase(placement_models_.begin() + index); 00095 } else 00096 { 00097 throw out_of_range("TaskModel::removePlacementModel : PlacementModel index does not exist."); 00098 } 00099 } 00100 00101 void TaskModel::getUniqueItems(vector<Item> &items) const 00102 { 00103 // parse out names of items 00104 map<string, Item> unique_items; 00105 Pose zero_pose; 00106 for (size_t i = 0; i < placement_models_.size(); i++) 00107 { 00108 const Item &placement_item = placement_models_[i].getPlacement().getItem(); 00109 const Object &placement_object = placement_models_[i].getPlacement().getObject(); 00110 const Surface &placement_surface = placement_models_[i].getPlacement().getSurface(); 00111 if (!placement_item.getName().empty() && unique_items.find(placement_item.getName()) == unique_items.end()) 00112 { 00113 // add the new item to the list 00114 unique_items[placement_item.getName()] = placement_item; 00115 // don't assign a pose or frame ID 00116 unique_items[placement_item.getName()].setPose(zero_pose); 00117 unique_items[placement_item.getName()].setFrameID(""); 00118 } 00119 } 00120 00121 // now add the unique list to the vector 00122 for (map<string, Item>::iterator iter = unique_items.begin(); iter != unique_items.end(); ++iter) 00123 { 00124 items.push_back(iter->second); 00125 } 00126 } 00127 00128 void TaskModel::getUniqueSurfaces(vector<Surface> &surfaces) const 00129 { 00130 // parse out names of surfaces 00131 map<string, Surface> unique_surfaces; 00132 Pose zero_pose; 00133 for (size_t i = 0; i < placement_models_.size(); i++) 00134 { 00135 const Surface &placement_surface = placement_models_[i].getPlacement().getSurface(); 00136 if (!placement_surface.getName().empty() 00137 && unique_surfaces.find(placement_surface.getName()) == unique_surfaces.end()) 00138 { 00139 // add the new surface to the list 00140 unique_surfaces[placement_surface.getName()] = placement_surface; 00141 // don't assign a pose or frame ID 00142 unique_surfaces[placement_surface.getName()].setPose(zero_pose); 00143 unique_surfaces[placement_surface.getName()].setFrameID(""); 00144 } 00145 } 00146 00147 // now add the unique list to the vector 00148 for (map<string, Surface>::iterator iter = unique_surfaces.begin(); iter != unique_surfaces.end(); ++iter) 00149 { 00150 surfaces.push_back(iter->second); 00151 } 00152 }