00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <humanoid_localization/MapModel.h>
00025
00026 namespace humanoid_localization{
00027 MapModel::MapModel(ros::NodeHandle* nh)
00028 : m_motionMeanZ(0.0),
00029 m_motionRangeZ(-1.0), m_motionRangeRoll(-1.0), m_motionRangePitch(-1.0),
00030 m_motionObstacleDist(0.2)
00031 {
00032
00033
00034 nh->param("motion_mean_z", m_motionMeanZ, m_motionMeanZ);
00035 nh->param("motion_range_z", m_motionRangeZ, m_motionRangeZ);
00036 nh->param("motion_range_roll", m_motionRangeRoll, m_motionRangeRoll);
00037 nh->param("motion_range_pitch", m_motionRangePitch, m_motionRangePitch);
00038
00039
00040
00041 }
00042
00043 MapModel::~MapModel(){
00044
00045 }
00046
00047
00048 boost::shared_ptr<octomap::OcTree> MapModel::getMap() const{
00049 return m_map;
00050 }
00051
00052 void MapModel::verifyPoses(Particles& particles){
00053 double minX, minY, minZ, maxX, maxY, maxZ;
00054 m_map->getMetricMin(minX, minY, minZ);
00055 m_map->getMetricMax(maxX, maxY, maxZ);
00056
00057
00058 double minWeight = std::numeric_limits<double>::max();
00059 for (Particles::iterator it = particles.begin(); it != particles.end(); ++it) {
00060 if (it->weight < minWeight)
00061 minWeight = it->weight;
00062
00063 }
00064
00065 minWeight -= 200;
00066
00067 unsigned numWall = 0;
00068 unsigned numOut = 0;
00069 unsigned numMotion = 0;
00070
00071
00072
00073
00074 #pragma omp parallel for
00075 for (unsigned i = 0; i < particles.size(); ++i){
00076
00077 octomap::point3d position(particles[i].pose.getOrigin().getX(),
00078 particles[i].pose.getOrigin().getY(),
00079 particles[i].pose.getOrigin().getZ());
00080
00081
00082 if (position(0) < minX || position(0) > maxX
00083 || position(1) < minY || position(1) > maxY
00084 || position(2) < minZ || position(2) > maxZ)
00085 {
00086 particles[i].weight = minWeight;
00087 #pragma omp atomic
00088 numOut++;
00089 } else {
00090
00091
00092 if (this->isOccupied(position)){
00093 particles[i].weight = minWeight;
00094 #pragma omp atomic
00095 numWall++;
00096 } else {
00097
00098 if (m_motionRangeZ >= 0.0 &&
00099 (std::abs(particles[i].pose.getOrigin().getZ() - getFloorHeight(particles[i].pose) - m_motionMeanZ)
00100 > m_motionRangeZ))
00101 {
00102 particles[i].weight = minWeight;
00103 #pragma omp atomic
00104 numMotion++;
00105 } else if (m_motionRangePitch >= 0.0 || m_motionRangeRoll >= 0.0){
00106
00107 double yaw, pitch, roll;
00108 particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
00109
00110 if ((m_motionRangePitch >= 0.0 && std::abs(pitch) > m_motionRangePitch)
00111 || (m_motionRangeRoll >= 0.0 && std::abs(roll) > m_motionRangeRoll))
00112 {
00113 particles[i].weight = minWeight;
00114 #pragma omp atomic
00115 numMotion++;
00116 }
00117 }
00118 }
00119 }
00120 }
00121
00122 if (numWall > 0 || numOut > 0 || numMotion > 0){
00123 ROS_INFO("Particle weights minimized: %d out of map, %d in obstacles, %d out of motion range", numOut, numWall, numMotion);
00124 }
00125
00126 if (numOut + numWall >= particles.size()){
00127 ROS_WARN("All particles are out of the valid map area or in obstacles!");
00128 }
00129
00130 }
00131
00132
00133 void MapModel::initGlobal(Particles& particles, double z, double roll, double pitch,
00134 const Vector6d& initNoise,
00135 UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal){
00136 double sizeX,sizeY,sizeZ, minX, minY, minZ;
00137 m_map->getMetricSize(sizeX,sizeY,sizeZ);
00138 m_map->getMetricMin(minX, minY, minZ);
00139
00140 double weight = 1.0 / particles.size();
00141 Particles::iterator it = particles.begin();
00142 while (true){
00143 if (it == particles.end())
00144 break;
00145
00146 double x = minX + sizeX * rngUniform();
00147 double y = minY + sizeY * rngUniform();
00148 std::vector<double> z_list;
00149 getHeightlist(x, y, 0.6,z_list);
00150
00151 for (unsigned zIdx = 0; zIdx < z_list.size(); zIdx++){
00152 if (it == particles.end())
00153 break;
00154
00155
00156
00157
00158
00159
00160 it->pose.getOrigin().setX(x);
00161 it->pose.getOrigin().setY(y);
00162
00163 it->pose.getOrigin().setZ(z_list.at(zIdx) + z + rngNormal() * initNoise(2));
00164 double yaw = rngUniform() * 2 * M_PI -M_PI;
00165 it->pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00166 it->weight = weight;
00167 it++;
00168 }
00169 }
00170
00171 }
00172
00173 void MapModel::getHeightlist(double x, double y, double totalHeight, std::vector<double>& heights){
00174 double minX, minY, minZ, maxX, maxY, maxZ;
00175 m_map->getMetricMin(minX, minY, minZ);
00176 m_map->getMetricMax(maxX, maxY, maxZ);
00177
00178 double res = m_map->getResolution();
00179
00180 double z = maxZ-res/2.0;
00181 double lastZ = z + res;
00182
00183 while (z >= minZ){
00184 if (isOccupied(octomap::point3d(x, y, z))){
00185 if (lastZ - z >= totalHeight + res){
00186 heights.push_back(z+ res/2.0);
00187 }
00188 lastZ = z;
00189 }
00190
00191 z -= res;
00192 }
00193 }
00194
00195 bool MapModel::isOccupied(const octomap::point3d& position) const{
00196 octomap::OcTreeNode* mapNode = m_map->search(position);
00197 if (mapNode)
00198 return isOccupied(mapNode);
00199 else return false;
00200 }
00201
00202
00204
00206
00207 DistanceMap::DistanceMap(ros::NodeHandle* nh)
00208 : MapModel(nh)
00209 {
00210 ROS_ERROR("Distance map implementation is currently not supported");
00211 std::string mapFileName;
00212 nh->getParam("map_file_dist", mapFileName);
00213
00214
00215
00216
00217
00218
00219
00220
00221 octomap::OcTree* tree = dynamic_cast<octomap::OcTree*>(octomap::AbstractOcTree::read(mapFileName));
00222 if (tree){
00223 m_map.reset(tree);
00224 }
00225
00226 if (!m_map|| m_map->size() <= 1){
00227 ROS_ERROR("Distance map file loaded from \"%s\" is erroneous, exiting...", mapFileName.c_str());
00228 exit(-1);
00229 }
00230 double x,y,z;
00231 m_map->getMetricSize(x,y,z);
00232 ROS_INFO("Distance map initialized with %zd nodes (%.2f x %.2f x %.2f m)", m_map->size(), x,y,z);
00233
00234 }
00235
00236 DistanceMap::~DistanceMap(){
00237
00238 }
00239
00240 bool DistanceMap::isOccupied(octomap::OcTreeNode* node) const{
00241 if (std::abs(node->getLogOdds()) < m_map->getResolution())
00242 return true;
00243 else
00244 return false;
00245 }
00246
00247 double DistanceMap::getFloorHeight(const tf::Transform& pose) const{
00248
00249 ROS_ERROR("DistanceMap::getFloorHeight not implemented yet!");
00250
00251 return 0.0;
00252 }
00253
00255
00257
00258
00259 OccupancyMap::OccupancyMap(ros::NodeHandle* nh)
00260 : MapModel(nh)
00261 {
00262 std::string servname = "octomap_binary";
00263 ROS_INFO("Requesting the map from %s...", nh->resolveName(servname).c_str());
00264 octomap_msgs::GetOctomap::Request req;
00265 octomap_msgs::GetOctomap::Response resp;
00266 while(nh->ok() && !ros::service::call(servname, req, resp))
00267 {
00268 ROS_WARN("Request to %s failed; trying again...", nh->resolveName(servname).c_str());
00269 usleep(1000000);
00270 }
00271
00272
00273
00274 #if ROS_VERSION_MINIMUM(1, 9, 0)
00275 m_map.reset(octomap_msgs::binaryMsgToMap(resp.map));
00276 #else // Fuerte:
00277 m_map.reset(octomap_msgs::binaryMsgDataToMap(resp.map.data));
00278 #endif
00279
00280 if (!m_map || m_map->size() <= 1){
00281 ROS_ERROR("Occupancy map is erroneous, exiting...");
00282 exit(-1);
00283 }
00284 double x,y,z;
00285 m_map->getMetricSize(x,y,z);
00286 ROS_INFO("Occupancy map initialized with %zd nodes (%.2f x %.2f x %.2f m), %f m res.", m_map->size(), x,y,z, m_map->getResolution());
00287
00288 }
00289
00290 OccupancyMap::~OccupancyMap(){
00291
00292 }
00293
00294 bool OccupancyMap::isOccupied(octomap::OcTreeNode* node) const{
00295 return m_map->isNodeOccupied(node);
00296 }
00297
00298
00299 double OccupancyMap::getFloorHeight(const tf::Transform& pose)const {
00300 octomap::point3d end;
00301 if (m_map->castRay(octomap::pointTfToOctomap(pose.getOrigin()), octomap::point3d(0.0, 0.0, -1.0), end, false)){
00302
00303 return end.z()+m_map->getResolution()/2.0;
00304 } else {
00305 ROS_WARN("getFloorHeight raycast did not succeed, using 0.0");
00306 return 0.0;
00307 }
00308
00309 }
00310
00311 }
00312