OctomapServerMultilayer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011-2013, A. Hornung, M. Philips
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the University of Freiburg nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <octomap_server/OctomapServerMultilayer.h>
00031 
00032 using namespace octomap;
00033 
00034 namespace octomap_server{
00035 
00036 
00037 
00038 OctomapServerMultilayer::OctomapServerMultilayer(ros::NodeHandle private_nh_)
00039 : OctomapServer(private_nh_)
00040 {
00041 
00042   // TODO: callback for arm_navigation attached objects was removed, is
00043   // there a replacement functionality?
00044 
00045   // TODO: param maps, limits
00046   // right now 0: base, 1: spine, 2: arms
00047   ProjectedMap m;
00048   m.name = "projected_base_map";
00049   m.minZ = 0.0;
00050   m.maxZ = 0.3;
00051   m.z = 0.0;
00052   m_multiGridmap.push_back(m);
00053 
00054   m.name = "projected_spine_map";
00055   m.minZ = 0.25;
00056   m.maxZ = 1.4;
00057   m.z = 0.6;
00058   m_multiGridmap.push_back(m);
00059 
00060   m.name = "projected_arm_map";
00061   m.minZ = 0.7;
00062   m.maxZ = 0.9;
00063   m.z = 0.8;
00064   m_multiGridmap.push_back(m);
00065 
00066 
00067   for (unsigned i = 0; i < m_multiGridmap.size(); ++i){
00068     ros::Publisher* pub = new ros::Publisher(m_nh.advertise<nav_msgs::OccupancyGrid>(m_multiGridmap.at(i).name, 5, m_latchedTopics));
00069     m_multiMapPub.push_back(pub);
00070   }
00071 
00072   // init arm links (could be params as well)
00073   m_armLinks.push_back("l_elbow_flex_link");
00074   m_armLinkOffsets.push_back(0.10);
00075   m_armLinks.push_back("l_gripper_l_finger_tip_link");
00076   m_armLinkOffsets.push_back(0.03);
00077   m_armLinks.push_back("l_gripper_r_finger_tip_link");
00078   m_armLinkOffsets.push_back(0.03);
00079   m_armLinks.push_back("l_upper_arm_roll_link");
00080   m_armLinkOffsets.push_back(0.16);
00081   m_armLinks.push_back("l_wrist_flex_link");
00082   m_armLinkOffsets.push_back(0.05);
00083   m_armLinks.push_back("r_elbow_flex_link");
00084   m_armLinkOffsets.push_back(0.10);
00085   m_armLinks.push_back("r_gripper_l_finger_tip_link");
00086   m_armLinkOffsets.push_back(0.03);
00087   m_armLinks.push_back("r_gripper_r_finger_tip_link");
00088   m_armLinkOffsets.push_back(0.03);
00089   m_armLinks.push_back("r_upper_arm_roll_link");
00090   m_armLinkOffsets.push_back(0.16);
00091   m_armLinks.push_back("r_wrist_flex_link");
00092   m_armLinkOffsets.push_back(0.05);
00093 
00094 
00095 }
00096 
00097 OctomapServerMultilayer::~OctomapServerMultilayer(){
00098   for (unsigned i = 0; i < m_multiMapPub.size(); ++i){
00099     delete m_multiMapPub[i];
00100   }
00101 
00102 }
00103 
00104 void OctomapServerMultilayer::handlePreNodeTraversal(const ros::Time& rostime){
00105   // multilayer server always publishes 2D maps:
00106   m_publish2DMap = true;
00107   nav_msgs::MapMetaData gridmapInfo = m_gridmap.info;
00108 
00109   OctomapServer::handlePreNodeTraversal(rostime);
00110 
00111 
00112   // recalculate height of arm layer (stub, TODO)
00113   geometry_msgs::PointStamped vin;
00114   vin.point.x = 0;
00115   vin.point.y = 0;
00116   vin.point.z = 0;
00117   vin.header.stamp = rostime;
00118   double link_padding = 0.03;
00119 
00120   double minArmHeight = 2.0;
00121   double maxArmHeight = 0.0;
00122 
00123   for (unsigned i = 0; i < m_armLinks.size(); ++i){
00124     vin.header.frame_id = m_armLinks[i];
00125     geometry_msgs::PointStamped vout;
00126     const bool found_trans =
00127         m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i),
00128                                       ros::Time(0), ros::Duration(1.0));
00129     ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s",
00130                    m_armLinks[i].c_str());
00131     m_tfListener.transformPoint("base_footprint",vin,vout);
00132     maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding));
00133     minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding));
00134   }
00135   ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight);
00136   m_multiGridmap.at(2).minZ = minArmHeight;
00137   m_multiGridmap.at(2).maxZ = maxArmHeight;
00138   m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0;
00139 
00140 
00141 
00142 
00143 
00144   // TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)?
00145 
00146   bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info);
00147 
00148   for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){
00149     it->map.header = m_gridmap.header;
00150     it->map.info = m_gridmap.info;
00151     it->map.info.origin.position.z = it->z;
00152     if (m_projectCompleteMap){
00153       ROS_INFO("Map resolution changed, rebuilding complete 2D maps");
00154       it->map.data.clear();
00155       // init to unknown:
00156       it->map.data.resize(it->map.info.width * it->map.info.height, -1);
00157     } else if (mapInfoChanged){
00158       adjustMapData(it->map, gridmapInfo);
00159     }
00160   }
00161 }
00162 
00163 void OctomapServerMultilayer::handlePostNodeTraversal(const ros::Time& rostime){
00164 
00165   // TODO: calc tall / short obs. cells for arm layer, => temp arm layer
00166 //  std::vector<int> shortObsCells;
00167 //  for(unsigned int i=0; i<arm_map.data.size(); i++){
00168 //    if(temp_arm_map.data[i] == 0){
00169 //      if(map.data[i] == -1)
00170 //        arm_map.data[i] = -1;
00171 //    }
00172 //    else if(arm_map.data[i] == 0)
00173 //      arm_map.data[i] = 0;
00174 //    else if(double(arm_map.data[i])/temp_arm_map.data[i] > 0.8)
00175 //      arm_map.data[i] = 101;
00176 //    else{
00177 //      arm_map.data[i] = 100;
00178 //      shortObsCells.push_back(i);
00179 //    }
00180 //  }
00181 //
00182 //  std::vector<int> tallObsCells;
00183 //  tallObsCells.reserve(shortObsCells.size());
00184 //  int dxy[8] = {-arm_map.info.width-1, -arm_map.info.width, -arm_map.info.width+1, -1, 1, arm_map.info.width-1, arm_map.info.width, arm_map.info.width+1};
00185 //  for(unsigned int i=0; i<shortObsCells.size(); i++){
00186 //    for(int j=0; j<8; j++){
00187 //      int temp = shortObsCells[i]+dxy[j];
00188 //      if(temp<0 || temp>=arm_map.data.size())
00189 //        continue;
00190 //      if(arm_map.data[temp]==101){
00191 //        tallObsCells.push_back(shortObsCells[i]);
00192 //        break;
00193 //      }
00194 //    }
00195 //  }
00196 //  for(unsigned int i=0; i<tallObsCells.size(); i++)
00197 //    arm_map.data[tallObsCells[i]] = 101;
00198 
00199 
00200 
00201   OctomapServer::handlePostNodeTraversal(rostime);
00202 
00203   for (unsigned i = 0; i < m_multiMapPub.size(); ++i){
00204     m_multiMapPub[i]->publish(m_multiGridmap.at(i).map);
00205   }
00206 
00207 }
00208 void OctomapServerMultilayer::update2DMap(const OcTreeT::iterator& it, bool occupied){
00209   double z = it.getZ();
00210   double s2 = it.getSize()/2.0;
00211 
00212   // create a mask on which maps to update:
00213   std::vector<bool> inMapLevel(m_multiGridmap.size(), false);
00214   for (unsigned i = 0; i < m_multiGridmap.size(); ++i){
00215     if (z+s2 >= m_multiGridmap[i].minZ && z-s2 <= m_multiGridmap[i].maxZ){
00216       inMapLevel[i] = true;
00217     }
00218   }
00219 
00220   if (it.getDepth() == m_maxTreeDepth){
00221     unsigned idx = mapIdx(it.getKey());
00222     if (occupied)
00223       m_gridmap.data[idx] = 100;
00224     else if (m_gridmap.data[idx] == -1){
00225       m_gridmap.data[idx] = 0;
00226     }
00227 
00228     for (unsigned i = 0; i < inMapLevel.size(); ++i){
00229       if (inMapLevel[i]){
00230         if (occupied)
00231           m_multiGridmap[i].map.data[idx] = 100;
00232         else if (m_multiGridmap[i].map.data[idx] == -1)
00233           m_multiGridmap[i].map.data[idx] = 0;
00234       }
00235     }
00236 
00237   } else {
00238     int intSize = 1 << (m_treeDepth - it.getDepth());
00239     octomap::OcTreeKey minKey=it.getIndexKey();
00240     for(int dx=0; dx < intSize; dx++){
00241       int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale;
00242       for(int dy=0; dy < intSize; dy++){
00243         unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale);
00244         if (occupied)
00245           m_gridmap.data[idx] = 100;
00246         else if (m_gridmap.data[idx] == -1){
00247           m_gridmap.data[idx] = 0;
00248         }
00249 
00250         for (unsigned i = 0; i < inMapLevel.size(); ++i){
00251           if (inMapLevel[i]){
00252             if (occupied)
00253               m_multiGridmap[i].map.data[idx] = 100;
00254             else if (m_multiGridmap[i].map.data[idx] == -1)
00255               m_multiGridmap[i].map.data[idx] = 0;
00256           }
00257         }
00258       }
00259     }
00260   }
00261 
00262 
00263 }
00264 
00265 }
00266 
00267 
00268 


octomap_server
Author(s): Armin Hornung
autogenerated on Wed Nov 23 2016 03:40:03