├── README.md ├── cloud_to_map ├── CMakeLists.txt ├── cfg │ ├── cloud_to_map_node.cfg │ └── cpp │ │ └── cloud_to_map │ │ └── cloud_to_map_nodeConfig.h ├── docs │ ├── cloud_to_map_nodeConfig-usage.dox │ ├── cloud_to_map_nodeConfig.dox │ ├── cloud_to_map_nodeConfig.wikidoc │ ├── cloud_to_map_node_backup_1224.cpp │ ├── cloud_to_map_node_with_groud_finder_bugs_with_big_map.cpp │ └── new1225pm9 ├── package.xml └── src │ ├── cloud_to_map │ └── cfg │ │ └── cloud_to_map_nodeConfig.py │ ├── cloud_to_map_node.cpp │ └── data_source.hpp ├── data ├── color │ ├── 1.png │ ├── 2.png │ ├── 3.png │ ├── 4.png │ └── 5.png ├── depth │ ├── 1.pgm │ ├── 2.pgm │ ├── 3.pgm │ ├── 4.pgm │ └── 5.pgm └── pose.txt └── pcl_test ├── CMakeLists.txt ├── package.xml └── src └── pcl_test.cpp /README.md: -------------------------------------------------------------------------------- 1 | # PointCloud_to_Map 2 | This project builds a point cloud map through depth map, and then uses the [cloud_to_map] function package to convert it into a 2D grid map that can be used for navigation. 3 | 4 | 5 | Blog：http://www.chenjianqu.com/show-118.html 6 | -------------------------------------------------------------------------------- /cloud_to_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cloud_to_map) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | dynamic_reconfigure 9 | nav_msgs 10 | ##pcl 11 | pcl_ros 12 | roscpp 13 | ) 14 | find_package(OpenCV 3 REQUIRED) 15 | #find_package(CUDA REQUIRED) 16 | #add dynamic reconfigure api 17 | generate_dynamic_reconfigure_options( 18 | cfg/cloud_to_map_node.cfg 19 | ) 20 | 21 | 22 | find_package(PCL REQUIRED) 23 | catkin_package( 24 | # INCLUDE_DIRS include 25 | # LIBRARIES cloud_to_map 26 | # CATKIN_DEPENDS dynamic_reconfigure nav_msgs pcl pcl_ros roscpp 27 | # DEPENDS system_lib 28 | ) 29 | 30 | ########### 31 | ## Build ## 32 | ########### 33 | 34 | ## Specify additional locations of header files 35 | ## Your package locations should be listed before other locations 36 | # include_directories(include) 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${OpenCV_INCLUDE_DIRS} 40 | ${PCL_INCLUDE_DIRS} 41 | #${CUDA_INCLUDE_DIRS} 42 | ) 43 | link_directories(${PCL_LIBRARY_DIRS}) 44 | add_definitions(${PCL_DEFINITIONS}) 45 | 46 | 47 | 48 | add_executable(cloud_to_map_node src/cloud_to_map_node.cpp) 49 | add_dependencies(cloud_to_map_node ${PROJECT_NAME}_gencfg) 50 | target_link_libraries(cloud_to_map_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES}) 51 | 52 | 53 | -------------------------------------------------------------------------------- /cloud_to_map/cfg/cloud_to_map_node.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "cloud_to_map" 3 | 4 | import roslib;roslib.load_manifest(PACKAGE) 5 | 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("frame", str_t, 0, "Frame the Occupancy Grid should connect to", "map") 11 | gen.add("search_radius", double_t, 0, "Search radius for approximating the normal vectors of a point", 12 | 0.06, 0.0001, 1) 13 | gen.add("deviation", double_t, 0, "Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)", 0.78539816339, 0, 1.57079632679) 14 | gen.add("buffer", int_t, 0, "Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)", 0, 0, 100) 15 | gen.add("loop_rate", double_t, 0, "Rate in Hz the node will attempt to run", 10, 0, 1000) 16 | gen.add("cell_resolution", double_t, 0, "Resolution of the Occupancy Grid output (in m/cell)", 0.05, 0, 1) 17 | 18 | 19 | exit(gen.generate(PACKAGE, "cloud_to_map", "cloud_to_map_node")) 20 | -------------------------------------------------------------------------------- /cloud_to_map/cfg/cpp/cloud_to_map/cloud_to_map_nodeConfig.h: -------------------------------------------------------------------------------- 1 | //#line 2 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" 2 | // ********************************************************* 3 | // 4 | // File autogenerated for the cloud_to_map package 5 | // by the dynamic_reconfigure package. 6 | // Please do not edit. 7 | // 8 | // ********************************************************/ 9 | 10 | #ifndef __cloud_to_map__CLOUD_TO_MAP_NODECONFIG_H__ 11 | #define __cloud_to_map__CLOUD_TO_MAP_NODECONFIG_H__ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace cloud_to_map 23 | { 24 | class cloud_to_map_nodeConfigStatics; 25 | 26 | class cloud_to_map_nodeConfig 27 | { 28 | public: 29 | class AbstractParamDescription : public dynamic_reconfigure::ParamDescription 30 | { 31 | public: 32 | AbstractParamDescription(std::string n, std::string t, uint32_t l, 33 | std::string d, std::string e) 34 | { 35 | name = n; 36 | type = t; 37 | level = l; 38 | description = d; 39 | edit_method = e; 40 | } 41 | 42 | virtual void clamp(cloud_to_map_nodeConfig &config, const cloud_to_map_nodeConfig &max, const cloud_to_map_nodeConfig &min) const = 0; 43 | virtual void calcLevel(uint32_t &level, const cloud_to_map_nodeConfig &config1, const cloud_to_map_nodeConfig &config2) const = 0; 44 | virtual void fromServer(const ros::NodeHandle &nh, cloud_to_map_nodeConfig &config) const = 0; 45 | virtual void toServer(const ros::NodeHandle &nh, const cloud_to_map_nodeConfig &config) const = 0; 46 | virtual bool fromMessage(const dynamic_reconfigure::Config &msg, cloud_to_map_nodeConfig &config) const = 0; 47 | virtual void toMessage(dynamic_reconfigure::Config &msg, const cloud_to_map_nodeConfig &config) const = 0; 48 | virtual void getValue(const cloud_to_map_nodeConfig &config, boost::any &val) const = 0; 49 | }; 50 | 51 | typedef boost::shared_ptr AbstractParamDescriptionPtr; 52 | typedef boost::shared_ptr AbstractParamDescriptionConstPtr; 53 | 54 | template 55 | class ParamDescription : public AbstractParamDescription 56 | { 57 | public: 58 | ParamDescription(std::string name, std::string type, uint32_t level, 59 | std::string description, std::string edit_method, T cloud_to_map_nodeConfig::* f) : 60 | AbstractParamDescription(name, type, level, description, edit_method), 61 | field(f) 62 | {} 63 | 64 | T (cloud_to_map_nodeConfig::* field); 65 | 66 | virtual void clamp(cloud_to_map_nodeConfig &config, const cloud_to_map_nodeConfig &max, const cloud_to_map_nodeConfig &min) const 67 | { 68 | if (config.*field > max.*field) 69 | config.*field = max.*field; 70 | 71 | if (config.*field < min.*field) 72 | config.*field = min.*field; 73 | } 74 | 75 | virtual void calcLevel(uint32_t &comb_level, const cloud_to_map_nodeConfig &config1, const cloud_to_map_nodeConfig &config2) const 76 | { 77 | if (config1.*field != config2.*field) 78 | comb_level |= level; 79 | } 80 | 81 | virtual void fromServer(const ros::NodeHandle &nh, cloud_to_map_nodeConfig &config) const 82 | { 83 | nh.getParam(name, config.*field); 84 | } 85 | 86 | virtual void toServer(const ros::NodeHandle &nh, const cloud_to_map_nodeConfig &config) const 87 | { 88 | nh.setParam(name, config.*field); 89 | } 90 | 91 | virtual bool fromMessage(const dynamic_reconfigure::Config &msg, cloud_to_map_nodeConfig &config) const 92 | { 93 | return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field); 94 | } 95 | 96 | virtual void toMessage(dynamic_reconfigure::Config &msg, const cloud_to_map_nodeConfig &config) const 97 | { 98 | dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field); 99 | } 100 | 101 | virtual void getValue(const cloud_to_map_nodeConfig &config, boost::any &val) const 102 | { 103 | val = config.*field; 104 | } 105 | }; 106 | 107 | class AbstractGroupDescription : public dynamic_reconfigure::Group 108 | { 109 | public: 110 | AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s) 111 | { 112 | name = n; 113 | type = t; 114 | parent = p; 115 | state = s; 116 | id = i; 117 | } 118 | 119 | std::vector abstract_parameters; 120 | bool state; 121 | 122 | virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0; 123 | virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0; 124 | virtual void updateParams(boost::any &cfg, cloud_to_map_nodeConfig &top) const= 0; 125 | virtual void setInitialState(boost::any &cfg) const = 0; 126 | 127 | 128 | void convertParams() 129 | { 130 | for(std::vector::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); ++i) 131 | { 132 | parameters.push_back(dynamic_reconfigure::ParamDescription(**i)); 133 | } 134 | } 135 | }; 136 | 137 | typedef boost::shared_ptr AbstractGroupDescriptionPtr; 138 | typedef boost::shared_ptr AbstractGroupDescriptionConstPtr; 139 | 140 | template 141 | class GroupDescription : public AbstractGroupDescription 142 | { 143 | public: 144 | GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::* f) : AbstractGroupDescription(name, type, parent, id, s), field(f) 145 | { 146 | } 147 | 148 | GroupDescription(const GroupDescription& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups) 149 | { 150 | parameters = g.parameters; 151 | abstract_parameters = g.abstract_parameters; 152 | } 153 | 154 | virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const 155 | { 156 | PT* config = boost::any_cast(cfg); 157 | if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field)) 158 | return false; 159 | 160 | for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) 161 | { 162 | boost::any n = &((*config).*field); 163 | if(!(*i)->fromMessage(msg, n)) 164 | return false; 165 | } 166 | 167 | return true; 168 | } 169 | 170 | virtual void setInitialState(boost::any &cfg) const 171 | { 172 | PT* config = boost::any_cast(cfg); 173 | T* group = &((*config).*field); 174 | group->state = state; 175 | 176 | for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) 177 | { 178 | boost::any n = boost::any(&((*config).*field)); 179 | (*i)->setInitialState(n); 180 | } 181 | 182 | } 183 | 184 | virtual void updateParams(boost::any &cfg, cloud_to_map_nodeConfig &top) const 185 | { 186 | PT* config = boost::any_cast(cfg); 187 | 188 | T* f = &((*config).*field); 189 | f->setParams(top, abstract_parameters); 190 | 191 | for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) 192 | { 193 | boost::any n = &((*config).*field); 194 | (*i)->updateParams(n, top); 195 | } 196 | } 197 | 198 | virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const 199 | { 200 | const PT config = boost::any_cast(cfg); 201 | dynamic_reconfigure::ConfigTools::appendGroup(msg, name, id, parent, config.*field); 202 | 203 | for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) 204 | { 205 | (*i)->toMessage(msg, config.*field); 206 | } 207 | } 208 | 209 | T (PT::* field); 210 | std::vector groups; 211 | }; 212 | 213 | class DEFAULT 214 | { 215 | public: 216 | DEFAULT() 217 | { 218 | state = true; 219 | name = "Default"; 220 | } 221 | 222 | void setParams(cloud_to_map_nodeConfig &config, const std::vector params) 223 | { 224 | for (std::vector::const_iterator _i = params.begin(); _i != params.end(); ++_i) 225 | { 226 | boost::any val; 227 | (*_i)->getValue(config, val); 228 | 229 | if("frame"==(*_i)->name){frame = boost::any_cast(val);} 230 | if("search_radius"==(*_i)->name){search_radius = boost::any_cast(val);} 231 | if("deviation"==(*_i)->name){deviation = boost::any_cast(val);} 232 | if("buffer"==(*_i)->name){buffer = boost::any_cast(val);} 233 | if("loop_rate"==(*_i)->name){loop_rate = boost::any_cast(val);} 234 | if("cell_resolution"==(*_i)->name){cell_resolution = boost::any_cast(val);} 235 | } 236 | } 237 | 238 | std::string frame; 239 | double search_radius; 240 | double deviation; 241 | int buffer; 242 | double loop_rate; 243 | double cell_resolution; 244 | 245 | bool state; 246 | std::string name; 247 | 248 | 249 | }groups; 250 | 251 | 252 | 253 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 254 | std::string frame; 255 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 256 | double search_radius; 257 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 258 | double deviation; 259 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 260 | int buffer; 261 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 262 | double loop_rate; 263 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 264 | double cell_resolution; 265 | //#line 218 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" 266 | 267 | bool __fromMessage__(dynamic_reconfigure::Config &msg) 268 | { 269 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 270 | const std::vector &__group_descriptions__ = __getGroupDescriptions__(); 271 | 272 | int count = 0; 273 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 274 | if ((*i)->fromMessage(msg, *this)) 275 | count++; 276 | 277 | for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++) 278 | { 279 | if ((*i)->id == 0) 280 | { 281 | boost::any n = boost::any(this); 282 | (*i)->updateParams(n, *this); 283 | (*i)->fromMessage(msg, n); 284 | } 285 | } 286 | 287 | if (count != dynamic_reconfigure::ConfigTools::size(msg)) 288 | { 289 | ROS_ERROR("cloud_to_map_nodeConfig::__fromMessage__ called with an unexpected parameter."); 290 | ROS_ERROR("Booleans:"); 291 | for (unsigned int i = 0; i < msg.bools.size(); i++) 292 | ROS_ERROR(" %s", msg.bools[i].name.c_str()); 293 | ROS_ERROR("Integers:"); 294 | for (unsigned int i = 0; i < msg.ints.size(); i++) 295 | ROS_ERROR(" %s", msg.ints[i].name.c_str()); 296 | ROS_ERROR("Doubles:"); 297 | for (unsigned int i = 0; i < msg.doubles.size(); i++) 298 | ROS_ERROR(" %s", msg.doubles[i].name.c_str()); 299 | ROS_ERROR("Strings:"); 300 | for (unsigned int i = 0; i < msg.strs.size(); i++) 301 | ROS_ERROR(" %s", msg.strs[i].name.c_str()); 302 | // @todo Check that there are no duplicates. Make this error more 303 | // explicit. 304 | return false; 305 | } 306 | return true; 307 | } 308 | 309 | // This version of __toMessage__ is used during initialization of 310 | // statics when __getParamDescriptions__ can't be called yet. 311 | void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector &__param_descriptions__, const std::vector &__group_descriptions__) const 312 | { 313 | dynamic_reconfigure::ConfigTools::clear(msg); 314 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 315 | (*i)->toMessage(msg, *this); 316 | 317 | for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) 318 | { 319 | if((*i)->id == 0) 320 | { 321 | (*i)->toMessage(msg, *this); 322 | } 323 | } 324 | } 325 | 326 | void __toMessage__(dynamic_reconfigure::Config &msg) const 327 | { 328 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 329 | const std::vector &__group_descriptions__ = __getGroupDescriptions__(); 330 | __toMessage__(msg, __param_descriptions__, __group_descriptions__); 331 | } 332 | 333 | void __toServer__(const ros::NodeHandle &nh) const 334 | { 335 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 336 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 337 | (*i)->toServer(nh, *this); 338 | } 339 | 340 | void __fromServer__(const ros::NodeHandle &nh) 341 | { 342 | static bool setup=false; 343 | 344 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 345 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 346 | (*i)->fromServer(nh, *this); 347 | 348 | const std::vector &__group_descriptions__ = __getGroupDescriptions__(); 349 | for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){ 350 | if (!setup && (*i)->id == 0) { 351 | setup = true; 352 | boost::any n = boost::any(this); 353 | (*i)->setInitialState(n); 354 | } 355 | } 356 | } 357 | 358 | void __clamp__() 359 | { 360 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 361 | const cloud_to_map_nodeConfig &__max__ = __getMax__(); 362 | const cloud_to_map_nodeConfig &__min__ = __getMin__(); 363 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 364 | (*i)->clamp(*this, __max__, __min__); 365 | } 366 | 367 | uint32_t __level__(const cloud_to_map_nodeConfig &config) const 368 | { 369 | const std::vector &__param_descriptions__ = __getParamDescriptions__(); 370 | uint32_t level = 0; 371 | for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) 372 | (*i)->calcLevel(level, config, *this); 373 | return level; 374 | } 375 | 376 | static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__(); 377 | static const cloud_to_map_nodeConfig &__getDefault__(); 378 | static const cloud_to_map_nodeConfig &__getMax__(); 379 | static const cloud_to_map_nodeConfig &__getMin__(); 380 | static const std::vector &__getParamDescriptions__(); 381 | static const std::vector &__getGroupDescriptions__(); 382 | 383 | private: 384 | static const cloud_to_map_nodeConfigStatics *__get_statics__(); 385 | }; 386 | 387 | template <> // Max and min are ignored for strings. 388 | inline void cloud_to_map_nodeConfig::ParamDescription::clamp(cloud_to_map_nodeConfig &config, const cloud_to_map_nodeConfig &max, const cloud_to_map_nodeConfig &min) const 389 | { 390 | return; 391 | } 392 | 393 | class cloud_to_map_nodeConfigStatics 394 | { 395 | friend class cloud_to_map_nodeConfig; 396 | 397 | cloud_to_map_nodeConfigStatics() 398 | { 399 | cloud_to_map_nodeConfig::GroupDescription Default("Default", "", 0, 0, true, &cloud_to_map_nodeConfig::groups); 400 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 401 | __min__.frame = ""; 402 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 403 | __max__.frame = ""; 404 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 405 | __default__.frame = "map"; 406 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 407 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("frame", "str", 0, "Frame the Occupancy Grid should connect to", "", &cloud_to_map_nodeConfig::frame))); 408 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 409 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("frame", "str", 0, "Frame the Occupancy Grid should connect to", "", &cloud_to_map_nodeConfig::frame))); 410 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 411 | __min__.search_radius = 0.0001; 412 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 413 | __max__.search_radius = 1.0; 414 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 415 | __default__.search_radius = 0.06; 416 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 417 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("search_radius", "double", 0, "Search radius for approximating the normal vectors of a point", "", &cloud_to_map_nodeConfig::search_radius))); 418 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 419 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("search_radius", "double", 0, "Search radius for approximating the normal vectors of a point", "", &cloud_to_map_nodeConfig::search_radius))); 420 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 421 | __min__.deviation = 0.0; 422 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 423 | __max__.deviation = 1.57079632679; 424 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 425 | __default__.deviation = 0.78539816339; 426 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 427 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("deviation", "double", 0, "Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)", "", &cloud_to_map_nodeConfig::deviation))); 428 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 429 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("deviation", "double", 0, "Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)", "", &cloud_to_map_nodeConfig::deviation))); 430 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 431 | __min__.buffer = 0; 432 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 433 | __max__.buffer = 100; 434 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 435 | __default__.buffer = 0; 436 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 437 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("buffer", "int", 0, "Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)", "", &cloud_to_map_nodeConfig::buffer))); 438 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 439 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("buffer", "int", 0, "Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)", "", &cloud_to_map_nodeConfig::buffer))); 440 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 441 | __min__.loop_rate = 0.0; 442 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 443 | __max__.loop_rate = 1000.0; 444 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 445 | __default__.loop_rate = 10.0; 446 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 447 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("loop_rate", "double", 0, "Rate in Hz the node will attempt to run", "", &cloud_to_map_nodeConfig::loop_rate))); 448 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 449 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("loop_rate", "double", 0, "Rate in Hz the node will attempt to run", "", &cloud_to_map_nodeConfig::loop_rate))); 450 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 451 | __min__.cell_resolution = 0.0; 452 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 453 | __max__.cell_resolution = 1.0; 454 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 455 | __default__.cell_resolution = 0.7; 456 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 457 | Default.abstract_parameters.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("cell_resolution", "double", 0, "Resolution of the Occupancy Grid output (in m/cell)", "", &cloud_to_map_nodeConfig::cell_resolution))); 458 | //#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 459 | __param_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractParamDescriptionConstPtr(new cloud_to_map_nodeConfig::ParamDescription("cell_resolution", "double", 0, "Resolution of the Occupancy Grid output (in m/cell)", "", &cloud_to_map_nodeConfig::cell_resolution))); 460 | //#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 461 | Default.convertParams(); 462 | //#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" 463 | __group_descriptions__.push_back(cloud_to_map_nodeConfig::AbstractGroupDescriptionConstPtr(new cloud_to_map_nodeConfig::GroupDescription(Default))); 464 | //#line 353 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" 465 | 466 | for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) 467 | { 468 | __description_message__.groups.push_back(**i); 469 | } 470 | __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__); 471 | __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__); 472 | __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__); 473 | } 474 | std::vector __param_descriptions__; 475 | std::vector __group_descriptions__; 476 | cloud_to_map_nodeConfig __max__; 477 | cloud_to_map_nodeConfig __min__; 478 | cloud_to_map_nodeConfig __default__; 479 | dynamic_reconfigure::ConfigDescription __description_message__; 480 | 481 | static const cloud_to_map_nodeConfigStatics *get_instance() 482 | { 483 | // Split this off in a separate function because I know that 484 | // instance will get initialized the first time get_instance is 485 | // called, and I am guaranteeing that get_instance gets called at 486 | // most once. 487 | static cloud_to_map_nodeConfigStatics instance; 488 | return &instance; 489 | } 490 | }; 491 | 492 | inline const dynamic_reconfigure::ConfigDescription &cloud_to_map_nodeConfig::__getDescriptionMessage__() 493 | { 494 | return __get_statics__()->__description_message__; 495 | } 496 | 497 | inline const cloud_to_map_nodeConfig &cloud_to_map_nodeConfig::__getDefault__() 498 | { 499 | return __get_statics__()->__default__; 500 | } 501 | 502 | inline const cloud_to_map_nodeConfig &cloud_to_map_nodeConfig::__getMax__() 503 | { 504 | return __get_statics__()->__max__; 505 | } 506 | 507 | inline const cloud_to_map_nodeConfig &cloud_to_map_nodeConfig::__getMin__() 508 | { 509 | return __get_statics__()->__min__; 510 | } 511 | 512 | inline const std::vector &cloud_to_map_nodeConfig::__getParamDescriptions__() 513 | { 514 | return __get_statics__()->__param_descriptions__; 515 | } 516 | 517 | inline const std::vector &cloud_to_map_nodeConfig::__getGroupDescriptions__() 518 | { 519 | return __get_statics__()->__group_descriptions__; 520 | } 521 | 522 | inline const cloud_to_map_nodeConfigStatics *cloud_to_map_nodeConfig::__get_statics__() 523 | { 524 | const static cloud_to_map_nodeConfigStatics *statics; 525 | 526 | if (statics) // Common case 527 | return statics; 528 | 529 | boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__); 530 | 531 | if (statics) // In case we lost a race. 532 | return statics; 533 | 534 | statics = cloud_to_map_nodeConfigStatics::get_instance(); 535 | 536 | return statics; 537 | } 538 | 539 | 540 | } 541 | 542 | #endif // __CLOUD_TO_MAP_NODERECONFIGURATOR_H__ 543 | -------------------------------------------------------------------------------- /cloud_to_map/docs/cloud_to_map_nodeConfig-usage.dox: -------------------------------------------------------------------------------- 1 | \subsubsection usage Usage 2 | \verbatim 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | \endverbatim 12 | 13 | -------------------------------------------------------------------------------- /cloud_to_map/docs/cloud_to_map_nodeConfig.dox: -------------------------------------------------------------------------------- 1 | \subsubsection parameters ROS parameters 2 | 3 | Reads and maintains the following parameters on the ROS server 4 | 5 | - \b "~frame" : \b [str] Frame the Occupancy Grid should connect to min: , default: map, max: 6 | - \b "~search_radius" : \b [double] Search radius for approximating the normal vectors of a point min: 0.0001, default: 0.06, max: 1.0 7 | - \b "~deviation" : \b [double] Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians) min: 0.0, default: 0.78539816339, max: 1.57079632679 8 | - \b "~buffer" : \b [int] Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective) min: 0, default: 0, max: 100 9 | - \b "~loop_rate" : \b [double] Rate in Hz the node will attempt to run min: 0.0, default: 10.0, max: 1000.0 10 | - \b "~cell_resolution" : \b [double] Resolution of the Occupancy Grid output (in m/cell) min: 0.0, default: 0.7, max: 1.0 11 | 12 | -------------------------------------------------------------------------------- /cloud_to_map/docs/cloud_to_map_nodeConfig.wikidoc: -------------------------------------------------------------------------------- 1 | # Autogenerated param section. Do not hand edit. 2 | param { 3 | group.0 { 4 | name=Dynamically Reconfigurable Parameters 5 | desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters. 6 | 0.name= ~frame 7 | 0.default= map 8 | 0.type= str 9 | 0.desc=Frame the Occupancy Grid should connect to 10 | 1.name= ~search_radius 11 | 1.default= 0.06 12 | 1.type= double 13 | 1.desc=Search radius for approximating the normal vectors of a point Range: 0.0001 to 1.0 14 | 2.name= ~deviation 15 | 2.default= 0.78539816339 16 | 2.type= double 17 | 2.desc=Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians) Range: 0.0 to 1.57079632679 18 | 3.name= ~buffer 19 | 3.default= 0 20 | 3.type= int 21 | 3.desc=Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective) Range: 0 to 100 22 | 4.name= ~loop_rate 23 | 4.default= 10.0 24 | 4.type= double 25 | 4.desc=Rate in Hz the node will attempt to run Range: 0.0 to 1000.0 26 | 5.name= ~cell_resolution 27 | 5.default= 0.7 28 | 5.type= double 29 | 5.desc=Resolution of the Occupancy Grid output (in m/cell) Range: 0.0 to 1.0 30 | } 31 | } 32 | # End of autogenerated section. You may edit below. 33 | -------------------------------------------------------------------------------- /cloud_to_map/docs/cloud_to_map_node_backup_1224.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | double threshold_z1 = 0; // should add a function to de terimnine the ground automatically 23 | int diffThresh = 4; // the value above certain height - the value under certain height 24 | int greenThresh; // the thereshold of "may we can pass" 25 | int orangeThresh = 20; // the thereshold of "high obstacle" 26 | int redThresh = 40; // the thereshold of "very high obstacle" 27 | int wallThresh =10; // the thereshold of "can't pass obstacle" 28 | 29 | /* Define the two point cloud types used in this code */ 30 | typedef pcl::PointCloud PointCloud; 31 | typedef pcl::PointCloud NormalCloud; 32 | 33 | /* Global */ 34 | PointCloud::ConstPtr currentPC; 35 | bool newPointCloud = false; 36 | bool reconfig = false; 37 | 38 | 39 | // ----------------------------------------------- 40 | // -----Define a structure to hold parameters----- 41 | // ----------------------------------------------- 42 | struct Param { 43 | std::string frame; 44 | double searchRadius; 45 | double deviation; 46 | int buffer; 47 | double loopRate; 48 | double cellResolution; 49 | 50 | }; 51 | 52 | // ----------------------------------- 53 | // -----Define default parameters----- 54 | // ----------------------------------- 55 | Param param; 56 | boost::mutex mutex; 57 | void loadDefaults(ros::NodeHandle& nh) { 58 | nh.param("frame", param.frame, "map"); 59 | nh.param("search_radius", param.searchRadius, 1.0); 60 | nh.param("deviation", param.deviation, 0.1); 61 | nh.param("buffer", param.buffer, 5); 62 | nh.param("loop_rate", param.loopRate, 10.0); 63 | nh.param("cell_resolution", param.cellResolution, 0.3); 64 | // nh.param("z1_thereshold", param.threshold_z1, 1); 65 | } 66 | // ----------------------------------------------- 67 | // ----- To find the ground abtomatcally ----- 68 | // ----------------------------------------------- 69 | void groundZfinder(){ 70 | std::vector > groundDifReg; 71 | 72 | 73 | 74 | 75 | } 76 | // ------------------------------------------------------ 77 | // -----Update current PointCloud if msg is received----- 78 | // ------------------------------------------------------ 79 | void callback(const PointCloud::ConstPtr& msg) { 80 | boost::unique_lock(mutex); 81 | currentPC = msg; 82 | newPointCloud = true; 83 | } 84 | 85 | // ------------------------------------------ 86 | // -----Callback for Dynamic Reconfigure----- 87 | // ------------------------------------------ 88 | void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) { 89 | ROS_INFO("Reconfigure Request: %s %f %f %f %f", config.frame.c_str(), config.deviation, 90 | config.loop_rate, config.cell_resolution, config.search_radius); 91 | boost::unique_lock(mutex); 92 | param.frame = config.frame.c_str(); 93 | param.searchRadius = config.search_radius; 94 | param.deviation = config.deviation; 95 | param.buffer = config.buffer; 96 | param.loopRate = config.loop_rate; 97 | param.cellResolution = config.cell_resolution; 98 | reconfig = true; 99 | } 100 | 101 | // ---------------------------------------------------------------- 102 | // -----Calculate surface normals with a search radius of 0.05----- 103 | // ---------------------------------------------------------------- 104 | void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud::Ptr normals) { 105 | pcl::NormalEstimation ne; 106 | ne.setInputCloud(cloud); 107 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 108 | ne.setSearchMethod(tree); 109 | ne.setRadiusSearch(param.searchRadius); 110 | ne.compute(*normals); 111 | } 112 | 113 | // --------------------------------------- 114 | // -----Initialize Occupancy Grid Msg----- 115 | // --------------------------------------- 116 | void initGrid(nav_msgs::OccupancyGridPtr grid) { 117 | grid->header.seq = 1; 118 | grid->header.frame_id = param.frame; 119 | grid->info.origin.position.z = 0; 120 | grid->info.origin.orientation.w = 1; 121 | grid->info.origin.orientation.x = 0; 122 | grid->info.origin.orientation.y = 0; 123 | grid->info.origin.orientation.z = 0; 124 | } 125 | 126 | // ----------------------------------- 127 | // -----Update Occupancy Grid Msg----- 128 | // ----------------------------------- 129 | void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells, 130 | double originX, double originY, std::vector *ocGrid) { 131 | grid->header.seq++; 132 | grid->header.stamp.sec = ros::Time::now().sec; 133 | grid->header.stamp.nsec = ros::Time::now().nsec; 134 | grid->info.map_load_time = ros::Time::now(); 135 | grid->info.resolution = cellRes; 136 | grid->info.width = xCells; 137 | grid->info.height = yCells; 138 | grid->info.origin.position.x = originX; 139 | grid->info.origin.position.y = originY; 140 | grid->data = *ocGrid; 141 | } 142 | 143 | // ------------------------------------------ 144 | // -----Calculate size of Occupancy Grid----- 145 | // ------------------------------------------ 146 | void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) { 147 | for (size_t i = 0; i < currentPC->size(); i++) { 148 | double x = currentPC->points[i].x; 149 | double y = currentPC->points[i].y; 150 | if (*xMax < x) { 151 | *xMax = x; 152 | } 153 | if (*xMin > x) { 154 | *xMin = x; 155 | } 156 | if (*yMax < y) { 157 | *yMax = y; 158 | } 159 | if (*yMin > y) { 160 | *yMin = y; 161 | } 162 | } 163 | } 164 | 165 | // --------------------------------------- 166 | // -----Populate map with cost values----- 167 | // --------------------------------------- 168 | void populateMap(NormalCloud::Ptr cloud_normals, std::vector &map,double xMax, double yMax, double xMin, double yMin, 169 | double cellResolution, int xCells, int yCells) { 170 | double deviation = param.deviation; 171 | std::vector > map_n; 172 | std::cout<<" 1 "<size()/cellResolution 183 | } 184 | std::cout<<" 2 "<size()/(cellResolution*cellResolution); 187 | for (size_t i = 0; i < currentPC->size(); i++) 188 | { 189 | double x = currentPC->points[i].x; 190 | double y = currentPC->points[i].y; 191 | double z = cloud_normals->points[i].normal_z; 192 | double z_axis = currentPC->points[i].z; 193 | 194 | double phi = acos(fabs(z)); 195 | int xCell, yCell; 196 | //std::cout<< i <<" i, the_size_of_map: "< (xCells * yCells)) { 203 | std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell << "\n"; 204 | } 205 | if (phi > deviation) 206 | { 207 | if(z_axissize(); m++) 227 | { 228 | if(map_n[0][m]>map_n[1][m]&&map_n[0][m] > diffThresh) 229 | { 230 | 231 | map[m] = -1; 232 | 233 | std::cout<size()<<" 4 "<20) { 245 | 246 | void genOccupancyGrid(std::vector &ocGrid, std::vector &countGrid, int size) { 247 | int buf = param.buffer; 248 | 249 | for (int i = 0; i < size; i++) { 250 | if (countGrid[i] < wallThresh&&countGrid[i]>0) 251 | { 252 | ocGrid[i] = 0; 253 | } else if (countGrid[i] > wallThresh&&countGrid[i]orangeThresh) 261 | { 262 | ocGrid[i] = 200; // TODO Should be -1 263 | } else if (countGrid[i]>redThresh) 264 | { 265 | ocGrid[i] = 150; // TODO Should be -1 266 | } else if (countGrid[i]=-1) 267 | { 268 | ocGrid[i] = 101;//125 269 | } 270 | 271 | } 272 | } 273 | 274 | // -------------- 275 | // -----Main----- 276 | // -------------- 277 | int main(int argc, char** argv) { 278 | /* Initialize ROS */ 279 | ros::init(argc, argv, "cloud_to_map_node"); 280 | ros::NodeHandle nh; 281 | ros::Subscriber sub = nh.subscribe("/voxel_filter", 1, callback); 282 | ros::Publisher pub = nh.advertise("map", 1); 283 | 284 | /* Initialize Dynamic Reconfigure */ 285 | dynamic_reconfigure::Server server; 286 | dynamic_reconfigure::Server::CallbackType f; 287 | f = boost::bind(&callbackReconfig, _1, _2); 288 | server.setCallback(f); 289 | 290 | /* Initialize Grid */ 291 | nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid); 292 | initGrid(grid); 293 | 294 | /* Finish initializing ROS */ 295 | mutex.lock(); 296 | ros::Rate loop_rate(param.loopRate); 297 | mutex.unlock(); 298 | 299 | /* Wait for first point cloud */ 300 | while(ros::ok() && newPointCloud == false){ 301 | ros::spinOnce(); 302 | loop_rate.sleep(); 303 | } 304 | 305 | /* Begin processing point clouds */ 306 | while (ros::ok()) { 307 | ros::spinOnce(); 308 | boost::unique_lock lck(mutex); 309 | if (newPointCloud || reconfig) { 310 | /* Update configuration status */ 311 | reconfig = false; 312 | newPointCloud = false; 313 | 314 | /* Record start time */ 315 | uint32_t sec = ros::Time::now().sec; 316 | uint32_t nsec = ros::Time::now().nsec; 317 | 318 | /* Calculate Surface Normals */ 319 | NormalCloud::Ptr cloud_normals(new NormalCloud); 320 | calcSurfaceNormals(currentPC, cloud_normals); 321 | 322 | /* Figure out size of matrix needed to store data. */ 323 | double xMax = 0, yMax = 0, xMin = 0, yMin = 0; 324 | calcSize(&xMax, &yMax, &xMin, &yMin); 325 | std::cout << "xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: " 326 | << yMin << "\n"; 327 | 328 | /* Determine resolution of grid (m/cell) */ 329 | double cellResolution = param.cellResolution; 330 | int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1; 331 | int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1; 332 | std::cout << "xCells: " << xCells << ", yCells: " << yCells << "\n"; 333 | 334 | /* Populate Map */ 335 | std::vector countGrid(yCells * xCells); 336 | 337 | populateMap(cloud_normals, countGrid, xMax, yMax, xMin, yMin, cellResolution, xCells, yCells); 338 | 339 | /* Generate OccupancyGrid Data Vector */ 340 | std::vector ocGrid(yCells * xCells); 341 | genOccupancyGrid(ocGrid, countGrid, xCells * yCells); 342 | 343 | /* Update OccupancyGrid Object */ 344 | updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid); 345 | 346 | /* Release lock */ 347 | lck.unlock(); 348 | 349 | /* Record end time */ 350 | uint32_t esec = ros::Time::now().sec; 351 | uint32_t ensec = ros::Time::now().nsec; 352 | std::cout << "Seconds: " << esec - sec << "\nNSeconds: " << ensec - nsec << "\n"; 353 | 354 | /* Publish Occupancy Grid */ 355 | pub.publish(grid); 356 | } 357 | loop_rate.sleep(); 358 | } 359 | } 360 | -------------------------------------------------------------------------------- /cloud_to_map/docs/cloud_to_map_node_with_groud_finder_bugs_with_big_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | //#include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | double threshold_z1 = 0; // should add a function to de terimnine the ground automatically 29 | double averange_height = 0.7; 30 | /* Define the two point cloud types used in this code */ 31 | typedef pcl::PointCloud PointCloud; 32 | typedef pcl::PointCloud NormalCloud; 33 | 34 | /* Global */ 35 | PointCloud::ConstPtr currentPC; 36 | bool newPointCloud = false; 37 | bool reconfig = false; 38 | //using namespace cv ; 39 | 40 | // ----------------------------------------------- 41 | // -----Define a structure to hold parameters----- 42 | // ----------------------------------------------- 43 | struct Param { 44 | std::string frame; 45 | double searchRadius; 46 | double deviation; 47 | int buffer; 48 | double loopRate; 49 | double cellResolution; 50 | 51 | }; 52 | 53 | // ----------------------------------- 54 | // -----Define default parameters----- 55 | // ----------------------------------- 56 | Param param; 57 | boost::mutex mutex; 58 | void loadDefaults(ros::NodeHandle& nh) { 59 | nh.param("frame", param.frame, "map"); 60 | nh.param("search_radius", param.searchRadius, 1.0); 61 | nh.param("deviation", param.deviation, 0.1); 62 | nh.param("buffer", param.buffer, 5); 63 | nh.param("loop_rate", param.loopRate, 10.0); 64 | nh.param("cell_resolution", param.cellResolution, 0.3); 65 | // nh.param("z1_thereshold", param.threshold_z1, 1); 66 | } 67 | // ----------------------------------------------- 68 | // ----- To find the ground automatcally ----- 69 | // ----------------------------------------------- 70 | void groundZfinder(int xMax, int yMax, int xMin, int yMin, NormalCloud::Ptr cloud_normals){ 71 | std::vector > groundDifReg; // to save the averange z aixs height 72 | double deviation = param.deviation; 73 | int xrange = xMax - xMin; 74 | int yrange = yMax - yMin; 75 | int blocks = 1; 76 | int rows =2; 77 | double averangeZheight; 78 | int counter_z = 0; 79 | int MapArea = (xMax-xMin)*(yMax-yMin); 80 | double matrix[MapArea]; 81 | 82 | groundDifReg.resize(rows); 83 | for(int j =0; j < rows; j++) 84 | { 85 | groundDifReg[j].resize(MapArea); //the size is rows*Map area the [0] saves the times appears 86 | } 87 | groundDifReg.clear(); 88 | 89 | for (size_t i = 0; i < currentPC->size(); i++) 90 | { 91 | 92 | int x = currentPC->points[i].x; 93 | int y = currentPC->points[i].y; 94 | int xCell = (x - xMin) ; 95 | int yCell = (y - yMin) ; 96 | 97 | double z = cloud_normals->points[i].normal_z; 98 | double z_axis = currentPC->points[i].z; 99 | double phi = acos(fabs(z)); 100 | 101 | if ( phi < deviation ){ 102 | 103 | double z_temp = 0; 104 | groundDifReg[0][yCell*xrange+xCell]++; 105 | //std::cout< the thereshold of Axis z 142 | 143 | 144 | // ------------------------------------------------------ 145 | // -----Update current PointCloud if msg is received----- 146 | // ------------------------------------------------------ 147 | void callback(const PointCloud::ConstPtr& msg) { 148 | boost::unique_lock(mutex); 149 | currentPC = msg; 150 | newPointCloud = true; 151 | } 152 | 153 | // ------------------------------------------ 154 | // -----Callback for Dynamic Reconfigure----- 155 | // ------------------------------------------ 156 | void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) { 157 | ROS_INFO("Reconfigure Request: %s %f %f %f %f", config.frame.c_str(), config.deviation, 158 | config.loop_rate, config.cell_resolution, config.search_radius); 159 | boost::unique_lock(mutex); 160 | param.frame = config.frame.c_str(); 161 | param.searchRadius = config.search_radius; 162 | param.deviation = config.deviation; 163 | param.buffer = config.buffer; 164 | param.loopRate = config.loop_rate; 165 | param.cellResolution = config.cell_resolution; 166 | reconfig = true; 167 | } 168 | 169 | // ---------------------------------------------------------------- 170 | // -----Calculate surface normals with a search radius of 0.05----- 171 | // ---------------------------------------------------------------- 172 | void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud::Ptr normals) { 173 | pcl::NormalEstimation ne; 174 | ne.setInputCloud(cloud); 175 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 176 | ne.setSearchMethod(tree); 177 | ne.setRadiusSearch(param.searchRadius); 178 | ne.compute(*normals); 179 | } 180 | 181 | // --------------------------------------- 182 | // -----Initialize Occupancy Grid Msg----- 183 | // --------------------------------------- 184 | void initGrid(nav_msgs::OccupancyGridPtr grid) { 185 | grid->header.seq = 1; 186 | grid->header.frame_id = param.frame; 187 | grid->info.origin.position.z = 0; 188 | grid->info.origin.orientation.w = 1; 189 | grid->info.origin.orientation.x = 0; 190 | grid->info.origin.orientation.y = 0; 191 | grid->info.origin.orientation.z = 0; 192 | } 193 | 194 | // ----------------------------------- 195 | // -----Update Occupancy Grid Msg----- 196 | // ----------------------------------- 197 | void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells, 198 | double originX, double originY, std::vector *ocGrid) { 199 | grid->header.seq++; 200 | grid->header.stamp.sec = ros::Time::now().sec; 201 | grid->header.stamp.nsec = ros::Time::now().nsec; 202 | grid->info.map_load_time = ros::Time::now(); 203 | grid->info.resolution = cellRes; 204 | grid->info.width = xCells; 205 | grid->info.height = yCells; 206 | grid->info.origin.position.x = originX; 207 | grid->info.origin.position.y = originY; 208 | grid->data = *ocGrid; 209 | } 210 | 211 | // ------------------------------------------ 212 | // -----Calculate size of Occupancy Grid----- 213 | // ------------------------------------------ 214 | void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) { 215 | for (size_t i = 0; i < currentPC->size(); i++) { 216 | double x = currentPC->points[i].x; 217 | double y = currentPC->points[i].y; 218 | if (*xMax < x) { 219 | *xMax = x; 220 | } 221 | if (*xMin > x) { 222 | *xMin = x; 223 | } 224 | if (*yMax < y) { 225 | *yMax = y; 226 | } 227 | if (*yMin > y) { 228 | *yMin = y; 229 | } 230 | } 231 | } 232 | 233 | // --------------------------------------- 234 | // -----Populate map with cost values----- 235 | // --------------------------------------- 236 | void populateMap(NormalCloud::Ptr cloud_normals, std::vector &map,double xMax, double yMax, double xMin, double yMin, 237 | double cellResolution, int xCells, int yCells) { 238 | double deviation = param.deviation; 239 | std::vector > map_n; 240 | 241 | int the_size_of_map; 242 | the_size_of_map = (xMax-xMin)*(yMax-yMin)/(cellResolution*cellResolution); 243 | 244 | int conditions=2; 245 | map_n.clear(); 246 | map_n.resize(conditions); 247 | 248 | std::cout<<" the averanger z axis height is :"<< threshold_z1<< std::endl; 249 | 250 | for(int j =0; j < conditions; j++) 251 | { 252 | map_n[j].resize(the_size_of_map); // whether it is not big enough to put the point cloud in？currentPC->size()/cellResolution 253 | } 254 | 255 | //std::cout<< "test point 1"<size()/(cellResolution*cellResolution); 257 | for (size_t i = 0; i < currentPC->size(); i++) 258 | { 259 | double x = currentPC->points[i].x; 260 | double y = currentPC->points[i].y; 261 | double z = cloud_normals->points[i].normal_z; 262 | double z_axis = currentPC->points[i].z; 263 | 264 | double phi = acos(fabs(z)); 265 | int xCell, yCell; 266 | //std::cout<< i <<" i, the_size_of_map: "< (xCells * yCells)) { 273 | std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell << "\n"; 274 | } 275 | if (phi > deviation) 276 | { 277 | if(z_axis < threshold_z1 + averange_height) // this part is to calculate the height of ground 278 | { 279 | map_n[0][yCell * xCells + xCell] ++; 280 | }else 281 | { 282 | map_n[1][yCell * xCells + xCell] ++; 283 | } 284 | map[yCell * xCells + xCell]++; 285 | } 286 | 287 | if(map[yCell * xCells + xCell]==0) 288 | { 289 | map[yCell * xCells + xCell]=1; 290 | // map[yCell * xCells + xCell]--; 291 | } 292 | } 293 | } 294 | 295 | for(int m =0; msize(); m++) 296 | { 297 | if(map_n[0][m] > map_n[1][m]&&map_n[0][m]>4) 298 | { 299 | map[m] = -1; 300 | } 301 | } 302 | } 303 | 304 | 305 | 306 | // --------------------------------- 307 | // -----Generate Occupancy Grid----- 308 | // -------------------else if (countGrid[i] < 40&&countGrid[i]>20) { 309 | 310 | void genOccupancyGrid(std::vector &ocGrid, std::vector &countGrid, int size) { 311 | int buf = param.buffer; 312 | 313 | for (int i = 0; i < size; i++) { 314 | if (countGrid[i] < 10&&countGrid[i]>0) //10 is not changeds 315 | { 316 | ocGrid[i] = 0; 317 | } else if (countGrid[i] > 10&&countGrid[i]<20) 318 | { 319 | // std::cout<20) 325 | { 326 | ocGrid[i] = 200; // TODO Should be -1 327 | } else if (countGrid[i]>40) 328 | { 329 | ocGrid[i] = 150; // TODO Should be -1 330 | } else if (countGrid[i]=-1) 331 | { 332 | ocGrid[i] = 101;//125 333 | } 334 | 335 | } 336 | } 337 | 338 | // -------------- 339 | // -----Main----- 340 | // -------------- 341 | int main(int argc, char** argv) { 342 | /* Initialize ROS */ 343 | ros::init(argc, argv, "cloud_to_map_node"); 344 | ros::NodeHandle nh; 345 | ros::Subscriber sub = nh.subscribe("/voxel_filter", 1, callback); 346 | ros::Publisher pub = nh.advertise("map", 1); 347 | 348 | /* Initialize Dynamic Reconfigure */ 349 | dynamic_reconfigure::Server server; 350 | dynamic_reconfigure::Server::CallbackType f; 351 | f = boost::bind(&callbackReconfig, _1, _2); 352 | server.setCallback(f); 353 | 354 | /* Initialize Grid */ 355 | nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid); 356 | initGrid(grid); 357 | 358 | /* Finish initializing ROS */ 359 | mutex.lock(); 360 | ros::Rate loop_rate(param.loopRate); 361 | mutex.unlock(); 362 | 363 | /* Wait for first point cloud */ 364 | while(ros::ok() && newPointCloud == false){ 365 | ros::spinOnce(); 366 | loop_rate.sleep(); 367 | } 368 | 369 | /* Begin processing point clouds */ 370 | while (ros::ok()) { 371 | ros::spinOnce(); 372 | boost::unique_lock lck(mutex); 373 | if (newPointCloud || reconfig) { 374 | /* Update configuration status */ 375 | reconfig = false; 376 | newPointCloud = false; 377 | 378 | /* Record start time */ 379 | uint32_t sec = ros::Time::now().sec; 380 | uint32_t nsec = ros::Time::now().nsec; 381 | 382 | /* Calculate Surface Normals */ 383 | NormalCloud::Ptr cloud_normals(new NormalCloud); 384 | calcSurfaceNormals(currentPC, cloud_normals); 385 | 386 | /* Figure out size of matrix needed to store data. */ 387 | double xMax = 0, yMax = 0, xMin = 0, yMin = 0; 388 | calcSize(&xMax, &yMax, &xMin, &yMin); 389 | std::cout << "xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: " 390 | << yMin << "\n"; 391 | 392 | /* Determine resolution of grid (m/cell) */ 393 | double cellResolution = param.cellResolution; 394 | int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1; 395 | int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1; 396 | std::cout << "xCells: " << xCells << ", yCells: " << yCells << "\n"; 397 | 398 | /* Populate Map */ 399 | std::vector countGrid(yCells * xCells); 400 | 401 | /* Detect the ground*/ 402 | groundZfinder(xMax, yMax, xMin, yMin,cloud_normals); 403 | 404 | populateMap(cloud_normals, countGrid, xMax, yMax, xMin, yMin, cellResolution, xCells, yCells); 405 | 406 | /* Generate OccupancyGrid Data Vector */ 407 | std::vector ocGrid(yCells * xCells); 408 | genOccupancyGrid(ocGrid, countGrid, xCells * yCells); 409 | 410 | /* Update OccupancyGrid Object */ 411 | updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid); 412 | 413 | /* Release lock */ 414 | lck.unlock(); 415 | 416 | /* Record end time */ 417 | uint32_t esec = ros::Time::now().sec; 418 | uint32_t ensec = ros::Time::now().nsec; 419 | std::cout << "Seconds: " << esec - sec << "\nNSeconds: " << ensec - nsec << "\n"; 420 | 421 | /* Publish Occupancy Grid */ 422 | pub.publish(grid); 423 | } 424 | loop_rate.sleep(); 425 | } 426 | } 427 | -------------------------------------------------------------------------------- /cloud_to_map/docs/new1225pm9: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | //#include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | double threshold_z1 = 0; // should add a function to de terimnine the ground automatically 29 | double averange_height = 0; //0.7 30 | int diffThresh = 4; // the value above certain height - the value under certain height 31 | int greenThresh; // the thereshold of "may we can pass" 32 | int orangeThresh = 20; // the thereshold of "high obstacle" 33 | int redThresh = 40; // the thereshold of "very high obstacle" 34 | int wallThresh =10; // the thereshold of "can't pass obstacle" 35 | float cutoff_height =4.5; //0.18 div float cutoff_height =4.5; 36 | /* Define the two point cloud types used in this code */ 37 | typedef pcl::PointCloud PointCloud; 38 | typedef pcl::PointCloud NormalCloud; 39 | 40 | /* Global */ 41 | PointCloud::ConstPtr currentPC; 42 | bool newPointCloud = false; 43 | bool reconfig = false; 44 | //using namespace cv ; 45 | 46 | // ----------------------------------------------- 47 | // -----Define a structure to hold parameters----- 48 | // ----------------------------------------------- 49 | struct Param { 50 | std::string frame; 51 | double searchRadius; 52 | double deviation; 53 | int buffer; 54 | double loopRate; 55 | double cellResolution; 56 | 57 | }; 58 | 59 | // ----------------------------------- 60 | // -----Define default parameters----- 61 | // ----------------------------------- 62 | Param param; 63 | boost::mutex mutex; 64 | void loadDefaults(ros::NodeHandle& nh) { 65 | nh.param("frame", param.frame, "map"); 66 | nh.param("search_radius", param.searchRadius, 1.0); 67 | nh.param("deviation", param.deviation, 0.1); 68 | nh.param("buffer", param.buffer, 5); 69 | nh.param("loop_rate", param.loopRate, 10.0); 70 | nh.param("cell_resolution", param.cellResolution, 0.3); 71 | // nh.param("z1_thereshold", param.threshold_z1, 1); 72 | } 73 | // ----------------------------------------------- 74 | // ----- To find the ground automatcally ----- 75 | // ----------------------------------------------- 76 | void groundZfinder(int xMax, int yMax, int xMin, int yMin, NormalCloud::Ptr cloud_normals){ 77 | std::vector > groundDifReg; // to save the averange z aixs height 78 | double deviation = param.deviation; 79 | int xrange = xMax - xMin; 80 | int yrange = yMax - yMin; 81 | int blocks = 1; 82 | int rows =2; 83 | double averangeZheight; 84 | int counter_z = 0; 85 | int MapArea = (xMax-xMin)*(yMax-yMin); 86 | double matrix[MapArea]; 87 | 88 | groundDifReg.resize(rows); 89 | for(int j =0; j < rows; j++) 90 | { 91 | groundDifReg[j].resize(MapArea); //the size is rows*Map area the [0] saves the times appears 92 | } 93 | groundDifReg.clear(); 94 | 95 | for (size_t i = 0; i < currentPC->size(); i++) 96 | { 97 | 98 | int x = currentPC->points[i].x; 99 | int y = currentPC->points[i].y; 100 | int xCell = (x - xMin) ; 101 | int yCell = (y - yMin) ; 102 | 103 | double z = cloud_normals->points[i].normal_z; 104 | double z_axis = currentPC->points[i].z; 105 | double phi = acos(fabs(z)); 106 | 107 | if ( phi < deviation ){ 108 | 109 | double z_temp = 0; 110 | groundDifReg[0][yCell*xrange+xCell]++; 111 | //std::cout< the thereshold of Axis z 146 | 147 | 148 | // ------------------------------------------------------ 149 | // -----Update current PointCloud if msg is received----- 150 | // ------------------------------------------------------ 151 | void callback(const PointCloud::ConstPtr& msg) { 152 | boost::unique_lock(mutex); 153 | currentPC = msg; 154 | newPointCloud = true; 155 | } 156 | 157 | // ------------------------------------------ 158 | // -----Callback for Dynamic Reconfigure----- 159 | // ------------------------------------------ 160 | void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) { 161 | ROS_INFO("Reconfigure Request: %s %f %f %f %f", config.frame.c_str(), config.deviation, 162 | config.loop_rate, config.cell_resolution, config.search_radius); 163 | boost::unique_lock(mutex); 164 | param.frame = config.frame.c_str(); 165 | param.searchRadius = config.search_radius; 166 | param.deviation = config.deviation; 167 | param.buffer = config.buffer; 168 | param.loopRate = config.loop_rate; 169 | param.cellResolution = config.cell_resolution; 170 | reconfig = true; 171 | } 172 | 173 | // ---------------------------------------------------------------- 174 | // -----Calculate surface normals with a search radius of 0.05----- 175 | // ---------------------------------------------------------------- 176 | void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud::Ptr normals) { 177 | pcl::NormalEstimation ne; 178 | ne.setInputCloud(cloud); 179 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 180 | ne.setSearchMethod(tree); 181 | ne.setRadiusSearch(param.searchRadius); 182 | ne.compute(*normals); 183 | } 184 | 185 | // --------------------------------------- 186 | // -----Initialize Occupancy Grid Msg----- 187 | // --------------------------------------- 188 | void initGrid(nav_msgs::OccupancyGridPtr grid) { 189 | grid->header.seq = 1; 190 | grid->header.frame_id = param.frame; 191 | grid->info.origin.position.z = 0; 192 | grid->info.origin.orientation.w = 1; 193 | grid->info.origin.orientation.x = 0; 194 | grid->info.origin.orientation.y = 0; 195 | grid->info.origin.orientation.z = 0; 196 | } 197 | 198 | // ----------------------------------- 199 | // -----Update Occupancy Grid Msg----- 200 | // ----------------------------------- 201 | void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells, 202 | double originX, double originY, std::vector *ocGrid) { 203 | grid->header.seq++; 204 | grid->header.stamp.sec = ros::Time::now().sec; 205 | grid->header.stamp.nsec = ros::Time::now().nsec; 206 | grid->info.map_load_time = ros::Time::now(); 207 | grid->info.resolution = cellRes; 208 | grid->info.width = xCells; 209 | grid->info.height = yCells; 210 | grid->info.origin.position.x = originX; 211 | grid->info.origin.position.y = originY; 212 | grid->data = *ocGrid; 213 | } 214 | 215 | // ------------------------------------------ 216 | // -----Calculate size of Occupancy Grid----- 217 | // ------------------------------------------ 218 | void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) { 219 | for (size_t i = 0; i < currentPC->size(); i++) { 220 | double x = currentPC->points[i].x; 221 | double y = currentPC->points[i].y; 222 | if (*xMax < x) { 223 | *xMax = x; 224 | } 225 | if (*xMin > x) { 226 | *xMin = x; 227 | } 228 | if (*yMax < y) { 229 | *yMax = y; 230 | } 231 | if (*yMin > y) { 232 | *yMin = y; 233 | } 234 | } 235 | } 236 | 237 | // --------------------------------------- 238 | // -----Populate map with cost values----- 239 | // --------------------------------------- 240 | void populateMap(NormalCloud::Ptr cloud_normals, std::vector &map,double xMax, double yMax, double xMin, double yMin, 241 | double cellResolution, int xCells, int yCells) { 242 | double deviation = param.deviation; 243 | std::vector > map_n; 244 | 245 | int the_size_of_map; 246 | the_size_of_map = (xMax-xMin)*(yMax-yMin)/(cellResolution*cellResolution); 247 | 248 | int conditions=2; 249 | map_n.clear(); 250 | map_n.resize(conditions); 251 | 252 | std::cout<<" the averanger z axis height is :"<< threshold_z1<< std::endl; 253 | 254 | for(int j =0; j < conditions; j++) 255 | { 256 | map_n[j].resize(the_size_of_map); // whether it is not big enough to put the point cloud in？currentPC->size()/cellResolution 257 | } 258 | 259 | //std::cout<< "test point 1"<size()/(cellResolution*cellResolution); 261 | for (size_t i = 0; i < currentPC->size(); i++) 262 | { 263 | double x = currentPC->points[i].x; 264 | double y = currentPC->points[i].y; 265 | double z = cloud_normals->points[i].normal_z; 266 | double z_axis = currentPC->points[i].z; 267 | 268 | double phi = acos(fabs(z)); 269 | int xCell, yCell; 270 | //std::cout<< i <<" i, the_size_.of_map: "< (xCells * yCells)) { 277 | std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell << "\n"; 278 | } 279 | if (phi > deviation) 280 | { 281 | if(z_axis < averange_height) // this part is to calculate the height of ground threshold_z1 + 282 | { 283 | map_n[0][yCell * xCells + xCell] ++; 284 | }else 285 | { 286 | map_n[1][yCell * xCells + xCell] ++; 287 | } 288 | map[yCell * xCells + xCell]++; 289 | } 290 | 291 | if(map[yCell * xCells + xCell]==0) 292 | { 293 | map[yCell * xCells + xCell]=1; 294 | // map[yCell * xCells + xCell]--; 295 | } 296 | } 297 | } 298 | 299 | for(int m =0; msize(); m++) 300 | { 301 | if(map_n[0][m] > map_n[1][m]&&map_n[0][m] > 1) 302 | { 303 | map[m] = -1; 304 | } 305 | } 306 | } 307 | 308 | 309 | 310 | // --------------------------------- 311 | // -----Generate Occupancy Grid----- 312 | // -------------------else if (countGrid[i] < 40&&countGrid[i]>20) { 313 | 314 | void genOccupancyGrid(std::vector &ocGrid, std::vector &countGrid, int size) { 315 | int buf = param.buffer; 316 | 317 | for (int i = 0; i < size; i++) { 318 | if (countGrid[i] < 10&&countGrid[i]>0) //10 is not changeds 319 | { 320 | ocGrid[i] = 0; 321 | } else if (countGrid[i] > 10&&countGrid[i]<20) 322 | { 323 | // std::cout<20) 329 | { 330 | ocGrid[i] = 200; // TODO Should be -1 331 | } else if (countGrid[i]>40) 332 | { 333 | ocGrid[i] = 150; // TODO Should be -1 334 | } else if (countGrid[i]=-1) 335 | { 336 | ocGrid[i] = 101;//125 337 | } 338 | 339 | } 340 | } 341 | 342 | // -------------- 343 | // -----Main----- 344 | // -------------- 345 | int main(int argc, char** argv) { 346 | /* Initialize ROS */ 347 | ros::init(argc, argv, "cloud_to_map_node"); 348 | ros::NodeHandle nh; 349 | ros::Subscriber sub = nh.subscribe("/voxel_filter", 1, callback); 350 | ros::Publisher pub = nh.advertise("map", 1); 351 | 352 | /* Initialize Dynamic Reconfigure */ 353 | dynamic_reconfigure::Server server; 354 | dynamic_reconfigure::Server::CallbackType f; 355 | f = boost::bind(&callbackReconfig, _1, _2); 356 | server.setCallback(f); 357 | 358 | /* Initialize Grid */ 359 | nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid); 360 | initGrid(grid); 361 | 362 | /* Finish initializing ROS */ 363 | mutex.lock(); 364 | ros::Rate loop_rate(param.loopRate); 365 | mutex.unlock(); 366 | 367 | /* Wait for first point cloud */ 368 | while(ros::ok() && newPointCloud == false){ 369 | ros::spinOnce(); 370 | loop_rate.sleep(); 371 | } 372 | 373 | /* Begin processing point clouds */ 374 | while (ros::ok()) { 375 | ros::spinOnce(); 376 | boost::unique_lock lck(mutex); 377 | if (newPointCloud || reconfig) { 378 | /* Update configuration status */ 379 | reconfig = false; 380 | newPointCloud = false; 381 | 382 | /* Record start time */ 383 | uint32_t sec = ros::Time::now().sec; 384 | uint32_t nsec = ros::Time::now().nsec; 385 | 386 | /* Calculate Surface Normals */ 387 | NormalCloud::Ptr cloud_normals(new NormalCloud); 388 | calcSurfaceNormals(currentPC, cloud_normals); 389 | 390 | /* Figure out size of matrix needed to store data. */ 391 | double xMax = 0, yMax = 0, xMin = 0, yMin = 0; 392 | calcSize(&xMax, &yMax, &xMin, &yMin); 393 | std::cout << "xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: " 394 | << yMin << "\n"; 395 | 396 | /* Determine resolution of grid (m/cell) */ 397 | double cellResolution = param.cellResolution; 398 | int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1; 399 | int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1; 400 | std::cout << "xCells: " << xCells << ", yCells: " << yCells << "\n"; 401 | 402 | /* Populate Map */ 403 | std::vector countGrid(yCells * xCells); 404 | 405 | /* Detect the ground*/ 406 | groundZfinder(xMax, yMax, xMin, yMin,cloud_normals); 407 | 408 | populateMap(cloud_normals, countGrid, xMax, yMax, xMin, yMin, cellResolution, xCells, yCells); 409 | 410 | /* Generate OccupancyGrid Data Vector */ 411 | std::vector ocGrid(yCells * xCells); 412 | genOccupancyGrid(ocGrid, countGrid, xCells * yCells); 413 | 414 | /* Update OccupancyGrid Object */ 415 | updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid); 416 | 417 | /* Release lock */ 418 | lck.unlock(); 419 | 420 | /* Record end time */ 421 | uint32_t esec = ros::Time::now().sec; 422 | uint32_t ensec = ros::Time::now().nsec; 423 | std::cout << "Seconds: " << esec - sec << "\nNSeconds: " << ensec - nsec << "\n"; 424 | 425 | /* Publish Occupancy Grid */ 426 | pub.publish(grid); 427 | } 428 | loop_rate.sleep(); 429 | } 430 | } 431 | -------------------------------------------------------------------------------- /cloud_to_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cloud_to_map 4 | 0.0.0 5 | The cloud_to_map package 6 | 7 | 8 | 9 | 10 | jake 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | dynamic_reconfigure 44 | nav_msgs 45 | pcl 46 | pcl_ros 47 | roscpp 48 | dynamic_reconfigure 49 | nav_msgs 50 | pcl 51 | pcl_ros 52 | roscpp 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /cloud_to_map/src/cloud_to_map/cfg/cloud_to_map_nodeConfig.py: -------------------------------------------------------------------------------- 1 | ## ********************************************************* 2 | ## 3 | ## File autogenerated for the cloud_to_map package 4 | ## by the dynamic_reconfigure package. 5 | ## Please do not edit. 6 | ## 7 | ## ********************************************************/ 8 | 9 | from dynamic_reconfigure.encoding import extract_params 10 | 11 | inf = float('inf') 12 | 13 | config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Frame the Occupancy Grid should connect to', 'max': '', 'cconsttype': 'const char * const', 'ctype': 'std::string', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'frame', 'edit_method': '', 'default': 'map', 'level': 0, 'min': '', 'type': 'str'}, {'srcline': 259, 'description': 'Search radius for approximating the normal vectors of a point', 'max': 1.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'search_radius', 'edit_method': '', 'default': 0.06, 'level': 0, 'min': 0.0001, 'type': 'double'}, {'srcline': 259, 'description': 'Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)', 'max': 1.57079632679, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'deviation', 'edit_method': '', 'default': 0.78539816339, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)', 'max': 100, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'buffer', 'edit_method': '', 'default': 0, 'level': 0, 'min': 0, 'type': 'int'}, {'srcline': 259, 'description': 'Rate in Hz the node will attempt to run', 'max': 1000.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'loop_rate', 'edit_method': '', 'default': 10.0, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Resolution of the Occupancy Grid output (in m/cell)', 'max': 1.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'cell_resolution', 'edit_method': '', 'default': 0.7, 'level': 0, 'min': 0.0, 'type': 'double'}], 'type': '', 'id': 0} 14 | 15 | min = {} 16 | max = {} 17 | defaults = {} 18 | level = {} 19 | type = {} 20 | all_level = 0 21 | 22 | #def extract_params(config): 23 | # params = [] 24 | # params.extend(config['parameters']) 25 | # for group in config['groups']: 26 | # params.extend(extract_params(group)) 27 | # return params 28 | 29 | for param in extract_params(config_description): 30 | min[param['name']] = param['min'] 31 | max[param['name']] = param['max'] 32 | defaults[param['name']] = param['default'] 33 | level[param['name']] = param['level'] 34 | type[param['name']] = param['type'] 35 | all_level = all_level | param['level'] 36 | 37 | -------------------------------------------------------------------------------- /cloud_to_map/src/cloud_to_map_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include //autogenerated for the cloud_to_map package by the dynamic_reconfigure package. 18 | 19 | /* Define the two point cloud types used in this code */ 20 | typedef pcl::PointCloud PointCloud; 21 | typedef pcl::PointCloud NormalCloud; 22 | 23 | /* Global */ 24 | PointCloud::ConstPtr currentPC; 25 | bool newPointCloud = false; 26 | bool reconfig = false; 27 | 28 | // ----------------------------------------------- 29 | // -----Define a structure to hold parameters----- 30 | // ----------------------------------------------- 31 | struct Param 32 | { 33 | std::string frame; 34 | double searchRadius; 35 | double deviation; 36 | int buffer; 37 | double loopRate; 38 | double cellResolution; 39 | }; 40 | 41 | // ----------------------------------- 42 | // -----Define default parameters----- 43 | //If the value cannot be retrieved from the server, default_val is used instead. 44 | // ----------------------------------- 45 | Param param; 46 | boost::mutex mutex; //多线程 47 | void loadDefaults(ros::NodeHandle& nh) 48 | { 49 | nh.param("frame", param.frame, "world"); 50 | nh.param("search_radius", param.searchRadius, 0.05); 51 | nh.param("deviation", param.deviation, 0.78539816339); 52 | nh.param("buffer", param.buffer, 5); 53 | nh.param("loop_rate", param.loopRate, 10.0); 54 | nh.param("cell_resolution", param.cellResolution, 0.05); 55 | } 56 | 57 | 58 | // ------------------------------------------ 59 | // -----Callback for Dynamic Reconfigure-----调用动态参数配置 60 | // ------------------------------------------ 61 | void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) 62 | { 63 | 64 | boost::unique_lock(mutex); 65 | param.frame = config.frame.c_str(); 66 | param.searchRadius = config.search_radius; 67 | param.deviation = config.deviation; 68 | param.buffer = config.buffer; 69 | param.loopRate = config.loop_rate; 70 | //param.cellResolution = config.cell_resolution; 71 | param.cellResolution=0.05; 72 | reconfig = true; 73 | 74 | 75 | ROS_INFO("\n\nReconfigure Request: \nframe:%s \ndeviation:%f \nloop_rate:%f \ncell_resolution:%f \nsearch_radius:%f", 76 | param.frame.c_str(), param.deviation,param.loopRate,param.cellResolution, param.searchRadius); 77 | 78 | } 79 | 80 | // ------------------------------------------------------ 81 | // -----Update current PointCloud if msg is received----- 82 | // ------------------------------------------------------ 83 | void callback(const PointCloud::ConstPtr& msg) 84 | { 85 | boost::unique_lock(mutex); //unique_lock是write lock。被锁后不允许其他线程执行被shared_lock或unique_lock的代码。 86 | //在写操作时，一般用这个，可以同时限制unique_lock的写和share_lock的读。 87 | //ROS_INFO("callback is finshed \n"); 88 | currentPC = msg; 89 | newPointCloud = true; 90 | } 91 | 92 | // ---------------------------------------------------------------- 93 | // -----Calculate surface normals with a search radius of 0.05----- 94 | // ---------------------------------------------------------------- 95 | void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud::Ptr normals) 96 | { 97 | pcl::NormalEstimation ne; 98 | ne.setInputCloud(cloud); 99 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 100 | ne.setSearchMethod(tree); 101 | ne.setRadiusSearch(param.searchRadius); 102 | ne.compute(*normals); 103 | } 104 | 105 | // --------------------------------------- 106 | // -----Initialize Occupancy Grid Msg----- 107 | // --------------------------------------- 108 | void initGrid(nav_msgs::OccupancyGridPtr grid) 109 | { 110 | grid->header.seq = 1; 111 | grid->header.frame_id = param.frame; //参考frame 112 | grid->info.origin.position.z = 0; 113 | grid->info.origin.orientation.w = 1; 114 | grid->info.origin.orientation.x = 0; 115 | grid->info.origin.orientation.y = 0; 116 | grid->info.origin.orientation.z = 0; 117 | std::cout<<"initGrid finshed!\n"<< std::endl; 118 | /* nav_msgs/OccupancyGrid 119 | # This represents a 2-D grid map, in which each cell represents the probability of occupancy. 120 | Header header 121 | 122 | #MetaData for the map 123 | MapMetaData info 124 | 125 | # The map data, in row-major order, starting with (0,0). Occupancy 126 | # probabilities are in the range [0,100]. Unknown is -1. 127 | int8[] data 128 | 129 | Header header 130 | uint32 seq 131 | time stamp 132 | string frame_id 133 | MapMetaData info 134 | time map_load_time 135 | float32 resolution 136 | uint32 width 137 | uint32 height 138 | geometry_msgs/Pose origin 139 | geometry_msgs/Point position 140 | float64 x 141 | float64 y 142 | float64 z 143 | geometry_msgs/Quaternion orientation 144 | float64 x 145 | float64 y 146 | float64 z 147 | float64 w 148 | int8[] data */ 149 | } 150 | 151 | // ------------------------------------------ 152 | // -----Calculate size of Occupancy Grid----- 153 | // ------------------------------------------ 154 | void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) 155 | { 156 | for (size_t i = 0; i < currentPC->size(); i++) 157 | { 158 | double x = currentPC->points[i].x; 159 | double y = currentPC->points[i].y; 160 | if (*xMax < x) { 161 | *xMax = x; 162 | } 163 | if (*xMin > x) { 164 | *xMin = x; 165 | } 166 | if (*yMax < y) { 167 | *yMax = y; 168 | } 169 | if (*yMin > y) { 170 | *yMin = y; 171 | } 172 | } 173 | } 174 | 175 | // --------------------------------------- 176 | // -----Populate 填充 map with cost values----- 177 | // --------------------------------------- 178 | void populateMap(NormalCloud::Ptr cloud_normals, std::vector &countGrid, double xMin, double yMin, 179 | double cellResolution, int xCells, int yCells) 180 | { 181 | double deviation = param.deviation; 182 | 183 | for (size_t i = 0; i < currentPC->size(); i++) //遍历每个点 184 | { 185 | double x = currentPC->points[i].x; 186 | double y = currentPC->points[i].y; 187 | double z = cloud_normals->points[i].normal_z; 188 | 189 | int xCell, yCell; 190 | //TODO implement cutoff height 191 | xCell = (int) ((x - xMin) / cellResolution); //取整后默认按照cellResolution将点分配到ｃｅｌｌ 192 | yCell = (int) ((y - yMin) / cellResolution); 193 | if ((yCell * xCells + xCell) > (xCells * yCells)) 194 | std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell<< "\n"; 195 | 196 | double normal_value = acos(fabs(z));//值域　０－－ｐｈｉ 地面fabs(z)应该是１　acos是０，最小值 197 | if (normal_value > deviation) //根据acos函数的递减性质，非地面点的值应该都比地面点大。可以设置deviation值，决定障碍物点的阈值 198 | countGrid[yCell * xCells + xCell]++; //统计一个ｃｅｌｌ中垂直方向满足条件的点数 199 | //else 200 | //countGrid[yCell * xCells + xCell]--; 201 | } 202 | } 203 | 204 | // --------------------------------- 205 | // -----Generate Occupancy Grid----- 206 | // --------------------------------- 207 | void genOccupancyGrid(std::vector &ocGrid, std::vector &countGrid, int size) 208 | { 209 | int buf = param.buffer; 210 | for (int i = 0; i < size; i++) //size:xCells * yCells 211 | { 212 | if (countGrid[i] < buf && countGrid[i]>0) 213 | ocGrid[i] = 0; 214 | else if (countGrid[i] > buf) 215 | ocGrid[i] = 100; 216 | else if (countGrid[i] == 0) 217 | ocGrid[i] = 0; // TODO Should be -1 218 | } 219 | //std::cout<<" genOccupancyGrid finshed!"<< std::endl; 220 | 221 | } 222 | // ----------------------------------- 223 | // -----Update Occupancy Grid Msg----- 224 | // ----------------------------------- 225 | void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells, 226 | double originX, double originY, std::vector *ocGrid) 227 | { 228 | grid->header.seq++; 229 | grid->header.stamp.sec = ros::Time::now().sec; 230 | grid->header.stamp.nsec = ros::Time::now().nsec; 231 | grid->info.map_load_time = ros::Time::now(); 232 | grid->info.resolution = cellRes; 233 | grid->info.width = xCells; 234 | grid->info.height = yCells; 235 | grid->info.origin.position.x = originX; //minx 236 | grid->info.origin.position.y = originY; //miny 237 | grid->data = *ocGrid; 238 | std::cout<<"updateGrid finshed!!"<< std::endl; 239 | 240 | } 241 | 242 | int main(int argc, char** argv) 243 | { 244 | /* Initialize ROS */ 245 | ros::init(argc, argv, "cloud_to_map_node"); 246 | ros::NodeHandle nh; 247 | ros::Subscriber sub = nh.subscribe("/point_cloud_raw", 1, callback); 248 | ros::Publisher pub = nh.advertise("/map", 1); 249 | 250 | /* Initialize Dynamic Reconfigure 　ＲＯＳ动态参数配置*/ 251 | dynamic_reconfigure::Server server; 252 | dynamic_reconfigure::Server::CallbackType f; 253 | f = boost::bind(&callbackReconfig, _1, _2); 254 | //std::cout<<"*************************\n"; //test 255 | server.setCallback(f); 256 | 257 | /* Initialize Grid */ 258 | nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid); 259 | initGrid(grid); 260 | /* Finish initializing ROS */ 261 | mutex.lock(); 262 | ros::Rate loop_rate(param.loopRate); 263 | mutex.unlock(); 264 | 265 | /* Wait for first point cloud */ 266 | while(ros::ok() && newPointCloud == false) 267 | { 268 | ros::spinOnce();//ROS消息回调处理函数。它俩通常会出现在ROS的主循环中，程序需要不断调用ros::spin() 或 ros::spinOnce()， 269 | //两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序。 270 | //所接到的消息并不是立刻就被处理，而是必须要等到ros::spin()或ros::spinOnce()执行的时候才被调用， 271 | loop_rate.sleep(); 272 | } 273 | 274 | /* Begin processing point clouds */ 275 | while (ros::ok()) 276 | { 277 | ros::spinOnce();//用后还可以继续执行之后的程序。 278 | boost::unique_lock lck(mutex); 279 | if (newPointCloud || reconfig) 280 | { 281 | /* Update configuration status */ 282 | reconfig = false; //调用动态参数配置时　＝true 283 | newPointCloud = false; //在回调函数=true 284 | 285 | /* Record start time */ 286 | uint32_t sec = ros::Time::now().sec; //秒 287 | uint32_t nsec = ros::Time::now().nsec;//纳秒 288 | 289 | /* Calculate Surface Normals */ 290 | NormalCloud::Ptr cloud_normals(new NormalCloud); 291 | calcSurfaceNormals(currentPC, cloud_normals); 292 | 293 | /* Figure out size of matrix needed to store data. */ 294 | double xMax = 0, yMax = 0, xMin = 0, yMin = 0; 295 | calcSize(&xMax, &yMax, &xMin, &yMin); //Calculate size of Occupancy Grid 296 | //std::cout <<"size of matrix needed to store data:"<<"xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: "<< yMin << "\n"; 297 | 298 | /* Determine resolution of grid (m/cell) */ 299 | double cellResolution = param.cellResolution; 300 | int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1; 301 | int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1; 302 | std::cout <<"the number of cells: " < countGrid(yCells * xCells); 306 | populateMap(cloud_normals, countGrid, xMin, yMin, cellResolution, xCells, yCells); 307 | 308 | /* Generate OccupancyGrid Data Vector */ 309 | std::vector ocGrid(yCells * xCells); //存储每个ｃｅｌｌ的值　　０或者１００ 310 | genOccupancyGrid(ocGrid, countGrid, xCells * yCells); 311 | 312 | /* Update OccupancyGrid Object */ 313 | updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid); 314 | 315 | /* Release lock */ 316 | lck.unlock(); 317 | 318 | /* Record end time */ 319 | uint32_t esec = ros::Time::now().sec; 320 | uint32_t ensec = ros::Time::now().nsec; 321 | std::cout << "Seconds: " << esec - sec << " NSeconds: " << ensec - nsec << "\n\n"; 322 | 323 | /* Publish Occupancy Grid */ 324 | pub.publish(grid); 325 | } 326 | loop_rate.sleep(); 327 | } 328 | } 329 | -------------------------------------------------------------------------------- /cloud_to_map/src/data_source.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 35 | */ 36 | 37 | 38 | #ifndef PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_ 39 | #define PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_ 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | #if defined (_WIN32) || defined(_WIN64) 55 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ) 56 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::Normal) 57 | #endif 58 | 59 | #include 60 | 61 | namespace pcl 62 | { 63 | namespace gpu 64 | { 65 | struct DataSource 66 | { 67 | const static int k = 32; 68 | const static int max_elements = 500; 69 | 70 | PointCloud::Ptr cloud; 71 | PointCloud::Ptr surface; 72 | IndicesPtr indices; 73 | 74 | PointCloud::Ptr normals; 75 | PointCloud::Ptr normals_surface; 76 | float radius; 77 | 78 | std::vector< std::vector > neighbors_all; 79 | std::vector sizes; 80 | int max_nn_size; 81 | 82 | DataSource(const std::string& file = "d:/office_chair_model.pcd") 83 | : cloud(new PointCloud()), surface(new PointCloud()), indices( new std::vector() ), 84 | normals(new PointCloud()), normals_surface(new PointCloud()) 85 | { 86 | PCDReader pcd; 87 | pcd.read(file, *cloud); 88 | 89 | PointXYZ minp, maxp; 90 | pcl::getMinMax3D(*cloud, minp, maxp); 91 | float sz = (maxp.x - minp.x + maxp.y - minp.y + maxp.z - minp.z) / 3; 92 | radius = sz / 15; 93 | } 94 | 95 | void generateColor() 96 | { 97 | size_t cloud_size = cloud->points.size(); 98 | for(size_t i = 0; i < cloud_size; ++i) 99 | { 100 | PointXYZ& p = cloud->points[i]; 101 | 102 | int r = std::max(1, std::min(255, static_cast((double(rand())/RAND_MAX)*255))); 103 | int g = std::max(1, std::min(255, static_cast((double(rand())/RAND_MAX)*255))); 104 | int b = std::max(1, std::min(255, static_cast((double(rand())/RAND_MAX)*255))); 105 | 106 | *reinterpret_cast(&p.data[3]) = (b << 16) + (g << 8) + r; 107 | } 108 | } 109 | 110 | void estimateNormals() 111 | { 112 | pcl::NormalEstimation ne; 113 | ne.setInputCloud (cloud); 114 | ne.setSearchMethod (pcl::search::KdTree::Ptr (new pcl::search::KdTree)); 115 | ne.setKSearch (k); 116 | //ne.setRadiusSearch (radius); 117 | 118 | ne.compute (*normals); 119 | } 120 | 121 | void runCloudViewer() const 122 | { 123 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 124 | viewer.showCloud (cloud); 125 | while (!viewer.wasStopped ()) {} 126 | } 127 | 128 | void findKNNeghbors() 129 | { 130 | KdTreeFLANN::Ptr kdtree(new KdTreeFLANN); 131 | kdtree->setInputCloud(cloud); 132 | 133 | size_t cloud_size = cloud->points.size(); 134 | 135 | std::vector dists; 136 | neighbors_all.resize(cloud_size); 137 | for(size_t i = 0; i < cloud_size; ++i) 138 | { 139 | kdtree->nearestKSearch(cloud->points[i], k, neighbors_all[i], dists); 140 | sizes.push_back((int)neighbors_all[i].size()); 141 | } 142 | max_nn_size = *max_element(sizes.begin(), sizes.end()); 143 | } 144 | 145 | void findRadiusNeghbors(float radius = -1) 146 | { 147 | radius = radius == -1 ? this->radius : radius; 148 | 149 | KdTreeFLANN::Ptr kdtree(new KdTreeFLANN); 150 | kdtree->setInputCloud(cloud); 151 | 152 | size_t cloud_size = cloud->points.size(); 153 | 154 | std::vector dists; 155 | neighbors_all.resize(cloud_size); 156 | for(size_t i = 0; i < cloud_size; ++i) 157 | { 158 | kdtree->radiusSearch(cloud->points[i], radius, neighbors_all[i], dists); 159 | sizes.push_back((int)neighbors_all[i].size()); 160 | } 161 | max_nn_size = *max_element(sizes.begin(), sizes.end()); 162 | } 163 | 164 | void getNeghborsArray(std::vector& data) 165 | { 166 | data.resize(max_nn_size * neighbors_all.size()); 167 | pcl::gpu::PtrStep ps(&data[0], max_nn_size * sizeof(int)); 168 | for(size_t i = 0; i < neighbors_all.size(); ++i) 169 | copy(neighbors_all[i].begin(), neighbors_all[i].end(), ps.ptr(i)); 170 | } 171 | 172 | void generateSurface() 173 | { 174 | surface->points.clear(); 175 | for(size_t i = 0; i < cloud->points.size(); i+= 10) 176 | surface->points.push_back(cloud->points[i]); 177 | surface->width = surface->points.size(); 178 | surface->height = 1; 179 | 180 | if (!normals->points.empty()) 181 | { 182 | normals_surface->points.clear(); 183 | for(size_t i = 0; i < normals->points.size(); i+= 10) 184 | normals_surface->points.push_back(normals->points[i]); 185 | 186 | normals_surface->width = surface->points.size(); 187 | normals_surface->height = 1; 188 | } 189 | } 190 | 191 | void generateIndices(size_t step = 100) 192 | { 193 | indices->clear(); 194 | for(size_t i = 0; i < cloud->points.size(); i += step) 195 | indices->push_back(i); 196 | } 197 | 198 | struct Normal2PointXYZ 199 | { 200 | PointXYZ operator()(const Normal& n) const 201 | { 202 | PointXYZ xyz; 203 | xyz.x = n.normal[0]; 204 | xyz.y = n.normal[1]; 205 | xyz.z = n.normal[2]; 206 | return xyz; 207 | } 208 | }; 209 | }; 210 | } 211 | } 212 | 213 | #endif /* PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_ */ 214 | -------------------------------------------------------------------------------- /data/color/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/color/1.png -------------------------------------------------------------------------------- /data/color/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/color/2.png -------------------------------------------------------------------------------- /data/color/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/color/3.png -------------------------------------------------------------------------------- /data/color/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/color/4.png -------------------------------------------------------------------------------- /data/color/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/color/5.png -------------------------------------------------------------------------------- /data/depth/1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/depth/1.pgm -------------------------------------------------------------------------------- /data/depth/2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/depth/2.pgm -------------------------------------------------------------------------------- /data/depth/3.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/depth/3.pgm -------------------------------------------------------------------------------- /data/depth/4.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/depth/4.pgm -------------------------------------------------------------------------------- /data/depth/5.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjianqu/PointCloud_to_Map/a85a75218875787ea435b27ce69ca76e784b67aa/data/depth/5.pgm -------------------------------------------------------------------------------- /data/pose.txt: -------------------------------------------------------------------------------- 1 | -0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042 2 | -0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662 3 | -0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536 4 | -1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178 5 | -1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741 -------------------------------------------------------------------------------- /pcl_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pcl_test) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | ## Compile as C++11, supported in ROS Kinetic and newer 7 | add_compile_options(-std=c++11) 8 | 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | pcl_conversions 13 | ) 14 | 15 | 16 | find_package(OpenCV 3.4 REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | find_package( PCL REQUIRED)#COMPONENTS )common io visualization filters) 19 | 20 | 21 | catkin_package( 22 | # INCLUDE_DIRS include 23 | # LIBRARIES serve_test 24 | # CATKIN_DEPENDS roscpp rospy std_msgs 25 | # DEPENDS system_lib 26 | ) 27 | 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | 33 | ${OpenCV_INCLUDE_DIRS} 34 | ${PCL_INCLUDE_DIRS} 35 | ) 36 | 37 | add_definitions( ${PCL_DEFINITIONS} ) 38 | 39 | 40 | set(LIBS 41 | ${OpenCV_LIBS} 42 | # ${PCL_COMMON_LIBRARIES} 43 | # ${PCL_IO_LIBRARIES} 44 | ${PCL_LIBRARIES} 45 | ${EIGEN3_LIBS} 46 | ) 47 | 48 | 49 | 50 | 51 | add_executable(pcl_test src/pcl_test.cpp) 52 | target_link_libraries(pcl_test ${catkin_LIBRARIES} ${LIBS}) 53 | 54 | -------------------------------------------------------------------------------- /pcl_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pcl_test 4 | 0.0.0 5 | The pcl_test package 6 | 7 | 8 | 9 | 10 | chen 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | pcl_conversions 55 | roscpp 56 | rospy 57 | pcl_conversions 58 | roscpp 59 | rospy 60 | pcl_conversions 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /pcl_test/src/pcl_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | //#include 25 | //#include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | 34 | // 定义点云使用的格式：这里用的是XYZRGB 35 | typedef pcl::PointXYZRGB PointT; 36 | typedef pcl::PointCloud PointCloud; 37 | 38 | 39 | using namespace std; 40 | 41 | void PCLTest(); 42 | ros::Publisher pub_pcs; 43 | ros::Publisher pub_pcr; 44 | 45 | int main(int argc, char** argv) 46 | { 47 | ros::init(argc, argv, "pcl_test_node");//初始化节点 48 | ros::start();//启动节点 49 | 50 | ros::NodeHandle nh; 51 | pub_pcs=nh.advertise("/point_cloud",1); 52 | pub_pcr=nh.advertise("/point_cloud_raw",1); 53 | 54 | ROS_INFO_STREAM("Initing"); 55 | 56 | 57 | PCLTest(); 58 | 59 | ROS_INFO_STREAM("pcl_test节点结束"); 60 | return 0; 61 | } 62 | 63 | void PCLTest() 64 | { 65 | 66 | vector colorImgs, depthImgs; // 彩色图和深度图 67 | vector poses; // 相机位姿 68 | 69 | ifstream fin("./data/pose.txt"); 70 | if (!fin) 71 | { 72 | cerr<<"cannot find pose file"<>data[i]; 86 | } 87 | Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); 88 | Eigen::Isometry3d T(q); 89 | T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] )); 90 | poses.push_back( T ); 91 | } 92 | 93 | // 计算点云并拼接 94 | // 相机内参 95 | double cx = 325.5; 96 | double cy = 253.5; 97 | double fx = 518.0; 98 | double fy = 519.0; 99 | double depthScale = 1000.0; 100 | 101 | cout<<"正在将图像转换为点云..."< ( v )[u]; // 深度值 121 | if ( d==0 ) continue; // 为0表示没有测量到 122 | if ( d >= 3500 ) continue; // 深度太大时不稳定，去掉 123 | Eigen::Vector3d point; 124 | point[2] = double(d)/depthScale; 125 | point[0] = (u-cx)*point[2]/fx; 126 | point[1] = (v-cy)*point[2]/fy; 127 | Eigen::Vector3d pointWorld = T*point; 128 | 129 | 130 | 131 | PointT p ; 132 | p.x = pointWorld[0]; 133 | p.y = pointWorld[1]; 134 | p.z = pointWorld[2]; 135 | p.b = color.data[ v*color.step+u*color.channels() ]; 136 | p.g = color.data[ v*color.step+u*color.channels()+1 ]; 137 | p.r = color.data[ v*color.step+u*color.channels()+2 ]; 138 | current->points.push_back( p ); 139 | } 140 | 141 | //利用统计滤波器方法去除孤立点。 142 | PointCloud::Ptr tmp ( new PointCloud ); 143 | pcl::StatisticalOutlierRemoval statistical_filter; 144 | statistical_filter.setMeanK(50); 145 | statistical_filter.setStddevMulThresh(1.0); 146 | statistical_filter.setInputCloud(current); 147 | statistical_filter.filter( *tmp ); 148 | (*pointCloud) += *tmp; 149 | 150 | 151 | //viewer.showCloud(pointCloud); 152 | //getchar(); 153 | } 154 | getchar(); 155 | pointCloud->is_dense = false; 156 | cout<<"点云共有"<size()<<"个点."< voxel_filter; 160 | voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); //分辨率 161 | PointCloud::Ptr tmp ( new PointCloud ); 162 | voxel_filter.setInputCloud( pointCloud ); 163 | voxel_filter.filter( *tmp ); 164 | tmp->swap(*pointCloud); 165 | 166 | cout<<"滤波之后，点云共有"<size()<<"个点."<