├── .github └── workflows │ └── industrial_ci_action.yml ├── .gitignore ├── LICENSE ├── README.md ├── feature_based_mapping ├── CMakeLists.txt ├── README.md ├── docs │ ├── fbm_cave.gif │ ├── fbm_cave_2.gif │ ├── fbm_cave_3.gif │ ├── fbm_loopclosure.gif │ ├── gt_vs_map.png │ ├── motivation.png │ └── motivation_keypoints.png ├── include │ └── feature_based_mapping │ │ ├── dataContainers.hpp │ │ └── fbm_icp.hpp ├── launch │ └── fbm_icp.launch ├── package.xml ├── rviz │ └── fbm_icp.rviz └── src │ └── fbm_icp.cpp ├── gazebo_ros_lidar_plugin ├── CMakeLists.txt ├── README.md ├── SDF_config_example │ ├── benwake_CE30D.md │ ├── images │ │ ├── benwakeCE30_rviz.gif │ │ ├── benwake_rviz_gazebo.png │ │ ├── velodyne16_rviz.gif │ │ ├── velodyne_16_gazebo.png │ │ └── velodyne_plugin_demo.gif │ └── velodyne16.md ├── include │ └── gazebo_ros_lidar_plugin │ │ ├── custom_lidar.h │ │ ├── dataContainers.h │ │ └── gazebo_ros_lidar_plugin.h ├── package.xml ├── setup.py └── src │ ├── custom_lidar.cpp │ └── gazebo_ros_lidar_plugin.cpp ├── gazebo_simulator ├── CMakeLists.txt ├── config │ ├── benwake_costmap_common_params.yaml │ ├── costmap_global_params.yaml │ ├── costmap_local_params.yaml │ ├── dwa_local_planner_params.yaml │ ├── gmapping_params.yaml │ ├── move_base_params.yaml │ ├── robot_control.yaml │ └── velodyne_costmap_common_params.yaml ├── docs │ ├── benwake_farming_demo.gif │ ├── slam_cave.gif │ ├── slam_farm.gif │ ├── velodyne_cave.gif │ └── velodyne_farming_demo.gif ├── launch │ ├── control │ │ └── robot_control.launch │ ├── description │ │ └── robot_description.launch │ ├── navigation │ │ ├── amcl.launch │ │ ├── move_base.launch │ │ └── robot_navigation.launch │ ├── sigmapilot_gazebo.launch │ ├── sigmapilot_rpi_replay.launch │ └── slam │ │ └── robot_gmapping.launch ├── maps │ ├── iscas_map.pgm │ ├── iscas_map.yaml │ ├── map.pgm │ ├── map.yaml │ ├── mymap_cave.pgm │ └── mymap_cave.yaml ├── meshes │ └── hokuyo.dae ├── package.xml ├── rviz │ ├── sigmapilot_basic_config.rviz │ └── sigmapilot_navigation.rviz ├── urdf │ ├── benwake │ │ ├── skid_steer_bot.gazebo │ │ └── skid_steer_bot.xacro │ └── velodyne │ │ ├── skid_steer_bot.gazebo │ │ └── skid_steer_bot.xacro └── worlds │ ├── 50m_long_mine_world.world │ ├── benchmarking.world │ ├── iscas_museum.world │ ├── jackal_race.world │ ├── mine_world.world │ ├── mine_world_notworking.world │ └── turtlebot3_world.world ├── laser_scans_fusion ├── CMakeLists.txt ├── README.md ├── include │ └── laser_scans_fusion │ │ ├── dataContainers.hpp │ │ ├── laser_scans_fusion.hpp │ │ └── scan_fusion.hpp ├── launch │ └── multi_scan_fusion.launch ├── package.xml └── src │ ├── laser_scans_fusion.cpp │ └── scan_fusion.cpp ├── point_cloud_assembler ├── CMakeLists.txt ├── include │ └── point_cloud_assembler │ │ ├── dataContainers.hpp │ │ └── point_cloud_assembler.hpp ├── launch │ └── assembler_node.launch ├── package.xml ├── rviz │ └── pointcloud_assembler.rviz └── src │ └── point_cloud_assembler.cpp └── sgm_lidar_clustering ├── CMakeLists.txt ├── README.md ├── docs ├── benwake_clustering_gazebo.gif ├── benwake_clustering_rviz.gif ├── euclidean_clustering_16_performance.png ├── euclidean_clustering_16_range.jpeg ├── euclidean_clustering_64_performance.png ├── euclidean_clustering_64_range.jpeg ├── rpi_benwake_demo.gif ├── rpi_velodyne_demo.gif ├── sgm_16_v2_performance.png ├── sgm_16_v2_range.png ├── sgm_64_v2_performance.png ├── sgm_v2_64_range.png ├── simulation_setup.jpeg ├── velodyne16_clustering_gazebo.gif └── velodyne16_clustering_rviz.gif ├── include └── sgm_lidar_clustering │ ├── dataContainers.hpp │ ├── ground_segmentation.hpp │ ├── sgm_lidar_clustering.hpp │ └── sgm_segmentation.hpp ├── launch ├── Cluster_node.launch ├── gtest.test └── multi_node_benwake.launch ├── nodes └── sgm_lidar_clustering.cpp ├── package.xml ├── rviz ├── Lidar.rviz ├── clustering.rviz ├── clustering_rpi.rviz ├── multi_LiDAR.rviz ├── multi_sensorclustering.rviz └── multi_sensorclustering_rpi.rviz ├── src ├── ground_segmentation.cpp └── sgm_segmentation.cpp └── tests └── sgm_unittests.cpp /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | env: 8 | ADDITIONAL_DEBS: ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control libgazebo11-dev 9 | TARGET_WORKSPACE: sgm_lidar_clustering laser_scans_fusion gazebo_simulator point_cloud_assembler 10 | strategy: 11 | matrix: 12 | env: 13 | - {ROS_DISTRO: noetic, ROS_REPO: testing} 14 | - {ROS_DISTRO: noetic, ROS_REPO: main} 15 | 16 | runs-on: ubuntu-latest 17 | steps: 18 | - uses: actions/checkout@v1 19 | - uses: 'ros-industrial/industrial_ci@master' 20 | env: ${{matrix.env}} 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | #CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![CI](https://github.com/SigmaEmbeddedEngineering/Sigmapilot/workflows/CI/badge.svg?branch=main) 2 | 3 | # Sigmapilot 4 | Sigmpilot is an online demo for Sigma embedded engineering clients representing our skills in autonomous robotics, sensor fusion and simulation environments. 5 | Sigmapilot is an all-in-one open-source software for autonomous UGVs and industrial robots. the target industries that Sigmapilot is working on are farming industry, mining industry, warehouses industry, and construction industry. 6 | 7 | we were always wondering why there isnt any framework that processes robust autonomous software stack on **Raspberry Pi** board that can be used in UGV industries, do we always need a **NVIDIA** board?. The result was **Sigmapilot** :) 8 | 9 | # Sigmapilot Featues 10 | 11 | - Modular software components can work with different sensors(benwake/velodyne) 12 | - 3D semantic segmentaiton object detection. 13 | - Freespace estimation. 14 | - Integration of Sigmapilot with ROS packages such as gmapping, move base, and amcl. 15 | - Free space fusion for cocoon Setup. 16 | - 3D point cloud assembly. 17 | - 3D mapping and matching. 18 | - Camera-LiDAR fusion. 19 | 20 | # Sigmapilot Planned Features 21 | 22 | - Object tracking. 23 | - new approaches for 2D mapping and matching. 24 | 25 | 26 | # Demos: 27 | 28 | ## Feature Based Mapping 29 | ### Run 30 | launch the **sigmapilot_gazebo.launch** file then the **fbm_icp.launch** files in two different terminals. 31 | 32 | roslaunch gazebo_simulator sigmapilot_gazebo.launch 33 | roslaunch feature_based_mapping fbm_icp.launch 34 | ### Loop closure 35 | in the below demo we present a loop closure of the FBM algorithm, where there is no deviation of the constructed map 36 | 37 | ![loop_closure](feature_based_mapping/docs/fbm_loopclosure.gif) 38 | 39 | ### Elivated Environments 40 | in the below demo we present the algorithm estimates in elevated environments, such as elevated caves 41 | 42 | ![elevated_cave1](feature_based_mapping/docs/fbm_cave.gif) 43 | 44 | ![elevated_cave2](feature_based_mapping/docs/fbm_cave_2.gif) 45 | 46 | in the below demo we present the qualitative validation of the accumlative map(Ground truth), and the generated FBM point cloud. 47 | 48 | ![elevated_cave_val2](feature_based_mapping/docs/fbm_cave_3.gif) 49 | 50 | ## Navigation 51 | ### Sigmapilot Farm simulation demo Velodyne 52 | 53 | roslaunch gazebo_simulator robot_navigation.launch sensor:=velodyne world_name:='$(find gazebo_simulator)/worlds/turtlebot3_world.world' map_file:='$(find gazebo_simulator)/maps/map.yaml' 54 | 55 | ![velo_farm](gazebo_simulator/docs/velodyne_farming_demo.gif) 56 | 57 | ### Sigmapilot Cave simulation demo velodyne 58 | 59 | roslaunch gazebo_simulator robot_navigation.launch sensor:=velodyne world_name:='$(find gazebo_simulator)/worlds/50m_long_mine_world.world' map_file:='$(find gazebo_simulator)/maps/mymap_cave.yaml' 60 | 61 | ![velo_cave](gazebo_simulator/docs/velodyne_cave.gif) 62 | 63 | ### Sigmapilot Farm simulation demo Benwake cocoon 64 | 65 | roslaunch gazebo_simulator robot_navigation.launch sensor:=benwake world_name:='$(find gazebo_simulator)/worlds/turtlebot3_world.world' map_file:='$(find gazebo_simulator)/maps/map.yaml' 66 | 67 | ![benwake_farm](gazebo_simulator/docs/benwake_farming_demo.gif) 68 | 69 | ## SLAM 70 | 71 | ### Sigmapilot map farm environment 72 | 73 | roslaunch gazebo_simulator Sigmapilot_gazebo.launch sensor:=velodyne 74 | 75 | ![benwake_farm](gazebo_simulator/docs/slam_farm.gif) 76 | 77 | 78 | ### Sigmapilot map cave environment 79 | 80 | roslaunch gazebo_simulator Sigmapilot_gazebo.launch sensor:=velodyne world_name:='$(find gazebo_simulator)/worlds/50m_long_mine_world.world' 81 | 82 | ![benwake_farm](gazebo_simulator/docs/slam_cave.gif) 83 | 84 | ## Raspberry pi benchmarking 85 | 86 | follow the instructions in [sgm_lidar_clustering](sgm_lidar_clustering/README.md) 87 | 88 | ## SGM clustering Vs lidar_euclidean_cluster_detect 89 | 90 | read the benchmarking section in [sgm_lidar_clustering](sgm_lidar_clustering/README.md) 91 | 92 | 93 | ## Contact us 94 | khalid.elmadawi@sigma.se 95 | -------------------------------------------------------------------------------- /feature_based_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(feature_based_mapping) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | tf 9 | message_filters 10 | laser_geometry 11 | pcl_conversions 12 | pcl_ros 13 | cv_bridge 14 | image_transport 15 | nav_msgs 16 | ) 17 | 18 | find_package(OpenCV REQUIRED 19 | NO_MODULE 20 | PATHS /usr/local 21 | NO_DEFAULT_PATH 22 | ) 23 | message("OpenCV version: ${OpenCV_VERSION}") 24 | message("OpenCV DIR: ${OpenCV_INCLUDE_DIRS}") 25 | message("OpenCV LIB DIR: ${OpenCV_LIB_DIR}") 26 | 27 | include_directories(${OpenCV_INCLUDE_DIRS}) 28 | link_directories(${OpenCV_LIB_DIR}) 29 | 30 | catkin_package( 31 | INCLUDE_DIRS include 32 | LIBRARIES feature_based_mapping 33 | CATKIN_DEPENDS roscpp rospy std_msgs 34 | ) 35 | 36 | include_directories( 37 | include/feature_based_mapping 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | 42 | add_executable(fbm_icp src/fbm_icp.cpp ) 43 | target_link_libraries(fbm_icp ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) -------------------------------------------------------------------------------- /feature_based_mapping/README.md: -------------------------------------------------------------------------------- 1 | # Feature Based Map 2 | feature based mapping is a new approach of 3D slam where it use only key points in surrounding environment to localize and construct a map of the surrounding environment, which make our constructed map 3 | 4 | 1) more accurate, immune from redundant data and noise from the surrounding environment. 5 | 2) faster and more computative efficient. 6 | 7 | ## Run 8 | launch the **sigmapilot_gazebo.launch** file then the **fbm_icp.launch** files in two different terminals. 9 | 10 | roslaunch gazebo_simulator sigmapilot_gazebo.launch 11 | roslaunch feature_based_mapping fbm_icp.launch 12 | ## Demo 13 | ### Loop closure 14 | in the below demo we present a loop closure of the FBM algorithm, where there is no deviation of the constructed map 15 | 16 | ![loop_closure](docs/fbm_loopclosure.gif) 17 | 18 | ### Elivated Environments 19 | in the below demo we present the algorithm estimates in elevated environments, such as elevated caves 20 | 21 | ![elevated_cave1](docs/fbm_cave.gif) 22 | 23 | ![elevated_cave2](docs/fbm_cave_2.gif) 24 | 25 | in the below demo we present the qualitative validation of the accumlative map(Ground truth), and the generated FBM point cloud. 26 | 27 | ![elevated_cave_val](docs/gt_vs_map.png) 28 | 29 | ![elevated_cave_val2](docs/fbm_cave_3.gif) 30 | 31 | 32 | ## Module Features 33 | ### High periority features: 34 | ------------------------ 35 | - [x] Estimate XYZRPY localization. 36 | - [x] Loop closure 37 | - [x] construct scalable map. 38 | - [x] validate the output map with the assmebler map qualitatively. 39 | - [x] initial position.(load parameter from lunch file) 40 | - [x] fuse IMU with point cloud. 41 | - [x] test on cave environment.(elevated environment) 42 | - [x] cropbox filter. 43 | - [ ] filter the accumilated map with a threshold to find the dominant key points. 44 | - [ ] save map. 45 | - [ ] fix the z localization estimate. 46 | 47 | ### Low periority features: 48 | ------------------------ 49 | 1)[ ]Dense maps. 50 | 2)[ ]test on rpi. 51 | 52 | ## Explaination 53 | ### Motivation 54 | if we have a two scattered pointclouds in a shape of a triangle, and we want to match these two point clouds with each other, we might not get an accurate translation and rotation resulting a non accurate matching, one of the main reasons for that is redundancy of data results in false statistical representations, which requires huge computation to fix. 55 | 56 | ![motivation1](docs/motivation.png) 57 | 58 | the proposed solution is to extract the keypoints from each point cloud and match them together, by that the statistical error will be reduced to the minimum, resulting an outstanding results. 59 | 60 | ![motivation2](docs/motivation_keypoints.png) 61 | 62 | ### Algorithm Steps 63 | 1) ponint cloud encoding in spherical grid map. 64 | 2) feature extraction of the slopes diffrentiation. 65 | 3) encode the keypoints in XYZRGB 66 | 4) downsample the point cloud 67 | 5) use imu rotation to get a better rotation estimate. 68 | 6) translate and rotate the current point cloud with the previous location. 69 | 7) calculate the translation and rotaiton with the old map 70 | 8) update the localization of the vehicle. 71 | 9) combine the two pointclouds. 72 | 10) downsample the combined pointcloud again. 73 | 11) publish the constructed map. 74 | -------------------------------------------------------------------------------- /feature_based_mapping/docs/fbm_cave.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/fbm_cave.gif -------------------------------------------------------------------------------- /feature_based_mapping/docs/fbm_cave_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/fbm_cave_2.gif -------------------------------------------------------------------------------- /feature_based_mapping/docs/fbm_cave_3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/fbm_cave_3.gif -------------------------------------------------------------------------------- /feature_based_mapping/docs/fbm_loopclosure.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/fbm_loopclosure.gif -------------------------------------------------------------------------------- /feature_based_mapping/docs/gt_vs_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/gt_vs_map.png -------------------------------------------------------------------------------- /feature_based_mapping/docs/motivation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/motivation.png -------------------------------------------------------------------------------- /feature_based_mapping/docs/motivation_keypoints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/feature_based_mapping/docs/motivation_keypoints.png -------------------------------------------------------------------------------- /feature_based_mapping/include/feature_based_mapping/dataContainers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: feature_based_mapping_icp. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 10 April 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef DATA_CONTAINER_HH 24 | #define DATA_CONTAINER_HH 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | typedef pcl::PointCloud PointCloud; 34 | typedef pcl::PointCloud PointCloud_C; 35 | typedef pcl::PointCloud MapPointCloud; 36 | 37 | class configuration { 38 | public: 39 | double voxel_size; 40 | double min_distance_range; 41 | int freq_ratio; 42 | std::string odom_frame; 43 | double slope_threshold; 44 | double euclidean_fitness_epsilon; 45 | double transformation_epsilon; 46 | double maximum_iterations; 47 | double max_correspondence_distance; 48 | double rejection_threshold; 49 | double map_score_threshold; 50 | double crop_box_threshold; 51 | bool use_external_imu; 52 | }; 53 | class euler_rot { 54 | public: 55 | double roll, pitch, yaw; 56 | euler_rot() : roll{0}, pitch{0}, yaw{0} {} 57 | }; 58 | 59 | class odom { 60 | public: 61 | double x, y, z, roll, pitch, yaw; 62 | odom() { 63 | this->x = 0; 64 | this->y = 0; 65 | this->z = 0; 66 | } 67 | void update_xyz(double dx, double dy, double dz) { 68 | this->x += dx; 69 | this->y += dy; 70 | this->z += dz; 71 | } 72 | void print_state() { std::cout << "odom: " << x << ", " << y << ", " << z << std::endl << std::endl; } 73 | }; 74 | class SGM { 75 | public: 76 | cv::Mat x, y, z, r, g, b, d; 77 | int height, width; 78 | SGM(int height, int width) { 79 | this->x = cv::Mat(height, width, CV_32F); 80 | this->y = cv::Mat(height, width, CV_32F); 81 | this->z = cv::Mat(height, width, CV_32F); 82 | this->r = cv::Mat(height, width, CV_32F); 83 | this->g = cv::Mat(height, width, CV_32F); 84 | this->b = cv::Mat(height, width, CV_32F); 85 | this->d = cv::Mat(height, width, CV_32F); 86 | this->height = height; 87 | this->width = width; 88 | } 89 | void Set(const PointCloud::ConstPtr input) { 90 | for (int rowIdx = 0; rowIdx < this->height; rowIdx++) { 91 | for (int colIdx = 0; colIdx < this->width; colIdx++) { 92 | int pointIdx = colIdx + this->width * rowIdx; 93 | this->x.at(rowIdx, colIdx) = input->points[pointIdx].x; 94 | this->y.at(rowIdx, colIdx) = input->points[pointIdx].y; 95 | this->z.at(rowIdx, colIdx) = input->points[pointIdx].z; 96 | this->r.at(rowIdx, colIdx) = input->points[pointIdx].r; 97 | this->g.at(rowIdx, colIdx) = input->points[pointIdx].g; 98 | this->b.at(rowIdx, colIdx) = input->points[pointIdx].b; 99 | this->d.at(rowIdx, colIdx) = sqrt(input->points[pointIdx].x * input->points[pointIdx].x + 100 | input->points[pointIdx].y * input->points[pointIdx].y + 101 | input->points[pointIdx].z * input->points[pointIdx].z); 102 | } 103 | } 104 | } 105 | }; 106 | 107 | enum ErrorCode { Success, Failed, Unknown, BadArgument }; 108 | 109 | #endif -------------------------------------------------------------------------------- /feature_based_mapping/include/feature_based_mapping/fbm_icp.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: feature_based_mapping_icp. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 10 April 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include // std::mutex 47 | #include 48 | #include 49 | 50 | #include "dataContainers.hpp" 51 | #include "sensor_msgs/Imu.h" 52 | 53 | class fbm_icp { 54 | public: 55 | fbm_icp(); 56 | ~fbm_icp(); 57 | ros::Publisher pub; 58 | ros::Publisher stampedPosePub; 59 | ros::Publisher LaserOdometryPub; 60 | void cloud_cb(const PointCloud::ConstPtr& input); 61 | void imu_cb(const sensor_msgs::Imu::ConstPtr& msg); 62 | ErrorCode feature_extraction(SGM const& sgm, MapPointCloud& keypoints); 63 | void icp_mapping(std::pair const& key_points_pair); 64 | 65 | ros::NodeHandle nh; 66 | ros::Subscriber imu_sub; 67 | std::vector sub_vec; 68 | std::vector> keypoints_vec; 69 | std::vector dense_pc_vec; 70 | MapPointCloud map; 71 | PointCloud dense_map; 72 | configuration config; 73 | 74 | odom odom_; 75 | euler_rot rot_; 76 | std::mutex mtx_map_odom, mtx_keypoints, mtx_imu; 77 | }; -------------------------------------------------------------------------------- /feature_based_mapping/launch/fbm_icp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | $(arg LiDAR_topicname) 6 | $(arg fbm_initial_pose) 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /feature_based_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | feature_based_mapping 4 | 0.1.0 5 | The feature_based_mapping package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | tf 19 | tf2_ros 20 | cv_bridge 21 | image_transport 22 | nav_msgs 23 | 24 | roscpp 25 | rospy 26 | std_msgs 27 | tf 28 | tf2_ros 29 | cv_bridge 30 | image_transport 31 | nav_msgs 32 | 33 | message_filters 34 | laser_geometry 35 | pcl_conversions 36 | pcl_ros 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /feature_based_mapping/rviz/fbm_icp.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /Pose1 11 | - /Odometry1 12 | - /Odometry1/Shape1 13 | Splitter Ratio: 0.5 14 | Tree Height: 719 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: PointCloud2 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 1 60 | Autocompute Intensity Bounds: true 61 | Autocompute Value Bounds: 62 | Max Value: 2.2006113529205322 63 | Min Value: -0.5811686515808105 64 | Value: true 65 | Axis: Z 66 | Channel Name: label 67 | Class: rviz/PointCloud2 68 | Color: 255; 255; 255 69 | Color Transformer: Intensity 70 | Decay Time: 0 71 | Enabled: true 72 | Invert Rainbow: false 73 | Max Color: 255; 255; 255 74 | Min Color: 0; 0; 0 75 | Name: PointCloud2 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 5 80 | Size (m): 0.009999999776482582 81 | Style: Points 82 | Topic: /icp_map 83 | Unreliable: false 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz/PointCloud2 96 | Color: 239; 41; 41 97 | Color Transformer: "" 98 | Decay Time: 0 99 | Enabled: false 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Min Color: 0; 0; 0 103 | Name: PointCloud2 104 | Position Transformer: XYZ 105 | Queue Size: 10 106 | Selectable: true 107 | Size (Pixels): 3 108 | Size (m): 0.009999999776482582 109 | Style: Points 110 | Topic: /test 111 | Unreliable: false 112 | Use Fixed Frame: true 113 | Use rainbow: true 114 | Value: false 115 | - Alpha: 1 116 | Axes Length: 0.30000001192092896 117 | Axes Radius: 0.05000000074505806 118 | Class: rviz/Pose 119 | Color: 255; 25; 0 120 | Enabled: true 121 | Head Length: 0.30000001192092896 122 | Head Radius: 0.10000000149011612 123 | Name: Pose 124 | Queue Size: 10 125 | Shaft Length: 0.30000001192092896 126 | Shaft Radius: 0.05000000074505806 127 | Shape: Axes 128 | Topic: /map_pose 129 | Unreliable: false 130 | Value: true 131 | - Angle Tolerance: 0.10000000149011612 132 | Class: rviz/Odometry 133 | Covariance: 134 | Orientation: 135 | Alpha: 0.5 136 | Color: 255; 255; 127 137 | Color Style: Unique 138 | Frame: Local 139 | Offset: 1 140 | Scale: 1 141 | Value: true 142 | Position: 143 | Alpha: 0.30000001192092896 144 | Color: 204; 51; 204 145 | Scale: 1 146 | Value: true 147 | Value: true 148 | Enabled: true 149 | Keep: 1000 150 | Name: Odometry 151 | Position Tolerance: 0.10000000149011612 152 | Queue Size: 10 153 | Shape: 154 | Alpha: 1 155 | Axes Length: 1 156 | Axes Radius: 0.10000000149011612 157 | Color: 252; 233; 79 158 | Head Length: 0.10000000149011612 159 | Head Radius: 0.10000000149011612 160 | Shaft Length: 0.10000000149011612 161 | Shaft Radius: 0.05000000074505806 162 | Value: Arrow 163 | Topic: /map_pose_odom 164 | Unreliable: false 165 | Value: true 166 | Enabled: true 167 | Global Options: 168 | Background Color: 48; 48; 48 169 | Default Light: true 170 | Fixed Frame: odom 171 | Frame Rate: 30 172 | Name: root 173 | Tools: 174 | - Class: rviz/Interact 175 | Hide Inactive Objects: true 176 | - Class: rviz/MoveCamera 177 | - Class: rviz/Select 178 | - Class: rviz/FocusCamera 179 | - Class: rviz/Measure 180 | - Class: rviz/SetInitialPose 181 | Theta std deviation: 0.2617993950843811 182 | Topic: /initialpose 183 | X std deviation: 0.5 184 | Y std deviation: 0.5 185 | - Class: rviz/SetGoal 186 | Topic: /move_base_simple/goal 187 | - Class: rviz/PublishPoint 188 | Single click: true 189 | Topic: /clicked_point 190 | Value: true 191 | Views: 192 | Current: 193 | Class: rviz/Orbit 194 | Distance: 34.77634048461914 195 | Enable Stereo Rendering: 196 | Stereo Eye Separation: 0.05999999865889549 197 | Stereo Focal Distance: 1 198 | Swap Stereo Eyes: false 199 | Value: false 200 | Field of View: 0.7853981852531433 201 | Focal Point: 202 | X: 5.160213947296143 203 | Y: 0.768585741519928 204 | Z: 2.974490165710449 205 | Focal Shape Fixed Size: true 206 | Focal Shape Size: 0.05000000074505806 207 | Invert Z Axis: false 208 | Name: Current View 209 | Near Clip Distance: 0.009999999776482582 210 | Pitch: 1.5697963237762451 211 | Target Frame: 212 | Yaw: 2.655378818511963 213 | Saved: ~ 214 | Window Geometry: 215 | Displays: 216 | collapsed: false 217 | Height: 1016 218 | Hide Left Dock: false 219 | Hide Right Dock: true 220 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002400000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 221 | Selection: 222 | collapsed: false 223 | Time: 224 | collapsed: false 225 | Tool Properties: 226 | collapsed: false 227 | Views: 228 | collapsed: true 229 | Width: 924 230 | X: 72 231 | Y: 27 232 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(gazebo_ros_lidar_plugin) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | gazebo_ros 12 | gazebo_plugins 13 | gazebo_msgs 14 | gazebo_dev 15 | geometry_msgs 16 | sensor_msgs 17 | trajectory_msgs 18 | nav_msgs 19 | urdf 20 | pcl_ros 21 | cv_bridge 22 | image_transport 23 | ) 24 | 25 | include (FindPkgConfig) 26 | if (PKG_CONFIG_FOUND) 27 | pkg_check_modules(XML libxml-2.0) 28 | pkg_check_modules(OGRE OGRE) 29 | pkg_check_modules(OGRE-Terrain OGRE-Terrain) 30 | pkg_check_modules(OGRE-Paging OGRE-Paging) 31 | else() 32 | message(FATAL_ERROR "pkg-config is required; please install it") 33 | endif() 34 | 35 | ## System dependencies are found with CMake's conventions 36 | find_package(Boost REQUIRED COMPONENTS thread) 37 | find_package(gazebo REQUIRED) 38 | find_package( PCL REQUIRED ) 39 | 40 | execute_process(COMMAND 41 | pkg-config --variable=plugindir OGRE 42 | OUTPUT_VARIABLE OGRE_PLUGIN_PATH 43 | OUTPUT_STRIP_TRAILING_WHITESPACE 44 | ) 45 | 46 | catkin_python_setup() 47 | 48 | catkin_package( 49 | INCLUDE_DIRS include 50 | 51 | CATKIN_DEPENDS 52 | gazebo_plugins 53 | gazebo_ros 54 | roscpp 55 | std_msgs 56 | urdf 57 | gazebo_msgs 58 | cv_bridge 59 | image_transport 60 | 61 | DEPENDS 62 | gazebo SDF 63 | ) 64 | 65 | ########### 66 | ## Build ## 67 | ########### 68 | 69 | ## Specify additional locations of header files 70 | ## Your package locations should be listed before other locations 71 | include_directories(include 72 | ${Boost_INCLUDE_DIRS} 73 | ${catkin_INCLUDE_DIRS} 74 | ${GAZEBO_INCLUDE_DIRS} 75 | ${OGRE_INCLUDE_DIRS} 76 | ${OGRE-Terrain_INCLUDE_DIRS} 77 | ${OGRE-Paging_INCLUDE_DIRS} 78 | ${PCL_INCLUDE_DIRS} 79 | ${Eigen3_INCLUDE_DIRS} 80 | include/gazebo_ros_lidar_plugin 81 | ) 82 | 83 | link_directories( 84 | ${GAZEBO_LIBRARY_DIRS} 85 | ${OGRE_LIBRARY_DIRS} 86 | ${OGRE-Terrain_LIBRARY_DIRS} 87 | ${OGRE-Paging_LIBRARY_DIRS} 88 | ${catkin_LIBRARY_DIRS} 89 | ${PCL_LIBRARY_DIRS} 90 | ${Eigen3_LIBRARY_DIRS} 91 | include/gazebo_ros_lidar_plugin 92 | 93 | ) 94 | 95 | add_library(gazebo_ros_lidar_plugin src/gazebo_ros_lidar_plugin.cpp src/custom_lidar.cpp) 96 | target_link_libraries(gazebo_ros_lidar_plugin RayPlugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 97 | 98 | 99 | 100 | ############# 101 | ## Install ## 102 | ############# 103 | 104 | # all install targets should use catkin DESTINATION variables 105 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 106 | 107 | ## Mark executable scripts (Python etc.) for installation 108 | ## in contrast to setup.py, you can choose the destination 109 | # catkin_install_python(PROGRAMS 110 | # scripts/my_python_script 111 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 112 | # ) 113 | 114 | ## Mark executables for installation 115 | ## Mark cpp header files for installation 116 | install(DIRECTORY include/ 117 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 118 | FILES_MATCHING PATTERN "*.h" 119 | ) 120 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/README.md: -------------------------------------------------------------------------------- 1 | ## gazebo_ros_quadcopter_lidar_plugin 2 | --- 3 | This repo is a custome plugin to simulate light weight LiDARs for Quadcopter mapping and object detection for different sensors, with different mounting positions, to see the best configuration for your solution/system. 4 | # Features 5 | - [x] Publish Pointcloud msg. 6 | - [x] Fuse Camera with LiDAR point cloud. 7 | - [x] Translate and rotate the fused point cloud in case SDF is used. 8 | - [x] Can use multiple sensors in the same robot. 9 | - [x] tf publish. 10 | - [ ] range lockup table processing. 11 | 12 | **TODO:** 13 | - Max-Min range lock-up table processing. 14 | 15 | # Build 16 | catkin build gazebo_ros_lidar_plugin 17 | 18 | # SDF configuration: 19 | you can find how to configure your LiDAR in the SDF file in **SDF_config** folder, where you can find **benwakeCE30** and **velodyne 16**, and you can extend your specs based on these two examples. 20 | 21 | ![Velodyne_PLugin_demo](SDF_config_example/images/velodyne_plugin_demo.gif) 22 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/benwake_CE30D.md: -------------------------------------------------------------------------------- 1 | ## Benwake CE30 SDF Configuration 2 | --- 3 | **Sensor Specs** 4 | - Method: Time of flight 5 | - Peak Wave Length: 850nm 6 | - FoV: 60*4 degrees 7 | - Pixel Resolution: 320*20 8 | - Angular Resolution (vertical): 0.2 degree 9 | - Frame Rate: 30fps 10 | - Ranging Resolution: 1cm 11 | - Detecting Range: 0.4~30m@90% reflectivity 12 | - Repeatability (1σ): ≤5cm 13 | - Accuracy: ≤15cm 14 | - Ambient Light Resistance: 60klux 15 | - Data Interface: UDP 16 | - Operating Temperature: 0-50 17 | - Supply Voltage: DC 12V ± 0.5V（≥3A） 18 | - Power Consumption: ≤8W 19 | - Enclosure Rating: IP65 20 | - Weight: 334g 21 | --- 22 | **SDF Sensor configuration** 23 | 24 | we will configure a camera and a lidar sensor sensors in the lidar link 25 | 26 | **Camera Sensor Configuration** 27 | 28 | - camera horizontal FOV same horizontal FOV of the LiDAR sensor. 29 | - camera image width is the same number of points per layer in LiDAR sensor. 30 | - camera has same x-pixel size, and y-pixel size, where the size of the pixel = (horizontal FOV/image width). 31 | - camera image height is set by (LidAR vertical FOV/pixel size) + some safety pixels. 32 | - camera rate and range are the same LiDAR rate and range. 33 | - camera frame name is the same LiDAR frame name. 34 | - make it easy don't add distortion. 35 | 36 | 37 | 38 | 0.2 0 0.3 1.57 0 0 39 | 40 | 1.0472 41 | 42 | 320 43 | 20 44 | 45 | 46 | 0.05 47 | 100 48 | 49 | 50 | 1 51 | 30 52 | true 53 | 54 | 57 | 58 | true 59 | 0.0 60 | webcam 61 | QuadCopterLiDAR_image_raw 62 | camera_info 63 | QuadCopterLiDAR_link 64 | 0.07 65 | 0.0 66 | 0.0 67 | 0.0 68 | 0.0 69 | 0.0 70 | 71 | 72 | 73 | **Benwake CE30 Sensor Configuration** 74 | 75 | - Simulate full horizontal FOV. 76 | - Each layer has 320 points. 77 | - sensor has 20 vertical layers. 78 | - sensor range is 30m. 79 | - If you are representing model in SDF then set the < pose > variable, else if xacro file then the tf msg will be published. 80 | - Set < cam_fov > as the same fov set in camera. 81 | - Camera fov might be slightly more than the LiDAR fov as a safety factor. 82 | - Set < sensorId >. 83 | 84 | 85 | 86 | 0.2 0 0.3 1.57 0 0 87 | true 88 | 30 89 | 90 | 91 | 92 | 320 93 | 1 94 | -0.523599 95 | 0.523599 96 | 97 | 98 | 20 99 | 1 100 | -0.0349066 101 | 0.0349066 102 | 103 | 104 | 105 | 0.4 106 | 30 107 | 0.1 108 | 109 | 114 | 115 | 116 | 117 | /spur/laser/scan 118 | /QuadCopterLiDAR_link 119 | 1 120 | 0.2 0 0.3 1.57 0 0 121 | 1.0472 122 | /webcam/QuadCopterLiDAR_image_raw 123 | 124 | 125 | 126 | Images below shows Benwake perception as the Quadcopter hover around a fire truck. 127 | ![BenwakeCE30](images/benwake_rviz_gazebo.png) 128 | 129 | ![BenwakeCE30](images/benwakeCE30_rviz.gif) 130 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/images/benwakeCE30_rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_ros_lidar_plugin/SDF_config_example/images/benwakeCE30_rviz.gif -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/images/benwake_rviz_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_ros_lidar_plugin/SDF_config_example/images/benwake_rviz_gazebo.png -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne16_rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne16_rviz.gif -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne_16_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne_16_gazebo.png -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne_plugin_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_ros_lidar_plugin/SDF_config_example/images/velodyne_plugin_demo.gif -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/SDF_config_example/velodyne16.md: -------------------------------------------------------------------------------- 1 | ## Velodyne 16 lite pack SDF Configuration 2 | --- 3 | **Sensor Specs** 4 | - Time of flight distance measurement with calibrated reflectivities 5 | - 16 channels 6 | - Measurement range up to 100 meters 7 | - Accuracy: +/- 3 cm (typical) 8 | - Dual returns 9 | - Field of view (vertical): 30° (+15° to -15°) 10 | - Angular resolution (vertical): 2° 11 | - Field of view (horizontal/azimuth): 360° 12 | - Angular resolution (horizontal/azimuth): 0.1° - 0.4° 13 | - Rotation rate: 5 - 20 Hz 14 | - Integrated web server for easy monitoring and configuration 15 | --- 16 | **SDF Sensor configuration** 17 | 18 | we will configure a camera and a lidar sensor sensors in the lidar link 19 | 20 | **Camera Sensor Configuration** 21 | 22 | - camera horizontal FOV same horizontal FOV of the LiDAR sensor. 23 | - camera image width is the same number of points per layer in LiDAR sensor. 24 | - camera has same x-pixel size, and y-pixel size, where the size of the pixel = (horizontal FOV/image width). 25 | - camera image height is set by (LidAR vertical FOV/pixel size) + some safety pixels. 26 | - camera rate and range are the same LiDAR rate and range. 27 | - camera frame name is the same LiDAR frame name. 28 | - make it easy don't add distortion. 29 | 30 | 31 | 32 | 33 | 34 | 0.2 0 0.3 1.57 0 0 35 | 36 | 1.59 37 | 38 | 512 39 | 200 40 | 41 | 42 | 0.05 43 | 100 44 | 45 | 46 | 1 47 | 10 48 | false 49 | 50 | 53 | 54 | true 55 | 0.0 56 | webcam 57 | QuadCopterLiDAR_image_raw 58 | camera_info 59 | QuadCopterLiDAR_link 60 | 0.07 61 | 0.0 62 | 0.0 63 | 0.0 64 | 0.0 65 | 0.0 66 | 67 | 68 | 69 | 70 | **Velodyne 16 Sensor Configuration** 71 | 72 | - Simulate 90 degree of Veldoyne horizontal FOV to reduce simulation processing. 73 | - each layer has 2048 points, 90 degrees contains 512. 74 | - sensor has 16 vertical layers. 75 | - sensor range is 100m. 76 | - If you are representing model in SDF then set the < pose > variable, else if xacro file then the tf msg will be published. 77 | - Set < cam_fov > as the same fov set in camera. 78 | - Camera fov might be slightly more than the LiDAR fov as a safety factor. 79 | - Set < sensorId >. 80 | 81 | 82 | 83 | 0.2 0 0.3 1.57 0 0 84 | true 85 | 10 86 | 87 | 88 | 89 | 512 90 | 1 91 | -0.785398 92 | 0.785398 93 | 94 | 95 | 16 96 | 1 97 | -0.261799 98 | 0.261799 99 | 100 | 101 | 102 | 0.1 103 | 100 104 | 0.1 105 | 106 | 111 | 112 | 113 | 114 | /spur/laser/scan 115 | /QuadCopterLiDAR_link 116 | 1 117 | 0.2 0 0.3 1.57 0 0 118 | 1.59 119 | /webcam/QuadCopterLiDAR_image_raw 120 | 121 | 122 | 123 | Images below shows Benwake perception as the Quadcopter hover around a fire truck. 124 | ![Velodyne16](images/velodyne_16_gazebo.png) 125 | 126 | ![Velodyne16](images/velodyne16_rviz.gif) 127 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/include/gazebo_ros_lidar_plugin/custom_lidar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: LiDAR sensor ROS Gazebo plugin. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 5 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef CUSTOM_LIDAR_HH 24 | #define CUSTOM_LIDAR_HH 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "dataContainers.h" 34 | 35 | class CustomLidar { 36 | public: 37 | /// \brief Constructor 38 | CustomLidar(); 39 | 40 | /// \brief Destructor 41 | ~CustomLidar(); 42 | 43 | /// \brief initialize from first scan 44 | void init_lidar_params(); 45 | 46 | /// \brief Lidar parameters data container 47 | LidarParameters LP; 48 | 49 | /// \brief create a frame 50 | ErrorCode construct_lidar_frame(std::vector const& ranges, cv::Mat const& cam_feed_, PointCloud& pcl_msg); 51 | 52 | /// \brief color a point cloud 53 | ErrorCode color_point(cv::Mat const& cam_feed, pcl::PointXYZ const& pt, pcl::PointXYZRGB& colored_pt); 54 | 55 | /// \brief mounting position point compensation 56 | ErrorCode mount_position_compensation(pcl::PointXYZ const& point, pcl::PointXYZ& calibrated_pt); 57 | 58 | private: 59 | ErrorCode arange(double const& minVal, double const& maxVal, double const& resolution, std::vector& vec); 60 | }; 61 | #endif -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/include/gazebo_ros_lidar_plugin/dataContainers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: LiDAR sensor ROS Gazebo plugin. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 5 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef DATA_CONTAINER_HH 24 | #define DATA_CONTAINER_HH 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | typedef pcl::PointCloud PointCloud; 34 | 35 | class mountingPositions { 36 | public: 37 | float x; 38 | float y; 39 | float z; 40 | float roll; 41 | float pitch; 42 | float yaw; 43 | void Set(std::string sensor_pose) { 44 | std::stringstream ss(sensor_pose); 45 | ss >> this->x; 46 | ss >> this->y; 47 | ss >> this->z; 48 | ss >> this->roll; 49 | ss >> this->pitch; 50 | ss >> this->yaw; 51 | }; 52 | }; 53 | 54 | class LidarParameters { 55 | public: 56 | float min_vertical_angle; 57 | float max_vertical_angle; 58 | float vertical_step_angle; 59 | 60 | float min_horizontal_angle; 61 | float max_horizontal_angle; 62 | float horizontal_step_angle; 63 | 64 | float horizontal_points_count; 65 | float vertical_points_count; 66 | 67 | bool parameters_initialized; 68 | 69 | float cam_fov; 70 | bool enable_fusion; 71 | 72 | mountingPositions mp; 73 | 74 | std::vector azimuthVec, elevationVec; 75 | }; 76 | 77 | enum ErrorCode { Success, Failed, Unknown, BadArgument }; 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/include/gazebo_ros_lidar_plugin/gazebo_ros_lidar_plugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: LiDAR sensor ROS Gazebo plugin. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 5 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef GAZEBO_ROS_LIDAR_HH 24 | #define GAZEBO_ROS_LIDAR_HH 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include // std::mutex 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "custom_lidar.h" 53 | 54 | namespace gazebo { 55 | 56 | class GazeboRosLiDAR : public RayPlugin { 57 | /// \brief Constructor 58 | public: 59 | GazeboRosLiDAR(); 60 | 61 | /// \brief Destructor 62 | public: 63 | virtual ~GazeboRosLiDAR(); 64 | 65 | /// \brief Load the plugin 66 | /// \param take in SDF root element 67 | public: 68 | void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 69 | 70 | /// \brief Keep track of number of connctions 71 | private: 72 | int laser_connect_count_; 73 | 74 | private: 75 | void LaserConnect(); 76 | 77 | private: 78 | void LaserDisconnect(); 79 | 80 | // Pointer to the model 81 | GazeboRosPtr gazebo_ros_; 82 | 83 | private: 84 | std::string world_name_; 85 | 86 | private: 87 | physics::WorldPtr world_; 88 | /// \brief The parent sensor 89 | private: 90 | sensors::RaySensorPtr parent_ray_sensor_; 91 | 92 | /// \brief pointer to ros node 93 | private: 94 | ros::NodeHandle *rosnode_; 95 | 96 | private: 97 | ros::Publisher pcl_pub_; 98 | 99 | private: 100 | PubQueue::Ptr pcl_pub_queue_; 101 | 102 | /// \brief topic name 103 | private: 104 | std::string topic_name_; 105 | 106 | /// \brief frame transform name, should match link name 107 | private: 108 | std::string frame_name_; 109 | 110 | /// \brief tf prefix 111 | private: 112 | std::string tf_prefix_; 113 | 114 | /// \brief for setting ROS name space 115 | private: 116 | std::string robot_namespace_; 117 | 118 | // deferred load in case ros is blocking 119 | private: 120 | sdf::ElementPtr sdf; 121 | 122 | private: 123 | void LoadThread(); 124 | 125 | private: 126 | void imageCb(const sensor_msgs::ImageConstPtr &msg); 127 | 128 | private: 129 | boost::thread deferred_load_thread_; 130 | 131 | private: 132 | gazebo::transport::NodePtr gazebo_node_; 133 | 134 | private: 135 | gazebo::transport::SubscriberPtr laser_scan_sub_; 136 | 137 | private: 138 | void OnScan(ConstLaserScanStampedPtr &_msg); 139 | 140 | private: 141 | PubMultiQueue pmq; 142 | std::string sensor_id; 143 | 144 | image_transport::ImageTransport *it_; 145 | image_transport::Subscriber image_sub_; 146 | cv::Mat cam_feed; 147 | 148 | CustomLidar custom_lidar_; 149 | 150 | std::mutex mtx; 151 | std::string cam_topic_name; 152 | }; 153 | 154 | } // namespace gazebo 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_ros_lidar_plugin 4 | 0.1.0 5 | The gazebo_ros_lidar_plugin package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | roscpp 19 | rospy 20 | std_msgs 21 | 22 | 23 | libgazebo11-dev 24 | gazebo_msgs 25 | geometry_msgs 26 | sensor_msgs 27 | trajectory_msgs 28 | std_srvs 29 | tf 30 | tf2_ros 31 | cv_bridge 32 | gazebo_ros 33 | gazebo_plugins 34 | rosconsole 35 | nodelet 36 | urdf 37 | image_transport 38 | 39 | libgazebo11-dev 40 | gazebo_ros 41 | gazebo_msgs 42 | sensor_msgs 43 | std_srvs 44 | tf 45 | tf2_ros 46 | rosconsole 47 | cv_bridge 48 | nodelet 49 | gazebo_plugins 50 | urdf 51 | image_transport 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | d['packages'] = ['gazebo_plugins'] 7 | d['package_dir'] = {'':'src'} 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /gazebo_ros_lidar_plugin/src/custom_lidar.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: LiDAR sensor ROS Gazebo plugin. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 5 September 2020 6 | * 7 | * Copyright 2020 SigmaEmbeddedEngineering. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "custom_lidar.h" 24 | 25 | CustomLidar::CustomLidar() { this->LP.parameters_initialized = false; } 26 | CustomLidar::~CustomLidar() {} 27 | void CustomLidar::init_lidar_params() { 28 | this->LP.parameters_initialized = true; 29 | (void)arange(this->LP.min_horizontal_angle, this->LP.max_horizontal_angle + this->LP.horizontal_step_angle, 30 | this->LP.horizontal_step_angle, this->LP.azimuthVec); 31 | (void)arange(this->LP.min_vertical_angle, this->LP.max_vertical_angle + this->LP.vertical_step_angle, 32 | this->LP.vertical_step_angle, this->LP.elevationVec); 33 | } 34 | 35 | ErrorCode CustomLidar::construct_lidar_frame(std::vector const& ranges, cv::Mat const& cam_feed_, 36 | PointCloud& pcl_msg) { 37 | if (ranges.empty()) { 38 | return ErrorCode::BadArgument; 39 | } 40 | for (int pointIdx = 0; pointIdx < ranges.size(); pointIdx++) { 41 | int aximuthIdx = pointIdx % static_cast(this->LP.horizontal_points_count); 42 | int elevationIdx = pointIdx / static_cast(this->LP.horizontal_points_count); 43 | float r{0}; 44 | if (std::numeric_limits::infinity() == ranges[pointIdx]) 45 | r = 0; 46 | else 47 | r = ranges[pointIdx]; 48 | 49 | // Polar to cartisian coordinates transformation. 50 | pcl::PointXYZ pt; 51 | pt.x = r * cos(this->LP.azimuthVec[aximuthIdx]) * cos(this->LP.elevationVec[elevationIdx]); 52 | pt.y = r * sin(this->LP.azimuthVec[aximuthIdx]) * cos(this->LP.elevationVec[elevationIdx]); 53 | pt.z = r * sin(this->LP.elevationVec[elevationIdx]); 54 | pcl::PointXYZRGB colored_pt; 55 | (void)color_point(cam_feed_, pt, colored_pt); 56 | pcl::PointXYZ calibrated_pt; 57 | (void)mount_position_compensation(pt, calibrated_pt); 58 | colored_pt.x = calibrated_pt.x; 59 | colored_pt.y = calibrated_pt.y; 60 | colored_pt.z = calibrated_pt.z; 61 | pcl_msg.points.push_back(colored_pt); 62 | } 63 | return ErrorCode::Success; 64 | } 65 | 66 | ErrorCode CustomLidar::color_point(cv::Mat const& cam_feed, pcl::PointXYZ const& pt, pcl::PointXYZRGB& colored_pt) { 67 | std::uint8_t R{0}, G{0}, B{0}; 68 | if (this->LP.enable_fusion) { 69 | // Fuse camera with lidar point cloud. 70 | float focalLength = (cam_feed.cols / 2) / tan(this->LP.cam_fov / 2); 71 | float cx = cam_feed.cols / 2; 72 | float cy = cam_feed.rows / 2; 73 | 74 | // Camera x-axis is LiDAR -1*y-axis, camera y-axis is LiDAR -1*z-axis, and 75 | // camera z-axis(deoth) is LiDAR x_axis. 76 | float X_cam = -pt.y; 77 | float Y_cam = -pt.z; 78 | float Z_cam = pt.x; 79 | X_cam = static_cast(cx + X_cam * focalLength / Z_cam); 80 | Y_cam = static_cast(cy + Y_cam * focalLength / Z_cam); 81 | if (Y_cam < cy * 2 - 1 && Y_cam > 0 && X_cam < cx * 2 - 1 && X_cam > 0 && Z_cam > 0) { 82 | B = cam_feed.at(Y_cam, X_cam)[0]; 83 | G = cam_feed.at(Y_cam, X_cam)[1]; 84 | R = cam_feed.at(Y_cam, X_cam)[2]; 85 | } 86 | colored_pt = pcl::PointXYZRGB(R, G, B); 87 | return ErrorCode::Success; 88 | } else { 89 | colored_pt = pcl::PointXYZRGB(R, G, B); 90 | return ErrorCode::Failed; 91 | } 92 | } 93 | ErrorCode CustomLidar::mount_position_compensation(pcl::PointXYZ const& point, pcl::PointXYZ& calibrated_pt) { 94 | calibrated_pt.x = point.x * cos(this->LP.mp.yaw) * cos(this->LP.mp.pitch) + 95 | point.y * (cos(this->LP.mp.yaw) * sin(this->LP.mp.pitch) * sin(this->LP.mp.roll) - 96 | sin(this->LP.mp.yaw) * cos(this->LP.mp.roll)) + 97 | point.z * (cos(this->LP.mp.yaw) * sin(this->LP.mp.pitch) * cos(this->LP.mp.roll) + 98 | sin(this->LP.mp.yaw) * sin(this->LP.mp.roll)) + 99 | this->LP.mp.x; 100 | calibrated_pt.y = point.x * sin(this->LP.mp.yaw) * cos(this->LP.mp.pitch) + 101 | point.y * (sin(this->LP.mp.yaw) * sin(this->LP.mp.pitch) * sin(this->LP.mp.roll) + 102 | cos(this->LP.mp.yaw) * cos(this->LP.mp.roll)) + 103 | point.z * (sin(this->LP.mp.yaw) * sin(this->LP.mp.pitch) * cos(this->LP.mp.roll) - 104 | cos(this->LP.mp.yaw) * sin(this->LP.mp.roll)) + 105 | this->LP.mp.y; 106 | calibrated_pt.z = point.x * (-sin(this->LP.mp.pitch)) + point.y * cos(this->LP.mp.pitch) * sin(this->LP.mp.roll) + 107 | point.z * cos(this->LP.mp.pitch) * cos(this->LP.mp.roll) + this->LP.mp.z; 108 | return ErrorCode::Success; 109 | } 110 | ErrorCode CustomLidar::arange(double const& minVal, double const& maxVal, double const& resolution, 111 | std::vector& vec) { 112 | // validate input arguments. 113 | if (!vec.empty()) { 114 | return ErrorCode::BadArgument; 115 | } 116 | // We get a vector varying from min val to max val with a step resolution. 117 | double minimum = std::min(minVal, maxVal); 118 | double maximum = std::max(minVal, maxVal); 119 | for (double i = minimum; i < maximum; i += resolution) { 120 | vec.push_back(i); 121 | } 122 | return ErrorCode::Success; 123 | } 124 | -------------------------------------------------------------------------------- /gazebo_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(gazebo_simulator) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES gazebo_simulator 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/gazebo_simulator.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/gazebo_simulator.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_simulator.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /gazebo_simulator/config/benwake_costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 3.0 2 | raytrace_range: 3.5 3 | 4 | footprint: [[-0.276, -0.23], [-0.276, 0.23], [0.276, 0.23], [0.276, -0.23]] 5 | #footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] 6 | #robot_radius: 0.17 7 | 8 | inflation_radius: 1.0 9 | cost_scaling_factor: 3.0 10 | 11 | map_type: costmap 12 | 13 | # The "observation_sources" parameter defines a list of sensors that are going to be passing information to 14 | # the costmap separated by spaces. Each sensor is defined in the next lines. 15 | observation_sources: scan 16 | # Definition of the "scan" observation source. Make sure to set the sensor_frame to the same 17 | # name of the laser frame link that is defined in the robot_description. Otherwise the local costamp won't work. 18 | #scan: {sensor_frame: coloredLidar, data_type: LaserScan, topic: clusteredPointCLoud_laser_scaner, marking: true, clearing: true} 19 | scan: {sensor_frame: fused_coloredLidar, data_type: LaserScan, topic: fused_laser_scan, marking: true, clearing: true} 20 | -------------------------------------------------------------------------------- /gazebo_simulator/config/costmap_global_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: robot_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: true -------------------------------------------------------------------------------- /gazebo_simulator/config/costmap_local_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: robot_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 -------------------------------------------------------------------------------- /gazebo_simulator/config/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | base_local_planner: dwa_local_planner/DWAPlannerROS 2 | DWAPlannerROS: 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.22 5 | min_vel_x: -0.22 6 | 7 | max_vel_y: 0.0 8 | min_vel_y: 0.0 9 | 10 | # The velocity when robot is moving in a straight line 11 | max_vel_trans: 0.22 12 | min_vel_trans: 0.11 13 | 14 | max_vel_theta: 2.75 15 | min_vel_theta: 1.37 16 | 17 | acc_lim_x: 2.5 18 | acc_lim_y: 0.0 19 | acc_lim_theta: 3.2 20 | 21 | # Goal Tolerance Parametes 22 | xy_goal_tolerance: 0.1 23 | yaw_goal_tolerance: 0.5 24 | latch_xy_goal_tolerance: false 25 | 26 | # Forward Simulation Parameters 27 | sim_time: 1.5 28 | vx_samples: 20 29 | vy_samples: 0 30 | vth_samples: 40 31 | controller_frequency: 10.0 32 | 33 | # Trajectory Scoring Parameters 34 | path_distance_bias: 32.0 35 | goal_distance_bias: 20.0 36 | occdist_scale: 0.02 37 | forward_point_distance: 0.325 38 | stop_time_buffer: 0.2 39 | scaling_speed: 0.25 40 | max_scaling_factor: 0.2 41 | 42 | # Oscillation Prevention Parameters 43 | oscillation_reset_dist: 0.05 44 | 45 | # Debugging 46 | publish_traj_pc : true 47 | publish_cost_grid_pc: true 48 | -------------------------------------------------------------------------------- /gazebo_simulator/config/gmapping_params.yaml: -------------------------------------------------------------------------------- 1 | map_update_interval: 2.0 2 | maxUrange: 30.0 3 | sigma: 0.05 4 | kernelSize: 1 5 | lstep: 0.05 6 | astep: 0.05 7 | iterations: 5 8 | lsigma: 0.075 9 | ogain: 3.0 10 | lskip: 0 11 | minimumScore: 50 12 | srr: 0.1 13 | srt: 0.2 14 | str: 0.1 15 | stt: 0.2 16 | linearUpdate: 1.0 17 | angularUpdate: 0.2 18 | temporalUpdate: 0.5 19 | resampleThreshold: 0.5 20 | particles: 100 21 | xmin: -15.0 22 | ymin: -15.0 23 | xmax: 15.0 24 | ymax: 15.0 25 | delta: 0.05 26 | llsamplerange: 0.01 27 | llsamplestep: 0.01 28 | lasamplerange: 0.005 29 | lasamplestep: 0.005 30 | -------------------------------------------------------------------------------- /gazebo_simulator/config/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | controller_frequency: 10.0 3 | planner_patience: 5.0 4 | controller_patience: 15.0 5 | conservative_reset_dist: 3.0 6 | planner_frequency: 5.0 7 | oscillation_timeout: 10.0 8 | oscillation_distance: 0.2 -------------------------------------------------------------------------------- /gazebo_simulator/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | skid_steer_bot: 2 | # Settings for ros_control hardware interface 3 | hardware_interface: 4 | joints: 5 | - left_back_wheel_hinge 6 | - right_back_wheel_hinge 7 | - left_front_wheel_hinge 8 | - right_front_wheel_hinge 9 | 10 | # Publish all joint states ----------------------------------- 11 | joint_state_controller: 12 | type: joint_state_controller/JointStateController 13 | publish_rate: 50 14 | 15 | 16 | mobile_base_controller: 17 | type: skid_steer_drive_controller/SkidSteerDriveController 18 | publish_rate: 50 19 | 20 | left_back_wheel: 'left_back_wheel_hinge' 21 | right_back_wheel: 'right_back_wheel_hinge' 22 | left_front_wheel: 'left_front_wheel_hinge' 23 | right_front_wheel: 'right_front_wheel_hinge' 24 | 25 | # Wheel separation and diameter. These are both optional. 26 | # skid_steer_drive_controller will attempt to read either one or both from the 27 | # URDF if not specified as a parameter 28 | wheel_separation : 0.46 29 | wheel_radius : 0.16 30 | 31 | # Odometry covariances for the encoder output of the robot. These values should 32 | # be tuned to your robot's sample odometry data, but these values are a good place 33 | # to start 34 | pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] 35 | twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] 36 | 37 | # Top level frame (link) of the robot description 38 | base_frame_id: robot_footprint 39 | 40 | # Velocity and acceleration limits for the robot 41 | linear: 42 | x: 43 | has_velocity_limits : true 44 | max_velocity : 0.2 # m/s 45 | has_acceleration_limits: true 46 | max_acceleration : 0.6 # m/s^2 47 | angular: 48 | z: 49 | has_velocity_limits : true 50 | max_velocity : 2.0 # rad/s 51 | has_acceleration_limits: true 52 | max_acceleration : 6.0 # rad/s^2 53 | -------------------------------------------------------------------------------- /gazebo_simulator/config/velodyne_costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 3.0 2 | raytrace_range: 3.5 3 | 4 | footprint: [[-0.276, -0.23], [-0.276, 0.23], [0.276, 0.23], [0.276, -0.23]] 5 | #footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] 6 | #robot_radius: 0.17 7 | 8 | inflation_radius: 1.0 9 | cost_scaling_factor: 3.0 10 | 11 | map_type: costmap 12 | 13 | # The "observation_sources" parameter defines a list of sensors that are going to be passing information to 14 | # the costmap separated by spaces. Each sensor is defined in the next lines. 15 | observation_sources: scan 16 | # Definition of the "scan" observation source. Make sure to set the sensor_frame to the same 17 | # name of the laser frame link that is defined in the robot_description. Otherwise the local costamp won't work. 18 | scan: {sensor_frame: coloredLidar, data_type: LaserScan, topic: clusteredPointCLoud_laser_scaner, marking: true, clearing: true} 19 | #scan: {sensor_frame: fused_coloredLidar, data_type: LaserScan, topic: fused_laser_scan, marking: true, clearing: true} 20 | -------------------------------------------------------------------------------- /gazebo_simulator/docs/benwake_farming_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/docs/benwake_farming_demo.gif -------------------------------------------------------------------------------- /gazebo_simulator/docs/slam_cave.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/docs/slam_cave.gif -------------------------------------------------------------------------------- /gazebo_simulator/docs/slam_farm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/docs/slam_farm.gif -------------------------------------------------------------------------------- /gazebo_simulator/docs/velodyne_cave.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/docs/velodyne_cave.gif -------------------------------------------------------------------------------- /gazebo_simulator/docs/velodyne_farming_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/docs/velodyne_farming_demo.gif -------------------------------------------------------------------------------- /gazebo_simulator/launch/control/robot_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/description/robot_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/navigation/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/navigation/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/navigation/robot_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/sigmapilot_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/sigmapilot_rpi_replay.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /gazebo_simulator/launch/slam/robot_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo_simulator/maps/iscas_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/maps/iscas_map.pgm -------------------------------------------------------------------------------- /gazebo_simulator/maps/iscas_map.yaml: -------------------------------------------------------------------------------- 1 | image: iscas_map.pgm 2 | resolution: 0.050000 3 | origin: [-15.000000, -15.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /gazebo_simulator/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/maps/map.pgm -------------------------------------------------------------------------------- /gazebo_simulator/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: ./map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /gazebo_simulator/maps/mymap_cave.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/gazebo_simulator/maps/mymap_cave.pgm -------------------------------------------------------------------------------- /gazebo_simulator/maps/mymap_cave.yaml: -------------------------------------------------------------------------------- 1 | image: mymap_cave.pgm 2 | resolution: 0.050000 3 | origin: [-15.000000, -15.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /gazebo_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_simulator 4 | 0.1.0 5 | The gazebo_simulator package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /gazebo_simulator/urdf/velodyne/skid_steer_bot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Gazebo/Green 5 | 6 | 7 | Gazebo/Black 8 | 9 | 10 | Gazebo/Blue 11 | 12 | 13 | Gazebo/Black 14 | 15 | 16 | Gazebo/Blue 17 | 18 | 19 | 20 | 21 | 100.0 22 | / 23 | left_front_wheel_hinge 24 | right_front_wheel_hinge 25 | left_back_wheel_hinge 26 | right_back_wheel_hinge 27 | 0.46 28 | 0.16 29 | robot_footprint 30 | 200 31 | 5.0 32 | cmd_vel 33 | odom 34 | odom 35 | true 36 | 0.001 37 | 0.001 38 | 0.01 39 | 40 | 41 | 42 | 43 | 44 | / 45 | 46 | 47 | 48 | 49 | Gazebo/Grey 50 | 51 | 30.0 52 | 53 | 1.3962634 54 | 55 | 800 56 | 800 57 | R8G8B8 58 | 59 | 60 | 0.02 61 | 300 62 | 63 | 64 | 65 | true 66 | 0.0 67 | skid_steer_bot/camera1 68 | image_raw 69 | camera_info 70 | camera 71 | 0.07 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.0 76 | 0.0 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0 0 0 0 0 0 85 | 10.0 86 | 87 | 1.59 88 | 89 | 512 90 | 200 91 | R8G8B8 92 | 93 | 94 | 0.02 95 | 30 96 | 97 | 98 | 99 | true 100 | 0.0 101 | skid_steer_bot/camera1 102 | image_raw 103 | camera_info 104 | coloredLidar 105 | 0.07 106 | 0.0 107 | 0.0 108 | 0.0 109 | 0.0 110 | 0.0 111 | 112 | 113 | 114 | 115 | 0 0 0 0 0 0 116 | false 117 | 10 118 | 119 | 120 | 121 | 2048 122 | 1 123 | -3.14159 124 | 3.14159 125 | 126 | 127 | 16 128 | 1 129 | -0.261799 130 | 0.261799 131 | 132 | 133 | 134 | 0.10 135 | 100.0 136 | 0.01 137 | 138 | 139 | gaussian 140 | 144 | 0.0 145 | 0.01 146 | 147 | 148 | 149 | /skid_steer_bot/laser/scan 150 | coloredLidar 151 | 1 152 | 0 0 0 0 0 0 153 | 1.59 154 | /skid_steer_bot/camera1/image_raw 155 | 156 | 157 | 158 | 159 | true 160 | 10 161 | true 162 | __default_topic__ 163 | 164 | imu 165 | coloredLidar 166 | 10.0 167 | 0.0 168 | 0 0 0 169 | 0 0 0 170 | coloredLidar 171 | false 172 | 173 | 0 0 0 0 0 0 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | -------------------------------------------------------------------------------- /gazebo_simulator/worlds/benchmarking.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 1000.0 15 | 0.001 16 | 1 17 | 18 | 19 | quick 20 | 150 21 | 0 22 | 1.400000 23 | 1 24 | 25 | 26 | 0.00001 27 | 0.2 28 | 2000.000000 29 | 0.01000 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | true 39 | model://mud_box 40 | 41 | 42 | 0 0 0 0 0 0 43 | 44 | grid 45 | 10 46 | 4 47 | 8 10 0 48 | 49 | 50 | 51 | 52 | 53 | 54 | true 55 | model://hatchback 56 | 57 | 58 | 0 -4.5 0 0 0 0 59 | 60 | grid 61 | 5 62 | 5 63 | 10 10 0 64 | 65 | 66 | 67 | 68 | 69 | 0.4 0.4 0.4 1 70 | 0.7 0.7 0.7 1 71 | true 72 | 73 | 74 | 75 | 76 | 0.8 0.0 12.0 0 1.5708 0 77 | orbit 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /gazebo_simulator/worlds/iscas_museum.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 1000.0 15 | 0.001 16 | 1 17 | 18 | 19 | quick 20 | 150 21 | 0 22 | 1.400000 23 | 1 24 | 25 | 26 | 0.00001 27 | 0.2 28 | 2000.000000 29 | 0.01000 30 | 31 | 32 | 33 | 34 | 35 | 36 | model://iscas_museum 37 | 2.5 2.5 0 0 -0 0 38 | 39 | 40 | 41 | 0.4 0.4 0.4 1 42 | 0.7 0.7 0.7 1 43 | true 44 | 45 | 46 | 47 | 48 | 0.8 0.0 12.0 0 1.5708 0 49 | orbit 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /gazebo_simulator/worlds/turtlebot3_world.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 1000.0 15 | 0.001 16 | 1 17 | 18 | 19 | quick 20 | 150 21 | 0 22 | 1.400000 23 | 1 24 | 25 | 26 | 0.00001 27 | 0.2 28 | 2000.000000 29 | 0.01000 30 | 31 | 32 | 33 | 34 | 35 | 36 | model://turtlebot3_world 37 | 38 | 39 | 40 | 0.4 0.4 0.4 1 41 | 0.7 0.7 0.7 1 42 | true 43 | 44 | 45 | 46 | 47 | 0.8 0.0 12.0 0 1.5708 0 48 | orbit 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /laser_scans_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(laser_scans_fusion) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | tf 9 | message_filters 10 | laser_geometry 11 | pcl_conversions 12 | pcl_ros 13 | tf2_geometry_msgs 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | LIBRARIES laser_scans_fusion 19 | CATKIN_DEPENDS roscpp rospy std_msgs message_filters laser_geometry pcl_conversions pcl_ros 20 | ) 21 | 22 | include_directories( 23 | include 24 | include/laser_scans_fusion 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | 29 | add_executable(laser_scans_fusion src/laser_scans_fusion.cpp src/scan_fusion.cpp) 30 | target_link_libraries(laser_scans_fusion ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /laser_scans_fusion/README.md: -------------------------------------------------------------------------------- 1 | # Laser Scan fusion 2 | 3 | Laser Scan fusion is a package that fuses Freespace detections in a cocoon sensor setup(multiple sensor covering different FOVs), to a **fused frame of reference** that is set in the launch file. 4 | 5 | [reference of the idea](https://github.com/ros-perception/slam_gmapping/issues/17#issuecomment-66199895) 6 | 7 | ![Freespace_fusion](https://cloud.githubusercontent.com/assets/3719094/5348469/e31fcbd8-7efd-11e4-8630-10fbe25fa361.jpeg) 8 | 9 | # Features: 10 | 11 | - [x] Fuses multiple sensors Covering multiple FOV and publish it to fused sensor_msgs::LaserScan. 12 | 13 | # Requirements: 14 | Required packages: 15 | 16 | * PCL 17 | * tf 18 | 19 | # Build: 20 | 21 | caktin build laser_scans_fusion 22 | 23 | # RUN: 24 | 25 | clone and build **gazebo_ros_package** and **Autobotware Gazebo Ros LiDAR Plugin**. 26 | 27 | roslaunch laser_scans_fusion multi_scan_fusion.launch 28 | 29 | -------------------------------------------------------------------------------- /laser_scans_fusion/include/laser_scans_fusion/dataContainers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Laser Scan fusion. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 30 march 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef DATA_CONTAINER_HH 24 | #define DATA_CONTAINER_HH 25 | #include 26 | 27 | class scan { 28 | public: 29 | float x; 30 | float y; 31 | float yaw; 32 | float depth; 33 | int pointIdx; 34 | }; 35 | class fusion_config { 36 | public: 37 | std::string fusion_frame; 38 | int number_fused_points; 39 | float range_min; 40 | float range_max; 41 | float dis_threshold; 42 | float tf_listener_rate; 43 | }; 44 | 45 | enum ErrorCode { Success, Failed, Unknown, BadArgument }; 46 | 47 | #endif -------------------------------------------------------------------------------- /laser_scans_fusion/include/laser_scans_fusion/laser_scans_fusion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Laser Scan fusion. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 30 march 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "dataContainers.hpp" 44 | #include "geometry_msgs/Twist.h" 45 | #include "ros/ros.h" 46 | #include "scan_fusion.hpp" 47 | #include "sensor_msgs/LaserScan.h" 48 | #include "std_msgs/String.h" 49 | 50 | using namespace message_filters; 51 | 52 | class Multi_Scan_Fusion { 53 | public: 54 | Multi_Scan_Fusion(); 55 | ~Multi_Scan_Fusion(); 56 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan1, const sensor_msgs::LaserScan::ConstPtr &scan2); 57 | 58 | private: 59 | ros::NodeHandle node_; 60 | laser_geometry::LaserProjection projector_; 61 | tf::TransformListener tfListener_; 62 | 63 | // ros::Publisher point_cloud_publisher_, point_cloud_publisher2_; 64 | ros::Publisher laserscan_publisher_; 65 | ros::Subscriber scan_sub_; 66 | 67 | message_filters::Subscriber scanSub1; 68 | message_filters::Subscriber scanSub2; 69 | 70 | typedef message_filters::sync_policies::ApproximateTime 71 | MySyncPolicy; 72 | typedef message_filters::Synchronizer Sync; 73 | boost::shared_ptr sync_; 74 | scan_fusion sf; 75 | fusion_config config; 76 | }; -------------------------------------------------------------------------------- /laser_scans_fusion/include/laser_scans_fusion/scan_fusion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Laser Scan fusion. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 30 march 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #ifndef Scan_FUSION_HH 24 | #define Scan_FUSION_HH 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "dataContainers.hpp" 35 | #include "ros/ros.h" 36 | #include "sensor_msgs/LaserScan.h" 37 | class scan_fusion { 38 | public: 39 | scan_fusion(); 40 | ~scan_fusion(); 41 | ErrorCode scans_accumulator(pcl::PointCloud::Ptr const& cloud, int const& cloud_offset, 42 | tf::StampedTransform const& transform, std::vector& fused_ls); 43 | ErrorCode fuse_scans(std::vector const& fused_ls, fusion_config const& config, 44 | sensor_msgs::LaserScan& fused_scans); 45 | }; 46 | #endif -------------------------------------------------------------------------------- /laser_scans_fusion/launch/multi_scan_fusion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /laser_scans_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_scans_fusion 4 | 0.1.0 5 | The laser_scans_fusion package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | tf 19 | tf2_ros 20 | tf2_geometry_msgs 21 | 22 | roscpp 23 | rospy 24 | std_msgs 25 | tf 26 | tf2_ros 27 | tf2_geometry_msgs 28 | 29 | message_filters 30 | laser_geometry 31 | pcl_conversions 32 | pcl_ros 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /laser_scans_fusion/src/laser_scans_fusion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Laser Scan fusion. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 30 march 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "laser_scans_fusion.hpp" 24 | 25 | Multi_Scan_Fusion::Multi_Scan_Fusion() { 26 | this->node_.getParam(ros::this_node::getName() + "/fusion_frame", this->config.fusion_frame); 27 | this->node_.getParam(ros::this_node::getName() + "/number_fused_points", this->config.number_fused_points); 28 | this->node_.getParam(ros::this_node::getName() + "/range_min", this->config.range_min); 29 | this->node_.getParam(ros::this_node::getName() + "/dis_threshold", this->config.dis_threshold); 30 | this->node_.getParam(ros::this_node::getName() + "/tf_listener_rate", this->config.tf_listener_rate); 31 | 32 | std::string input_topic1, input_topic2, fused_topic; 33 | this->node_.getParam(ros::this_node::getName() + "/input_topic1", input_topic1); 34 | this->node_.getParam(ros::this_node::getName() + "/input_topic2", input_topic2); 35 | this->node_.getParam(ros::this_node::getName() + "/fused_topic", fused_topic); 36 | 37 | tfListener_.setExtrapolationLimit(ros::Duration(this->config.tf_listener_rate)); 38 | 39 | scanSub1.subscribe(node_, input_topic1, 1); 40 | scanSub2.subscribe(node_, input_topic2, 1); 41 | 42 | sync_.reset(new Sync(MySyncPolicy(10), scanSub1, scanSub2)); 43 | sync_->registerCallback(boost::bind(&Multi_Scan_Fusion::scanCallback, this, _1, _2)); 44 | 45 | laserscan_publisher_ = node_.advertise(fused_topic, 1, false); 46 | } 47 | Multi_Scan_Fusion::~Multi_Scan_Fusion() {} 48 | 49 | void Multi_Scan_Fusion::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan1, 50 | const sensor_msgs::LaserScan::ConstPtr &scan2) { 51 | tf::StampedTransform transform, transform2; 52 | tfListener_.lookupTransform(this->config.fusion_frame, scan1->header.frame_id, ros::Time(0), transform); 53 | tfListener_.lookupTransform(this->config.fusion_frame, scan2->header.frame_id, ros::Time(0), transform2); 54 | 55 | sensor_msgs::PointCloud2 cloud, cloud2; 56 | projector_.transformLaserScanToPointCloud(this->config.fusion_frame, *scan1, cloud, tfListener_); 57 | projector_.transformLaserScanToPointCloud(this->config.fusion_frame, *scan2, cloud2, tfListener_); 58 | 59 | pcl::PCLPointCloud2 pcl_pc, pcl_pc2; 60 | pcl_conversions::toPCL(cloud, pcl_pc); 61 | pcl_conversions::toPCL(cloud2, pcl_pc2); 62 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 63 | pcl::PointCloud::Ptr temp_cloud2(new pcl::PointCloud); 64 | 65 | pcl::fromPCLPointCloud2(pcl_pc, *temp_cloud); 66 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud2); 67 | 68 | std::vector fused_ls; 69 | (void)sf.scans_accumulator(temp_cloud, 0, transform, fused_ls); 70 | (void)sf.scans_accumulator(temp_cloud2, fused_ls.size(), transform2, fused_ls); 71 | 72 | config.range_max = 73 | std::max(scan1->range_max, scan2->range_max) + 74 | std::max(std::sqrt(std::pow(transform.getOrigin().x(), 2) + std::pow(transform.getOrigin().y(), 2)), 75 | std::sqrt(std::pow(transform2.getOrigin().x(), 2) + std::pow(transform2.getOrigin().y(), 2))) + 76 | 0.1; 77 | sensor_msgs::LaserScan fused_scans; 78 | (void)sf.fuse_scans(fused_ls, config, fused_scans); 79 | laserscan_publisher_.publish(fused_scans); 80 | } 81 | 82 | int main(int argc, char **argv) { 83 | ros::init(argc, argv, "laser_scans_fusion"); 84 | 85 | std::cout << "initialization" << std::endl; 86 | 87 | Multi_Scan_Fusion filter; 88 | 89 | ros::spin(); 90 | 91 | return 0; 92 | } -------------------------------------------------------------------------------- /laser_scans_fusion/src/scan_fusion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Laser Scan fusion. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 30 march 2021 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "scan_fusion.hpp" 24 | scan_fusion::scan_fusion() { 25 | // 26 | } 27 | scan_fusion::~scan_fusion() {} 28 | 29 | ErrorCode scan_fusion::scans_accumulator(pcl::PointCloud::Ptr const& cloud, int const& offset, 30 | tf::StampedTransform const& transform, std::vector& fused_ls) { 31 | float origin_distance = 32 | std::sqrt(std::pow(transform.getOrigin().x(), 2) + std::pow(transform.getOrigin().y(), 2)) + 0.1; 33 | 34 | for (int i = 0; i < cloud->points.size(); i++) { 35 | int sign = ((0 < cloud->points[i].y) - ((cloud->points[i].y < 0))); 36 | if (cloud->points[i].y * sign > 0) { 37 | scan temp_scan; 38 | temp_scan.x = cloud->points[i].x; 39 | temp_scan.y = cloud->points[i].y; 40 | temp_scan.yaw = std::atan2(cloud->points[i].y, cloud->points[i].x); 41 | temp_scan.depth = std::sqrt(std::pow(cloud->points[i].y, 2) + std::pow(cloud->points[i].x, 2)); 42 | temp_scan.pointIdx = i + offset; 43 | if (temp_scan.pointIdx == 0) 44 | fused_ls.push_back(temp_scan); 45 | else if ((temp_scan.yaw > fused_ls[fused_ls.size() - 1].yaw) && temp_scan.depth > origin_distance) 46 | fused_ls.push_back(temp_scan); 47 | } 48 | } 49 | return ErrorCode::Success; 50 | } 51 | ErrorCode scan_fusion::fuse_scans(std::vector const& fused_ls, fusion_config const& config, 52 | sensor_msgs::LaserScan& fused_scans) { 53 | fused_scans.header.stamp = ros::Time::now(); 54 | fused_scans.header.frame_id = config.fusion_frame; 55 | fused_scans.angle_min = fused_ls[0].yaw; 56 | fused_scans.angle_max = fused_ls[fused_ls.size() - 1].yaw; 57 | fused_scans.angle_increment = (fused_scans.angle_max - fused_scans.angle_min) / config.number_fused_points; 58 | fused_scans.range_min = config.range_min; 59 | fused_scans.range_max = config.range_max; 60 | 61 | fused_scans.ranges.resize(config.number_fused_points); 62 | std::fill(fused_scans.ranges.begin(), fused_scans.ranges.end(), fused_scans.range_max); 63 | fused_scans.intensities.resize(config.number_fused_points); 64 | std::fill(fused_scans.intensities.begin(), fused_scans.intensities.end(), 0); 65 | 66 | int non_fused_scan_idx = 0; 67 | float theta_lower_range = fused_ls[non_fused_scan_idx].yaw; 68 | float theta_upper_range = fused_ls[non_fused_scan_idx + 1].yaw; 69 | float theta = fused_scans.angle_min; 70 | for (int i = 0; i < config.number_fused_points; i++) { 71 | bool exit_condition = false; 72 | while (theta_upper_range < fused_scans.angle_max && !exit_condition) { 73 | if (theta <= theta_upper_range && theta >= theta_lower_range) { 74 | if (((fused_ls[non_fused_scan_idx + 1].pointIdx - fused_ls[non_fused_scan_idx].pointIdx) > 1)) 75 | fused_scans.ranges[i] = fused_scans.range_max; 76 | else if ((abs(fused_ls[non_fused_scan_idx + 1].depth - fused_ls[non_fused_scan_idx].depth) > 77 | config.dis_threshold)) 78 | fused_scans.ranges[i] = fused_scans.range_max; 79 | else { 80 | fused_scans.ranges[i] = 81 | (theta - theta_lower_range) * 82 | (fused_ls[non_fused_scan_idx + 1].depth - fused_ls[non_fused_scan_idx].depth) / 83 | (theta_upper_range - theta_lower_range) + 84 | fused_ls[non_fused_scan_idx].depth; 85 | } 86 | 87 | theta = theta + fused_scans.angle_increment; 88 | exit_condition = true; 89 | } else { 90 | non_fused_scan_idx += 1; 91 | theta_lower_range = fused_ls[non_fused_scan_idx].yaw; 92 | theta_upper_range = 93 | fused_ls[std::min(non_fused_scan_idx + 1, static_cast(fused_ls.size() - 1))].yaw; 94 | } 95 | } 96 | } 97 | return ErrorCode::Success; 98 | } 99 | -------------------------------------------------------------------------------- /point_cloud_assembler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(point_cloud_assembler) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | tf 9 | message_filters 10 | laser_geometry 11 | pcl_conversions 12 | pcl_ros 13 | ) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES point_cloud_assembler 18 | CATKIN_DEPENDS roscpp rospy std_msgs message_filters laser_geometry pcl_conversions pcl_ros 19 | ) 20 | 21 | include_directories( 22 | include/point_cloud_assembler 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | 27 | add_executable(point_cloud_assembler src/point_cloud_assembler.cpp ) 28 | target_link_libraries(point_cloud_assembler ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /point_cloud_assembler/include/point_cloud_assembler/dataContainers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Pointcloud assembler. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 14 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | 25 | class configuration { 26 | public: 27 | double voxel_size; 28 | double tf_duration; 29 | std::string odom_frame; 30 | }; 31 | 32 | enum ErrorCode { Success, Failed, Unknown, BadArgument }; -------------------------------------------------------------------------------- /point_cloud_assembler/include/point_cloud_assembler/point_cloud_assembler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Pointcloud assembler. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 14 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "dataContainers.hpp" 40 | 41 | typedef pcl::PointCloud PointCloud; 42 | typedef pcl::PointCloud PointCloud_C; 43 | class pointCloudAssembler { 44 | public: 45 | pointCloudAssembler(); 46 | ~pointCloudAssembler(); 47 | ros::Publisher pub; 48 | void cloud_cb(const PointCloud::ConstPtr &input); 49 | 50 | ros::NodeHandle nh; 51 | std::vector sub_vec; 52 | tf::TransformListener tfListener_; 53 | pcl::PointCloud cloud_accumilated; 54 | configuration config; 55 | }; -------------------------------------------------------------------------------- /point_cloud_assembler/launch/assembler_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | $(arg LiDAR_topicname) 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /point_cloud_assembler/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | point_cloud_assembler 4 | 0.1.0 5 | The point_cloud_assembler package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | tf 19 | tf2_ros 20 | 21 | roscpp 22 | rospy 23 | std_msgs 24 | tf 25 | tf2_ros 26 | 27 | message_filters 28 | laser_geometry 29 | pcl_conversions 30 | pcl_ros 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /point_cloud_assembler/rviz/pointcloud_assembler.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | Splitter Ratio: 0.5 11 | Tree Height: 719 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: true 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: true 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: label 64 | Class: rviz/PointCloud2 65 | Color: 255; 255; 255 66 | Color Transformer: Intensity 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: PointCloud2 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 5 77 | Size (m): 0.009999999776482582 78 | Style: Points 79 | Topic: /assemblerMap 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 48; 48; 48 87 | Default Light: true 88 | Fixed Frame: odom 89 | Frame Rate: 30 90 | Name: root 91 | Tools: 92 | - Class: rviz/Interact 93 | Hide Inactive Objects: true 94 | - Class: rviz/MoveCamera 95 | - Class: rviz/Select 96 | - Class: rviz/FocusCamera 97 | - Class: rviz/Measure 98 | - Class: rviz/SetInitialPose 99 | Theta std deviation: 0.2617993950843811 100 | Topic: /initialpose 101 | X std deviation: 0.5 102 | Y std deviation: 0.5 103 | - Class: rviz/SetGoal 104 | Topic: /move_base_simple/goal 105 | - Class: rviz/PublishPoint 106 | Single click: true 107 | Topic: /clicked_point 108 | Value: true 109 | Views: 110 | Current: 111 | Class: rviz/Orbit 112 | Distance: 11.693883895874023 113 | Enable Stereo Rendering: 114 | Stereo Eye Separation: 0.05999999865889549 115 | Stereo Focal Distance: 1 116 | Swap Stereo Eyes: false 117 | Value: false 118 | Field of View: 0.7853981852531433 119 | Focal Point: 120 | X: 1.9684412479400635 121 | Y: -0.08030711859464645 122 | Z: 0.001964612863957882 123 | Focal Shape Fixed Size: true 124 | Focal Shape Size: 0.05000000074505806 125 | Invert Z Axis: false 126 | Name: Current View 127 | Near Clip Distance: 0.009999999776482582 128 | Pitch: 1.5697963237762451 129 | Target Frame: 130 | Yaw: 3.1753921508789062 131 | Saved: ~ 132 | Window Geometry: 133 | Displays: 134 | collapsed: true 135 | Height: 1016 136 | Hide Left Dock: true 137 | Hide Right Dock: true 138 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 139 | Selection: 140 | collapsed: false 141 | Time: 142 | collapsed: false 143 | Tool Properties: 144 | collapsed: false 145 | Views: 146 | collapsed: true 147 | Width: 924 148 | X: 72 149 | Y: 27 150 | -------------------------------------------------------------------------------- /point_cloud_assembler/src/point_cloud_assembler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: Pointcloud assembler. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 14 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "point_cloud_assembler.hpp" 24 | 25 | pointCloudAssembler::pointCloudAssembler() { 26 | std::cout << "initialize" << std::endl; 27 | 28 | std::string pub_topicname = "assemblerMap"; 29 | this->nh.getParam(ros::this_node::getName() + "/pub_topicname", pub_topicname); 30 | this->nh.getParam(ros::this_node::getName() + "/voxel_size", this->config.voxel_size); 31 | this->nh.getParam(ros::this_node::getName() + "/tf_duration", this->config.tf_duration); 32 | this->nh.getParam(ros::this_node::getName() + "/odom_frame", this->config.odom_frame); 33 | 34 | tfListener_.setExtrapolationLimit(ros::Duration(this->config.tf_duration)); 35 | 36 | this->pub = this->nh.advertise(pub_topicname, 1); 37 | 38 | this->cloud_accumilated.width = 0; 39 | this->cloud_accumilated.height = 0; 40 | this->cloud_accumilated.points.clear(); 41 | XmlRpc::XmlRpcValue v; 42 | nh.param("/LiDAR_topicname", v, v); 43 | for (int i = 0; i < v.size(); i++) { 44 | sub_vec.push_back(this->nh.subscribe(v[i], 1, &pointCloudAssembler::cloud_cb, this)); 45 | std::cout << "topic_names: " << v[i] << std::endl; 46 | } 47 | ros::Duration(this->config.tf_duration * 2).sleep(); 48 | } 49 | pointCloudAssembler::~pointCloudAssembler() {} 50 | 51 | void pointCloudAssembler::cloud_cb(const PointCloud::ConstPtr &input) { 52 | // down sample the point cloud 53 | PointCloud::Ptr downsampledPC(new pcl::PointCloud); 54 | pcl::VoxelGrid sor; 55 | sor.setInputCloud(input); 56 | sor.setLeafSize(this->config.voxel_size, this->config.voxel_size, this->config.voxel_size); 57 | sor.filter(*downsampledPC); 58 | 59 | pcl::PointCloud cloud_in = *downsampledPC; 60 | pcl::PointCloud cloud_trans; 61 | tf::StampedTransform transform, transform2; 62 | 63 | tfListener_.lookupTransform(this->config.odom_frame, input->header.frame_id, ros::Time(0), transform); 64 | 65 | // translate and rotate the downsampled point cloud. 66 | pcl_ros::transformPointCloud(cloud_in, cloud_trans, transform); 67 | // assemble point cloud. 68 | this->cloud_accumilated.width = this->cloud_accumilated.width + cloud_trans.width; 69 | this->cloud_accumilated.height = 1; 70 | this->cloud_accumilated.points.insert(this->cloud_accumilated.points.end(), cloud_trans.points.begin(), 71 | cloud_trans.points.end()); 72 | 73 | std::cout << "PointCloud after filtering: " << downsampledPC->width << " " << downsampledPC->height 74 | << " before filtering:" << input->width * input->height 75 | << " accumilative filtering:" << this->cloud_accumilated.width * this->cloud_accumilated.height 76 | << std::endl; 77 | 78 | // down sample the assembled point cloud. 79 | PointCloud::Ptr accumilatedPC(new pcl::PointCloud); 80 | PointCloud::Ptr downsampledAccumilatedPC(new pcl::PointCloud); 81 | pcl::VoxelGrid sor_accumilated; 82 | *accumilatedPC = this->cloud_accumilated; 83 | sor_accumilated.setInputCloud(accumilatedPC); 84 | sor_accumilated.setLeafSize(this->config.voxel_size, this->config.voxel_size, this->config.voxel_size); 85 | sor_accumilated.filter(*downsampledAccumilatedPC); 86 | this->cloud_accumilated = *downsampledAccumilatedPC; 87 | this->cloud_accumilated.header = input->header; 88 | this->cloud_accumilated.header.frame_id = this->config.odom_frame; 89 | // publish point cloud. 90 | this->pub.publish(this->cloud_accumilated); 91 | } 92 | 93 | int main(int argc, char **argv) { 94 | // Initialize ROS 95 | ros::init(argc, argv, "point_cloud_assembler"); 96 | 97 | // Create a pointcloud assembler object. 98 | pointCloudAssembler assembler; 99 | 100 | // Spin 101 | ros::spin(); 102 | } 103 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sgm_lidar_clustering) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | add_definitions(-Wall -g) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | trajectory_msgs 18 | nav_msgs 19 | pcl_ros 20 | cv_bridge 21 | image_transport 22 | visualization_msgs 23 | ) 24 | 25 | find_package(OpenCV REQUIRED 26 | #NO_MODULE 27 | #PATHS /usr/local 28 | #NO_DEFAULT_PATH 29 | ) 30 | message("OpenCV version: ${OpenCV_VERSION}") 31 | message("OpenCV DIR: ${OpenCV_INCLUDE_DIRS}") 32 | message("OpenCV LIB DIR: ${OpenCV_LIB_DIR}") 33 | 34 | include_directories(${OpenCV_INCLUDE_DIRS}) 35 | link_directories(${OpenCV_LIB_DIR}) 36 | 37 | 38 | ################################### 39 | ## catkin specific configuration ## 40 | ################################### 41 | ## The catkin_package macro generates cmake config files for your package 42 | ## Declare things to be passed to dependent projects 43 | ## INCLUDE_DIRS: uncomment this if your package contains header files 44 | ## LIBRARIES: libraries you create in this project that dependent projects also need 45 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 46 | ## DEPENDS: system dependencies of this project that dependent projects also need 47 | catkin_package( 48 | INCLUDE_DIRS include 49 | LIBRARIES sgm_lidar_clustering 50 | CATKIN_DEPENDS roscpp rospy std_msgs 51 | DEPENDS system_lib 52 | ) 53 | 54 | include_directories( 55 | include 56 | include/sgm_lidar_clustering 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | 60 | add_library(sgm_clustering_lib 61 | src/ground_segmentation.cpp 62 | src/sgm_segmentation.cpp 63 | ) 64 | target_link_libraries(sgm_clustering_lib ${catkin_LIBRARIES}) 65 | 66 | add_executable(sgm_lidar_clustering nodes/sgm_lidar_clustering.cpp) 67 | target_link_libraries(sgm_lidar_clustering ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} sgm_clustering_lib) 68 | 69 | install(TARGETS sgm_clustering_lib 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | ) 74 | 75 | ## Mark cpp header files for installation 76 | install(DIRECTORY include/${PROJECT_NAME}/ 77 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | FILES_MATCHING PATTERN "*.h" 79 | PATTERN ".svn" EXCLUDE 80 | ) 81 | 82 | if(CATKIN_ENABLE_TESTING) 83 | find_package(rostest REQUIRED) 84 | add_rostest_gtest(sgm-test launch/gtest.test tests/sgm_unittests.cpp) 85 | target_link_libraries(sgm-test ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} sgm_clustering_lib) 86 | endif() -------------------------------------------------------------------------------- /sgm_lidar_clustering/README.md: -------------------------------------------------------------------------------- 1 | # sgm_lidar_clustering 2 | LiDAR clustering algorithm based on the assumption of LiDAR as a sphirical, designed to subscribe a pcl::PointXYZRGB point cloud,and publish a clustered pcl::PointXYZRGBL point cloud. 3 | 4 | SGM LiDAR clustering act only on sorted ordered set of points, and doesn't act on unsorted points, or downsampled point clouds. 5 | 6 | # Features: 7 | 8 | - [x] Semantic Clustered Point Cloud Encoded in pcl::PointXYZRGBL. 9 | - [x] FreeSpace Encoded in sensor_msgs::LaserScan. 10 | - [x] Clustered Objects are filtered based on size constrains. 11 | - [x] Objectlevel detections Encoded in visualization_msgs::MarkerArray Message. 12 | 13 | # Requirements: 14 | Required packages: 15 | 16 | * OpenCV 17 | * PCL 18 | 19 | # Build: 20 | 21 | caktin build sgm_lidar_lidar_clustering 22 | 23 | # RUN: 24 | 25 | clone and build **gazebo_ros_package** and **Sigmapilot Gazebo Ros LiDAR Plugin**. 26 | 27 | 28 | to run the Clustering Node for Velodyne(Single Sensor): 29 | 30 | roslaunch sgm_lidar_lidar_clustering Cluster_node.launch 31 | or for multi benwake sensor: 32 | 33 | roslaunch sgm_lidar_lidar_clustering multi_node_benwake.launch 34 | 35 | 36 | # Benwake LiDAR clustering Demo: 37 | 38 | image below represent benwake LiDAR clustering 39 | 40 | ![BenwakeCE30_rviz](docs/benwake_clustering_rviz.gif) 41 | 42 | and the below image shows how it looks in simulation 43 | 44 | ![BenwakeCE30_gazebo](docs/benwake_clustering_gazebo.gif) 45 | 46 | # Velodyne16 LiDAR clustering Demo: 47 | 48 | image below represent Veldoyne16 LiDAR clustering 49 | 50 | ![Velodyne16_rviz](docs/velodyne16_clustering_rviz.gif) 51 | 52 | and the below image shows how it looks in simulation 53 | 54 | ![Velodyne16_gazebo](docs/velodyne16_clustering_gazebo.gif) 55 | 56 | # Benchmarking on Raspberry Pi: 57 | 58 | you can download ros_bags from: 59 | 60 | wget https://www.dropbox.com/s/yzl5eceb7j4qxlq/benwake_farm.bag 61 | 62 | or 63 | 64 | wget https://www.dropbox.com/s/srb028hw6i85zwl/velodyne_farm.bag 65 | 66 | 67 | 68 | rpi setup: 69 | 70 | * ubuntu 20 71 | * ros noetic 72 | 73 | clone and build Sigmapilot, and run the launch files: 74 | 75 | roslaunch gazebo_simulator Sigmapilot_rpi_replay.launch rosbag_path:='path_to_bag_files' sensor:= 76 | 77 | raspberry pi for velodyne16 78 | setup: 79 | * rosbag publish tf, tf_static, velodyne point cloud sensor. 80 | * roscore master runs on raspberry pi. 81 | * velodyne sensor point cloud is 16 layer, each layer has 2048 points, 32768 scan points in total. 82 | * velodyne input frequency is 10hz, output clustered point cloud is 10 hz. 83 | * clustering algorithm runs in 20 to 23 hz( 2x to 2.3x required speed). 84 | * rviz just for visualizing what is happening on rpi, rviz runs on pc. 85 | 86 | ![VelodyneFreq](docs/rpi_velodyne_demo.gif) 87 | 88 | 89 | raspberry pi for two benwake sensor CE30C 90 | setup: 91 | * rosbag publish tf, tf_static, left and right benwake point cloud sensors. 92 | * roscore master runs on raspberry pi. 93 | * each benwake sensor is 24 layer, each layer has 320 points, 7680 scan points in total. 94 | * benwake input frequency is 30 hz, output clustered point cloud is 30 hz. 95 | * clustering algorithm runs in 70 to 100 hz( 2.3x to 3.3x required speed). 96 | * rviz just for visualizing what is happening on rpi, rviz runs on pc. 97 | 98 | ![BenwakeFreq](docs/rpi_benwake_demo.gif) 99 | 100 | # Benchmarking: 101 | Benchmarking between SGM clustering and lidar_euclidean_cluster_detect in autoware. 102 | 103 | ## Simulation test rig setup: 104 | test rig is an array of cars setup in a 5x5 grid each grid with a distance of 10 meters between each cone, and the robot is in the middle of it 105 | as shown in the below figure. 106 | 107 | ![simulationsetup](docs/simulation_setup.jpeg) 108 | 109 | 110 | ## Sensor model: 111 | we used two sensors first is 16 layer velodyne sensor, second is 64 layer velodyne sensor, to measure the change of point cloud density on the detection algorithms. 112 | 113 | ## Sigmapilot setup: 114 | clone the repo and “catkin build” 115 | will set two sets of configurations: 116 | 117 | * “use_morphological_filter” = false. 118 | * “ground_segmentation_threshold” = 0.1 119 | 120 | 121 | ## Autoware setup: 122 | Follow the setup guide and build the lidar_euclidean_cluster_detect package. 123 | will set two sets of configurations: 124 | 125 | * “remove_ground” = true. 126 | * “keep_lanes” = false. 127 | * “clip_min_height” = -5. 128 | * “clip_max_height” = 5. 129 | * “output_frame” = 3. 130 | * “downsample_cloud” = true. 131 | 132 | 133 | 134 | ## KPIs: 135 | the KPI that we think fit the most is range of detection, as both are detection algorithms, so simply how far can each algorithm can detect, and how the frequency will be. 136 | 137 | 138 | 139 | ## Experiment1: 140 | 141 | * velodyne16-SGM algorithm with ground segmentation threshold of 0.1 142 | 143 | ### result: 144 | 145 | SGM effective detection range around 48m, detection frequency beween 143 and 160 hz 146 | 147 | ![sgm_16](docs/sgm_16_v2_performance.png) 148 | ![sgm_16range](docs/sgm_16_v2_range.png) 149 | 150 | * velodyne16-euclidean algorithm with downsample = true 151 | 152 | ### result: 153 | effective detection range 25.5m, detection frequency between 74 and 90 hz 154 | 155 | ![euclidean_16](docs/euclidean_clustering_16_performance.png) 156 | ![euclidean_16range](docs/euclidean_clustering_16_range.jpeg) 157 | 158 | 159 | ## Experiment2: 160 | 161 | * velodyne64-SGM algorithm with ground segmentation threshold of 0.1 162 | 163 | ### result: 164 | effective detection range around 99.3m(max range), detection frequency beween 35 and 38 hz 165 | 166 | ![sgm_64](docs/sgm_64_v2_performance.png) 167 | ![sgm_64range](docs/sgm_v2_64_range.png) 168 | 169 | 170 | * velodyne64-euclidean algorithm with downsample = true 171 | ### result: 172 | effective detection range around 63.2m, detection frequency beween 12 and 15 hz 173 | 174 | ![euclidean_64](docs/euclidean_clustering_64_performance.png) 175 | ![euclidean_64range](docs/euclidean_clustering_64_range.jpeg) 176 | 177 | 178 | # Conclusion: 179 | 180 | * SGM V2 clustering ~2x longer range than euclidean clustering in velodyne 16 and ~1.57x longer detection range in velodyne 64. 181 | * SGM V2 better in performance than euclidean clustering in velodyne16 by around 66%~84%, and in velodyne64 by around 150%~190%. 182 | * SGM is losless, doesnot lose pointcloud textures or downsample it, euclidean clustering requires downsampling to actually work, which may result to a deformation or object splitting into multiple objects. 183 | * SGM provides freespace euclidean clustering doesn’t, euclidean clustering provides pose estimation, SGM doesn’t. -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/benwake_clustering_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/benwake_clustering_gazebo.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/benwake_clustering_rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/benwake_clustering_rviz.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/euclidean_clustering_16_performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/euclidean_clustering_16_performance.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/euclidean_clustering_16_range.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/euclidean_clustering_16_range.jpeg -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/euclidean_clustering_64_performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/euclidean_clustering_64_performance.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/euclidean_clustering_64_range.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/euclidean_clustering_64_range.jpeg -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/rpi_benwake_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/rpi_benwake_demo.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/rpi_velodyne_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/rpi_velodyne_demo.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/sgm_16_v2_performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/sgm_16_v2_performance.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/sgm_16_v2_range.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/sgm_16_v2_range.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/sgm_64_v2_performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/sgm_64_v2_performance.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/sgm_v2_64_range.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/sgm_v2_64_range.png -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/simulation_setup.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/simulation_setup.jpeg -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/velodyne16_clustering_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/velodyne16_clustering_gazebo.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/docs/velodyne16_clustering_rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SigmaEmbeddedEngineering-eg/Sigmapilot/be1fbf0e27117b16a8fcaf50844a4a39ca89a05d/sgm_lidar_clustering/docs/velodyne16_clustering_rviz.gif -------------------------------------------------------------------------------- /sgm_lidar_clustering/include/sgm_lidar_clustering/dataContainers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #ifndef DATA_CONTAINER_HH 34 | #define DATA_CONTAINER_HH 35 | typedef pcl::PointCloud PointCloud; 36 | typedef pcl::PointCloud PointCloud_C; 37 | 38 | class SGM { 39 | public: 40 | cv::Mat x, y, z, r, g, b, d; 41 | int height, width; 42 | SGM(int height, int width) { 43 | this->x = cv::Mat(height, width, CV_32F); 44 | this->y = cv::Mat(height, width, CV_32F); 45 | this->z = cv::Mat(height, width, CV_32F); 46 | this->r = cv::Mat(height, width, CV_32F); 47 | this->g = cv::Mat(height, width, CV_32F); 48 | this->b = cv::Mat(height, width, CV_32F); 49 | this->d = cv::Mat(height, width, CV_32F); 50 | this->height = height; 51 | this->width = width; 52 | } 53 | void Set(const PointCloud::ConstPtr input) { 54 | for (int rowIdx = 0; rowIdx < this->height; rowIdx++) { 55 | for (int colIdx = 0; colIdx < this->width; colIdx++) { 56 | int pointIdx = colIdx + this->width * rowIdx; 57 | this->x.at(rowIdx, colIdx) = input->points[pointIdx].x; 58 | this->y.at(rowIdx, colIdx) = input->points[pointIdx].y; 59 | this->z.at(rowIdx, colIdx) = input->points[pointIdx].z; 60 | this->r.at(rowIdx, colIdx) = input->points[pointIdx].r; 61 | this->g.at(rowIdx, colIdx) = input->points[pointIdx].g; 62 | this->b.at(rowIdx, colIdx) = input->points[pointIdx].b; 63 | this->d.at(rowIdx, colIdx) = sqrt(input->points[pointIdx].x * input->points[pointIdx].x + 64 | input->points[pointIdx].y * input->points[pointIdx].y + 65 | input->points[pointIdx].z * input->points[pointIdx].z); 66 | } 67 | } 68 | } 69 | }; 70 | 71 | class configuration { 72 | public: 73 | std::string LiDAR_topicname; 74 | std::string pub_topicname; 75 | std::string pub_Objtopicname; 76 | bool obj_level_filter_flag; 77 | double ground_segmentation_threshold; 78 | double distance_threshold; 79 | double distance_threshold_type; 80 | double obj_level_x_min; 81 | double obj_level_y_min; 82 | double obj_level_z_min; 83 | double obj_level_x_max; 84 | double obj_level_y_max; 85 | double obj_level_z_max; 86 | double obj_level_vol_threshold; 87 | double min_horizontal_angle; 88 | double max_horizontal_angle; 89 | double lidar_max_range; 90 | bool use_morphological_filter; 91 | }; 92 | 93 | class object { 94 | public: 95 | double pos_x; 96 | double pos_y; 97 | double pos_z; 98 | double size_x; 99 | double size_y; 100 | double size_z; 101 | double min_x; 102 | double min_y; 103 | double min_z; 104 | double max_x; 105 | double max_y; 106 | double max_z; 107 | }; 108 | 109 | class clustered_objects { 110 | public: 111 | cv::Mat labels; 112 | std::vector objects; 113 | std::vector> cluster_indices; 114 | }; 115 | 116 | enum ErrorCode { Success, Failed, Unknown, BadArgument }; 117 | 118 | #endif -------------------------------------------------------------------------------- /sgm_lidar_clustering/include/sgm_lidar_clustering/ground_segmentation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "dataContainers.hpp" 31 | #ifndef GROUND_SEGMENTATION_HH 32 | #define GROUND_SEGMENTATION_HH 33 | 34 | class GroundSegmentation { 35 | public: 36 | GroundSegmentation(configuration config); 37 | ~GroundSegmentation(); 38 | ErrorCode spherical_grid_map_slopes(SGM const& sgm, cv::Mat& slope); 39 | ErrorCode ground_segmentation(SGM const& sgm, cv::Mat& ground_label); 40 | ErrorCode freespace(cv::Mat const& ground_label, SGM const& sgm, std::vector& freespace_); 41 | 42 | private: 43 | configuration config_; 44 | }; 45 | #endif -------------------------------------------------------------------------------- /sgm_lidar_clustering/include/sgm_lidar_clustering/sgm_lidar_clustering.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include "dataContainers.hpp" 40 | #include "ground_segmentation.hpp" 41 | #include "sgm_segmentation.hpp" 42 | class SGMClustering { 43 | public: 44 | SGMClustering(); 45 | ~SGMClustering(); 46 | ros::Publisher pub; 47 | ros::Publisher laser_scan_pub; 48 | ros::Publisher marker_pub; 49 | 50 | void cloud_cb(const PointCloud::ConstPtr& input); 51 | 52 | ros::NodeHandle nh; 53 | ros::Subscriber sub; 54 | configuration config; 55 | std::unique_ptr gs_ptr; 56 | std::unique_ptr sgm_ss_ptr; 57 | 58 | }; -------------------------------------------------------------------------------- /sgm_lidar_clustering/include/sgm_lidar_clustering/sgm_segmentation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "dataContainers.hpp" 30 | #ifndef SGM_SEGMENTATION_HH 31 | #define SGM_SEGMENTATION_HH 32 | class SGMSegmentation { 33 | public: 34 | SGMSegmentation(configuration config); 35 | ~SGMSegmentation(); 36 | ErrorCode semantic_segmentation(SGM const& sgm, cv::Mat const& ground_label, 37 | clustered_objects& ss_clustered_objects); 38 | ErrorCode create_object(double const& x, double const& y, double const& z, object& obj); 39 | ErrorCode update_object(object& obj, double const& x, double const& y, double const& z); 40 | ErrorCode clear_object(object& obj); 41 | 42 | private: 43 | configuration config_; 44 | }; 45 | #endif -------------------------------------------------------------------------------- /sgm_lidar_clustering/launch/Cluster_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/launch/gtest.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/launch/multi_node_benwake.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/nodes/sgm_lidar_clustering.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "sgm_lidar_clustering.hpp" 24 | 25 | SGMClustering::SGMClustering() { 26 | std::cout << "initialize" << std::endl; 27 | this->nh.getParam(ros::this_node::getName() + "/LiDAR_topicname", this->config.LiDAR_topicname); 28 | this->nh.getParam(ros::this_node::getName() + "/pub_pc_topicname", this->config.pub_topicname); 29 | this->nh.getParam(ros::this_node::getName() + "/pub_ma_topicname", this->config.pub_Objtopicname); 30 | 31 | this->nh.getParam(ros::this_node::getName() + "/ground_segmentation_threshold", 32 | this->config.ground_segmentation_threshold); 33 | 34 | this->nh.getParam(ros::this_node::getName() + "/distance_threshold", this->config.distance_threshold); 35 | this->nh.getParam(ros::this_node::getName() + "/distance_threshold_type", this->config.distance_threshold_type); 36 | 37 | this->nh.getParam(ros::this_node::getName() + "/obj_level_filter_flag", this->config.obj_level_filter_flag); 38 | this->nh.getParam(ros::this_node::getName() + "/obj_level_x_min", this->config.obj_level_x_min); 39 | this->nh.getParam(ros::this_node::getName() + "/obj_level_y_min", this->config.obj_level_y_min); 40 | this->nh.getParam(ros::this_node::getName() + "/obj_level_z_min", this->config.obj_level_z_min); 41 | this->nh.getParam(ros::this_node::getName() + "/obj_level_x_max", this->config.obj_level_x_max); 42 | this->nh.getParam(ros::this_node::getName() + "/obj_level_y_max", this->config.obj_level_y_max); 43 | this->nh.getParam(ros::this_node::getName() + "/obj_level_z_max", this->config.obj_level_z_max); 44 | this->nh.getParam(ros::this_node::getName() + "/obj_level_vol_threshold", this->config.obj_level_vol_threshold); 45 | this->nh.getParam(ros::this_node::getName() + "/min_horizontal_angle", this->config.min_horizontal_angle); 46 | this->nh.getParam(ros::this_node::getName() + "/max_horizontal_angle", this->config.max_horizontal_angle); 47 | this->nh.getParam(ros::this_node::getName() + "/lidar_max_range", this->config.lidar_max_range); 48 | this->nh.getParam(ros::this_node::getName() + "/use_morphological_filter", this->config.use_morphological_filter); 49 | 50 | std::cout << "obj_level_filter_flag: " << this->config.obj_level_filter_flag << std::endl; 51 | std::cout << "gnd_seg_threshold: " << this->config.ground_segmentation_threshold << std::endl; 52 | std::cout << "distance_threshold: " << this->config.distance_threshold << std::endl; 53 | std::cout << "point cloud sub topic: " << this->config.LiDAR_topicname << std::endl; 54 | std::cout << "point cloud pub topic: " << this->config.pub_topicname << std::endl; 55 | std::cout << "object pub topic: " << this->config.pub_Objtopicname << std::endl; 56 | 57 | this->sub = this->nh.subscribe(this->config.LiDAR_topicname, 1, &SGMClustering::cloud_cb, this); 58 | this->pub = this->nh.advertise(this->config.pub_topicname, 1); 59 | this->laser_scan_pub = this->nh.advertise(this->config.pub_topicname + "_laser_scaner", 1); 60 | this->marker_pub = this->nh.advertise(this->config.pub_Objtopicname, 1); 61 | this->gs_ptr = std::make_unique(this->config); 62 | this->sgm_ss_ptr = std::make_unique(this->config); 63 | } 64 | SGMClustering::~SGMClustering() {} 65 | void SGMClustering::cloud_cb(const PointCloud::ConstPtr &input) { 66 | // 67 | clock_t begin = clock(); 68 | // Map the point cloud from Pointcloud container to Spherical grid map 69 | // representation, where it is a matrix form with 70 | SGM sgm(input->height, input->width); 71 | 72 | // Set Spherical grid map containers with the point cloud. 73 | sgm.Set(input); 74 | cv::Mat ground_label; 75 | (void)this->gs_ptr->ground_segmentation(sgm, ground_label); 76 | std::vector freespace_d; 77 | (void)this->gs_ptr->freespace(ground_label, sgm, freespace_d); 78 | clustered_objects labels; 79 | (void)this->sgm_ss_ptr->semantic_segmentation(sgm, ground_label, labels); 80 | 81 | visualization_msgs::MarkerArray ma; 82 | for (int i = 0; i < labels.objects.size(); i++) { 83 | visualization_msgs::Marker marker; 84 | marker.header.frame_id = input->header.frame_id; 85 | marker.header.stamp = ros::Time::now(); 86 | marker.ns = "object"; 87 | marker.id = i; 88 | marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 89 | marker.action = visualization_msgs::Marker::MODIFY; 90 | marker.text = "Detected Obj" + std::to_string(i); 91 | 92 | marker.pose.position.x = labels.objects[i].pos_x; 93 | marker.pose.position.y = labels.objects[i].pos_y; 94 | marker.pose.position.z = labels.objects[i].pos_z; 95 | marker.scale.x = labels.objects[i].size_x; 96 | marker.scale.y = labels.objects[i].size_y; 97 | marker.scale.z = labels.objects[i].size_z; 98 | 99 | marker.pose.orientation.x = 0.0; 100 | marker.pose.orientation.y = 0.0; 101 | marker.pose.orientation.z = 0.0; 102 | marker.pose.orientation.w = 1.0; 103 | 104 | marker.color.r = 0.0f; 105 | marker.color.g = 1.0f; 106 | marker.color.b = 0.0f; 107 | marker.color.a = 1.0; 108 | 109 | marker.lifetime = ros::Duration(0.1); 110 | if ((marker.scale.x > this->config.obj_level_x_min && marker.scale.x < this->config.obj_level_x_max) && 111 | (marker.scale.y > this->config.obj_level_y_min && marker.scale.y < this->config.obj_level_y_max) && 112 | (marker.scale.z > this->config.obj_level_z_min && marker.scale.z < this->config.obj_level_z_max)) 113 | ma.markers.push_back(marker); 114 | } 115 | this->marker_pub.publish(ma); 116 | 117 | PointCloud_C pcl_msg; 118 | pcl_msg.header = input->header; 119 | for (int rowIdx = 0; rowIdx < sgm.height; rowIdx++) { 120 | for (int colIdx = 0; colIdx < sgm.width; colIdx++) { 121 | pcl::PointXYZRGBL pt; 122 | pt = pcl::PointXYZRGBL(); 123 | pt.x = sgm.x.at(rowIdx, colIdx); 124 | pt.y = sgm.y.at(rowIdx, colIdx); 125 | pt.z = sgm.z.at(rowIdx, colIdx); 126 | pt.r = sgm.r.at(rowIdx, colIdx); 127 | pt.g = sgm.g.at(rowIdx, colIdx); 128 | pt.b = sgm.b.at(rowIdx, colIdx); 129 | if (labels.labels.at(rowIdx, colIdx)) 130 | pt.label = int(labels.labels.at(rowIdx, colIdx)) % 5 + 1; 131 | else 132 | pt.label = 0; 133 | pcl_msg.points.push_back(pt); 134 | } 135 | } 136 | 137 | // Publish the data. 138 | this->pub.publish(pcl_msg); 139 | 140 | sensor_msgs::LaserScan scans; 141 | scans.header.stamp = ros::Time::now(); 142 | scans.header.frame_id = input->header.frame_id; 143 | scans.angle_min = this->config.min_horizontal_angle; 144 | scans.angle_max = this->config.max_horizontal_angle; 145 | scans.angle_increment = (this->config.max_horizontal_angle - this->config.min_horizontal_angle) / sgm.width; 146 | // scans.time_increment = (1 / 10) / (input->width); 147 | scans.range_min = 0.0; 148 | scans.range_max = this->config.lidar_max_range; 149 | scans.ranges = freespace_d; 150 | std::vector intensities(sgm.width, 0); 151 | scans.intensities = intensities; 152 | this->laser_scan_pub.publish(scans); 153 | 154 | clock_t end = clock(); 155 | double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC; 156 | std::cout << "Elapsed_time: " << elapsed_secs << " sec, Freq:" << 1 / elapsed_secs << " Hz" << std::endl; 157 | std::cout << "******************************************************" << std::endl; 158 | } 159 | 160 | int main(int argc, char **argv) { 161 | // Initialize ROS 162 | ros::init(argc, argv, "sgm_lidar_clustering"); 163 | 164 | // Create a LiDAR clustering object. 165 | SGMClustering cluster; 166 | 167 | // Spin 168 | ros::spin(); 169 | } 170 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sgm_lidar_clustering 4 | 0.1.0 5 | The sgm_lidar_clustering package 6 | 7 | khalid 8 | 9 | Apache 2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | visualization_msgs 16 | roscpp 17 | rospy 18 | std_msgs 19 | roscpp 20 | rospy 21 | std_msgs 22 | visualization_msgs 23 | 24 | 25 | geometry_msgs 26 | sensor_msgs 27 | trajectory_msgs 28 | std_srvs 29 | 30 | nodelet 31 | angles 32 | nav_msgs 33 | urdf 34 | tf 35 | tf2_ros 36 | dynamic_reconfigure 37 | rosgraph_msgs 38 | image_transport 39 | rosconsole 40 | message_generation 41 | cv_bridge 42 | polled_camera 43 | diagnostic_updater 44 | camera_info_manager 45 | pcl_ros 46 | geometry_msgs 47 | 48 | sensor_msgs 49 | trajectory_msgs 50 | std_srvs 51 | nodelet 52 | angles 53 | nav_msgs 54 | urdf 55 | tf 56 | tf2_ros 57 | dynamic_reconfigure 58 | rosgraph_msgs 59 | image_transport 60 | rosconsole 61 | message_generation 62 | cv_bridge 63 | pcl_ros 64 | polled_camera 65 | camera_info_manager 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/rviz/Lidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /Image1 11 | Splitter Ratio: 0.5 12 | Tree Height: 544 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Image 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: true 59 | Autocompute Value Bounds: 60 | Max Value: 10 61 | Min Value: -10 62 | Value: true 63 | Axis: Z 64 | Channel Name: label 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: RGB8 68 | Decay Time: 0 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Max Intensity: 244 73 | Min Color: 0; 0; 0 74 | Min Intensity: 0 75 | Name: PointCloud2 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 3 80 | Size (m): 0.05000000074505806 81 | Style: Points 82 | Topic: /husky/LiDAR1/pcl 83 | Unreliable: false 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Class: rviz/Image 88 | Enabled: true 89 | Image Topic: /webcam/QuadCopterLiDAR_image_raw 90 | Max Value: 1 91 | Median window: 5 92 | Min Value: 0 93 | Name: Image 94 | Normalize Range: true 95 | Queue Size: 2 96 | Transport Hint: raw 97 | Unreliable: false 98 | Value: true 99 | Enabled: true 100 | Global Options: 101 | Background Color: 48; 48; 48 102 | Default Light: true 103 | Fixed Frame: QuadCopterLiDAR_link 104 | Frame Rate: 30 105 | Name: root 106 | Tools: 107 | - Class: rviz/Interact 108 | Hide Inactive Objects: true 109 | - Class: rviz/MoveCamera 110 | - Class: rviz/Select 111 | - Class: rviz/FocusCamera 112 | - Class: rviz/Measure 113 | - Class: rviz/SetInitialPose 114 | Theta std deviation: 0.2617993950843811 115 | Topic: /initialpose 116 | X std deviation: 0.5 117 | Y std deviation: 0.5 118 | - Class: rviz/SetGoal 119 | Topic: /move_base_simple/goal 120 | - Class: rviz/PublishPoint 121 | Single click: true 122 | Topic: /clicked_point 123 | Value: true 124 | Views: 125 | Current: 126 | Class: rviz/Orbit 127 | Distance: 21.65574073791504 128 | Enable Stereo Rendering: 129 | Stereo Eye Separation: 0.05999999865889549 130 | Stereo Focal Distance: 1 131 | Swap Stereo Eyes: false 132 | Value: false 133 | Field of View: 0.7853981852531433 134 | Focal Point: 135 | X: 0.2552507221698761 136 | Y: -0.8350999355316162 137 | Z: 2.1539220809936523 138 | Focal Shape Fixed Size: true 139 | Focal Shape Size: 0.05000000074505806 140 | Invert Z Axis: false 141 | Name: Current View 142 | Near Clip Distance: 0.009999999776482582 143 | Pitch: 0.12979641556739807 144 | Target Frame: 145 | Yaw: 3.1653940677642822 146 | Saved: ~ 147 | Window Geometry: 148 | Displays: 149 | collapsed: false 150 | Height: 1016 151 | Hide Left Dock: false 152 | Hide Right Dock: true 153 | Image: 154 | collapsed: false 155 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000ec000002ab000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000039f000000a9fc0100000002fb0000000a0049006d00610067006501000000000000039f0000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000243000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 156 | Selection: 157 | collapsed: false 158 | Time: 159 | collapsed: false 160 | Tool Properties: 161 | collapsed: false 162 | Views: 163 | collapsed: true 164 | Width: 927 165 | X: 993 166 | Y: 27 167 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/rviz/clustering.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /Image1 11 | - /MarkerArray1 12 | - /MarkerArray2 13 | - /LaserScan1 14 | Splitter Ratio: 0.5 15 | Tree Height: 130 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: PointCloud2 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 10 58 | Reference Frame: 59 | Value: true 60 | - Alpha: 1 61 | Autocompute Intensity Bounds: true 62 | Autocompute Value Bounds: 63 | Max Value: 10 64 | Min Value: -10 65 | Value: true 66 | Axis: Z 67 | Channel Name: label 68 | Class: rviz/PointCloud2 69 | Color: 255; 255; 255 70 | Color Transformer: Intensity 71 | Decay Time: 0 72 | Enabled: true 73 | Invert Rainbow: false 74 | Max Color: 255; 255; 255 75 | Min Color: 0; 0; 0 76 | Name: PointCloud2 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.05000000074505806 82 | Style: Points 83 | Topic: /clusteredPointCLoud 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Class: rviz/Image 89 | Enabled: false 90 | Image Topic: /webcam/QuadCopterLiDAR_image_raw 91 | Max Value: 1 92 | Median window: 5 93 | Min Value: 0 94 | Name: Image 95 | Normalize Range: true 96 | Queue Size: 2 97 | Transport Hint: raw 98 | Unreliable: false 99 | Value: false 100 | - Class: rviz/MarkerArray 101 | Enabled: true 102 | Marker Topic: /clusteredObjectList 103 | Name: MarkerArray 104 | Namespaces: 105 | object: true 106 | Queue Size: 100 107 | Value: true 108 | - Class: rviz/MarkerArray 109 | Enabled: false 110 | Marker Topic: /TrackedObjectList 111 | Name: MarkerArray 112 | Namespaces: 113 | {} 114 | Queue Size: 100 115 | Value: false 116 | - Alpha: 1 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz/LaserScan 125 | Color: 255; 255; 255 126 | Color Transformer: Intensity 127 | Decay Time: 0 128 | Enabled: true 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Min Color: 238; 238; 236 132 | Name: LaserScan 133 | Position Transformer: XYZ 134 | Queue Size: 10 135 | Selectable: true 136 | Size (Pixels): 3 137 | Size (m): 0.009999999776482582 138 | Style: Points 139 | Topic: /clusteredPointCLoud_laser_scaner 140 | Unreliable: false 141 | Use Fixed Frame: true 142 | Use rainbow: true 143 | Value: true 144 | Enabled: true 145 | Global Options: 146 | Background Color: 48; 48; 48 147 | Default Light: true 148 | Fixed Frame: odom 149 | Frame Rate: 30 150 | Name: root 151 | Tools: 152 | - Class: rviz/Interact 153 | Hide Inactive Objects: true 154 | - Class: rviz/MoveCamera 155 | - Class: rviz/Select 156 | - Class: rviz/FocusCamera 157 | - Class: rviz/Measure 158 | - Class: rviz/SetInitialPose 159 | Theta std deviation: 0.2617993950843811 160 | Topic: /initialpose 161 | X std deviation: 0.5 162 | Y std deviation: 0.5 163 | - Class: rviz/SetGoal 164 | Topic: /move_base_simple/goal 165 | - Class: rviz/PublishPoint 166 | Single click: true 167 | Topic: /clicked_point 168 | Value: true 169 | Views: 170 | Current: 171 | Class: rviz/Orbit 172 | Distance: 8.576340675354004 173 | Enable Stereo Rendering: 174 | Stereo Eye Separation: 0.05999999865889549 175 | Stereo Focal Distance: 1 176 | Swap Stereo Eyes: false 177 | Value: false 178 | Field of View: 0.7853981852531433 179 | Focal Point: 180 | X: -0.5879124402999878 181 | Y: 0.061921268701553345 182 | Z: 0.04140102490782738 183 | Focal Shape Fixed Size: true 184 | Focal Shape Size: 0.05000000074505806 185 | Invert Z Axis: false 186 | Name: Current View 187 | Near Clip Distance: 0.009999999776482582 188 | Pitch: 0.7497969269752502 189 | Target Frame: 190 | Yaw: 2.7953872680664062 191 | Saved: ~ 192 | Window Geometry: 193 | Displays: 194 | collapsed: true 195 | Height: 427 196 | Hide Left Dock: true 197 | Hide Right Dock: true 198 | Image: 199 | collapsed: false 200 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000010dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000010d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000039f000000a9fc0100000002fb0000000a0049006d00610067006500000000000000039f0000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039f0000010d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 201 | Selection: 202 | collapsed: false 203 | Time: 204 | collapsed: false 205 | Tool Properties: 206 | collapsed: false 207 | Views: 208 | collapsed: true 209 | Width: 927 210 | X: 996 211 | Y: 618 212 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/rviz/clustering_rpi.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /Image1 11 | - /MarkerArray1 12 | - /MarkerArray2 13 | - /LaserScan1 14 | Splitter Ratio: 0.5 15 | Tree Height: 719 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: PointCloud2 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 1 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: true 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 10 58 | Reference Frame: 59 | Value: true 60 | - Alpha: 1 61 | Autocompute Intensity Bounds: true 62 | Autocompute Value Bounds: 63 | Max Value: 10 64 | Min Value: -10 65 | Value: true 66 | Axis: Z 67 | Channel Name: label 68 | Class: rviz/PointCloud2 69 | Color: 255; 255; 255 70 | Color Transformer: Intensity 71 | Decay Time: 0 72 | Enabled: true 73 | Invert Rainbow: false 74 | Max Color: 255; 255; 255 75 | Min Color: 0; 0; 0 76 | Name: PointCloud2 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.05000000074505806 82 | Style: Points 83 | Topic: /clusteredPointCLoud 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Class: rviz/Image 89 | Enabled: false 90 | Image Topic: /webcam/QuadCopterLiDAR_image_raw 91 | Max Value: 1 92 | Median window: 5 93 | Min Value: 0 94 | Name: Image 95 | Normalize Range: true 96 | Queue Size: 2 97 | Transport Hint: raw 98 | Unreliable: false 99 | Value: false 100 | - Class: rviz/MarkerArray 101 | Enabled: true 102 | Marker Topic: /clusteredObjectList 103 | Name: MarkerArray 104 | Namespaces: 105 | object: true 106 | Queue Size: 100 107 | Value: true 108 | - Class: rviz/MarkerArray 109 | Enabled: false 110 | Marker Topic: /TrackedObjectList 111 | Name: MarkerArray 112 | Namespaces: 113 | {} 114 | Queue Size: 100 115 | Value: false 116 | - Alpha: 1 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz/LaserScan 125 | Color: 255; 255; 255 126 | Color Transformer: Intensity 127 | Decay Time: 0 128 | Enabled: true 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Min Color: 238; 238; 236 132 | Name: LaserScan 133 | Position Transformer: XYZ 134 | Queue Size: 10 135 | Selectable: true 136 | Size (Pixels): 3 137 | Size (m): 0.009999999776482582 138 | Style: Points 139 | Topic: /clusteredPointCLoud_laser_scaner 140 | Unreliable: false 141 | Use Fixed Frame: true 142 | Use rainbow: true 143 | Value: true 144 | Enabled: true 145 | Global Options: 146 | Background Color: 48; 48; 48 147 | Default Light: true 148 | Fixed Frame: robot_footprint 149 | Frame Rate: 30 150 | Name: root 151 | Tools: 152 | - Class: rviz/Interact 153 | Hide Inactive Objects: true 154 | - Class: rviz/MoveCamera 155 | - Class: rviz/Select 156 | - Class: rviz/FocusCamera 157 | - Class: rviz/Measure 158 | - Class: rviz/SetInitialPose 159 | Theta std deviation: 0.2617993950843811 160 | Topic: /initialpose 161 | X std deviation: 0.5 162 | Y std deviation: 0.5 163 | - Class: rviz/SetGoal 164 | Topic: /move_base_simple/goal 165 | - Class: rviz/PublishPoint 166 | Single click: true 167 | Topic: /clicked_point 168 | Value: true 169 | Views: 170 | Current: 171 | Class: rviz/Orbit 172 | Distance: 8.576340675354004 173 | Enable Stereo Rendering: 174 | Stereo Eye Separation: 0.05999999865889549 175 | Stereo Focal Distance: 1 176 | Swap Stereo Eyes: false 177 | Value: false 178 | Field of View: 0.7853981852531433 179 | Focal Point: 180 | X: -0.5879124402999878 181 | Y: 0.061921268701553345 182 | Z: 0.04140102490782738 183 | Focal Shape Fixed Size: true 184 | Focal Shape Size: 0.05000000074505806 185 | Invert Z Axis: false 186 | Name: Current View 187 | Near Clip Distance: 0.009999999776482582 188 | Pitch: 0.7497969269752502 189 | Target Frame: 190 | Yaw: 2.7953872680664062 191 | Saved: ~ 192 | Window Geometry: 193 | Displays: 194 | collapsed: false 195 | Height: 1016 196 | Hide Left Dock: false 197 | Hide Right Dock: true 198 | Image: 199 | collapsed: false 200 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000039f000000a9fc0100000002fb0000000a0049006d00610067006500000000000000039f0000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002400000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 201 | Selection: 202 | collapsed: false 203 | Time: 204 | collapsed: false 205 | Tool Properties: 206 | collapsed: false 207 | Views: 208 | collapsed: true 209 | Width: 924 210 | X: 72 211 | Y: 27 212 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/rviz/multi_LiDAR.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /Image1 11 | - /PointCloud22 12 | Splitter Ratio: 0.5 13 | Tree Height: 544 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Image 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 10 62 | Min Value: -10 63 | Value: true 64 | Axis: Z 65 | Channel Name: label 66 | Class: rviz/PointCloud2 67 | Color: 255; 255; 255 68 | Color Transformer: RGB8 69 | Decay Time: 0 70 | Enabled: true 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 244 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0 76 | Name: PointCloud2 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.05000000074505806 82 | Style: Points 83 | Topic: /husky/LiDAR1/pcl 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Class: rviz/Image 89 | Enabled: true 90 | Image Topic: /webcam_r/QuadCopterLiDAR_image_raw 91 | Max Value: 1 92 | Median window: 5 93 | Min Value: 0 94 | Name: Image 95 | Normalize Range: true 96 | Queue Size: 2 97 | Transport Hint: raw 98 | Unreliable: false 99 | Value: true 100 | - Alpha: 1 101 | Autocompute Intensity Bounds: true 102 | Autocompute Value Bounds: 103 | Max Value: 10 104 | Min Value: -10 105 | Value: true 106 | Axis: Z 107 | Channel Name: intensity 108 | Class: rviz/PointCloud2 109 | Color: 255; 255; 255 110 | Color Transformer: RGB8 111 | Decay Time: 0 112 | Enabled: true 113 | Invert Rainbow: false 114 | Max Color: 255; 255; 255 115 | Max Intensity: 4096 116 | Min Color: 0; 0; 0 117 | Min Intensity: 0 118 | Name: PointCloud2 119 | Position Transformer: XYZ 120 | Queue Size: 10 121 | Selectable: true 122 | Size (Pixels): 3 123 | Size (m): 0.009999999776482582 124 | Style: Points 125 | Topic: /husky/LiDAR2/pcl 126 | Unreliable: false 127 | Use Fixed Frame: true 128 | Use rainbow: true 129 | Value: true 130 | Enabled: true 131 | Global Options: 132 | Background Color: 48; 48; 48 133 | Default Light: true 134 | Fixed Frame: QuadCopterLiDAR_link 135 | Frame Rate: 30 136 | Name: root 137 | Tools: 138 | - Class: rviz/Interact 139 | Hide Inactive Objects: true 140 | - Class: rviz/MoveCamera 141 | - Class: rviz/Select 142 | - Class: rviz/FocusCamera 143 | - Class: rviz/Measure 144 | - Class: rviz/SetInitialPose 145 | Theta std deviation: 0.2617993950843811 146 | Topic: /initialpose 147 | X std deviation: 0.5 148 | Y std deviation: 0.5 149 | - Class: rviz/SetGoal 150 | Topic: /move_base_simple/goal 151 | - Class: rviz/PublishPoint 152 | Single click: true 153 | Topic: /clicked_point 154 | Value: true 155 | Views: 156 | Current: 157 | Class: rviz/Orbit 158 | Distance: 10.325029373168945 159 | Enable Stereo Rendering: 160 | Stereo Eye Separation: 0.05999999865889549 161 | Stereo Focal Distance: 1 162 | Swap Stereo Eyes: false 163 | Value: false 164 | Field of View: 0.7853981852531433 165 | Focal Point: 166 | X: 0.2552507221698761 167 | Y: -0.8350999355316162 168 | Z: 2.1539220809936523 169 | Focal Shape Fixed Size: true 170 | Focal Shape Size: 0.05000000074505806 171 | Invert Z Axis: false 172 | Name: Current View 173 | Near Clip Distance: 0.009999999776482582 174 | Pitch: 0.4547962546348572 175 | Target Frame: 176 | Yaw: 3.1153931617736816 177 | Saved: ~ 178 | Window Geometry: 179 | Displays: 180 | collapsed: false 181 | Height: 1016 182 | Hide Left Dock: false 183 | Hide Right Dock: true 184 | Image: 185 | collapsed: false 186 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000ec000002ab000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000039f000000a9fc0100000002fb0000000a0049006d00610067006501000000000000039f0000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000243000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 187 | Selection: 188 | collapsed: false 189 | Time: 190 | collapsed: false 191 | Tool Properties: 192 | collapsed: false 193 | Views: 194 | collapsed: true 195 | Width: 927 196 | X: 993 197 | Y: 27 198 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/src/ground_segmentation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Desc: SGM LiDAR clustering. 3 | * Author: khaled elmadawi 4 | * mail: khalid.elmadawi@sigma.se 5 | * Date: 12 September 2020 6 | * 7 | * Copyright 2020 sigma embedded engineering-se. All rights reserved. 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | * 21 | */ 22 | 23 | #include "ground_segmentation.hpp" 24 | GroundSegmentation::GroundSegmentation(configuration config) { this->config_ = config; } 25 | 26 | GroundSegmentation::~GroundSegmentation() {} 27 | 28 | ErrorCode GroundSegmentation::spherical_grid_map_slopes(SGM const& sgm, cv::Mat& slope) { 29 | // slope up direction 30 | int rows = sgm.x.rows; 31 | 32 | cv::Mat deltaX = cv::Mat::zeros(sgm.x.rows, sgm.x.cols, CV_32F); 33 | 34 | cv::absdiff(sgm.x(cv::Range(1, rows), cv::Range::all()), sgm.x(cv::Range(0, rows - 1), cv::Range::all()), 35 | deltaX(cv::Range(1, rows), cv::Range::all())); 36 | cv::pow(deltaX, 2, deltaX); 37 | 38 | cv::Mat deltaY = cv::Mat::zeros(sgm.x.rows, sgm.x.cols, CV_32F); 39 | cv::absdiff(sgm.y(cv::Range(1, rows), cv::Range::all()), sgm.y(cv::Range(0, rows - 1), cv::Range::all()), 40 | deltaY(cv::Range(1, rows), cv::Range::all())); 41 | cv::pow(deltaY, 2, deltaY); 42 | cv::Mat deltaXY = cv::Mat::zeros(sgm.x.rows, sgm.x.cols, CV_32F); 43 | deltaXY = deltaX + deltaY; 44 | cv::sqrt(deltaXY, deltaXY); 45 | 46 | cv::Mat deltaZ = cv::Mat::zeros(sgm.x.rows, sgm.x.cols, CV_32F); 47 | cv::absdiff(sgm.z(cv::Range(1, rows), cv::Range::all()), sgm.z(cv::Range(0, rows - 1), cv::Range::all()), 48 | deltaZ(cv::Range(1, rows), cv::Range::all())); 49 | cv::pow(deltaZ, 2, deltaZ); 50 | cv::sqrt(deltaZ, deltaZ); 51 | 52 | slope = deltaZ / deltaXY; 53 | return ErrorCode::Success; 54 | } 55 | 56 | ErrorCode GroundSegmentation::ground_segmentation(SGM const& sgm, cv::Mat& ground_label) { 57 | cv::Mat slope; 58 | (void)this->spherical_grid_map_slopes(sgm, slope); 59 | // taking threshold for the ground vectors 60 | cv::threshold(slope, ground_label, this->config_.ground_segmentation_threshold, 1, 0); 61 | if (this->config_.use_morphological_filter) { 62 | cv::Mat element = cv::Mat::ones(3, 3, CV_8U); 63 | cv::erode(ground_label, ground_label, element, cv::Point(-1, -1), 2); 64 | cv::dilate(ground_label, ground_label, element, cv::Point(-1, -1), 2); 65 | } 66 | 67 | return ErrorCode::Success; 68 | } 69 | ErrorCode GroundSegmentation::freespace(cv::Mat const& ground_label, SGM const& sgm, std::vector& freespace_) { 70 | cv::Mat freespace_edges = cv::Mat::zeros(sgm.height, sgm.width, CV_32F); 71 | 72 | cv::absdiff(ground_label(cv::Range(1, sgm.height), cv::Range::all()), 73 | ground_label(cv::Range(0, sgm.height - 1), cv::Range::all()), 74 | freespace_edges(cv::Range(1, sgm.height), cv::Range::all())); 75 | 76 | cv::threshold(freespace_edges, freespace_edges, 0.5, 255, 0); 77 | 78 | std::vector free_space_idx; 79 | cv::findNonZero(freespace_edges, free_space_idx); 80 | freespace_ = std::vector(sgm.width, 0); 81 | for (long unsigned int i = 0; i < free_space_idx.size(); i++) { 82 | if (freespace_[free_space_idx[i].x] < 0.001) { 83 | freespace_[free_space_idx[i].x] = 84 | std::sqrt(std::pow(sgm.x.at(free_space_idx[i].y, free_space_idx[i].x), 2) + 85 | std::pow(sgm.y.at(free_space_idx[i].y, free_space_idx[i].x), 2)); 86 | } 87 | } 88 | return ErrorCode::Success; 89 | } 90 | -------------------------------------------------------------------------------- /sgm_lidar_clustering/tests/sgm_unittests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "dataContainers.hpp" 13 | #include "ground_segmentation.hpp" 14 | #include "sgm_segmentation.hpp" 15 | 16 | // using namespace std; 17 | 18 | class MyTestSuite : public ::testing::Test { 19 | public: 20 | MyTestSuite() { 21 | configuration config; 22 | config.ground_segmentation_threshold = 0.1; 23 | config.use_morphological_filter = false; 24 | this->gs_ptr = std::make_unique(config); 25 | this->sgm_ss_ptr = std::make_unique(config); 26 | } 27 | ~MyTestSuite() {} 28 | 29 | std::unique_ptr gs_ptr; 30 | std::unique_ptr sgm_ss_ptr; 31 | }; 32 | TEST_F(MyTestSuite, ground_slopes) { 33 | SGM sgm(3, 3); 34 | float x[9] = {0, 1, 1, 0, 1, 1, 0, 1, 1}; 35 | float y[9] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; 36 | float z[9] = {0, 0, 1, 0, 0, 1, 0, 0, 1}; 37 | sgm.x = cv::Mat(3, 3, CV_32F, x); 38 | sgm.y = cv::Mat(3, 3, CV_32F, y); 39 | sgm.z = cv::Mat(3, 3, CV_32F, z); 40 | cv::Mat slopes; 41 | ErrorCode error_ = this->gs_ptr->spherical_grid_map_slopes(sgm, slopes); 42 | 43 | ASSERT_EQ(error_, ErrorCode::Success); 44 | ASSERT_EQ(slopes.rows, sgm.height); 45 | ASSERT_EQ(slopes.cols, sgm.width); 46 | } 47 | 48 | TEST_F(MyTestSuite, ground_segmentation) { 49 | SGM sgm(3, 3); 50 | float x[9] = {0, 0, 0, 1, 1, 1, 1, 1, 1}; 51 | float y[9] = {0, 0, 0, 1, 1, 1, 2, 2, 2}; 52 | float z[9] = {0, 0, 0, 0, 0, 0, 1, 1, 1}; 53 | float r[9] = {0, 0, 0, 0, 0, 0, 1, 1, 1}; 54 | cv::Mat result = cv::Mat(3, 3, CV_32F, r); 55 | 56 | sgm.x = cv::Mat(3, 3, CV_32F, x); 57 | sgm.y = cv::Mat(3, 3, CV_32F, y); 58 | sgm.z = cv::Mat(3, 3, CV_32F, z); 59 | 60 | cv::Mat ground_label; 61 | ErrorCode error_ = this->gs_ptr->ground_segmentation(sgm, ground_label); 62 | 63 | cv::Mat diff = ground_label != result; 64 | 65 | ASSERT_EQ(error_, ErrorCode::Success); 66 | ASSERT_EQ(cv::countNonZero(diff), 0); 67 | ASSERT_EQ(ground_label.rows, sgm.height); 68 | ASSERT_EQ(ground_label.cols, sgm.width); 69 | } 70 | 71 | int main(int argc, char** argv) { 72 | ros::init(argc, argv, "TestNode"); 73 | 74 | testing::InitGoogleTest(&argc, argv); 75 | 76 | std::thread t([] { 77 | while (ros::ok()) ros::spin(); 78 | }); 79 | 80 | auto res = RUN_ALL_TESTS(); 81 | 82 | ros::shutdown(); 83 | 84 | return res; 85 | } 86 | --------------------------------------------------------------------------------