OctomapServerMultilayer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011-2012, 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   m_haveAttachedObject(false)
00041 {
00042 
00043   m_attachedObjectsSub = m_nh.subscribe("attached_collision_object", 1, &OctomapServerMultilayer::attachedCallback, this);
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::attachedCallback(const arm_navigation_msgs::AttachedCollisionObjectConstPtr& msg){
00105   ROS_DEBUG("AttachedCollisionObjects received");
00106   m_haveAttachedObject = (msg->object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT);
00107   if(m_haveAttachedObject){
00108     m_attachedFrame = msg->link_name;
00109     m_attachedMaxOffset = msg->object.poses.back().position.z + msg->object.shapes.back().dimensions[1];
00110     m_attachedMinOffset = msg->object.poses.back().position.z - msg->object.shapes.back().dimensions[1];
00111   }
00112 }
00113 
00114 void OctomapServerMultilayer::handlePreNodeTraversal(const ros::Time& rostime){
00115   // multilayer server always publishes 2D maps:
00116   m_publish2DMap = true;
00117   nav_msgs::MapMetaData gridmapInfo = m_gridmap.info;
00118 
00119   OctomapServer::handlePreNodeTraversal(rostime);
00120 
00121 
00122   // recalculate height of arm layer (stub, TODO)
00123   geometry_msgs::PointStamped vin;
00124   vin.point.x = 0;
00125   vin.point.y = 0;
00126   vin.point.z = 0;
00127   vin.header.stamp = rostime;
00128   double link_padding = 0.03;
00129 
00130   double minArmHeight = 2.0;
00131   double maxArmHeight = 0.0;
00132   if(m_haveAttachedObject){
00133     printf("adjust for attached object\n");
00134     vin.header.frame_id = m_attachedFrame;
00135     geometry_msgs::PointStamped vout;
00136     m_tfListener.transformPoint("base_footprint",vin,vout);
00137     //ROS_ERROR("link %s with height %f\n",arm_links[i],vout.point.z);
00138     maxArmHeight = vout.point.z + (m_attachedMaxOffset + link_padding);
00139     minArmHeight = vout.point.z - (m_attachedMinOffset + link_padding);
00140   }
00141 
00142   for (unsigned i = 0; i < m_armLinks.size(); ++i){
00143     vin.header.frame_id = m_armLinks[i];
00144     geometry_msgs::PointStamped vout;
00145     const bool found_trans =
00146         m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i),
00147                                       ros::Time(0), ros::Duration(1.0));
00148     ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s",
00149                    m_armLinks[i].c_str());
00150     m_tfListener.transformPoint("base_footprint",vin,vout);
00151     maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding));
00152     minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding));
00153   }
00154   ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight);
00155   m_multiGridmap.at(2).minZ = minArmHeight;
00156   m_multiGridmap.at(2).maxZ = maxArmHeight;
00157   m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0;
00158 
00159 
00160 
00161 
00162 
00163   // TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)?
00164 
00165   bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info);
00166 
00167   for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){
00168     it->map.header = m_gridmap.header;
00169     it->map.info = m_gridmap.info;
00170     it->map.info.origin.position.z = it->z;
00171     if (m_projectCompleteMap){
00172       ROS_INFO("Map resolution changed, rebuilding complete 2D maps");
00173       it->map.data.clear();
00174       // init to unknown:
00175       it->map.data.resize(it->map.info.width * it->map.info.height, -1);
00176     } else if (mapInfoChanged){
00177       adjustMapData(it->map, gridmapInfo);
00178     }
00179   }
00180 }
00181 
00182 void OctomapServerMultilayer::handlePostNodeTraversal(const ros::Time& rostime){
00183 
00184   // TODO: calc tall / short obs. cells for arm layer, => temp arm layer
00185 //  std::vector<int> shortObsCells;
00186 //  for(unsigned int i=0; i<arm_map.data.size(); i++){
00187 //    if(temp_arm_map.data[i] == 0){
00188 //      if(map.data[i] == -1)
00189 //        arm_map.data[i] = -1;
00190 //    }
00191 //    else if(arm_map.data[i] == 0)
00192 //      arm_map.data[i] = 0;
00193 //    else if(double(arm_map.data[i])/temp_arm_map.data[i] > 0.8)
00194 //      arm_map.data[i] = 101;
00195 //    else{
00196 //      arm_map.data[i] = 100;
00197 //      shortObsCells.push_back(i);
00198 //    }
00199 //  }
00200 //
00201 //  std::vector<int> tallObsCells;
00202 //  tallObsCells.reserve(shortObsCells.size());
00203 //  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};
00204 //  for(unsigned int i=0; i<shortObsCells.size(); i++){
00205 //    for(int j=0; j<8; j++){
00206 //      int temp = shortObsCells[i]+dxy[j];
00207 //      if(temp<0 || temp>=arm_map.data.size())
00208 //        continue;
00209 //      if(arm_map.data[temp]==101){
00210 //        tallObsCells.push_back(shortObsCells[i]);
00211 //        break;
00212 //      }
00213 //    }
00214 //  }
00215 //  for(unsigned int i=0; i<tallObsCells.size(); i++)
00216 //    arm_map.data[tallObsCells[i]] = 101;
00217 
00218 
00219 
00220   OctomapServer::handlePostNodeTraversal(rostime);
00221 
00222   for (unsigned i = 0; i < m_multiMapPub.size(); ++i){
00223     m_multiMapPub[i]->publish(m_multiGridmap.at(i).map);
00224   }
00225 
00226 }
00227 void OctomapServerMultilayer::update2DMap(const OcTreeT::iterator& it, bool occupied){
00228   double z = it.getZ();
00229   double s2 = it.getSize()/2.0;
00230 
00231   // create a mask on which maps to update:
00232   std::vector<bool> inMapLevel(m_multiGridmap.size(), false);
00233   for (unsigned i = 0; i < m_multiGridmap.size(); ++i){
00234     if (z+s2 >= m_multiGridmap[i].minZ && z-s2 <= m_multiGridmap[i].maxZ){
00235       inMapLevel[i] = true;
00236     }
00237   }
00238 
00239   if (it.getDepth() == m_maxTreeDepth){
00240     unsigned idx = mapIdx(it.getKey());
00241     if (occupied)
00242       m_gridmap.data[idx] = 100;
00243     else if (m_gridmap.data[idx] == -1){
00244       m_gridmap.data[idx] = 0;
00245     }
00246 
00247     for (unsigned i = 0; i < inMapLevel.size(); ++i){
00248       if (inMapLevel[i]){
00249         if (occupied)
00250           m_multiGridmap[i].map.data[idx] = 100;
00251         else if (m_multiGridmap[i].map.data[idx] == -1)
00252           m_multiGridmap[i].map.data[idx] = 0;
00253       }
00254     }
00255 
00256   } else {
00257     int intSize = 1 << (m_treeDepth - it.getDepth());
00258     octomap::OcTreeKey minKey=it.getIndexKey();
00259     for(int dx=0; dx < intSize; dx++){
00260       int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale;
00261       for(int dy=0; dy < intSize; dy++){
00262         unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale);
00263         if (occupied)
00264           m_gridmap.data[idx] = 100;
00265         else if (m_gridmap.data[idx] == -1){
00266           m_gridmap.data[idx] = 0;
00267         }
00268 
00269         for (unsigned i = 0; i < inMapLevel.size(); ++i){
00270           if (inMapLevel[i]){
00271             if (occupied)
00272               m_multiGridmap[i].map.data[idx] = 100;
00273             else if (m_multiGridmap[i].map.data[idx] == -1)
00274               m_multiGridmap[i].map.data[idx] = 0;
00275           }
00276         }
00277       }
00278     }
00279   }
00280 
00281 
00282 }
00283 
00284 }
00285 
00286 
00287 


octomap_server
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 02:56:34