OctomapServerMultilayer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2013, A. Hornung, M. Philips
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the University of Freiburg nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
31 
32 using namespace octomap;
33 
34 namespace octomap_server{
35 
36 
37 
38 OctomapServerMultilayer::OctomapServerMultilayer(ros::NodeHandle private_nh_)
39 : OctomapServer(private_nh_)
40 {
41 
42  // TODO: callback for arm_navigation attached objects was removed, is
43  // there a replacement functionality?
44 
45  // TODO: param maps, limits
46  // right now 0: base, 1: spine, 2: arms
47  ProjectedMap m;
48  m.name = "projected_base_map";
49  m.minZ = 0.0;
50  m.maxZ = 0.3;
51  m.z = 0.0;
52  m_multiGridmap.push_back(m);
53 
54  m.name = "projected_spine_map";
55  m.minZ = 0.25;
56  m.maxZ = 1.4;
57  m.z = 0.6;
58  m_multiGridmap.push_back(m);
59 
60  m.name = "projected_arm_map";
61  m.minZ = 0.7;
62  m.maxZ = 0.9;
63  m.z = 0.8;
64  m_multiGridmap.push_back(m);
65 
66 
67  for (unsigned i = 0; i < m_multiGridmap.size(); ++i){
68  ros::Publisher* pub = new ros::Publisher(m_nh.advertise<nav_msgs::OccupancyGrid>(m_multiGridmap.at(i).name, 5, m_latchedTopics));
69  m_multiMapPub.push_back(pub);
70  }
71 
72  // init arm links (could be params as well)
73  m_armLinks.push_back("l_elbow_flex_link");
74  m_armLinkOffsets.push_back(0.10);
75  m_armLinks.push_back("l_gripper_l_finger_tip_link");
76  m_armLinkOffsets.push_back(0.03);
77  m_armLinks.push_back("l_gripper_r_finger_tip_link");
78  m_armLinkOffsets.push_back(0.03);
79  m_armLinks.push_back("l_upper_arm_roll_link");
80  m_armLinkOffsets.push_back(0.16);
81  m_armLinks.push_back("l_wrist_flex_link");
82  m_armLinkOffsets.push_back(0.05);
83  m_armLinks.push_back("r_elbow_flex_link");
84  m_armLinkOffsets.push_back(0.10);
85  m_armLinks.push_back("r_gripper_l_finger_tip_link");
86  m_armLinkOffsets.push_back(0.03);
87  m_armLinks.push_back("r_gripper_r_finger_tip_link");
88  m_armLinkOffsets.push_back(0.03);
89  m_armLinks.push_back("r_upper_arm_roll_link");
90  m_armLinkOffsets.push_back(0.16);
91  m_armLinks.push_back("r_wrist_flex_link");
92  m_armLinkOffsets.push_back(0.05);
93 
94 
95 }
96 
98  for (unsigned i = 0; i < m_multiMapPub.size(); ++i){
99  delete m_multiMapPub[i];
100  }
101 
102 }
103 
105  // multilayer server always publishes 2D maps:
106  m_publish2DMap = true;
107  nav_msgs::MapMetaData gridmapInfo = m_gridmap.info;
108 
110 
111 
112  // recalculate height of arm layer (stub, TODO)
113  geometry_msgs::PointStamped vin;
114  vin.point.x = 0;
115  vin.point.y = 0;
116  vin.point.z = 0;
117  vin.header.stamp = rostime;
118  double link_padding = 0.03;
119 
120  double minArmHeight = 2.0;
121  double maxArmHeight = 0.0;
122 
123  for (unsigned i = 0; i < m_armLinks.size(); ++i){
124  vin.header.frame_id = m_armLinks[i];
125  geometry_msgs::PointStamped vout;
126  const bool found_trans =
127  m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i),
128  ros::Time(0), ros::Duration(1.0));
129  ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s",
130  m_armLinks[i].c_str());
131  m_tfListener.transformPoint("base_footprint",vin,vout);
132  maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding));
133  minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding));
134  }
135  ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight);
136  m_multiGridmap.at(2).minZ = minArmHeight;
137  m_multiGridmap.at(2).maxZ = maxArmHeight;
138  m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0;
139 
140 
141 
142 
143 
144  // TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)?
145 
146  bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info);
147 
148  for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){
149  it->map.header = m_gridmap.header;
150  it->map.info = m_gridmap.info;
151  it->map.info.origin.position.z = it->z;
153  ROS_INFO("Map resolution changed, rebuilding complete 2D maps");
154  it->map.data.clear();
155  // init to unknown:
156  it->map.data.resize(it->map.info.width * it->map.info.height, -1);
157  } else if (mapInfoChanged){
158  adjustMapData(it->map, gridmapInfo);
159  }
160  }
161 }
162 
164 
165  // TODO: calc tall / short obs. cells for arm layer, => temp arm layer
166 // std::vector<int> shortObsCells;
167 // for(unsigned int i=0; i<arm_map.data.size(); i++){
168 // if(temp_arm_map.data[i] == 0){
169 // if(map.data[i] == -1)
170 // arm_map.data[i] = -1;
171 // }
172 // else if(arm_map.data[i] == 0)
173 // arm_map.data[i] = 0;
174 // else if(double(arm_map.data[i])/temp_arm_map.data[i] > 0.8)
175 // arm_map.data[i] = 101;
176 // else{
177 // arm_map.data[i] = 100;
178 // shortObsCells.push_back(i);
179 // }
180 // }
181 //
182 // std::vector<int> tallObsCells;
183 // tallObsCells.reserve(shortObsCells.size());
184 // 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};
185 // for(unsigned int i=0; i<shortObsCells.size(); i++){
186 // for(int j=0; j<8; j++){
187 // int temp = shortObsCells[i]+dxy[j];
188 // if(temp<0 || temp>=arm_map.data.size())
189 // continue;
190 // if(arm_map.data[temp]==101){
191 // tallObsCells.push_back(shortObsCells[i]);
192 // break;
193 // }
194 // }
195 // }
196 // for(unsigned int i=0; i<tallObsCells.size(); i++)
197 // arm_map.data[tallObsCells[i]] = 101;
198 
199 
200 
202 
203  for (unsigned i = 0; i < m_multiMapPub.size(); ++i){
204  m_multiMapPub[i]->publish(m_multiGridmap.at(i).map);
205  }
206 
207 }
209  double z = it.getZ();
210  double s2 = it.getSize()/2.0;
211 
212  // create a mask on which maps to update:
213  std::vector<bool> inMapLevel(m_multiGridmap.size(), false);
214  for (unsigned i = 0; i < m_multiGridmap.size(); ++i){
215  if (z+s2 >= m_multiGridmap[i].minZ && z-s2 <= m_multiGridmap[i].maxZ){
216  inMapLevel[i] = true;
217  }
218  }
219 
220  if (it.getDepth() == m_maxTreeDepth){
221  unsigned idx = mapIdx(it.getKey());
222  if (occupied)
223  m_gridmap.data[idx] = 100;
224  else if (m_gridmap.data[idx] == -1){
225  m_gridmap.data[idx] = 0;
226  }
227 
228  for (unsigned i = 0; i < inMapLevel.size(); ++i){
229  if (inMapLevel[i]){
230  if (occupied)
231  m_multiGridmap[i].map.data[idx] = 100;
232  else if (m_multiGridmap[i].map.data[idx] == -1)
233  m_multiGridmap[i].map.data[idx] = 0;
234  }
235  }
236 
237  } else {
238  int intSize = 1 << (m_treeDepth - it.getDepth());
239  octomap::OcTreeKey minKey=it.getIndexKey();
240  for(int dx=0; dx < intSize; dx++){
241  int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale;
242  for(int dy=0; dy < intSize; dy++){
243  unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale);
244  if (occupied)
245  m_gridmap.data[idx] = 100;
246  else if (m_gridmap.data[idx] == -1){
247  m_gridmap.data[idx] = 0;
248  }
249 
250  for (unsigned i = 0; i < inMapLevel.size(); ++i){
251  if (inMapLevel[i]){
252  if (occupied)
253  m_multiGridmap[i].map.data[idx] = 100;
254  else if (m_multiGridmap[i].map.data[idx] == -1)
255  m_multiGridmap[i].map.data[idx] = 0;
256  }
257  }
258  }
259  }
260  }
261 
262 
263 }
264 
265 }
266 
267 
268 
tf::TransformListener m_tfListener
virtual void handlePostNodeTraversal(const ros::Time &rostime)
hook that is called after traversing all nodes
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
bool mapChanged(const nav_msgs::MapMetaData &oldMapInfo, const nav_msgs::MapMetaData &newMapInfo)
std::vector< ros::Publisher * > m_multiMapPub
void transformPoint(const std::string &target_frame, const geometry_msgs::PointStamped &stamped_in, geometry_msgs::PointStamped &stamped_out) const
void adjustMapData(nav_msgs::OccupancyGrid &map, const nav_msgs::MapMetaData &oldMapInfo) const
virtual void update2DMap(const OcTreeT::iterator &it, bool occupied)
updates the downprojected 2D map as either occupied or free
virtual void handlePreNodeTraversal(const ros::Time &rostime)
hook that is called after traversing all nodes
virtual void handlePreNodeTraversal(const ros::Time &rostime)
hook that is called before traversing all nodes
octomap::OcTreeKey m_paddedMinKey
#define ROS_INFO(...)
#define ROS_ASSERT_MSG(cond,...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned mapIdx(int i, int j) const
virtual void handlePostNodeTraversal(const ros::Time &rostime)
hook that is called after traversing all nodes
nav_msgs::OccupancyGrid m_gridmap


octomap_server
Author(s): Armin Hornung
autogenerated on Tue Dec 27 2022 03:15:28