├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── data ├── ParameterTuner.png ├── downloadData.txt ├── example.png └── street.png ├── display └── app1.mlapp ├── include ├── dsp_dynamic.h ├── dsp_dynamic_multiple_neighbors.h └── dsp_static.h ├── launch └── mapping.launch ├── package.xml ├── readme.md ├── rviz ├── boxes.rviz ├── future_status.rviz └── original_pointcloud.rviz ├── script └── set_map_parameters.py └── src └── map_sim_example.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | /cmake-build-debug/ 2 | /cmake-build-release/ 3 | /.idea/ 4 | *.bag 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dynamic_occpuancy_map) 3 | 4 | add_definitions(-std=c++14 -g -O3 -ftree-vectorize -ffast-math -march=native -lpthread) 5 | #add_definitions(-std=c++14) 6 | 7 | find_package(PCL REQUIRED) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | message_filters 14 | sensor_msgs # 15 | image_transport 16 | ) 17 | 18 | 19 | add_definitions(${PCL_DEFINITIONS}) 20 | 21 | catkin_package( 22 | 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ${PCL_INCLUDE_DIRS} 29 | ) 30 | 31 | include_directories(/usr/local/include/munkres) 32 | set(MunkresLIB 33 | /usr/local/lib/libmunkres.a 34 | ) 35 | 36 | add_executable(map_sim_example src/map_sim_example.cpp) 37 | target_link_libraries(map_sim_example ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${MunkresLIB}) 38 | 39 | 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Gang Chen 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /data/ParameterTuner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/g-ch/DSP-map/85507e180435614087da3b96ea89baf0ca02ea59/data/ParameterTuner.png -------------------------------------------------------------------------------- /data/downloadData.txt: -------------------------------------------------------------------------------- 1 | 2 | You can download the following bag file and put it in the data folder to test our map with launch file. 3 | 4 | https://drive.google.com/file/d/1go4ALTe8CqaBY2wjZJzkUCmdlBI7yAAU/view?usp=sharing -------------------------------------------------------------------------------- /data/example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/g-ch/DSP-map/85507e180435614087da3b96ea89baf0ca02ea59/data/example.png -------------------------------------------------------------------------------- /data/street.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/g-ch/DSP-map/85507e180435614087da3b96ea89baf0ca02ea59/data/street.png -------------------------------------------------------------------------------- /display/app1.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/g-ch/DSP-map/85507e180435614087da3b96ea89baf0ca02ea59/display/app1.mlapp -------------------------------------------------------------------------------- /include/dsp_static.h: -------------------------------------------------------------------------------- 1 | /************************************************************************** 2 | 3 | Copyright <2022> 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | 12 | Author: Gang Chen 13 | 14 | Date: 2021/8/19 15 | 16 | Description: This is the head file for the DSP map with static model. The current occupancy status of dynamic obstacles can represented very quickly without generating any trail noise. But you cannot have prediction result. Therefore, this is a Type one dynamic occupancy map. 17 | 18 | **************************************************************************/ 19 | 20 | 21 | #include 22 | #include 23 | #include 24 | #include "iostream" 25 | #include 26 | #include 27 | #include "Eigen/Eigen" 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include "munkres.h" 34 | 35 | using namespace std; 36 | 37 | /** Parameters for the map **/ 38 | #define MAP_LENGTH_VOXEL_NUM 50 39 | #define MAP_WIDTH_VOXEL_NUM 50 40 | #define MAP_HEIGHT_VOXEL_NUM 30 41 | #define VOXEL_RESOLUTION 0.2 42 | #define ANGLE_RESOLUTION 3 43 | #define MAX_PARTICLE_NUM_VOXEL 10 44 | 45 | /// Note: Prediction is meaningless when using a static model! But we still leave the interface so you can directly switch between dsp_dynamic and dsp_static by using a different head file. 46 | #define PREDICTION_TIMES 1 47 | static const float prediction_future_time[PREDICTION_TIMES] = {0.05f}; //unit: second. The first value is used to compensate the delay caused by the map. 48 | 49 | const int half_fov_h = 42; // can be divided by ANGLE_RESOLUTION. If not, modify ANGLE_RESOLUTION or make half_fov_h a smaller value than the real FOV angle 50 | const int half_fov_v = 27; // can be divided by ANGLE_RESOLUTION. If not, modify ANGLE_RESOLUTION or make half_fov_h a smaller value than the real FOV angle 51 | 52 | string particle_save_folder = "."; 53 | /** END **/ 54 | 55 | 56 | static const int observation_pyramid_num_h = (int)half_fov_h * 2 / ANGLE_RESOLUTION; 57 | static const int observation_pyramid_num_v = (int)half_fov_v * 2 / ANGLE_RESOLUTION; 58 | static const int observation_pyramid_num = observation_pyramid_num_h * observation_pyramid_num_v; 59 | 60 | static const int VOXEL_NUM = MAP_LENGTH_VOXEL_NUM*MAP_WIDTH_VOXEL_NUM*MAP_HEIGHT_VOXEL_NUM; 61 | static const int PYRAMID_NUM = 360*180/ANGLE_RESOLUTION/ANGLE_RESOLUTION; 62 | static const int SAFE_PARTICLE_NUM = VOXEL_NUM * MAX_PARTICLE_NUM_VOXEL + 1e5; 63 | static const int SAFE_PARTICLE_NUM_VOXEL = MAX_PARTICLE_NUM_VOXEL * 5; 64 | static const int SAFE_PARTICLE_NUM_PYRAMID = SAFE_PARTICLE_NUM/PYRAMID_NUM * 2; 65 | 66 | //An estimated number. If the observation points are too dense (over 100 points in one pyramid), the overflowed points will be ignored. It is suggested to use a voxel filter to the original point cloud. 67 | static const int observation_max_points_num_one_pyramid = 100; 68 | 69 | #define GAUSSIAN_RANDOMS_NUM 10000000 70 | 71 | #define O_MAKE_VALID 1 // use |= operator 72 | #define O_MAKE_INVALID 0 // use &= operator 73 | 74 | # define M_PIf32 3.14159265358979323846 /* pi */ 75 | # define M_PI_2f32 1.57079632679489661923 /* pi/2 */ 76 | 77 | using namespace std; 78 | 79 | /** Struct for an individual particle**/ 80 | struct Particle{ 81 | float px; 82 | float py; 83 | float pz; 84 | float vx; 85 | float vy; 86 | float vz; 87 | float weight; 88 | int voxel_index; 89 | }; 90 | 91 | struct ClusterFeature{ 92 | float center_x = 0.f; 93 | float center_y = 0.f; 94 | float center_z = 0.f; 95 | int point_num = 0; 96 | int match_cluster_seq = -1; 97 | float vx = -10000.f; 98 | float vy = -10000.f; 99 | float vz = -10000.f; 100 | float v = 0.f; 101 | }; 102 | 103 | 104 | // flag value 0: invalid, value 1: valid but not newborn, value 3: valid newborn 7: Recently predicted 105 | /// Container for voxels h particles 106 | // 1.flag 2.vx 3.vy 4.vz 5.px 6.py 7.pz 107 | // 8.weight 9.update time 108 | static float voxels_with_particle[VOXEL_NUM][SAFE_PARTICLE_NUM_VOXEL][9]; 109 | 110 | // 1. objects number; 2-4. Avg vx, vy, vz; 5-. Future objects number 111 | static const int voxels_objects_number_dimension = 4 + PREDICTION_TIMES; 112 | static float voxels_objects_number[VOXEL_NUM][voxels_objects_number_dimension]; 113 | 114 | /// Container for pyramids 115 | // 0.flag 1.particle voxel index 2.particle index inside voxel 116 | static int pyramids_in_fov[observation_pyramid_num][SAFE_PARTICLE_NUM_PYRAMID][3]; 117 | 118 | // 1.neighbors num 2-10:neighbor indexes 119 | static int observation_pyramid_neighbors[observation_pyramid_num][10]{}; 120 | 121 | /// Variables for velocity estimation 122 | pcl::PointCloud::Ptr cloud_in_current_view_rotated(new pcl::PointCloud()); 123 | static float current_position[3] = {0.f, 0.f, 0.f}; 124 | static float voxel_filtered_resolution = 0.15; 125 | static float delt_t_from_last_observation = 0.f; 126 | pcl::PointCloud::Ptr input_cloud_with_velocity(new pcl::PointCloud()); 127 | 128 | 129 | /** Storage for Gaussian randoms and Gaussian PDF**/ 130 | static float p_gaussian_randoms[GAUSSIAN_RANDOMS_NUM]; 131 | static float v_gaussian_randoms[GAUSSIAN_RANDOMS_NUM]; 132 | static float standard_gaussian_pdf[20000]; 133 | 134 | class DSPMap{ 135 | public: 136 | 137 | DSPMap(int init_particle_num = 0, float init_weight=0.01f) 138 | : voxel_num_x(MAP_LENGTH_VOXEL_NUM), /// Must be odd. voxel_resolution_set*voxel_num_x_set = map length 139 | voxel_num_y(MAP_WIDTH_VOXEL_NUM), /// Must be odd. voxel_resolution_set*voxel_num_y_set = map width 140 | voxel_num_z(MAP_HEIGHT_VOXEL_NUM), /// Must be odd. voxel_resolution_set*voxel_num_z_set = map height 141 | voxel_resolution(VOXEL_RESOLUTION), 142 | angle_resolution(ANGLE_RESOLUTION), /// degree, should be completely divided by 360 degrees. 143 | max_particle_num_voxel(MAX_PARTICLE_NUM_VOXEL), 144 | velocity_gaussian_random_seq(0), 145 | position_gaussian_random_seq(0), 146 | position_prediction_stddev(0.2f), 147 | velocity_prediction_stddev(0.1f), 148 | sigma_ob(0.2f), 149 | kappa(0.01f), 150 | P_detection(0.95f), 151 | update_time(0.f), 152 | update_counter(0), 153 | expected_new_born_objects(0.f), 154 | new_born_particle_weight(0.04f), 155 | new_born_particle_number_each_point(20), 156 | if_record_particle_csv(0), 157 | record_time(1.f), 158 | new_born_each_object_weight(0.f), 159 | total_time(0.0), 160 | update_times(0) 161 | { 162 | setInitParameters(); 163 | 164 | addRandomParticles(init_particle_num, init_weight); 165 | 166 | cout << "Map is ready to update!" << endl; 167 | } 168 | 169 | ~DSPMap(){ 170 | cout << "\n See you ;)" < 1.001f || fabs(sensor_quaternion_x) > 1.001f || fabs(sensor_quaternion_y) > 1.001f || fabs(sensor_quaternion_z) > 1.001f){ 186 | cout << "Invalid quaternion." < 10.f || fabs(odom_delt_py) > 10.f || fabs(odom_delt_pz) > 10.f || delt_t < 0.f || delt_t > 10.f){ 196 | cout << "!!! delt_t = "<< delt_t <= 0){ 311 | mapAddNewBornParticlesByObservation(); 312 | } 313 | 314 | 315 | /** Calculate object number and Resample **/ 316 | 317 | /// NOTE in this step the flag which is set to be 7.f in prediction step will be changed to 1.f or 0.6f. 318 | /// Removing this step will make prediction malfunction unless the flag is reset somewhere else. 319 | mapOccupancyCalculationAndResample(); 320 | 321 | 322 | /*** Record particles for analysis ***/ 323 | static int recorded_once_flag = 0; 324 | 325 | if(if_record_particle_csv){ 326 | if(if_record_particle_csv < 0 || (update_time > record_time && !recorded_once_flag)){ 327 | recorded_once_flag = 1; 328 | 329 | ofstream particle_log_writer; 330 | string file_name = particle_save_folder + "particles_update_t_" + to_string(update_counter) + "_"+ to_string((int)(update_time*1000)) + ".csv"; 331 | particle_log_writer.open(file_name, ios::out | ios::trunc); 332 | 333 | for(int i=0; i 0.1f){ 336 | for(int k=0; k<8; k++){ 337 | // 1.flag 2.vx 3.vy 4.vz 5.px 6.py 7.pz 338 | // 8.weight 9.update time 339 | particle_log_writer << voxels_with_particle[i][j][k] <<","; 340 | } 341 | particle_log_writer << i <<"\n"; 342 | } 343 | } 344 | } 345 | particle_log_writer.close(); 346 | } 347 | } 348 | 349 | return 1; 350 | } 351 | 352 | void setPredictionVariance(float p_stddev, float v_stddev){ 353 | position_prediction_stddev = p_stddev; 354 | velocity_prediction_stddev = v_stddev; 355 | // regenerate randoms 356 | generateGaussianRandomsVectorZeroCenter(); 357 | } 358 | 359 | void setObservationStdDev(float ob_stddev){ 360 | sigma_ob = ob_stddev; 361 | } 362 | 363 | void setNewBornParticleWeight(float weight){ 364 | new_born_particle_weight = weight; 365 | } 366 | 367 | void setNewBornParticleNumberofEachPoint(int num){ 368 | new_born_particle_number_each_point = num; 369 | } 370 | 371 | /// record_particle_flag O: don't record; -1 or other negative value: record all; positive value: record a time 372 | void setParticleRecordFlag(int record_particle_flag, float record_csv_time = 1.f){ 373 | if_record_particle_csv = record_particle_flag; 374 | record_time = record_csv_time; 375 | } 376 | 377 | static void setOriginalVoxelFilterResolution(float res){ 378 | voxel_filtered_resolution = res; 379 | } 380 | 381 | 382 | void getOccupancyMap(int &obstacles_num, pcl::PointCloud &cloud, const float threshold=0.7){ 383 | obstacles_num = 0; 384 | for(int i=0; i threshold){ 386 | pcl::PointXYZ pcl_point; 387 | getVoxelPositionFromIndex(i, pcl_point.x, pcl_point.y, pcl_point.z); 388 | cloud.push_back(pcl_point); 389 | 390 | ++ obstacles_num; 391 | } 392 | 393 | /// Clear weights for next prediction 394 | for(int j=4; j &cloud, float *future_status, const float threshold=0.7){ 403 | obstacles_num = 0; 404 | for(int i=0; i threshold){ 406 | pcl::PointXYZ pcl_point; 407 | getVoxelPositionFromIndex(i, pcl_point.x, pcl_point.y, pcl_point.z); 408 | cloud.push_back(pcl_point); 409 | 410 | ++ obstacles_num; 411 | } 412 | 413 | for(int n=0; n < PREDICTION_TIMES; ++n){ // Set future weights 414 | *(future_status + i * PREDICTION_TIMES + n) = voxels_objects_number[i][n + 4]; 415 | } 416 | 417 | /// Clear weights for next prediction 418 | for(int j=4; j (time(0))); //TEST 577 | generateGaussianRandomsVectorZeroCenter(); 578 | calculateNormalPDFBuffer(); 579 | 580 | 581 | } 582 | 583 | 584 | void addRandomParticles(int particle_num, float avg_weight) 585 | { 586 | /*** Initialize some particles ***/ 587 | int successfully_added_num = 0; 588 | int voxel_overflow_num = 0; 589 | 590 | for(int i=0; i particle_ptr{new Particle}; 592 | 593 | particle_ptr->px = generateRandomFloat(-map_length_x_half, map_length_x_half); 594 | particle_ptr->py = generateRandomFloat(-map_length_y_half, map_length_y_half); 595 | particle_ptr->pz = generateRandomFloat(-map_length_z_half, map_length_z_half); 596 | particle_ptr->vx = generateRandomFloat(-1.f, 1.f); 597 | particle_ptr->vy = generateRandomFloat(-1.f, 1.f); 598 | particle_ptr->vz = generateRandomFloat(-1.f, 1.f); 599 | particle_ptr->weight = avg_weight; 600 | 601 | if (getParticleVoxelsIndex(*particle_ptr, particle_ptr->voxel_index)) { 602 | 603 | int test = addAParticle(*particle_ptr, particle_ptr->voxel_index); 604 | if(test>0){ 605 | successfully_added_num ++; 606 | }else{ 607 | voxel_overflow_num ++; 608 | } 609 | } 610 | } 611 | } 612 | 613 | 614 | void mapPrediction(float odom_delt_px, float odom_delt_py, float odom_delt_pz, float delt_t) 615 | { 616 | int operation_counter = 0; 617 | int exist_particles = 0; 618 | int voxel_full_remove_counter = 0, pyramid_full_remove_counter = 0; 619 | int moves_out_counter = 0; 620 | 621 | update_time += delt_t; 622 | update_counter += 1; 623 | 624 | /// Clear pyramids first 625 | for(auto & j : pyramids_in_fov){ 626 | for(auto & i : j){ 627 | i[0] &= O_MAKE_INVALID; 628 | } 629 | } 630 | 631 | /// Update Particles' state and index in both voxels and pyramids 632 | for(int v_index=0; v_index0.1f && voxels_with_particle[v_index][p][0] <6.f){ /// exsit, but not new moved 637 | voxels_with_particle[v_index][p][0] = 1.f; // If valid, remove resample flag. 638 | ++ operation_counter; 639 | 640 | voxels_with_particle[v_index][p][1] = 0.f; //vx 641 | voxels_with_particle[v_index][p][2] = 0.f; //vy 642 | voxels_with_particle[v_index][p][3] = 0.f; //vz 643 | 644 | voxels_with_particle[v_index][p][4] += odom_delt_px; //px 645 | voxels_with_particle[v_index][p][5] += odom_delt_py; //py 646 | voxels_with_particle[v_index][p][6] += odom_delt_pz; //pz 647 | 648 | int particle_voxel_index_new; 649 | if(getParticleVoxelsIndex(voxels_with_particle[v_index][p][4], voxels_with_particle[v_index][p][5], 650 | voxels_with_particle[v_index][p][6], particle_voxel_index_new)) 651 | { 652 | // move particle. If moved, the flag turns to 7.f. If should move but failed because target voxel is full, delete the voxel. 653 | int move_flag = moveParticle(particle_voxel_index_new, v_index, p, &voxels_with_particle[v_index][p][0]); 654 | if(move_flag == -2){ 655 | // Move the particle, if fails, "moveParticleByVoxel" will delete the particle 656 | ++ pyramid_full_remove_counter; 657 | continue; 658 | }else if(move_flag == -1){ 659 | ++ voxel_full_remove_counter; 660 | continue; 661 | } 662 | ++ exist_particles; 663 | 664 | } 665 | else{ 666 | /// Particle moves out 667 | removeParticle(&voxels_with_particle[v_index][p][0]); 668 | ++ moves_out_counter; 669 | } 670 | 671 | } 672 | } 673 | } 674 | 675 | // cout << "exist_particles=" << exist_particles<9.9f) corrected_x=9.9f; 1206 | else if(corrected_x<-9.9f) corrected_x=-9.9f; 1207 | 1208 | return standard_gaussian_pdf[(int)(corrected_x*1000+10000)]; 1209 | } 1210 | 1211 | static void rotateVectorByQuaternion(const float *ori_vector, const float *quaternion, float *rotated_vector) 1212 | { 1213 | //Lazy. Use Eigen directly 1214 | Eigen::Quaternionf ori_vector_quaternion, vector_quaternion; 1215 | ori_vector_quaternion.w() = 0; 1216 | ori_vector_quaternion.x() = *ori_vector; 1217 | ori_vector_quaternion.y() = *(ori_vector+1); 1218 | ori_vector_quaternion.z() = *(ori_vector+2); 1219 | 1220 | Eigen::Quaternionf att; 1221 | att.w() = *quaternion; 1222 | att.x() = *(quaternion+1); 1223 | att.y() = *(quaternion+2); 1224 | att.z() = *(quaternion+3); 1225 | 1226 | vector_quaternion = att * ori_vector_quaternion * att.inverse(); 1227 | *rotated_vector = vector_quaternion.x(); 1228 | *(rotated_vector+1) = vector_quaternion.y(); 1229 | *(rotated_vector+2) = vector_quaternion.z(); 1230 | } 1231 | 1232 | static float vectorMultiply(float &x1, float &y1, float &z1, float &x2, float &y2, float &z2){ 1233 | return x1*x2 + y1*y2 + z1*z2; 1234 | } 1235 | 1236 | 1237 | int ifInPyramidsArea(float &x, float &y, float &z) 1238 | { 1239 | if(vectorMultiply(x,y,z, pyramid_BPnorm_params_h[0][0], pyramid_BPnorm_params_h[0][1], pyramid_BPnorm_params_h[0][2]) >= 0.f 1240 | && vectorMultiply(x,y,z, pyramid_BPnorm_params_h[observation_pyramid_num_h][0], pyramid_BPnorm_params_h[observation_pyramid_num_h][1], pyramid_BPnorm_params_h[observation_pyramid_num_h][2]) <= 0.f 1241 | && vectorMultiply(x,y,z, pyramid_BPnorm_params_v[0][0], pyramid_BPnorm_params_v[0][1], pyramid_BPnorm_params_v[0][2]) <= 0.f 1242 | && vectorMultiply(x,y,z, pyramid_BPnorm_params_v[observation_pyramid_num_v][0], pyramid_BPnorm_params_v[observation_pyramid_num_v][1], pyramid_BPnorm_params_v[observation_pyramid_num_v][2]) >= 0.f){ 1243 | return 1; 1244 | }else{ 1245 | return 0; 1246 | } 1247 | } 1248 | 1249 | int findPointPyramidHorizontalIndex(float &x, float &y, float &z){ /// The point should already be inside of Pyramids Area 1250 | float last_dot_multiply = 1.f; // for horizontal direction, if the point is inside of Pyramids Area. The symbol of the first dot multiplication should be positive 1251 | for(int i=0; i< observation_pyramid_num_h; i++){ 1252 | float this_dot_multiply = vectorMultiply(x, y, z, pyramid_BPnorm_params_h[i+1][0], pyramid_BPnorm_params_h[i+1][1], pyramid_BPnorm_params_h[i+1][2]); 1253 | if(last_dot_multiply * this_dot_multiply <= 0.f){ 1254 | return i; 1255 | } 1256 | last_dot_multiply = this_dot_multiply; 1257 | } 1258 | 1259 | cout << "!!!!!! Please use Function ifInPyramidsArea() to filter the points first before using findPointPyramidHorizontalIndex()" <points.empty()) return; 1289 | 1290 | input_cloud_with_velocity->clear(); 1291 | 1292 | for(auto &p : cloud_in_current_view_rotated->points){ 1293 | pcl::PointXYZ transformed_p; 1294 | transformed_p.x = p.x + current_position[0]; 1295 | transformed_p.y = p.y + current_position[1]; 1296 | transformed_p.z = p.z + current_position[2]; 1297 | 1298 | pcl::PointXYZINormal p2; 1299 | p2.x = transformed_p.x; 1300 | p2.y = transformed_p.y; 1301 | p2.z = transformed_p.z; 1302 | p2.normal_x = 0.f; 1303 | p2.normal_y = 0.f; 1304 | p2.normal_z = 0.f; 1305 | p2.intensity = 0.f; 1306 | input_cloud_with_velocity->push_back(p2); 1307 | } 1308 | 1309 | } 1310 | 1311 | 1312 | 1313 | 1314 | /*** For test ***/ 1315 | public: 1316 | static float generateRandomFloat(float min, float max){ 1317 | return min + static_cast (rand()) /( static_cast (RAND_MAX/(max-min))); 1318 | } 1319 | 1320 | 1321 | void getVoxelPositionFromIndexPublic(const int &index, float &px, float &py, float &pz) const{ 1322 | static const int z_change_storage_taken = voxel_num_y*voxel_num_x; 1323 | static const int y_change_storage_taken = voxel_num_x; 1324 | 1325 | int z_index = index / z_change_storage_taken; 1326 | int yx_left_indexes = index - z_index * z_change_storage_taken; 1327 | int y_index = yx_left_indexes / y_change_storage_taken; 1328 | int x_index = yx_left_indexes - y_index * y_change_storage_taken; 1329 | 1330 | static const float correction_x = -map_length_x_half + voxel_resolution*0.5f; 1331 | static const float correction_y = -map_length_y_half + voxel_resolution*0.5f; 1332 | static const float correction_z = -map_length_z_half + voxel_resolution*0.5f; 1333 | 1334 | px = (float)x_index * voxel_resolution + correction_x; 1335 | py = (float)y_index * voxel_resolution + correction_y; 1336 | pz = (float)z_index * voxel_resolution + correction_z; 1337 | } 1338 | 1339 | int getPointVoxelsIndexPublic(const float &px, const float &py, const float &pz, int & index){ 1340 | if(ifParticleIsOut(px, py, pz)) {return 0;} 1341 | auto x = (int)((px + map_length_x_half) / voxel_resolution); 1342 | auto y = (int)((py + map_length_y_half) / voxel_resolution); 1343 | auto z = (int)((pz + map_length_z_half) / voxel_resolution); 1344 | index = z*voxel_num_y*voxel_num_x + y*voxel_num_x + x; 1345 | if(index<0 || index>=VOXEL_NUM){ 1346 | return 0; 1347 | } 1348 | return 1; 1349 | } 1350 | 1351 | }; 1352 | 1353 | 1354 | 1355 | -------------------------------------------------------------------------------- /launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamic_occpuancy_map 4 | 0.0.0 5 | dynamic_occpuancy_map package 6 | 7 | 8 | 9 | 10 | clarence 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 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Description 2 | This repository contains the head files of the Dual-Structure Particle-based (DSP) map 3 | and a ROS node example to use this map. For more information about the DSP map, see [video](https://www.youtube.com/watch?v=seF_Oy4YbXo&t=5s) and [preprint](https://arxiv.org/abs/2202.06273). 4 | 5 | There are three __head files__ in the `include` folder. 6 | 1. ``dsp_dynamic.h`` is the head file for the DSP map with a constant velocity model (called DSP-Dynamic map in our paper). (__Recommended__ for Type II dynamic occupancy map.) 7 | 2. ``dsp_dynamic_multiple_neighbors.h`` is the head file for the DSP map with a constant velocity model. Check the Supplementary section below to know the difference between `dsp_dynamic` and `dsp_dynamic_multiple_neighbors`. 8 | 3. ``dsp_static.h`` is the head file for the DSP map with the static model (called DSP-Static map in our paper). (Type I dynamic occupancy map.) 9 | 10 | Just include one of the head files in your source file and use the map. We write everything in a single head! 11 | 12 | A ROS __node example__ `map_sim_example.cpp` can be found in the `src` folder. In the example, we subscribe point cloud from topic `/camera_front/depth/points` and pose from `/mavros/local_position/pose` to build the map. We publish the current occupancy status with topic `/my_map/cloud_ob` and one layer of the predicted future occupancy maps with topic `/my_map/future_status` in the point cloud form. 13 | 14 | 15 | # Progress 16 | __New Progress!__ 2024.9.22 We have released the code of [Semantic DSP Map](https://github.com/tud-amr/semantic_dsp_map), which is an upgraded version of the DSP map. Compared to DSP map, Semantic DSP map contains instance-aware semantic information and has less noise. Please check [here](https://github.com/tud-amr/semantic_dsp_map) for details. 17 | 18 | A UAV simulation tool (third party) made by RicardoLEE123 using the DSP map is [here](https://github.com/SmartGroupSystems/Dsp-map-simulator). 19 | 20 | # Compile 21 | __Tested environment__: Ubuntu 18.04 + ROS Melodic and Ubuntu 20.04 + ROS Noetic 22 | 23 | To compile the source code of our map, you need: 24 | 1. PCL and Mavros. PCL is included in the desktop-full version of ROS. 25 | Mavros is only used for ROS message subscriptions in the example node. Check [mavros](https://github.com/mavlink/mavros) for installation guidance. 26 | 27 | 2. Install [munkers-cpp](https://github.com/saebyn/munkres-cpp) with the following steps. 28 | ``` 29 | git clone https://github.com/saebyn/munkres-cpp.git 30 | cd munkres-cpp 31 | mkdir build && cd build 32 | cmake .. 33 | make 34 | sudo make install 35 | ``` 36 | 37 | 3. Download and compile the example node 38 | ``` 39 | mkdir -p map_ws/src 40 | cd map_ws/src 41 | git clone https://github.com/g-ch/DSP-map.git 42 | cd .. 43 | catkin_make 44 | ``` 45 | 46 | # Test 47 | Download a bag file named `street.bag` containing the point cloud and pose data collected with a drone in Gazebo. [Download](https://drive.google.com/file/d/1go4ALTe8CqaBY2wjZJzkUCmdlBI7yAAU/view?usp=sharing). 48 | Save the bag file in `data` folder. 49 | 50 | Launch a test by 51 | ``` 52 | cd map_ws 53 | source devel/setup.bash 54 | roslaunch dynamic_occpuancy_map mapping.launch 55 | ``` 56 | 57 | The launch file will start the example mapping node and open three RVIZ windows to show the current occupancy status (3D), predicted future occupancy status (2D, layer of a height set in `map_sim_example.cpp`), and the raw point cloud from the camera. 58 | 59 | 60 | 61 | # Parameters 62 | There are quite a few parameters in this map. Below we provide two ways to set the parameters. 63 | 64 | __NOTE:__ Only Parameter `Camera FOV`, including a horizontal angle and a vertical angle of the FOV, is coupled with hardware and should be set according to the real FOV of your camera. 65 | You can use default values for other parameters. 66 | 67 | ## Set Parameters with a Tool 68 | Some of the parameters are hard to understand if you are not very familiar with the DSP map. We provide a "Parameter Tuner" tool to set the parameters in the DSP-Dynamic map in an easily-understood way. To use the tool, open the `script` folder and run 69 | 70 | ``` 71 | python set_map_parameters.py 72 | ``` 73 | You will see the following UI. 74 | 75 | 76 | 77 | The meaning of parameters in the UI are: 78 | 79 | 1. _Map Resolution_ controls the size of the voxel subspace in the map. The voxel subspace is used to voxelize the map for visulaization and usage in voxel-based motion planners. Changing this parameter will change voxel resolution. (When _Map Resolution_ is changed, the tool will also change the occupancy threshold in `map_sim_example.cpp` to a nice value automatically.) 80 | 81 | 2. _Map Length/Width/Height_ control the size of the map. The mapping process gets slower when the size gets larger. 82 | 83 | 3. _Performance/Efficiency_ control the performance level and efficiency level of the map, respectively. Improving the performance will lower the efficiency. 84 | 85 | 4. _FOV Horizontal/Vertical Angle_ describe the FOV range of the camera. Set the parameters according to the real FOV size of your depth camera. Since the depth points close to the edge of the FOV are usually very noisy, the tool will clip the FOV size a little bit after the parameters are saved. 86 | 87 | 5. _(Optional) Max cluster point number / center height_ are parameters in the initial velocity estimator. The initial velocity estimator clusters the input point cloud. The cluster whose size is larger than `Max cluster point number` or center height is larger than `Max cluster enter height` will be regarded as a cluster representing static obstacles. 88 | 89 | Press the `Save` button to save parameters. Some parameters in `map_sim_example.cpp` and `dsp_dynamic.h` will be changed accordingly. Then you need to recompile the code with `catkin_make`. 90 | 91 | Press the `Reset to default` button to show the default parameters. Then press the `Save` button to use the default parameters. 92 | 93 | 94 | ## Set Parameters in the Code Directly 95 | You can also modify the parameters in the code directly. For DSP-Static map and DSP-Dynamic map using multiple neighbors, you can only use this way currently. 96 | 97 | ### Static parameters 98 | The following parameters can be found at the top of the map head file. 99 | 1. Camera FOV. It is necessary to set the camera FOV angle for your camera. The unit is degree and we set the half-angle value. 100 | ``` 101 | const int half_fov_h = 45; // Half of the horizental angle. should be able to be divided by ANGLE_RESOLUTION. If not, modify ANGLE_RESOLUTION or make half_fov_h a little smaller value than the real FOV angle 102 | const int half_fov_v = 27; // Half of the vertical angle. Should be able to be divided by ANGLE_RESOLUTION. If not, modify ANGLE_RESOLUTION or make half_fov_h a little smaller value than the real FOV angle 103 | ``` 104 | * The ``ANGLE_RESOLUTION`` is 3 in the head files. Don't change ``ANGLE_RESOLUTION`` unless you are very familiar with the way our map works. 105 | * Tips: Depth camera usually contains large noise near the edge of the image. You can make the FOV parameter a little smaller than the real FOV angle. But never make it larger. 106 | 107 | 2. Change the size and resolution of the map by changing the following parameters: 108 | ``` 109 | #define MAP_LENGTH_VOXEL_NUM 66 110 | #define MAP_WIDTH_VOXEL_NUM 66 111 | #define MAP_HEIGHT_VOXEL_NUM 40 112 | #define VOXEL_RESOLUTION 0.15 113 | ``` 114 | * The default resolution is `0.15m`. The real size of the Map length is `MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION`. 115 | * With different resolutions, you also need to change the threshold in Function "getOccupancyMapWithFutureStatus" in the ROS node. 116 | 117 | __Note:__ Although the DSP map is continuous and composed of particles. We usually acquire the current or future occupancy status from the voxel structure. The voxel resolution is theoretically arbitrary but you need to tune the parameter for threshold and maximum particle number if the resolution is changed. 118 | To maintain the same efficiency when the resolution changes (the real map size is the same), the maximum particle number in the map shouldn't change, which means you need to change the maximum number of particles in each voxel space by changing the Parameter `MAX_PARTICLE_NUM_VOXEL`. 119 | 120 | 121 | 3. Change the time stamp to predict the future status 122 | ``` 123 | #define PREDICTION_TIMES 6 124 | static const float prediction_future_time[PREDICTION_TIMES] = {0.05f, 0.2f, 0.5f, 1.f, 1.5f, 2.f}; //unit: second 125 | ``` 126 | 127 | ### Dynamic parameters 128 | The following parameters can be changed dynamically in the node. 129 | ``` 130 | my_map.setPredictionVariance(0.05, 0.05); // StdDev for prediction. velocity StdDev, position StdDev, respectively. 131 | my_map.setObservationStdDev(0.1); // StdDev for update. position StdDev. 132 | my_map.setNewBornParticleNumberofEachPoint(20); // Number of new particles generated from one measurement point. 133 | my_map.setNewBornParticleWeight(0.0001); // Initial weight of particles. 134 | DSPMap::setOriginalVoxelFilterResolution(res); // Resolution of the voxel filter used for point cloud pre-process. 135 | ``` 136 | 137 | # Record Particles 138 | In our ROS node, we publish point cloud from occupancy status. The particles are not published because they are too many. 139 | But you can record the particles at one time to a CSV file for analysis. 140 | 141 | ``` 142 | my_map.setParticleRecordFlag(1, 19.2); // Uncomment this to save particle file of a moment. Saving will take a long time. Don't use it in real-time applications. 143 | ``` 144 | 145 | You can use the Matlab app tool ``app1.mlapp`` in the `display` folder to open the CSV and view and analyze the particles. 146 | 147 | # Citation 148 | If you use our code in your research, please cite 149 | ``` 150 | @article{chen2022continuous, 151 | title={Continuous Occupancy Mapping in Dynamic Environments Using Particles}, 152 | author={Chen, Gang and Dong, Wei and Peng, Peng and Alonso-Mora, Javier and Zhu, Xiangyang}, 153 | journal={arXiv preprint arXiv:2202.06273}, 154 | year={2022} 155 | } 156 | ``` 157 | 158 | # License 159 | MIT license. 160 | 161 | # Supplementary 162 | * __Difference between `dsp_dynamic` and `dsp_dynamic_multiple_neighbors`__: 163 | In `dsp_dynamic.h`, Parameter ANGLE_RESOLUTION (3 degrees) is not as small as the real sensor angle resolution (may be smaller than 1 degree). This is not ideal to handle occlusion when very tiny obstacles exist but is efficient and sufficient in most scenarios. 164 | In `dsp_dynamic_multiple_neighbors`, Parameter ANGLE_RESOLUTION can be as small as the real sensor angle resolution. A (2*PYRAMID_NEIGHBOR_N+1)^2 neighborhood pyramid space will be considered in the update. This is ideal to handle occlusion when very tiny obstacles exist but is less efficient. 165 | 166 | * __Difference between Type I and Type II dynamic occupancy map__: The type I map considers only to model the current status of dynamic obstacles. Type II considers short-term prediction of the future status of dynamic obstacles. 167 | 168 | 169 | 170 | -------------------------------------------------------------------------------- /rviz/boxes.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1/Offset1 10 | - /PointCloud21 11 | - /MarkerArray1 12 | - /MarkerArray1/Namespaces1 13 | - /Marker1 14 | - /MarkerArray2 15 | - /Image2 16 | Splitter Ratio: 0.5 17 | Tree Height: 296 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | - /Publish Point1 25 | Name: Tool Properties 26 | Splitter Ratio: 0.5886790156364441 27 | - Class: rviz/Views 28 | Expanded: 29 | - /Current View1 30 | Name: Views 31 | Splitter Ratio: 0.5 32 | - Class: rviz/Time 33 | Experimental: false 34 | Name: Time 35 | SyncMode: 0 36 | SyncSource: PointCloud2 37 | Preferences: 38 | PromptSaveOnExit: true 39 | Toolbars: 40 | toolButtonStyle: 2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz/Grid 47 | Color: 160; 160; 164 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.029999999329447746 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: -2 58 | Plane: XY 59 | Plane Cell Count: 10 60 | Reference Frame: 61 | Value: true 62 | - Alpha: 1 63 | Autocompute Intensity Bounds: true 64 | Autocompute Value Bounds: 65 | Max Value: 2.4750003814697266 66 | Min Value: -1.8749998807907104 67 | Value: true 68 | Axis: Z 69 | Channel Name: intensity 70 | Class: rviz/PointCloud2 71 | Color: 255; 255; 255 72 | Color Transformer: AxisColor 73 | Decay Time: 0 74 | Enabled: true 75 | Invert Rainbow: false 76 | Max Color: 255; 255; 255 77 | Max Intensity: 4096 78 | Min Color: 0; 0; 0 79 | Min Intensity: 0 80 | Name: PointCloud2 81 | Position Transformer: XYZ 82 | Queue Size: 10 83 | Selectable: true 84 | Size (Pixels): 3 85 | Size (m): 0.15000000596046448 86 | Style: Boxes 87 | Topic: /my_map/cloud_ob 88 | Unreliable: false 89 | Use Fixed Frame: true 90 | Use rainbow: true 91 | Value: true 92 | - Class: rviz/Image 93 | Enabled: false 94 | Image Topic: /iris/vi_sensor/camera_depth/camera/image_raw 95 | Max Value: 1 96 | Median window: 5 97 | Min Value: 0 98 | Name: Image 99 | Normalize Range: true 100 | Queue Size: 2 101 | Transport Hint: raw 102 | Unreliable: false 103 | Value: false 104 | - Class: rviz/MarkerArray 105 | Enabled: true 106 | Marker Topic: /my_map/velocity_marker 107 | Name: MarkerArray 108 | Namespaces: 109 | {} 110 | Queue Size: 100 111 | Value: true 112 | - Alpha: 1 113 | Class: rviz/Axes 114 | Enabled: true 115 | Length: 1 116 | Name: Axes 117 | Radius: 0.10000000149011612 118 | Reference Frame: 119 | Value: true 120 | - Class: rviz/Marker 121 | Enabled: true 122 | Marker Topic: /visualization_fov 123 | Name: Marker 124 | Namespaces: 125 | lines_and_points: true 126 | Queue Size: 100 127 | Value: true 128 | - Class: rviz/MarkerArray 129 | Enabled: false 130 | Marker Topic: /visualization_marker 131 | Name: MarkerArray 132 | Namespaces: 133 | {} 134 | Queue Size: 100 135 | Value: false 136 | - Class: rviz/Image 137 | Enabled: false 138 | Image Topic: /d400/color/image_raw 139 | Max Value: 1 140 | Median window: 5 141 | Min Value: 0 142 | Name: Image 143 | Normalize Range: true 144 | Queue Size: 2 145 | Transport Hint: raw 146 | Unreliable: false 147 | Value: false 148 | Enabled: true 149 | Global Options: 150 | Background Color: 238; 238; 236 151 | Default Light: true 152 | Fixed Frame: map 153 | Frame Rate: 30 154 | Name: root 155 | Tools: 156 | - Class: rviz/Interact 157 | Hide Inactive Objects: true 158 | - Class: rviz/MoveCamera 159 | - Class: rviz/Select 160 | - Class: rviz/FocusCamera 161 | - Class: rviz/Measure 162 | - Class: rviz/SetInitialPose 163 | Theta std deviation: 0.2617993950843811 164 | Topic: /initialpose 165 | X std deviation: 0.5 166 | Y std deviation: 0.5 167 | - Class: rviz/SetGoal 168 | Topic: /move_base_simple/goal 169 | - Class: rviz/PublishPoint 170 | Single click: true 171 | Topic: /clicked_point 172 | Value: true 173 | Views: 174 | Current: 175 | Class: rviz/Orbit 176 | Distance: 18.0529842376709 177 | Enable Stereo Rendering: 178 | Stereo Eye Separation: 0.05999999865889549 179 | Stereo Focal Distance: 1 180 | Swap Stereo Eyes: false 181 | Value: false 182 | Field of View: 0.7853981852531433 183 | Focal Point: 184 | X: 1.4222959280014038 185 | Y: -0.2956828474998474 186 | Z: -0.9762728214263916 187 | Focal Shape Fixed Size: false 188 | Focal Shape Size: 0.05000000074505806 189 | Invert Z Axis: false 190 | Name: Current View 191 | Near Clip Distance: 0.009999999776482582 192 | Pitch: 0.8297968506813049 193 | Target Frame: 194 | Yaw: 3.1453869342803955 195 | Saved: ~ 196 | Window Geometry: 197 | Displays: 198 | collapsed: true 199 | Height: 633 200 | Hide Left Dock: true 201 | Hide Right Dock: true 202 | Image: 203 | collapsed: false 204 | QMainWindow State: 000000ff00000000fd0000000400000000000001f700000174fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004700000174000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001da0000012d0000001900fffffffb0000000a0049006d00610067006500000002c1000000d00000001900ffffff000000010000015f0000025bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e0000025b000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003710000005afc0100000002fb0000000800540069006d00650100000000000003710000037100fffffffb0000000800540069006d0065010000000000000450000000000000000000000371000001b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 205 | Selection: 206 | collapsed: false 207 | Time: 208 | collapsed: false 209 | Tool Properties: 210 | collapsed: false 211 | Views: 212 | collapsed: true 213 | Width: 881 214 | X: 1001 215 | Y: 34 216 | -------------------------------------------------------------------------------- /rviz/future_status.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 753 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: PointCloud2 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 10 60 | Min Value: -10 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 255; 255; 255 66 | Color Transformer: RGB8 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Max Intensity: 4096 72 | Min Color: 0; 0; 0 73 | Min Intensity: 0 74 | Name: PointCloud2 75 | Position Transformer: XYZ 76 | Queue Size: 10 77 | Selectable: true 78 | Size (Pixels): 3 79 | Size (m): 0.20000000298023224 80 | Style: Boxes 81 | Topic: /my_map/future_status 82 | Unreliable: false 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | - Alpha: 1 87 | Class: rviz/Axes 88 | Enabled: true 89 | Length: 1 90 | Name: Axes 91 | Radius: 0.10000000149011612 92 | Reference Frame: 93 | Value: true 94 | Enabled: true 95 | Global Options: 96 | Background Color: 255; 255; 255 97 | Default Light: true 98 | Fixed Frame: map 99 | Frame Rate: 30 100 | Name: root 101 | Tools: 102 | - Class: rviz/Interact 103 | Hide Inactive Objects: true 104 | - Class: rviz/MoveCamera 105 | - Class: rviz/Select 106 | - Class: rviz/FocusCamera 107 | - Class: rviz/Measure 108 | - Class: rviz/SetInitialPose 109 | Theta std deviation: 0.2617993950843811 110 | Topic: /initialpose 111 | X std deviation: 0.5 112 | Y std deviation: 0.5 113 | - Class: rviz/SetGoal 114 | Topic: /move_base_simple/goal 115 | - Class: rviz/PublishPoint 116 | Single click: true 117 | Topic: /clicked_point 118 | Value: true 119 | Views: 120 | Current: 121 | Class: rviz/Orbit 122 | Distance: 107.37020111083984 123 | Enable Stereo Rendering: 124 | Stereo Eye Separation: 0.05999999865889549 125 | Stereo Focal Distance: 1 126 | Swap Stereo Eyes: false 127 | Value: false 128 | Field of View: 0.7853981852531433 129 | Focal Point: 130 | X: 29.367382049560547 131 | Y: -1.1195290088653564 132 | Z: -14.458261489868164 133 | Focal Shape Fixed Size: true 134 | Focal Shape Size: 0.05000000074505806 135 | Invert Z Axis: false 136 | Name: Current View 137 | Near Clip Distance: 0.009999999776482582 138 | Pitch: 1.5697963237762451 139 | Target Frame: 140 | Yaw: 3.143578290939331 141 | Saved: ~ 142 | Window Geometry: 143 | Displays: 144 | collapsed: false 145 | Height: 1163 146 | Hide Left Dock: false 147 | Hide Right Dock: true 148 | QMainWindow State: 000000ff00000000fd000000040000000000000161000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000047000003c2000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015600000341fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004700000341000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003710000005afc0100000002fb0000000800540069006d00650100000000000003710000037100fffffffb0000000800540069006d0065010000000000000450000000000000000000000209000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 149 | Selection: 150 | collapsed: false 151 | Time: 152 | collapsed: false 153 | Tool Properties: 154 | collapsed: false 155 | Views: 156 | collapsed: true 157 | Width: 881 158 | X: 116 159 | Y: 34 160 | -------------------------------------------------------------------------------- /rviz/original_pointcloud.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 349 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: PointCloud2 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 10 60 | Min Value: -10 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 255; 255; 255 66 | Color Transformer: RGB8 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Max Intensity: 4096 72 | Min Color: 0; 0; 0 73 | Min Intensity: 0 74 | Name: PointCloud2 75 | Position Transformer: XYZ 76 | Queue Size: 10 77 | Selectable: true 78 | Size (Pixels): 3 79 | Size (m): 0.05000000074505806 80 | Style: Flat Squares 81 | Topic: /camera_front/depth/points 82 | Unreliable: false 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 238; 238; 236 89 | Default Light: true 90 | Fixed Frame: front_camera_link 91 | Frame Rate: 30 92 | Name: root 93 | Tools: 94 | - Class: rviz/Interact 95 | Hide Inactive Objects: true 96 | - Class: rviz/MoveCamera 97 | - Class: rviz/Select 98 | - Class: rviz/FocusCamera 99 | - Class: rviz/Measure 100 | - Class: rviz/SetInitialPose 101 | Theta std deviation: 0.2617993950843811 102 | Topic: /initialpose 103 | X std deviation: 0.5 104 | Y std deviation: 0.5 105 | - Class: rviz/SetGoal 106 | Topic: /move_base_simple/goal 107 | - Class: rviz/PublishPoint 108 | Single click: true 109 | Topic: /clicked_point 110 | Value: true 111 | Views: 112 | Current: 113 | Class: rviz/Orbit 114 | Distance: 17.836992263793945 115 | Enable Stereo Rendering: 116 | Stereo Eye Separation: 0.05999999865889549 117 | Stereo Focal Distance: 1 118 | Swap Stereo Eyes: false 119 | Value: false 120 | Field of View: 0.7853981852531433 121 | Focal Point: 122 | X: 1.8818532228469849 123 | Y: -1.3879152536392212 124 | Z: 2.4410722255706787 125 | Focal Shape Fixed Size: true 126 | Focal Shape Size: 0.05000000074505806 127 | Invert Z Axis: false 128 | Name: Current View 129 | Near Clip Distance: 0.009999999776482582 130 | Pitch: -1.5697963237762451 131 | Target Frame: 132 | Yaw: 5.115452289581299 133 | Saved: ~ 134 | Window Geometry: 135 | Displays: 136 | collapsed: true 137 | Height: 591 138 | Hide Left Dock: true 139 | Hide Right Dock: true 140 | QMainWindow State: 000000ff00000000fd0000000400000000000001610000029ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000470000029f000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000470000029f000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000037100000040fc0100000002fb0000000800540069006d00650100000000000003710000037100fffffffb0000000800540069006d0065010000000000000450000000000000000000000371000001a000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 141 | Selection: 142 | collapsed: false 143 | Time: 144 | collapsed: false 145 | Tool Properties: 146 | collapsed: false 147 | Views: 148 | collapsed: true 149 | Width: 881 150 | X: 1001 151 | Y: 599 152 | -------------------------------------------------------------------------------- /script/set_map_parameters.py: -------------------------------------------------------------------------------- 1 | ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' 2 | 3 | Copyright <2022> 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | 12 | Author: Gang Chen 13 | 14 | Date: 2021/12/16 15 | 16 | Description: This script is a tool designed to quickly modify the parameters in the DSP-Dynamic Map. There are many parameters in the DSP-Dynamic Map. This tool presents a UI using performance and efficiency levels to calculate and set parameters automatically. 17 | 18 | ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' 19 | 20 | from PyQt5.QtCore import * 21 | from PyQt5.QtWidgets import * 22 | from PyQt5.QtGui import * 23 | import sys 24 | 25 | def changeALineInAnyFile(file, unique_keywords, new_line): 26 | file_data = "" 27 | line_changed = False 28 | with open(file, "r") as f: 29 | for line in f: 30 | if unique_keywords in line and not line_changed: 31 | line = new_line 32 | line_changed = True 33 | file_data += line 34 | with open(file, "w") as f: 35 | f.write(file_data) 36 | return line_changed 37 | 38 | 39 | def readParameterinFile(file, unique_keywords_of_the_parameter_definition_line, type="int"): 40 | parameter_found = False 41 | with open(file, "r") as f: 42 | for line in f: 43 | if unique_keywords_of_the_parameter_definition_line in line: 44 | copy_line = line 45 | parameter_value_str = copy_line.split("\n")[0].split("//")[0].split(";")[0].split(" ")[-1] 46 | print(parameter_value_str) 47 | parameter_found = True 48 | 49 | if type == "int": 50 | return int(parameter_value_str) 51 | elif type == "float" or type == "double": 52 | return float(parameter_value_str) 53 | elif type == "string" or type == "str": 54 | return parameter_value_str 55 | else: 56 | print("Unknown type in Function readCurrentParameter()") 57 | return parameter_found 58 | 59 | 60 | class Q_Window(QWidget): 61 | 62 | def __init__(self): 63 | super(Q_Window, self).__init__() 64 | self.initializeParameters() 65 | self.readCurrentParameters() 66 | self.initUI() 67 | 68 | def initializeParameters(self): 69 | # File Path 70 | self.cpp_file = "../src/map_sim_example.cpp" 71 | self.head_file_dynamic = "../include/dsp_dynamic.h" 72 | 73 | # Parameters to modify in the UI 74 | self.resolution = 0.15 75 | self.map_length_x = 0.15*66 76 | self.map_length_y = 0.15*66 77 | self.map_length_z = 0.15*40 78 | self.efficiency = 45 79 | self.performance = 75 80 | 81 | self.fov_angle_h = 84 82 | self.fov_angle_v = 48 83 | 84 | self.filter_max_cluster_point_num = 200 85 | self.filter_max_cluster_center_height = 1.5 86 | 87 | # Map parameters related to performance and efficiency 88 | self.pyramid_resolution = 3 89 | self.max_particle_density = 3000 90 | self.voxel_filter = 0.1 91 | self.max_particle_num_voxel = int(self.max_particle_density * (self.resolution*self.resolution*self.resolution)) 92 | 93 | def readCurrentParameters(self): 94 | self.resolution = readParameterinFile(self.head_file_dynamic, "#define VOXEL_RESOLUTION", type="float") 95 | self.map_length_x = readParameterinFile(self.head_file_dynamic, "#define MAP_LENGTH_VOXEL_NUM", type="int")*self.resolution 96 | self.map_length_y = readParameterinFile(self.head_file_dynamic, "#define MAP_WIDTH_VOXEL_NUM", type="int")*self.resolution 97 | self.map_length_z = readParameterinFile(self.head_file_dynamic, "#define MAP_HEIGHT_VOXEL_NUM", type="int")*self.resolution 98 | self.fov_angle_h = readParameterinFile(self.head_file_dynamic, "const int half_fov_h", type="int")*2 99 | self.fov_angle_v = readParameterinFile(self.head_file_dynamic, "const int half_fov_v", type="int")*2 100 | self.filter_max_cluster_point_num = readParameterinFile(self.head_file_dynamic, "DYNAMIC_CLUSTER_MAX_POINT_NUM", type="int") 101 | self.filter_max_cluster_center_height = readParameterinFile(self.head_file_dynamic, "DYNAMIC_CLUSTER_MAX_CENTER_HEIGHT", type="float") 102 | 103 | self.pyramid_resolution = readParameterinFile(self.head_file_dynamic, "#define ANGLE_RESOLUTION", type="int") 104 | self.max_particle_num_voxel = readParameterinFile(self.head_file_dynamic, "#define MAX_PARTICLE_NUM_VOXEL", type="int") 105 | self.voxel_filter = readParameterinFile(self.cpp_file, "const float res", type="float") 106 | 107 | self.max_particle_density = int(self.max_particle_num_voxel / (self.resolution*self.resolution*self.resolution)) 108 | 109 | self.fov_angle_h_last = self.fov_angle_h 110 | self.fov_angle_v_last = self.fov_angle_v 111 | self.pyramid_resolution_last = self.pyramid_resolution 112 | 113 | # Calculate performance 114 | self.mapParametersToPerformanceLevel() 115 | 116 | def initUI(self): 117 | global desktop_size 118 | window_size = [1280, 720] 119 | 120 | ''' Resolution slider ''' 121 | self.sl_res = QSlider(Qt.Horizontal, self) 122 | self.sl_res.setMinimum(10) 123 | self.sl_res.setMaximum(30) 124 | self.sl_res.setSingleStep(5) 125 | self.sl_res.setValue(int(self.resolution*100)) 126 | self.sl_res.valueChanged[int].connect(self.resValueChange) 127 | 128 | slide_size = [600,40] 129 | slider_res_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 300)] 130 | self.sl_res.setGeometry(slider_res_position[0], slider_res_position[1], slide_size[0], slide_size[1]) 131 | self.sl_res.setFocusPolicy(Qt.NoFocus) 132 | 133 | self.label_res = QLabel("Map Resolution (m)", self) 134 | self.label_res.move(slider_res_position[0]-300, slider_res_position[1]+10) 135 | self.label_res.setToolTip("Resolution of the voxel subspace. Range=[0.1,0.3]") 136 | self.line_edit_res = QLineEdit(str(self.resolution), self) 137 | self.line_edit_res.move(slider_res_position[0]-130, slider_res_position[1]+10) 138 | self.line_edit_res.resize(100,30) 139 | self.line_edit_res.textChanged.connect(self.resTextValueChanged) 140 | self.line_edit_res.setToolTip("Resolution of the voxel subspace. Range=[0.1,0.3]") 141 | 142 | res_validator = QDoubleValidator() 143 | res_validator.setRange(0.1, 0.3) 144 | res_validator.setDecimals(2) 145 | self.line_edit_res.setValidator(res_validator) 146 | 147 | ''' Map size ''' 148 | ### x 149 | self.sl_map_size_x = QSlider(Qt.Horizontal, self) 150 | self.sl_map_size_y = QSlider(Qt.Horizontal, self) 151 | self.sl_map_size_z = QSlider(Qt.Horizontal, self) 152 | 153 | self.sl_map_size_x.setMinimum(20) 154 | self.sl_map_size_x.setMaximum(200) 155 | self.sl_map_size_x.setSingleStep(int(self.resolution*100)) 156 | self.sl_map_size_x.setValue(int(self.map_length_x*10)) 157 | self.sl_map_size_x.valueChanged.connect(self.mapSizeChanged) 158 | sl_map_size_x_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 250)] 159 | self.sl_map_size_x.setGeometry(sl_map_size_x_position[0], sl_map_size_x_position[1], slide_size[0], slide_size[1]) 160 | self.sl_map_size_x.setFocusPolicy(Qt.NoFocus) 161 | 162 | self.label_map_size_x = QLabel("Map Length (m)", self) 163 | self.label_map_size_x.move(sl_map_size_x_position[0]-300, sl_map_size_x_position[1]+10) 164 | self.label_value_map_size_x = QLabel(str(self.map_length_x), self) 165 | self.label_value_map_size_x.move(sl_map_size_x_position[0]-130, sl_map_size_x_position[1]+10) 166 | self.label_value_map_size_x.resize(100,30) 167 | 168 | ### y 169 | self.sl_map_size_y.setMinimum(20) 170 | self.sl_map_size_y.setMaximum(200) 171 | self.sl_map_size_y.setSingleStep(int(self.resolution*100)) 172 | self.sl_map_size_y.setValue(int(self.map_length_y*10)) 173 | self.sl_map_size_y.valueChanged.connect(self.mapSizeChanged) 174 | sl_map_size_y_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 200)] 175 | self.sl_map_size_y.setGeometry(sl_map_size_y_position[0], sl_map_size_y_position[1], slide_size[0], slide_size[1]) 176 | self.sl_map_size_y.setFocusPolicy(Qt.NoFocus) 177 | 178 | self.label_map_size_y = QLabel("Map Width (m)", self) 179 | self.label_map_size_y.move(sl_map_size_y_position[0]-300, sl_map_size_y_position[1]+10) 180 | self.label_value_map_size_y = QLabel(str(self.map_length_y), self) 181 | self.label_value_map_size_y.move(sl_map_size_y_position[0]-130, sl_map_size_y_position[1]+10) 182 | self.label_value_map_size_y.resize(100,30) 183 | 184 | ### z 185 | self.sl_map_size_z.setMinimum(20) 186 | self.sl_map_size_z.setMaximum(200) 187 | self.sl_map_size_z.setSingleStep(int(self.resolution*100)) 188 | self.sl_map_size_z.setValue(int(self.map_length_z*10)) 189 | self.sl_map_size_z.valueChanged.connect(self.mapSizeChanged) 190 | sl_map_size_z_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 150)] 191 | self.sl_map_size_z.setGeometry(sl_map_size_z_position[0], sl_map_size_z_position[1], slide_size[0], slide_size[1]) 192 | self.sl_map_size_z.setFocusPolicy(Qt.NoFocus) 193 | 194 | self.label_map_size_z = QLabel("Map Height (m)", self) 195 | self.label_map_size_z.move(sl_map_size_z_position[0]-300, sl_map_size_z_position[1]+10) 196 | self.label_value_map_size_z = QLabel(str(self.map_length_z), self) 197 | self.label_value_map_size_z.move(sl_map_size_z_position[0]-130, sl_map_size_z_position[1]+10) 198 | self.label_value_map_size_z.resize(100,30) 199 | 200 | 201 | ''' Performance and Efficiency ''' 202 | self.sl_performance = QSlider(Qt.Horizontal, self) 203 | self.sl_performance.setMinimum(20) 204 | self.sl_performance.setMaximum(100) 205 | self.sl_performance.setSingleStep(2) 206 | self.sl_performance.setValue(self.performance) 207 | self.sl_performance.valueChanged.connect(self.performanceValueChange) 208 | 209 | slider_performance_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 100)] 210 | self.sl_performance.setGeometry(slider_performance_position[0], slider_performance_position[1], slide_size[0], slide_size[1]) 211 | self.sl_performance.setFocusPolicy(Qt.NoFocus) 212 | 213 | self.label_performance = QLabel("Performance", self) 214 | self.label_performance.move(slider_performance_position[0]-300, slider_performance_position[1]+10) 215 | self.label_performance = QLabel(str(self.performance), self) 216 | self.label_performance.move(slider_performance_position[0]-130, slider_performance_position[1]+10) 217 | self.label_performance.resize(100,30) 218 | 219 | self.sl_efficiency = QSlider(Qt.Horizontal, self) 220 | self.sl_efficiency.setMinimum(20) 221 | self.sl_efficiency.setMaximum(100) 222 | self.sl_efficiency.setSingleStep(2) 223 | self.sl_efficiency.setValue(self.efficiency) 224 | self.sl_efficiency.valueChanged.connect(self.efficiencyValueChange) 225 | 226 | slider_efficiency_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2 - 50)] 227 | self.sl_efficiency.setGeometry(slider_efficiency_position[0], slider_efficiency_position[1], slide_size[0], slide_size[1]) 228 | self.sl_efficiency.setFocusPolicy(Qt.NoFocus) 229 | 230 | self.label_efficiency = QLabel("Efficiency", self) 231 | self.label_efficiency.move(slider_efficiency_position[0]-300, slider_efficiency_position[1]+10) 232 | self.label_efficiency = QLabel(str(self.efficiency), self) 233 | self.label_efficiency.move(slider_efficiency_position[0]-130, slider_efficiency_position[1]+10) 234 | self.label_efficiency.resize(100,30) 235 | 236 | ''' FOV ''' 237 | fov_edit_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2)+30] 238 | 239 | self.label_fov_h = QLabel("FOV Horizontal Angle", self) 240 | self.label_fov_h.move(fov_edit_position[0]-300, fov_edit_position[1]+10) 241 | self.label_fov_h.setToolTip("Camera parameter. Degree. Range=[1,120]") 242 | self.line_edit_fov_h = QLineEdit(str(self.fov_angle_h), self) 243 | self.line_edit_fov_h.move(fov_edit_position[0]-100, fov_edit_position[1]+10) 244 | self.line_edit_fov_h.resize(100,30) 245 | self.line_edit_fov_h.textChanged.connect(self.fovHTextValueChanged) 246 | self.line_edit_fov_h.setToolTip("Camera parameter. Degree. Range=[1,120]") 247 | 248 | fov_h_validator = QIntValidator() 249 | fov_h_validator.setRange(1, 120) 250 | self.line_edit_fov_h.setValidator(fov_h_validator) 251 | 252 | self.label_fov_v = QLabel("FOV Vertical Angle", self) 253 | self.label_fov_v.move(fov_edit_position[0]+100, fov_edit_position[1]+10) 254 | self.label_fov_v.setToolTip("Camera parameter. Degree. Range=[1,90]") 255 | self.line_edit_fov_v = QLineEdit(str(self.fov_angle_v), self) 256 | self.line_edit_fov_v.move(fov_edit_position[0]+300, fov_edit_position[1]+10) 257 | self.line_edit_fov_v.resize(100,30) 258 | self.line_edit_fov_v.textChanged.connect(self.fovVTextValueChanged) 259 | self.line_edit_fov_v.setToolTip("Camera parameter. Degree. Range=[1,90]") 260 | 261 | fov_v_validator = QIntValidator() 262 | fov_v_validator.setRange(1, 90) 263 | self.line_edit_fov_h.setValidator(fov_v_validator) 264 | 265 | ''' Filter for initial velocity estimation ''' 266 | filter_label_position = [int(window_size[0]/2 - slide_size[0]/2)+100, int(window_size[1]/2 - slide_size[1]/2)+130] 267 | self.label_filter = QLabel("*Optional: paramters of the filter for initial velocity estimation", self) 268 | self.label_filter.move(filter_label_position[0]-300, filter_label_position[1]) 269 | 270 | filter_edit_position = [int(window_size[0]/2 - slide_size[0]/2)+120, int(window_size[1]/2 - slide_size[1]/2)+160] 271 | self.label_filter_max_number = QLabel("Max cluster point number", self) 272 | self.label_filter_max_number.move(filter_edit_position[0]-300, filter_edit_position[1]+10) 273 | self.line_filter_max_number = QLineEdit(str(self.filter_max_cluster_point_num), self) 274 | self.line_filter_max_number.move(filter_edit_position[0]-50, filter_edit_position[1]+10) 275 | self.line_filter_max_number.resize(100,30) 276 | self.line_filter_max_number.textChanged.connect(self.filterMaxNumberTextValueChanged) 277 | self.line_filter_max_number.setToolTip("The allowed maximum point number in a dynamic obstacle cluster.") 278 | 279 | filter_max_number_validator = QIntValidator() 280 | filter_max_number_validator.setRange(1, 10000) 281 | self.line_filter_max_number.setValidator(filter_max_number_validator) 282 | 283 | 284 | self.label_filter_max_height = QLabel("Max cluster center height", self) 285 | self.label_filter_max_height.move(filter_edit_position[0] + 100, filter_edit_position[1]+10) 286 | self.line_filter_max_height = QLineEdit(str(self.filter_max_cluster_center_height), self) 287 | self.line_filter_max_height.move(filter_edit_position[0]+350, filter_edit_position[1]+10) 288 | self.line_filter_max_height.resize(100,30) 289 | self.line_filter_max_height.textChanged.connect(self.filterCenterHeightTextValueChanged) 290 | self.line_filter_max_number.setToolTip("The allowed maximum center height of a dynamic obstacle cluster.") 291 | 292 | 293 | filter_max_height_validator = QDoubleValidator() 294 | filter_max_height_validator.setRange(0.1, 100.0) 295 | filter_max_height_validator.setDecimals(2) 296 | self.line_filter_max_height.setValidator(filter_max_height_validator) 297 | 298 | ''' Reset and Confirm button ''' 299 | button_position = [int(window_size[0]/2 - slide_size[0]/2)-100, int(window_size[1]/2 - slide_size[1]/2)+250] 300 | self.reset_button = QPushButton("Reset to default", self) 301 | self.reset_button.move(button_position[0], button_position[1]) 302 | self.reset_button.resize(250,80) 303 | self.reset_button.clicked.connect(self.resetButtonClicked); 304 | 305 | self.save_button = QPushButton("Save", self) 306 | self.save_button.move(button_position[0]+500, button_position[1]) 307 | self.save_button.resize(250,80) 308 | self.save_button.clicked.connect(self.saveButtonClicked); 309 | 310 | ''' Main Window''' 311 | self.setGeometry(int(desktop_size[0]/2 - window_size[0]/2), int(desktop_size[1]/2 - window_size[1]/2), window_size[0], window_size[1]) 312 | self.setWindowTitle('MapParameterQuickTuner') 313 | self.show() 314 | 315 | def resValueChange(self, value): 316 | self.resolution = value/100 317 | self.line_edit_res.setText(str(value/100)) 318 | self.sl_map_size_x.setSingleStep(int(self.resolution*100)) 319 | self.sl_map_size_y.setSingleStep(int(self.resolution*100)) 320 | self.sl_map_size_z.setSingleStep(int(self.resolution*100)) 321 | 322 | def resTextValueChanged(self): 323 | self.resolution = float(self.line_edit_res.text()) 324 | self.sl_res.setValue(int(self.resolution*100)) 325 | self.sl_map_size_x.setSingleStep(int(self.resolution*100)) 326 | self.sl_map_size_y.setSingleStep(int(self.resolution*100)) 327 | self.sl_map_size_z.setSingleStep(int(self.resolution*100)) 328 | 329 | def mapSizeChanged(self): 330 | self.map_length_x = self.sl_map_size_x.value()/10 331 | self.map_length_y = self.sl_map_size_y.value()/10 332 | self.map_length_z = self.sl_map_size_z.value()/10 333 | 334 | self.label_value_map_size_x.setText(str(self.map_length_x)) 335 | self.label_value_map_size_y.setText(str(self.map_length_y)) 336 | self.label_value_map_size_z.setText(str(self.map_length_z)) 337 | 338 | def efficiencyValueChange(self): 339 | self.efficiency = self.sl_efficiency.value() 340 | self.label_efficiency.setText(str(self.efficiency)) 341 | self.performance = 120 - self.sl_efficiency.value() 342 | self.label_performance.setText(str(self.performance)) 343 | self.sl_performance.setValue(self.performance) 344 | 345 | def performanceValueChange(self): 346 | self.performance = self.sl_performance.value() 347 | self.label_performance.setText(str(self.performance)) 348 | self.efficiency = 120 - self.performance 349 | self.label_efficiency.setText(str(self.efficiency)) 350 | self.sl_efficiency.setValue(self.efficiency) 351 | 352 | def fovHTextValueChanged(self): 353 | self.fov_angle_h = int(self.line_edit_fov_h.text()) 354 | 355 | def fovVTextValueChanged(self): 356 | self.fov_angle_v = int(self.line_edit_fov_v.text()) 357 | 358 | def filterMaxNumberTextValueChanged(self): 359 | self.filter_max_cluster_point_num = int(self.line_filter_max_number.text()) 360 | 361 | def filterCenterHeightTextValueChanged(self): 362 | self.filter_max_cluster_center_height = float(self.line_filter_max_height.text()) 363 | 364 | def resetButtonClicked(self): 365 | A = QMessageBox.question(self,'Confirm','Are you sure to reset the parameters？',QMessageBox.Yes | QMessageBox.No) 366 | if A == QMessageBox.Yes: 367 | self.initializeParameters() 368 | self.updateUI() 369 | 370 | def saveButtonClicked(self): 371 | if self.resolution > 0.3 or self.resolution < 0.1: 372 | msg = QMessageBox.warning(self, "Warning", "Map resolution should be in the range [0.1, 0.3]", buttons=QMessageBox.Ok) 373 | return 374 | 375 | if self.fov_angle_h > 120 or self.fov_angle_h < 0: 376 | msg = QMessageBox.warning(self, "Warning", "Horizontal FOV angle should be in the range [0, 120]", buttons=QMessageBox.Ok) 377 | return 378 | 379 | if self.fov_angle_v > 90 or self.fov_angle_v < 0: 380 | msg = QMessageBox.warning(self, "Warning", "Vertical FOV angle should be in the range [0, 90]", buttons=QMessageBox.Ok) 381 | return 382 | 383 | ''' Calculate Specific Map parameters and save ''' 384 | A = QMessageBox.question(self,'Confirm','Are you sure to save the parameter？',QMessageBox.Yes | QMessageBox.No) 385 | if A == QMessageBox.Yes: 386 | self.performanceLevelToMapParameters() 387 | self.max_particle_num_voxel = int(self.max_particle_density * (self.resolution*self.resolution*self.resolution)) 388 | # Set a minimum particle number in a voxel 389 | if self.max_particle_num_voxel < 5: 390 | self.max_particle_num_voxel = 5 391 | 392 | if not changeALineInAnyFile(self.head_file_dynamic, "#define VOXEL_RESOLUTION", "#define VOXEL_RESOLUTION " + str(self.resolution) + "\n"): 393 | QMessageBox.critical(self, "Error", "Cannot find VOXEL_RESOLUTION in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 394 | return 395 | 396 | if not changeALineInAnyFile(self.head_file_dynamic, "#define MAP_LENGTH_VOXEL_NUM", "#define MAP_LENGTH_VOXEL_NUM " + str(int(self.map_length_x/self.resolution)) + "\n"): 397 | QMessageBox.critical(self, "Error", "Cannot find MAP_LENGTH_VOXEL_NUM in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 398 | return 399 | 400 | if not changeALineInAnyFile(self.head_file_dynamic, "#define MAP_WIDTH_VOXEL_NUM", "#define MAP_WIDTH_VOXEL_NUM " + str(int(self.map_length_y/self.resolution)) + "\n"): 401 | QMessageBox.critical(self, "Error", "Cannot find MAP_WIDTH_VOXEL_NUM in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 402 | return 403 | 404 | if not changeALineInAnyFile(self.head_file_dynamic, "#define MAP_HEIGHT_VOXEL_NUM", "#define MAP_HEIGHT_VOXEL_NUM " + str(int(self.map_length_z/self.resolution)) + "\n"): 405 | QMessageBox.critical(self, "Error", "Cannot find MAP_HEIGHT_VOXEL_NUM in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 406 | return 407 | 408 | if not changeALineInAnyFile(self.head_file_dynamic, "#define DYNAMIC_CLUSTER_MAX_POINT_NUM", "#define DYNAMIC_CLUSTER_MAX_POINT_NUM " + str(self.filter_max_cluster_point_num) + "\n"): 409 | QMessageBox.critical(self, "Error", "Cannot find DYNAMIC_CLUSTER_MAX_POINT_NUM in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 410 | return 411 | 412 | if not changeALineInAnyFile(self.head_file_dynamic, "#define DYNAMIC_CLUSTER_MAX_CENTER_HEIGHT", "#define DYNAMIC_CLUSTER_MAX_CENTER_HEIGHT " + str(self.filter_max_cluster_center_height) + "\n"): 413 | QMessageBox.critical(self, "Error", "Cannot find DYNAMIC_CLUSTER_MAX_CENTER_HEIGHT in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 414 | return 415 | 416 | if not changeALineInAnyFile(self.head_file_dynamic, "#define ANGLE_RESOLUTION", "#define ANGLE_RESOLUTION " + str(self.pyramid_resolution) + "\n"): 417 | QMessageBox.critical(self, "Error", "Cannot find ANGLE_RESOLUTION in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 418 | return 419 | 420 | if not changeALineInAnyFile(self.head_file_dynamic, "#define MAX_PARTICLE_NUM_VOXEL", "#define MAX_PARTICLE_NUM_VOXEL " + str(self.max_particle_num_voxel) + "\n"): 421 | QMessageBox.critical(self, "Error", "Cannot find MAX_PARTICLE_NUM_VOXEL in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 422 | return 423 | 424 | if not changeALineInAnyFile(self.cpp_file, "const float res", "const float res = " + str(self.voxel_filter) + ";\n"): 425 | QMessageBox.critical(self, "Error", "Cannot find float res in " + self.cpp_file +"!", buttons=QMessageBox.Ok) 426 | return 427 | 428 | ''' Change threshold and new born particle number ''' 429 | occupancy_threshold = 0.2 430 | if self.resolution > 0.18: 431 | occupancy_threshold = 0.5 432 | if self.resolution > 0.28: 433 | occupancy_threshold = 0.6 434 | 435 | if not changeALineInAnyFile(self.cpp_file, "my_map.getOccupancyMapWithFutureStatus(occupied_num", 436 | " my_map.getOccupancyMapWithFutureStatus(occupied_num, cloud_to_publish, &future_status[0][0], " 437 | + str(occupancy_threshold) + ");\n"): 438 | QMessageBox.critical(self, "Error", "Cannot find Function getOccupancyMapWithFutureStatus in " + self.cpp_file +"!", buttons=QMessageBox.Ok) 439 | return 440 | 441 | ''' Change FOV ''' 442 | if self.fov_angle_h != self.fov_angle_h_last or self.pyramid_resolution != self.pyramid_resolution_last: 443 | half_fov_angle_h = int((self.fov_angle_h-self.pyramid_resolution) / 2 / self.pyramid_resolution)*self.pyramid_resolution # Abort the measurement close to fov's edge. 444 | if not changeALineInAnyFile(self.head_file_dynamic, "const int half_fov_h", "const int half_fov_h = " + str(half_fov_angle_h) + ";\n"): 445 | QMessageBox.critical(self, "Error", "Cannot find Parameter half_fov_h in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 446 | return 447 | 448 | if self.fov_angle_v != self.fov_angle_v_last or self.pyramid_resolution != self.pyramid_resolution_last: 449 | half_fov_angle_v = int((self.fov_angle_v-self.pyramid_resolution) / 2 / self.pyramid_resolution)*self.pyramid_resolution # Abort the measurement close to fov's edge. 450 | if not changeALineInAnyFile(self.head_file_dynamic, "const int half_fov_v", "const int half_fov_v = " + str(half_fov_angle_v) + ";\n"): 451 | QMessageBox.critical(self, "Error", "Cannot find Parameter half_fov_v in " + self.head_file_dynamic +"!", buttons=QMessageBox.Ok) 452 | return 453 | 454 | QMessageBox.information(self, "Note", "Parameters modified successfully! Please recompile the map using catkin_make or catkin build.", buttons=QMessageBox.Ok) 455 | 456 | self.readCurrentParameters() 457 | self.updateUI() 458 | 459 | def performanceLevelToMapParameters(self): 460 | if self.performance < 35: 461 | self.pyramid_resolution = 1 462 | self.voxel_filter = 0.2 463 | self.max_particle_density = int((3000-1000)*(self.performance-20)/15 + 1000) 464 | elif self.performance < 50: 465 | self.pyramid_resolution = 1 466 | self.voxel_filter = 0.15 467 | self.max_particle_density = int((3000-2000)*(self.performance-35)/15 + 2000) 468 | elif self.performance < 70: 469 | self.pyramid_resolution = 3 470 | self.voxel_filter = 0.15 471 | self.max_particle_density = int((3000-2000)*(self.performance-50)/20 + 2000) 472 | else: 473 | self.pyramid_resolution = 3 474 | self.voxel_filter = 0.1 475 | self.max_particle_density = int((6000-2500)*(self.performance-70)/30 + 2500) 476 | 477 | def mapParametersToPerformanceLevel(self): 478 | if self.pyramid_resolution < 2: 479 | if self.voxel_filter > 0.18: # 20 < performance < 35 480 | self.performance = int((self.max_particle_density - 1000)/(3000-1000)*15+20) 481 | else: # 35 < performance < 50 482 | self.performance = int((self.max_particle_density - 2000)/(3000-2000)*15+35) 483 | else: 484 | if self.voxel_filter > 0.12: # 50 < performance < 70 485 | self.performance = int((self.max_particle_density - 2000)/(3000-2000)*20+50) 486 | else: # 70 < performance < 100 487 | self.performance = int((self.max_particle_density - 2500)/(6000-2500)*30+70) 488 | if self.performance > 100: 489 | self.performance = 100 490 | 491 | self.efficiency = 120-self.performance 492 | 493 | def updateUI(self): 494 | self.sl_res.setValue(int(self.resolution*100)) 495 | self.sl_map_size_x.setValue(int(self.map_length_x*10)) 496 | self.sl_map_size_y.setValue(int(self.map_length_y*10)) 497 | self.sl_map_size_z.setValue(int(self.map_length_z*10)) 498 | self.sl_efficiency.setValue(self.efficiency) 499 | self.sl_performance.setValue(self.performance) 500 | self.line_edit_fov_h.setText(str(self.fov_angle_h)) 501 | self.line_edit_fov_v.setText(str(self.fov_angle_v)) 502 | self.line_filter_max_number.setText(str(self.filter_max_cluster_point_num)) 503 | self.line_filter_max_height.setText(str(self.filter_max_cluster_center_height)) 504 | 505 | if __name__ == '__main__': 506 | desktop_size=[0,0] 507 | app = QApplication(sys.argv) 508 | desktop_size[0] = app.desktop().width() 509 | desktop_size[1] = app.desktop().height() 510 | 511 | ex = Q_Window() 512 | sys.exit(app.exec_()) -------------------------------------------------------------------------------- /src/map_sim_example.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************** 2 | 3 | Copyright <2022> 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | 12 | Author: Gang Chen 13 | 14 | Date: 2021/8/19 15 | 16 | Description: This is a ROS example to use the DSP map. The map object is my_map and updated in Function cloudCallback. We also add some visualization functions in this file. The visualization results can be viewed with RVIZ. 17 | 18 | **************************************************************************/ 19 | 20 | 21 | #include "ros/ros.h" 22 | #include "dsp_dynamic.h" // You can change the head file to "dsp_dynamic_multiple_neighbors.h" or "dsp_static.h" to use different map types. For more information, please refer to the readme file. 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "geometry_msgs/Pose.h" 29 | #include 30 | #include 31 | #include 32 | #include "gazebo_msgs/ModelStates.h" 33 | #include "geometry_msgs/PoseStamped.h" 34 | #include 35 | #include "nav_msgs/Odometry.h" 36 | #include "std_msgs/Float64.h" 37 | 38 | /// Define a map object 39 | DSPMap my_map; 40 | const float res = 0.1; 41 | /// Set global variables 42 | queue pose_att_time_queue; 43 | queue uav_position_global_queue; 44 | queue uav_att_global_queue; 45 | Eigen::Vector3d uav_position_global; 46 | Eigen::Quaternionf uav_att_global; 47 | 48 | const unsigned int MAX_POINT_NUM = 5000; // Estimated max point cloud number after down sample. To define the vector below. 49 | float point_clouds[MAX_POINT_NUM*3]; // Container for point cloud. We use naive vector for efficiency purpose. 50 | 51 | // The following range parameters are calculated with map parameters to remove the point cloud outside of the map range. 52 | float x_min = -MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2; 53 | float x_max = MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2; 54 | float y_min = -MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2; 55 | float y_max = MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2; 56 | float z_min = -MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2; 57 | float z_max = MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2; 58 | 59 | ros::Publisher cloud_pub, map_center_pub, gazebo_model_states_pub, current_velocity_pub, single_object_velocity_pub, single_object_velocity_truth_pub; 60 | ros::Publisher future_status_pub, current_marker_pub, fov_pub, update_time_pub; 61 | gazebo_msgs::ModelStates ground_truth_model_states; 62 | 63 | int ground_truth_updated = 0; 64 | bool state_locked = false; 65 | 66 | /*** 67 | * Summary: This function is for actor true position visualization 68 | */ 69 | void actor_publish(const vector &actors, int id, float r, float g, float b, float width, int publish_num) 70 | { 71 | if(actors.empty()) return; 72 | 73 | visualization_msgs::MarkerArray marker_array; 74 | 75 | visualization_msgs::Marker marker; 76 | 77 | marker.header.frame_id = "map"; 78 | marker.header.stamp = ros::Time::now(); 79 | marker.type = visualization_msgs::Marker::CYLINDER; 80 | marker.action = visualization_msgs::Marker::ADD; 81 | marker.ns = "actors"; 82 | 83 | marker.scale.x = 0.4; 84 | marker.scale.y = 0.4; 85 | marker.scale.z = 1.7; 86 | marker.color.a = 0.6; 87 | marker.color.r = 0.3; 88 | marker.color.g = 0.3; 89 | marker.color.b = 0.9; 90 | 91 | marker.pose.orientation.x = 0.0; 92 | marker.pose.orientation.y = 0.0; 93 | marker.pose.orientation.z = 0.0; 94 | marker.pose.orientation.w = 1.0; 95 | 96 | for(int i=0; i low && x < high){ 193 | return 1; 194 | }else{ 195 | return 0; 196 | } 197 | } 198 | 199 | /*** 200 | * Summary: This function is for future status visualization 201 | */ 202 | void colorAssign(int &r, int &g, int &b, float v, float value_min=0.f, float value_max=1.f, int reverse_color=0) 203 | { 204 | v = std::max(v, value_min); 205 | v = std::min(v, value_max); 206 | 207 | float v_range = value_max - value_min; 208 | int value = floor((v - value_min) / v_range * 240); // Mapping 0~1.0 to 0~240 209 | value = std::min(value, 240); 210 | 211 | if(reverse_color){ 212 | value = 240 - value; 213 | } 214 | 215 | int section = value / 60; 216 | float float_key = (value % 60) / (float)60 * 255; 217 | int key = floor(float_key); 218 | int nkey = 255 - key; 219 | 220 | switch(section) { 221 | case 0: // G increase 222 | r = 255; 223 | g = key; 224 | b = 0; 225 | break; 226 | case 1: // R decrease 227 | r = nkey; 228 | g = 255; 229 | b = 0; 230 | break; 231 | case 2: // B increase 232 | r = 0; 233 | g = 255; 234 | b = key; 235 | break; 236 | case 3: // G decrease 237 | r = 0; 238 | g = nkey; 239 | b = 255; 240 | break; 241 | case 4: // Sky blue 242 | r = 0; 243 | g = 255; 244 | b = 255; 245 | break; 246 | default: // White 247 | r = 255; 248 | g = 255; 249 | b = 255; 250 | } 251 | 252 | } 253 | 254 | /*** 255 | * Summary: This is the main callback to update map. 256 | */ 257 | pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud()); 258 | void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud) 259 | { 260 | /// Simple synchronizer for point cloud data and pose 261 | Eigen::Vector3d uav_position = uav_position_global; 262 | Eigen::Quaternionf uav_att = uav_att_global; 263 | 264 | static Eigen::Quaternionf quad_last_popped(-10.f, -10.f, -10.f, -10.f); 265 | static Eigen::Vector3d position_last_popped(-10000.f, -10000.f, -10000.f); 266 | static double last_popped_time = 0.0; 267 | 268 | ros::Rate loop_rate(500); 269 | while(state_locked){ 270 | loop_rate.sleep(); 271 | ros::spinOnce(); 272 | } 273 | state_locked = true; 274 | 275 | while(!pose_att_time_queue.empty()){ //Synchronize pose by queue 276 | double time_stamp_pose = pose_att_time_queue.front(); 277 | if(time_stamp_pose >= cloud->header.stamp.toSec()){ 278 | uav_att = uav_att_global_queue.front(); 279 | uav_position = uav_position_global_queue.front(); 280 | 281 | // linear interpolation 282 | if(quad_last_popped.x() >= -1.f){ 283 | double time_interval_from_last_time = time_stamp_pose - last_popped_time; 284 | double time_interval_cloud = cloud->header.stamp.toSec() - last_popped_time; 285 | double factor = time_interval_cloud / time_interval_from_last_time; 286 | uav_att = quad_last_popped.slerp(factor, uav_att); 287 | uav_position = position_last_popped * (1.0 - factor) + uav_position*factor; 288 | } 289 | 290 | ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - time_stamp_pose); 291 | 292 | break; 293 | } 294 | 295 | quad_last_popped = uav_att_global_queue.front(); 296 | position_last_popped = uav_position_global_queue.front(); 297 | last_popped_time = time_stamp_pose; 298 | 299 | pose_att_time_queue.pop(); 300 | uav_att_global_queue.pop(); 301 | uav_position_global_queue.pop(); 302 | } 303 | state_locked = false; 304 | 305 | 306 | /// Point cloud preprocess 307 | double data_time_stamp = cloud->header.stamp.toSec(); 308 | 309 | // convert cloud to pcl form 310 | pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud()); 311 | pcl::fromROSMsg(*cloud, *cloud_in); 312 | 313 | // down-sample for all 314 | pcl::VoxelGrid sor; 315 | sor.setInputCloud(cloud_in); 316 | sor.setLeafSize(res, res, res); 317 | sor.filter(*cloud_filtered); 318 | 319 | int useful_point_num = 0; 320 | for(int i=0; iwidth; i++){ 321 | float x = cloud_filtered->points.at(i).z; 322 | float y = -cloud_filtered->points.at(i).x; 323 | float z = -cloud_filtered->points.at(i).y; 324 | 325 | if(inRange(x_min, x_max, x) && inRange(y_min, y_max, y) && inRange(z_min, z_max, z)) 326 | { 327 | point_clouds[useful_point_num*3] = x; 328 | point_clouds[useful_point_num*3+1] = y; 329 | point_clouds[useful_point_num*3+2] = z; 330 | ++ useful_point_num; 331 | 332 | if(useful_point_num >= MAX_POINT_NUM){ // In case the buffer overflows 333 | break; 334 | } 335 | } 336 | } 337 | 338 | /// Update map 339 | clock_t start1, finish1; 340 | start1 = clock(); 341 | 342 | std::cout << "uav_position="< cloud_to_publish; 370 | sensor_msgs::PointCloud2 cloud_to_pub_transformed; 371 | static float future_status[VOXEL_NUM][PREDICTION_TIMES]; 372 | /** Note: The future status is stored with voxel structure. 373 | * The voxels are indexed with one dimension. 374 | * You can use Function getVoxelPositionFromIndexPublic() to convert index to real position. 375 | * future_status[*][0] is current status considering delay compensation. 376 | **/ 377 | 378 | my_map.getOccupancyMapWithFutureStatus(occupied_num, cloud_to_publish, &future_status[0][0], 0.2); 379 | 380 | /// Publish Point cloud and center position 381 | pcl::toROSMsg(cloud_to_publish, cloud_to_pub_transformed); 382 | cloud_to_pub_transformed.header.frame_id = "map"; 383 | cloud_to_pub_transformed.header.stamp = cloud->header.stamp; 384 | cloud_pub.publish(cloud_to_pub_transformed); 385 | 386 | geometry_msgs::PoseStamped map_pose; 387 | map_pose.header.stamp = cloud_to_pub_transformed.header.stamp; 388 | map_pose.pose.position.x = uav_position.x(); 389 | map_pose.pose.position.y = uav_position.y(); 390 | map_pose.pose.position.z = uav_position.z(); 391 | map_pose.pose.orientation.x = uav_att.x(); 392 | map_pose.pose.orientation.y = uav_att.y(); 393 | map_pose.pose.orientation.z = uav_att.z(); 394 | map_pose.pose.orientation.w = uav_att.w(); 395 | map_center_pub.publish(map_pose); 396 | 397 | 398 | /// Publish future status of one layer 399 | pcl::PointCloud future_status_cloud; 400 | static const int z_index_to_show = MAP_HEIGHT_VOXEL_NUM / 2 - 1; ///Layer 401 | for(int j=0; jheader.stamp; 427 | future_status_pub.publish(cloud_future_transformed); 428 | 429 | finish2 = clock(); 430 | 431 | double duration2 = (double)(finish2 - start2) / CLOCKS_PER_SEC; 432 | printf( "****** Map publish time %f seconds\n \n", duration2); 433 | 434 | /// Publish update time for evaluation tools 435 | std_msgs::Float64 update_time; 436 | update_time.data = duration1 + duration2; 437 | update_time_pub.publish(update_time); 438 | } 439 | 440 | /*** 441 | * Summary: This function is used to tell pedestrians' names in simObjectStateCallback 442 | */ 443 | static void split(const string& s, vector& tokens, const string& delimiters = " ") 444 | { 445 | string::size_type lastPos = s.find_first_not_of(delimiters, 0); 446 | string::size_type pos = s.find_first_of(delimiters, lastPos); 447 | while (string::npos != pos || string::npos != lastPos) { 448 | tokens.push_back(s.substr(lastPos, pos - lastPos)); 449 | lastPos = s.find_first_not_of(delimiters, pos); 450 | pos = s.find_first_of(delimiters, lastPos); 451 | } 452 | } 453 | 454 | /*** 455 | * Summary: Ros callback function to get true position of pedestrians in Gazebo. Just for visualization 456 | */ 457 | void simObjectStateCallback(const gazebo_msgs::ModelStates &msg) 458 | { 459 | ground_truth_model_states = msg; 460 | ground_truth_updated = 1; 461 | 462 | vector actor_visualization_points; 463 | 464 | for(int i=0; i name_splited; 467 | split(msg.name[i], name_splited, "_"); 468 | if(name_splited[0] == "actor"){ 469 | Eigen::Vector3d p; 470 | p.x() = msg.pose[i].position.x - uav_position_global.x(); 471 | p.y() = msg.pose[i].position.y - uav_position_global.y(); 472 | p.z() = msg.pose[i].position.z - uav_position_global.z(); 473 | actor_visualization_points.push_back(p); 474 | } 475 | } 476 | actor_publish(actor_visualization_points, 6, 1.f, 0.f, 0.f, 0.8, -1); 477 | gazebo_model_states_pub.publish(ground_truth_model_states); 478 | } 479 | 480 | /*** 481 | * Summary: Ros callback function to get pose of the drone (camera) to update map. 482 | */ 483 | void simPoseCallback(const geometry_msgs::PoseStamped &msg) 484 | { 485 | if(!state_locked) 486 | { 487 | state_locked = true; 488 | uav_position_global.x() = msg.pose.position.x; 489 | uav_position_global.y() = msg.pose.position.y; 490 | uav_position_global.z() = msg.pose.position.z; 491 | 492 | uav_att_global.x() = msg.pose.orientation.x; 493 | uav_att_global.y() = msg.pose.orientation.y; 494 | uav_att_global.z() = msg.pose.orientation.z; 495 | uav_att_global.w() = msg.pose.orientation.w; 496 | 497 | uav_position_global_queue.push(uav_position_global); 498 | uav_att_global_queue.push(uav_att_global); 499 | pose_att_time_queue.push(msg.header.stamp.toSec()); 500 | ROS_INFO("Pose updated"); 501 | } 502 | 503 | state_locked = false; 504 | 505 | Eigen::Quaternionf axis; //= quad * q1 * quad.inverse(); 506 | axis.w() = cos(-M_PI/4.0); 507 | axis.x() = 0.0; 508 | axis.y() = 0.0; 509 | axis.z() = sin(-M_PI/4.0); 510 | Eigen::Quaternionf rotated_att = uav_att_global * axis; 511 | 512 | showFOV(uav_position_global, rotated_att, 90.0 / 180.0 * M_PI, 54.0 / 180.0 * M_PI , 5); 513 | } 514 | 515 | 516 | int main(int argc, char **argv) 517 | { 518 | ros::init(argc, argv, "map_sim_example_with_cluster"); 519 | ros::NodeHandle n; 520 | 521 | /// Map parameters that can be changed dynamically. But usually we still use them as static parameters. 522 | my_map.setPredictionVariance(0.05, 0.05); // StdDev for prediction. velocity StdDev, position StdDev, respectively. 523 | my_map.setObservationStdDev(0.1); // StdDev for update. position StdDev. 524 | my_map.setNewBornParticleNumberofEachPoint(20); // Number of new particles generated from one measurement point. 525 | my_map.setNewBornParticleWeight(0.0001); // Initial weight of particles. 526 | DSPMap::setOriginalVoxelFilterResolution(res); // Resolution of the voxel filter used for point cloud pre-process. 527 | 528 | my_map.setParticleRecordFlag(0, 19.0); // Set the first parameter to 1 to save particles at a time: e.g. 19.0s. Saving will take a long time. Don't use it in realtime applications. 529 | 530 | 531 | /// Gazebo pedestrain's pose. Just for visualization 532 | ros::Subscriber object_states_sub = n.subscribe("/gazebo/model_states", 1, simObjectStateCallback); 533 | 534 | /// Input data for the map 535 | ros::Subscriber point_cloud_sub = n.subscribe("/camera_front/depth/points", 1, cloudCallback); 536 | ros::Subscriber pose_sub = n.subscribe("/mavros/local_position/pose", 1, simPoseCallback); 537 | 538 | /// Visualization topics 539 | cloud_pub = n.advertise("/my_map/cloud_ob", 1, true); 540 | map_center_pub = n.advertise("/my_map/map_center", 1, true); 541 | gazebo_model_states_pub = n.advertise("/my_map/model_states", 1, true); 542 | 543 | future_status_pub = n.advertise("/my_map/future_status", 1, true); 544 | 545 | current_velocity_pub = n.advertise("/my_map/velocity_marker", 1); 546 | single_object_velocity_pub = n.advertise("/my_map/single_object_velocity", 1); 547 | single_object_velocity_truth_pub = n.advertise("/my_map/single_object_velocity_ground_truth", 1); 548 | current_marker_pub = n.advertise("/visualization_marker", 1); 549 | fov_pub = n.advertise("/visualization_fov", 1); 550 | 551 | update_time_pub = n.advertise("/map_update_time", 1); 552 | 553 | /// Ros spin 554 | ros::AsyncSpinner spinner(3); // Use 3 threads 555 | spinner.start(); 556 | ros::waitForShutdown(); 557 | 558 | return 0; 559 | } 560 | --------------------------------------------------------------------------------