├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── forestI_config.yaml └── forestI_seg_param.yaml ├── docs ├── 2022_IEEE_RAL_video_Lucas.mp4 └── forest_localisation_diagram.png ├── launch ├── pfilter.launch └── pfilter_rviz.launch ├── msg └── gator_data.msg ├── package.xml ├── rviz ├── pfilter_particles_rviz.rviz ├── pfilter_rviz.rviz └── submap_rviz.rviz └── src ├── config_server.h ├── csv.h ├── gator_handler.cpp ├── ground_extraction.cpp ├── ground_extraction.h ├── imu_interface.h ├── localization_node.cpp ├── map.cpp ├── map.h ├── pfilter_localization.cpp ├── pfilter_localization.h ├── tree.h ├── tree_segmentation.cpp ├── tree_segmentation.h ├── utils.h ├── visualisation.cpp └── visualisation.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(forest_localisation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | add_definitions(-std=c++14) 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 | nav_msgs 13 | roscpp 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | pcl_ros 18 | visualization_msgs 19 | cv_bridge 20 | message_generation 21 | linefit_ground_segmentation 22 | ) 23 | 24 | find_package(OpenCV REQUIRED) 25 | 26 | find_package(PCL REQUIRED) 27 | include_directories(${PCL_INCLUDE_DIRS}) 28 | link_directories(${PCL_LIBRARY_DIRS}) 29 | add_definitions(${PCL_DEFINITIONS}) 30 | 31 | ## Generate messages in the 'msg' folder 32 | add_message_files( 33 | FILES 34 | gator_data.msg 35 | ) 36 | 37 | ## Generate added messages and services with any dependencies listed here 38 | generate_messages( 39 | DEPENDENCIES 40 | std_msgs 41 | ) 42 | 43 | ################################### 44 | ## catkin specific configuration ## 45 | ################################### 46 | ## The catkin_package macro generates cmake config files for your package 47 | ## Declare things to be passed to dependent projects 48 | ## INCLUDE_DIRS: uncomment this if your package contains header files 49 | ## LIBRARIES: libraries you create in this project that dependent projects also need 50 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 51 | ## DEPENDS: system dependencies of this project that dependent projects also need 52 | catkin_package( 53 | INCLUDE_DIRS src 54 | # LIBRARIES pfilter_localization 55 | CATKIN_DEPENDS message_runtime nav_msgs roscpp rospy sensor_msgs std_msgs tf2 tf2_ros cv_bridge pcl_ros linefit_ground_segmentation DEPENDS OpenCV 56 | # DEPENDS system_lib 57 | ) 58 | 59 | ########### 60 | ## Build ## 61 | ########### 62 | 63 | ## Specify additional locations of header files 64 | ## Your package locations should be listed before other locations 65 | include_directories( 66 | src 67 | #include 68 | ${catkin_INCLUDE_DIRS} 69 | ${OpenCV_INCLUDE_DIRS} 70 | ${PCL_LIBRARY_DIRS} 71 | ) 72 | 73 | ## Declare a C++ executable 74 | ## With catkin_make all packages are built within a single CMake context 75 | ## The recommended prefix ensures that target names across packages don't collide 76 | add_executable(forest_localisation_node src/localization_node.cpp src/pfilter_localization.cpp src/map.cpp src/tree_segmentation.cpp src/ground_extraction.cpp src/visualisation.cpp) 77 | 78 | ## Add cmake target dependencies of the executable 79 | ## same as for the library above 80 | add_dependencies(forest_localisation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 81 | 82 | ## Specify libraries to link a library or executable target against 83 | target_link_libraries(forest_localisation_node 84 | ${catkin_LIBRARIES} 85 | ${OpenCV_LIBS} 86 | ${PCL_LIBRARIES} 87 | 88 | ) 89 | 90 | 91 | ############# 92 | ## Install ## 93 | ############# 94 | 95 | # all install targets should use catkin DESTINATION variables 96 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 97 | 98 | ## Mark executable scripts (Python etc.) for installation 99 | ## in contrast to setup.py, you can choose the destination 100 | # catkin_install_python(PROGRAMS 101 | # scripts/my_python_script 102 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 103 | # ) 104 | 105 | ## Mark executables for installation 106 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 107 | # install(TARGETS ${PROJECT_NAME}_node 108 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 109 | # ) 110 | 111 | ## Mark libraries for installation 112 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 113 | # install(TARGETS ${PROJECT_NAME} 114 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 115 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 116 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 117 | # ) 118 | 119 | ## Mark cpp header files for installation 120 | # install(DIRECTORY include/${PROJECT_NAME}/ 121 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 122 | # FILES_MATCHING PATTERN "*.h" 123 | # PATTERN ".svn" EXCLUDE 124 | # ) 125 | 126 | ## Mark other files for installation (e.g. launch and bag files, etc.) 127 | # install(FILES 128 | # # myfile1 129 | # # myfile2 130 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 131 | # ) 132 | 133 | ############# 134 | ## Testing ## 135 | ############# 136 | 137 | ## Add gtest based cpp test target and link libraries 138 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pfilter_localization.cpp) 139 | # if(TARGET ${PROJECT_NAME}-test) 140 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 141 | # endif() 142 | 143 | ## Add folders to be run by python nosetests 144 | # catkin_add_nosetests(test) 145 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | 3 | CSIRO Open Source Software Licence Agreement (variation of the BSD / MIT License) 4 | Copyright (c) 2022, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230. 5 | All rights reserved. CSIRO is willing to grant you a licence to this software (Forest_Localisation) on the following terms, except where otherwise indicated for third party material. 6 | Redistribution and use of this software in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | • Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | • Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | • Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission of CSIRO. 10 | EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE, OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE. 11 | TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR OTHER LIABILITY HOWSOEVER INCURRED. WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL, INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. 12 | APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED, RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES". TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO’S OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES: 13 | (a) THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN; 14 | (b) THE REPAIR OF THE SOFTWARE; 15 | (c) THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED. 16 | IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED WITH IT. CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY. 17 | 18 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 19 | 20 | Third Party Components 21 | The following third party components are distributed with the Software. You agree to comply with the licence terms for these components as part of accessing the Software. Other third party software may also be identified in separate files distributed with the Software. 22 | _____________________________________________________________________________________ 23 | ROS https://www.ros.org/ 24 | This software is licensed under the BSD 3-Clause. 25 | _____________________________________________________________________________________ 26 | _____________________________________________________________________________________ 27 | PCL https://pointclouds.org/ 28 | 29 | This software is licensed under the BSD license. 30 | _____________________________________________________________________________________ 31 | _____________________________________________________________________________________ 32 | Eigen https://eigen.tuxfamily.org/ 33 | 34 | This software is licensed under the Mozilla Public License (MPL) version 2.0. 35 | _____________________________________________________________________________________ 36 | _____________________________________________________________________________________ 37 | OpenCV https://opencv.org/ 38 | 39 | This software is licensed under the Apache 2. 40 | _____________________________________________________________________________________ 41 | _____________________________________________________________________________________ 42 | linefit_ground_extraction https://github.com/lorenwel/linefit_ground_segmentation 43 | 44 | This software is licensed under the BSD 3-Clause. 45 | _____________________________________________________________________________________ 46 | 47 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 48 | 49 | 50 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Forest_Localisation 2 | 3 | This repository contains the open-source implementation of the localisation method for forest environments published in RAL 2023: 4 | 5 | - [Air-Ground Collaborative Localisation in Forests Using Lidar Canopy Maps](https://doi.org/10.1109/LRA.2023.3243498) 6 | 7 | We presented a collaborative localisation framework that matches an Above Canopy Map (ACM) (captures height information) collected from aerial lidar scans, as a reference 8 | map, with Under Canopy Maps (UCM) constructed from sensed trees (positions and crown heights) using mobile lidar data from the ground robot system. Experimental evaluation on distinct forest environments and platforms demonstrated the method is effective in geo-localising robots with low position errors. 9 | 10 | ## Method overview 11 | ![](./docs/forest_localisation_diagram.png) 12 | 13 | - Obtain the Above Canopy Map (ACM): Heightmap constructed from aerial lidar data 14 | - Tree detection and estimation of position and crown height to construct the local Under Canopy Maps (UCM) 15 | - Map Matching between the ACM and UCMs using the SSD similarity measure 16 | - Monte Carlo Localisation implementation to estimate the robot positions using the Map Matching scores and the robot's motion model 17 | 18 | 19 | ## Installation 20 | 21 | ### Requirements 22 | Install the following packages (dependencies) to build this project: 23 | - [ROS](https://www.ros.org/) (Noetic) 24 | - [PCL](https://pointclouds.org/downloads/) (1.10.0) 25 | - [Eigen](https://eigen.tuxfamily.org/dox/GettingStarted.html) (3.3.7-2) 26 | - [OpenCV](https://opencv.org/) (4.2.0) 27 | 28 | Install the following ROS packages (dependencies) to build this project: 29 | 30 | - Install the robot-localization ros package 31 | ```bash 32 | sudo apt-get install -y ros-noetic-robot-localization 33 | ``` 34 | Many dependencies such as (tf2, tf2_ros, geometry_msgs, sensor_msgs, etc) will be installed along with the robot-localization. 35 | 36 | - Install other ros package dependencies: 37 | ```bash 38 | sudo apt-get install -y ros-noetic-pcl-ros 39 | sudo apt-get install -y ros-noetic-cv-bridge 40 | ``` 41 | 42 | - Install [linefit_ground_segmentation](https://github.com/lorenwel/linefit_ground_segmentation) ros package 43 | 44 | 45 | Clone and install the project: 46 | ```bash 47 | git clone https://github.com/csiro-robotics/Forest_Localisation.git 48 | catkin build pfilter_localization 49 | ``` 50 | 51 | ### Dataset 52 | 53 | - Download the dataset from: [Forest Localisation Dataset](https://doi.org/10.25919/fbwy-rk04) 54 | - Copy the dataset folder `forestI` to the project folder `(your_catkin_ws)/forest_localisation/datasets/` 55 | 56 | 57 | ### Running the code 58 | 59 | Launch the main node: 60 | ```bash 61 | roslaunch forest_localisation pfilter.launch 62 | ``` 63 | 64 | Launch the rviz nodes for visualisation: 65 | ```bash 66 | roslaunch forest_localisation pfilter_rviz.launch 67 | ``` 68 | To visualise the ACM and UCM images, launch the ROS rqt_image_vew node: 69 | ```bash 70 | rqt_image_vew 71 | ``` 72 | then, select the topic `/map_local_images` for visualisation. 73 | 74 | Play the rosbag file: 75 | ```bash 76 | cd (you_catkin_ws)/forest_localisation/datasets/forestI/ 77 | rosbag play forestI.bag 78 | ``` 79 | 80 | ### Parameters 81 | 82 | The parameters are set in the `/config/forestI_config.yaml` file. Please refer to the file for more details. 83 | 84 | 85 | ## Citation 86 | 87 | If you use any of this code and/or find this work helpful to your research, please cite: 88 | 89 | ``` 90 | @ARTICLE{Lima23, 91 | author={de Lima, Lucas Carvalho and Ramezani, Milad and Borges, Paulo and Brünig, Michael}, 92 | journal={IEEE Robotics and Automation Letters}, 93 | title={Air-Ground Collaborative Localisation in Forests Using Lidar Canopy Maps}, 94 | year={2023}, 95 | volume={}, 96 | number={}, 97 | pages={1-8}, 98 | doi={10.1109/LRA.2023.3243498}} 99 | 100 | ``` 101 | 102 | ## Acknowledgment 103 | Functions or code snippets from 3rd party have been acknowledged at the respective function definitions. 104 | 105 | ## Contact 106 | For questions or feedback: 107 | ``` 108 | lucas.lima@data61.csiro.au 109 | ``` 110 | -------------------------------------------------------------------------------- /config/forestI_config.yaml: -------------------------------------------------------------------------------- 1 | dataset_name: 'forestI' 2 | 3 | #ACM map params 4 | map_file_name: "forestI_acm.png" # Name of ACM image file (.png) 5 | map_image_max_height: 34.28 # [meters] Maximum height above ground observed in the aerial 3D pointcloud 6 | map_image_frame_x: -231.43 # [meters] x coordinate of ACM origin frame (top left point) 7 | map_image_frame_y: 177.08 # [meters] y coordinate of ACM origin frame (top left point) 8 | map_image_grid_size: 0.25 # [meters] Grid size of ACM map image 9 | map_display_window: [617,310,1310,1100] # [meters] coordinates to display a smaller version acm (visualisation only) 10 | 11 | #Ground truth trajectory parameters (for evaluation) 12 | groundtruth_trajectory_name: 'global_groundtruth_traj.csv' # Ground truth trajectory file name 13 | trajectory_rotation: [] #not used 14 | trajectory_translation: [] #not used 15 | 16 | # Car like model params (Gator robot) 17 | wheels_axis_distance: 1.96 # [meters] Distance between the front and rear wheels axis of the robot car 18 | gator_data_topic: "gator_info" # topic of data from the wheeled robot (gator) 19 | 20 | #Frames 21 | base_frame: "base_link" # Robot's frame 22 | map_frame: "map" # Map frame 23 | 24 | #Lidar accumulation params 25 | input_lidar_topic: "cloud" # topic of input cloud 26 | laser_accumulate_time: 1.5 # [seconds] Time to accumulate the sweep cloud 27 | compensate_cloud: true # Use gravity alignment of sweeps or not 28 | 29 | #Particle filtering params 30 | number_particles: 500 # Number of particles in the Monte Carlo Localisation 31 | odom_alpha1: 0.1 # robot motion model uncertainty parameters (Probabilistic Robotics book) 32 | odom_alpha2: 0.1 33 | odom_alpha3: 0.2 34 | odom_alpha4: 0.01 35 | particle_range_x: 1.0 # [meters] Range distance in x coordiante to initiate particles around the robot's initial position 36 | particle_range_y: 1.0 # [meters] Range distance in y coordiante to initiate particles around the robot's initial position 37 | distance_threshold_stopped: 0.1 # [meters] Distance between consecutive odometry poses to determine if the robot has moved 38 | angle_threshold_stopped: 0.1 # [radians] Angle between consecutive odometry orientations to determine if the robot has moved 39 | min_landmarks_threshold: 3 # Minimum number of segmented trees to construct UCM and perform resampling 40 | initial_pose: [0, 0, 0] # [meters,meters,radians] [x,y,yaw] robots initial position 41 | 42 | #Tree segmentation params 43 | ground_z_down_threshold: 1.3 # [meters] Minimun height above ground to crop pointcloud 44 | ground_z_up_threshold: 4.5 # [meters] Maximum height above ground to crop pointcloud 45 | downsample_voxel_size: 0.25 # [meters] Voxel size for downsampling the sweep (pointcloud) 46 | tree_height: 3.0 # [meters] minimum height of cluster to classify as a tree 47 | tolerance: 0.45 # [meters] minimum distance for clustering points. 48 | min_cluster_size: 50 # minimum points in euclidean clustering to consider a cluster 49 | max_cluster_size: 20000 # maximum points in euclidean clustering to consider a cluster 50 | trunks_color: [255,255,0] 51 | 52 | #Map Matching and UCM params 53 | ssd_beta: 0.7 # Similarity parameter beta for map matching 54 | crown_radius: 2.0 # Crown radius of trees represented in the UCM map images 55 | number_of_threads: 6 # Number of threads used in the Map Matching algorithm 56 | 57 | -------------------------------------------------------------------------------- /config/forestI_seg_param.yaml: -------------------------------------------------------------------------------- 1 | #Config file extracted and adapted from (https://github.com/lorenwel/linefit_ground_segmentation) 2 | 3 | n_threads: 4 # number of threads to use. 4 | 5 | r_min: 0.5 # minimum point distance. 6 | r_max: 50 # maximum point distance. 7 | n_bins: 120 # number of radial bins. 8 | n_segments: 360 # number of radial segments. 9 | 10 | max_dist_to_line: 0.25 # maximum vertical distance of point to line to be considered ground. 11 | 12 | sensor_height: 2.0 # sensor height above ground. 13 | min_slope: -0.4 # minimum slope of a ground line. 14 | max_slope: 0.4 # maximum slope of a ground line. 15 | max_fit_error: 0.25 #0.15 # maximum error of a point during line fit. 16 | long_threshold: 2.5 # distance between points after which they are considered far from each other. 17 | max_long_height: 0.25 #0.15 # maximum height change to previous point in long line. 18 | max_start_height: 0.25 #0.15 # maximum difference to estimated ground height to start a new line. 19 | line_search_angle: 0.1 # how far to search in angular direction to find a line [rad]. 20 | 21 | gravity_aligned_frame: "" # Not used since sweeps are gravity aligned 22 | 23 | latch: false # latch output topics or not 24 | visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING 25 | 26 | -------------------------------------------------------------------------------- /docs/2022_IEEE_RAL_video_Lucas.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/csiro-robotics/Forest_Localisation/c441760eb840c5cfa9f3e5b604c68bc35e7ed3e8/docs/2022_IEEE_RAL_video_Lucas.mp4 -------------------------------------------------------------------------------- /docs/forest_localisation_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/csiro-robotics/Forest_Localisation/c441760eb840c5cfa9f3e5b604c68bc35e7ed3e8/docs/forest_localisation_diagram.png -------------------------------------------------------------------------------- /launch/pfilter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/pfilter_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /msg/gator_data.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 steering_angle # in Radians, -ve left, +ve right 3 | float32 inst_velocity # m/sec current speed fast average -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | forest_localisation 4 | 0.0.0 5 | The forest_localisation package 6 | 7 | 8 | 9 | 10 | lucas.lima 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | nav_msgs 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | tf2 58 | tf2_ros 59 | pcl_ros 60 | visualization_msgs 61 | 62 | cv_bridge 63 | linefit_ground_segmentation 64 | 65 | nav_msgs 66 | roscpp 67 | rospy 68 | sensor_msgs 69 | std_msgs 70 | tf2 71 | tf2_ros 72 | visualization_msgs 73 | 74 | nav_msgs 75 | roscpp 76 | rospy 77 | sensor_msgs 78 | std_msgs 79 | tf2 80 | tf2_ros 81 | pcl_ros 82 | visualization_msgs 83 | 84 | message_generation 85 | message_runtime 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /rviz/pfilter_particles_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PoseArray1 10 | - /Axes1 11 | - /PointCloud21 12 | - /PoseArray2 13 | - /PoseArray3 14 | - /Axes2 15 | Splitter Ratio: 0.33753502368927 16 | Tree Height: 441 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 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: false 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: false 60 | - Alpha: 1 61 | Arrow Length: 0.5 62 | Axes Length: 0.30000001192092896 63 | Axes Radius: 0.009999999776482582 64 | Class: rviz/PoseArray 65 | Color: 252; 246; 16 66 | Enabled: false 67 | Head Length: 0.07000000029802322 68 | Head Radius: 0.029999999329447746 69 | Name: PoseArray 70 | Queue Size: 10 71 | Shaft Length: 0.23000000417232513 72 | Shaft Radius: 0.009999999776482582 73 | Shape: Arrow (Flat) 74 | Topic: /pf/particles 75 | Unreliable: false 76 | Value: false 77 | - Alpha: 1 78 | Class: rviz/Axes 79 | Enabled: false 80 | Length: 1 81 | Name: Axes 82 | Radius: 0.10000000149011612 83 | Reference Frame: base_link 84 | Show Trail: false 85 | Value: false 86 | - Alpha: 1 87 | Autocompute Intensity Bounds: true 88 | Autocompute Value Bounds: 89 | Max Value: 10 90 | Min Value: -10 91 | Value: true 92 | Axis: Z 93 | Channel Name: intensity 94 | Class: rviz/PointCloud2 95 | Color: 255; 255; 255 96 | Color Transformer: RGB8 97 | Decay Time: 0 98 | Enabled: true 99 | Invert Rainbow: false 100 | Max Color: 255; 255; 255 101 | Min Color: 0; 0; 0 102 | Name: PointCloud2 103 | Position Transformer: XYZ 104 | Queue Size: 10 105 | Selectable: true 106 | Size (Pixels): 3 107 | Size (m): 0.10000000149011612 108 | Style: Flat Squares 109 | Topic: /particles_weights 110 | Unreliable: false 111 | Use Fixed Frame: true 112 | Use rainbow: true 113 | Value: true 114 | - Alpha: 1 115 | Arrow Length: 0.5 116 | Axes Length: 0.30000001192092896 117 | Axes Radius: 0.009999999776482582 118 | Class: rviz/PoseArray 119 | Color: 110; 255; 255 120 | Enabled: true 121 | Head Length: 0.07000000029802322 122 | Head Radius: 0.029999999329447746 123 | Name: PoseArray 124 | Queue Size: 10 125 | Shaft Length: 0.23000000417232513 126 | Shaft Radius: 0.009999999776482582 127 | Shape: Arrow (Flat) 128 | Topic: /particles_resample 129 | Unreliable: false 130 | Value: true 131 | - Alpha: 1 132 | Arrow Length: 0.5 133 | Axes Length: 0.30000001192092896 134 | Axes Radius: 0.009999999776482582 135 | Class: rviz/PoseArray 136 | Color: 252; 246; 16 137 | Enabled: true 138 | Head Length: 0.07000000029802322 139 | Head Radius: 0.029999999329447746 140 | Name: PoseArray 141 | Queue Size: 10 142 | Shaft Length: 0.23000000417232513 143 | Shaft Radius: 0.009999999776482582 144 | Shape: Arrow (Flat) 145 | Topic: /particles_update 146 | Unreliable: false 147 | Value: true 148 | - Alpha: 1 149 | Class: rviz/Axes 150 | Enabled: true 151 | Length: 1 152 | Name: Axes 153 | Radius: 0.10000000149011612 154 | Reference Frame: base_link 155 | Show Trail: false 156 | Value: true 157 | Enabled: true 158 | Global Options: 159 | Background Color: 0; 0; 0 160 | Default Light: true 161 | Fixed Frame: base_link 162 | Frame Rate: 30 163 | Name: root 164 | Tools: 165 | - Class: rviz/Interact 166 | Hide Inactive Objects: true 167 | - Class: rviz/MoveCamera 168 | - Class: rviz/Select 169 | - Class: rviz/FocusCamera 170 | - Class: rviz/Measure 171 | - Class: rviz/SetInitialPose 172 | Theta std deviation: 0.2617993950843811 173 | Topic: /initialpose 174 | X std deviation: 0.5 175 | Y std deviation: 0.5 176 | - Class: rviz/SetGoal 177 | Topic: /move_base_simple/goal 178 | - Class: rviz/PublishPoint 179 | Single click: true 180 | Topic: /clicked_point 181 | Value: true 182 | Views: 183 | Current: 184 | Angle: 0.015000020153820515 185 | Class: rviz/TopDownOrtho 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.05999999865889549 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Invert Z Axis: false 192 | Name: Current View 193 | Near Clip Distance: 0.009999999776482582 194 | Scale: 74.04893493652344 195 | Target Frame: 196 | X: 0.2802550196647644 197 | Y: 0.03264422342181206 198 | Saved: ~ 199 | Window Geometry: 200 | Displays: 201 | collapsed: false 202 | Height: 297 203 | Hide Left Dock: false 204 | Hide Right Dock: true 205 | QMainWindow State: 000000ff00000000fd000000040000000000000156000000f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000016000000f5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000016000003c5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002eb0000003efc0100000002fb0000000800540069006d00650000000000000002eb0000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000186000000f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 206 | Selection: 207 | collapsed: false 208 | Time: 209 | collapsed: false 210 | Tool Properties: 211 | collapsed: false 212 | Views: 213 | collapsed: false 214 | Width: 390 215 | X: 2878 216 | Y: 0 217 | -------------------------------------------------------------------------------- /rviz/pfilter_rviz.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 | - /Grid1 10 | Splitter Ratio: 0.529411792755127 11 | Tree Height: 363 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 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: false 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: false 55 | - Alpha: 1 56 | Axes Length: 8 57 | Axes Radius: 0.5 58 | Class: rviz/Pose 59 | Color: 114; 159; 207 60 | Enabled: true 61 | Head Length: 3 62 | Head Radius: 3 63 | Name: robot_pose 64 | Queue Size: 10 65 | Shaft Length: 6 66 | Shaft Radius: 0.800000011920929 67 | Shape: Arrow 68 | Topic: /pf/robot_pose 69 | Unreliable: false 70 | Value: true 71 | - Alpha: 1 72 | Axes Length: 1 73 | Axes Radius: 0.10000000149011612 74 | Class: rviz/Pose 75 | Color: 255; 25; 0 76 | Enabled: true 77 | Head Length: 3 78 | Head Radius: 3 79 | Name: odom_pose 80 | Queue Size: 10 81 | Shaft Length: 6 82 | Shaft Radius: 0.800000011920929 83 | Shape: Arrow 84 | Topic: /pf/odom 85 | Unreliable: false 86 | Value: true 87 | - Alpha: 0.800000011920929 88 | Axes Length: 1 89 | Axes Radius: 0.10000000149011612 90 | Class: rviz/Pose 91 | Color: 138; 226; 52 92 | Enabled: true 93 | Head Length: 3 94 | Head Radius: 3 95 | Name: groundtruth_pose 96 | Queue Size: 10 97 | Shaft Length: 6 98 | Shaft Radius: 0.800000011920929 99 | Shape: Arrow 100 | Topic: /pf/groundtruth_pose 101 | Unreliable: false 102 | Value: true 103 | - Alpha: 1 104 | Buffer Length: 1 105 | Class: rviz/Path 106 | Color: 114; 159; 207 107 | Enabled: true 108 | Head Diameter: 0.30000001192092896 109 | Head Length: 0.20000000298023224 110 | Length: 0.30000001192092896 111 | Line Style: Billboards 112 | Line Width: 0.30000001192092896 113 | Name: robot_path 114 | Offset: 115 | X: 0 116 | Y: 0 117 | Z: 0 118 | Pose Color: 255; 85; 255 119 | Pose Style: None 120 | Queue Size: 10 121 | Radius: 0.029999999329447746 122 | Shaft Diameter: 0.10000000149011612 123 | Shaft Length: 0.10000000149011612 124 | Topic: /pf/robot/path 125 | Unreliable: false 126 | Value: true 127 | - Alpha: 1 128 | Buffer Length: 1 129 | Class: rviz/Path 130 | Color: 25; 255; 0 131 | Enabled: true 132 | Head Diameter: 0.30000001192092896 133 | Head Length: 0.20000000298023224 134 | Length: 0.30000001192092896 135 | Line Style: Billboards 136 | Line Width: 0.30000001192092896 137 | Name: groundtruth_path 138 | Offset: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Pose Color: 255; 85; 255 143 | Pose Style: None 144 | Queue Size: 10 145 | Radius: 0.029999999329447746 146 | Shaft Diameter: 0.10000000149011612 147 | Shaft Length: 0.10000000149011612 148 | Topic: /pf/groundtruth/path 149 | Unreliable: false 150 | Value: true 151 | - Alpha: 1 152 | Buffer Length: 1 153 | Class: rviz/Path 154 | Color: 255; 25; 0 155 | Enabled: true 156 | Head Diameter: 0.30000001192092896 157 | Head Length: 0.20000000298023224 158 | Length: 0.30000001192092896 159 | Line Style: Billboards 160 | Line Width: 0.30000001192092896 161 | Name: odom_path 162 | Offset: 163 | X: 0 164 | Y: 0 165 | Z: 0 166 | Pose Color: 255; 85; 255 167 | Pose Style: None 168 | Queue Size: 10 169 | Radius: 0.029999999329447746 170 | Shaft Diameter: 0.10000000149011612 171 | Shaft Length: 0.10000000149011612 172 | Topic: /pf/odom/path 173 | Unreliable: false 174 | Value: true 175 | - Alpha: 1 176 | Arrow Length: 5 177 | Axes Length: 0.30000001192092896 178 | Axes Radius: 0.009999999776482582 179 | Class: rviz/PoseArray 180 | Color: 114; 159; 207 181 | Enabled: false 182 | Head Length: 0.07000000029802322 183 | Head Radius: 0.029999999329447746 184 | Name: PoseArray 185 | Queue Size: 10 186 | Shaft Length: 0.23000000417232513 187 | Shaft Radius: 0.009999999776482582 188 | Shape: Arrow (Flat) 189 | Topic: /pf/particles 190 | Unreliable: false 191 | Value: false 192 | - Class: rviz/MarkerArray 193 | Enabled: false 194 | Marker Topic: trunks_map_markers 195 | Name: MarkerArray 196 | Namespaces: 197 | {} 198 | Queue Size: 100 199 | Value: false 200 | - Class: rviz/MarkerArray 201 | Enabled: false 202 | Marker Topic: /trunks_extracted_world 203 | Name: MarkerArray 204 | Namespaces: 205 | {} 206 | Queue Size: 100 207 | Value: false 208 | - Alpha: 1 209 | Autocompute Intensity Bounds: true 210 | Autocompute Value Bounds: 211 | Max Value: 10 212 | Min Value: -10 213 | Value: true 214 | Axis: Z 215 | Channel Name: intensity 216 | Class: rviz/PointCloud2 217 | Color: 255; 255; 255 218 | Color Transformer: Intensity 219 | Decay Time: 0 220 | Enabled: false 221 | Invert Rainbow: false 222 | Max Color: 255; 255; 255 223 | Min Color: 0; 0; 0 224 | Name: PointCloud2 225 | Position Transformer: XYZ 226 | Queue Size: 10 227 | Selectable: true 228 | Size (Pixels): 3 229 | Size (m): 0.009999999776482582 230 | Style: Flat Squares 231 | Topic: subscan_cloud_world 232 | Unreliable: false 233 | Use Fixed Frame: true 234 | Use rainbow: true 235 | Value: false 236 | - Class: rviz/MarkerArray 237 | Enabled: true 238 | Marker Topic: /trunks_extracted_world/estimate 239 | Name: MarkerArray 240 | Namespaces: 241 | {} 242 | Queue Size: 100 243 | Value: true 244 | - Alpha: 1 245 | Arrow Length: 0.30000001192092896 246 | Axes Length: 0.30000001192092896 247 | Axes Radius: 0.009999999776482582 248 | Class: rviz/PoseArray 249 | Color: 173; 127; 168 250 | Enabled: true 251 | Head Length: 0.10000000149011612 252 | Head Radius: 0.10000000149011612 253 | Name: PoseArray 254 | Queue Size: 10 255 | Shaft Length: 0.10000000149011612 256 | Shaft Radius: 0.05000000074505806 257 | Shape: Arrow (3D) 258 | Topic: /pf/particles/max_weight 259 | Unreliable: false 260 | Value: true 261 | - Alpha: 1 262 | Autocompute Intensity Bounds: true 263 | Autocompute Value Bounds: 264 | Max Value: 10 265 | Min Value: -10 266 | Value: true 267 | Axis: Z 268 | Channel Name: intensity 269 | Class: rviz/PointCloud2 270 | Color: 255; 255; 255 271 | Color Transformer: RGB8 272 | Decay Time: 0 273 | Enabled: true 274 | Invert Rainbow: false 275 | Max Color: 255; 255; 255 276 | Min Color: 0; 0; 0 277 | Name: PointCloud2 278 | Position Transformer: XYZ 279 | Queue Size: 10 280 | Selectable: true 281 | Size (Pixels): 3 282 | Size (m): 0.10000000149011612 283 | Style: Flat Squares 284 | Topic: /particles_weights 285 | Unreliable: false 286 | Use Fixed Frame: true 287 | Use rainbow: true 288 | Value: true 289 | Enabled: true 290 | Global Options: 291 | Background Color: 0; 0; 0 292 | Default Light: true 293 | Fixed Frame: map 294 | Frame Rate: 30 295 | Name: root 296 | Tools: 297 | - Class: rviz/Interact 298 | Hide Inactive Objects: true 299 | - Class: rviz/MoveCamera 300 | - Class: rviz/Select 301 | - Class: rviz/FocusCamera 302 | - Class: rviz/Measure 303 | - Class: rviz/SetInitialPose 304 | Theta std deviation: 0.2617993950843811 305 | Topic: /initialpose 306 | X std deviation: 0.5 307 | Y std deviation: 0.5 308 | - Class: rviz/SetGoal 309 | Topic: /move_base_simple/goal 310 | - Class: rviz/PublishPoint 311 | Single click: true 312 | Topic: /clicked_point 313 | Value: true 314 | Views: 315 | Current: 316 | Angle: 15.650015830993652 317 | Class: rviz/TopDownOrtho 318 | Enable Stereo Rendering: 319 | Stereo Eye Separation: 0.05999999865889549 320 | Stereo Focal Distance: 1 321 | Swap Stereo Eyes: false 322 | Value: false 323 | Invert Z Axis: false 324 | Name: Current View 325 | Near Clip Distance: 0.009999999776482582 326 | Scale: -7.758509635925293 327 | Target Frame: 328 | X: 46.67472457885742 329 | Y: 13.184698104858398 330 | Saved: ~ 331 | Window Geometry: 332 | Displays: 333 | collapsed: true 334 | Height: 1107 335 | Hide Left Dock: true 336 | Hide Right Dock: false 337 | QMainWindow State: 000000ff00000000fd0000000400000000000002d400000458fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000001600000458000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003fe0000003efc0100000002fb0000000800540069006d00650000000000000003fe0000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000004c00000042000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 338 | Selection: 339 | collapsed: false 340 | Time: 341 | collapsed: false 342 | Tool Properties: 343 | collapsed: false 344 | Views: 345 | collapsed: false 346 | Width: 1216 347 | X: 1992 348 | Y: 0 349 | -------------------------------------------------------------------------------- /rviz/submap_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /MarkerArray1 11 | - /Marker1 12 | - /PointCloud23 13 | - /MarkerArray2 14 | - /PointCloud24 15 | - /PointCloud25 16 | - /PointCloud26 17 | - /PointCloud27 18 | - /PointCloud28 19 | - /PointCloud29 20 | Splitter Ratio: 0.520588219165802 21 | Tree Height: 441 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /2D Nav Goal1 28 | - /Publish Point1 29 | Name: Tool Properties 30 | Splitter Ratio: 0.5886790156364441 31 | - Class: rviz/Views 32 | Expanded: 33 | - /Current View1 34 | Name: Views 35 | Splitter Ratio: 0.5 36 | - Class: rviz/Time 37 | Name: Time 38 | SyncMode: 0 39 | SyncSource: sweep 40 | - Class: rviz/Displays 41 | Help Height: 70 42 | Name: Displays 43 | Property Tree Widget: 44 | Expanded: ~ 45 | Splitter Ratio: 0.5 46 | Tree Height: 371 47 | Preferences: 48 | PromptSaveOnExit: true 49 | Toolbars: 50 | toolButtonStyle: 2 51 | Visualization Manager: 52 | Class: "" 53 | Displays: 54 | - Alpha: 0.5 55 | Cell Size: 1 56 | Class: rviz/Grid 57 | Color: 160; 160; 164 58 | Enabled: true 59 | Line Style: 60 | Line Width: 0.029999999329447746 61 | Value: Lines 62 | Name: Grid 63 | Normal Cell Count: 0 64 | Offset: 65 | X: 0 66 | Y: 0 67 | Z: 0 68 | Plane: XY 69 | Plane Cell Count: 40 70 | Reference Frame: 71 | Value: true 72 | - Alpha: 1 73 | Autocompute Intensity Bounds: true 74 | Autocompute Value Bounds: 75 | Max Value: 10 76 | Min Value: -10 77 | Value: true 78 | Axis: Z 79 | Channel Name: intensity 80 | Class: rviz/PointCloud2 81 | Color: 255; 255; 255 82 | Color Transformer: Intensity 83 | Decay Time: 0 84 | Enabled: false 85 | Invert Rainbow: false 86 | Max Color: 255; 255; 255 87 | Min Color: 0; 0; 0 88 | Name: PointCloud2 89 | Position Transformer: XYZ 90 | Queue Size: 10 91 | Selectable: true 92 | Size (Pixels): 3 93 | Size (m): 0.05000000074505806 94 | Style: Flat Squares 95 | Topic: submap_cloud 96 | Unreliable: false 97 | Use Fixed Frame: true 98 | Use rainbow: true 99 | Value: false 100 | - Alpha: 1 101 | Buffer Length: 1 102 | Class: rviz/Path 103 | Color: 25; 255; 0 104 | Enabled: false 105 | Head Diameter: 0.30000001192092896 106 | Head Length: 0.20000000298023224 107 | Length: 0.30000001192092896 108 | Line Style: Lines 109 | Line Width: 0.029999999329447746 110 | Name: Path 111 | Offset: 112 | X: 0 113 | Y: 0 114 | Z: 0 115 | Pose Color: 255; 85; 255 116 | Pose Style: None 117 | Queue Size: 10 118 | Radius: 0.029999999329447746 119 | Shaft Diameter: 0.10000000149011612 120 | Shaft Length: 0.10000000149011612 121 | Topic: /sim/odom/path 122 | Unreliable: false 123 | Value: false 124 | - Class: rviz/MarkerArray 125 | Enabled: false 126 | Marker Topic: trunks_positions_markers 127 | Name: MarkerArray 128 | Namespaces: 129 | {} 130 | Queue Size: 100 131 | Value: false 132 | - Class: rviz/Marker 133 | Enabled: false 134 | Marker Topic: clusters_segmented 135 | Name: Marker 136 | Namespaces: 137 | {} 138 | Queue Size: 100 139 | Value: false 140 | - Alpha: 0.5 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz/PointCloud2 149 | Color: 255; 255; 255 150 | Color Transformer: Intensity 151 | Decay Time: 0 152 | Enabled: false 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Min Color: 0; 0; 0 156 | Name: PointCloud2 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 2 161 | Size (m): 0.019999999552965164 162 | Style: Points 163 | Topic: submap_cloud_input 164 | Unreliable: true 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: false 168 | - Class: rviz/Marker 169 | Enabled: false 170 | Marker Topic: visualization_marker 171 | Name: Marker 172 | Namespaces: 173 | {} 174 | Queue Size: 100 175 | Value: false 176 | - Alpha: 1 177 | Autocompute Intensity Bounds: true 178 | Autocompute Value Bounds: 179 | Max Value: 10 180 | Min Value: -10 181 | Value: true 182 | Axis: Z 183 | Channel Name: intensity 184 | Class: rviz/PointCloud2 185 | Color: 255; 255; 255 186 | Color Transformer: RGB8 187 | Decay Time: 0 188 | Enabled: false 189 | Invert Rainbow: false 190 | Max Color: 255; 255; 255 191 | Min Color: 0; 0; 0 192 | Name: PointCloud2 193 | Position Transformer: XYZ 194 | Queue Size: 10 195 | Selectable: true 196 | Size (Pixels): 3 197 | Size (m): 1 198 | Style: Spheres 199 | Topic: /pfilter_localization_node/centroids 200 | Unreliable: false 201 | Use Fixed Frame: true 202 | Use rainbow: true 203 | Value: false 204 | - Class: rviz/MarkerArray 205 | Enabled: false 206 | Marker Topic: visualization_marker_array 207 | Name: MarkerArray 208 | Namespaces: 209 | {} 210 | Queue Size: 100 211 | Value: false 212 | - Alpha: 1 213 | Autocompute Intensity Bounds: true 214 | Autocompute Value Bounds: 215 | Max Value: 10 216 | Min Value: -10 217 | Value: true 218 | Axis: Z 219 | Channel Name: intensity 220 | Class: rviz/PointCloud2 221 | Color: 255; 255; 255 222 | Color Transformer: "" 223 | Decay Time: 0 224 | Enabled: false 225 | Invert Rainbow: false 226 | Max Color: 255; 255; 255 227 | Min Color: 0; 0; 0 228 | Name: PointCloud2 229 | Position Transformer: "" 230 | Queue Size: 10 231 | Selectable: true 232 | Size (Pixels): 3 233 | Size (m): 0.10000000149011612 234 | Style: Flat Squares 235 | Topic: /pfilter_localization_node/input 236 | Unreliable: false 237 | Use Fixed Frame: true 238 | Use rainbow: true 239 | Value: false 240 | - Alpha: 1 241 | Autocompute Intensity Bounds: true 242 | Autocompute Value Bounds: 243 | Max Value: 10 244 | Min Value: -10 245 | Value: true 246 | Axis: Z 247 | Channel Name: intensity 248 | Class: rviz/PointCloud2 249 | Color: 255; 255; 255 250 | Color Transformer: "" 251 | Decay Time: 0 252 | Enabled: false 253 | Invert Rainbow: false 254 | Max Color: 255; 255; 255 255 | Min Color: 0; 0; 0 256 | Name: PointCloud2 257 | Position Transformer: "" 258 | Queue Size: 10 259 | Selectable: true 260 | Size (Pixels): 3 261 | Size (m): 0.10000000149011612 262 | Style: Flat Squares 263 | Topic: /pfilter_localization_node/forest 264 | Unreliable: false 265 | Use Fixed Frame: true 266 | Use rainbow: true 267 | Value: false 268 | - Alpha: 1 269 | Autocompute Intensity Bounds: true 270 | Autocompute Value Bounds: 271 | Max Value: 10 272 | Min Value: -10 273 | Value: true 274 | Axis: Z 275 | Channel Name: intensity 276 | Class: rviz/PointCloud2 277 | Color: 255; 255; 255 278 | Color Transformer: RGB8 279 | Decay Time: 0 280 | Enabled: false 281 | Invert Rainbow: false 282 | Max Color: 255; 255; 255 283 | Min Color: 0; 0; 0 284 | Name: PointCloud2 285 | Position Transformer: XYZ 286 | Queue Size: 10 287 | Selectable: true 288 | Size (Pixels): 3 289 | Size (m): 0.20000000298023224 290 | Style: Flat Squares 291 | Topic: /pfilter_localization_node/segments 292 | Unreliable: false 293 | Use Fixed Frame: true 294 | Use rainbow: true 295 | Value: false 296 | - Alpha: 1 297 | Autocompute Intensity Bounds: true 298 | Autocompute Value Bounds: 299 | Max Value: 10 300 | Min Value: -10 301 | Value: true 302 | Axis: Z 303 | Channel Name: intensity 304 | Class: rviz/PointCloud2 305 | Color: 255; 255; 255 306 | Color Transformer: RGB8 307 | Decay Time: 0 308 | Enabled: false 309 | Invert Rainbow: false 310 | Max Color: 255; 255; 255 311 | Min Color: 0; 0; 0 312 | Name: PointCloud2 313 | Position Transformer: XYZ 314 | Queue Size: 10 315 | Selectable: true 316 | Size (Pixels): 3 317 | Size (m): 0.10000000149011612 318 | Style: Flat Squares 319 | Topic: /submap_cloud_sor 320 | Unreliable: false 321 | Use Fixed Frame: true 322 | Use rainbow: true 323 | Value: false 324 | - Alpha: 1 325 | Autocompute Intensity Bounds: true 326 | Autocompute Value Bounds: 327 | Max Value: 10 328 | Min Value: -10 329 | Value: true 330 | Axis: Z 331 | Channel Name: intensity 332 | Class: rviz/PointCloud2 333 | Color: 255; 255; 255 334 | Color Transformer: Intensity 335 | Decay Time: 0 336 | Enabled: false 337 | Invert Rainbow: false 338 | Max Color: 255; 255; 255 339 | Min Color: 0; 0; 0 340 | Name: PointCloud2 341 | Position Transformer: XYZ 342 | Queue Size: 10 343 | Selectable: true 344 | Size (Pixels): 3 345 | Size (m): 0.10000000149011612 346 | Style: Flat Squares 347 | Topic: /ground_segmentation/ground_cloud 348 | Unreliable: false 349 | Use Fixed Frame: true 350 | Use rainbow: true 351 | Value: false 352 | - Alpha: 1 353 | Autocompute Intensity Bounds: true 354 | Autocompute Value Bounds: 355 | Max Value: 10 356 | Min Value: -10 357 | Value: true 358 | Axis: Z 359 | Channel Name: intensity 360 | Class: rviz/PointCloud2 361 | Color: 255; 255; 255 362 | Color Transformer: Intensity 363 | Decay Time: 0 364 | Enabled: false 365 | Invert Rainbow: false 366 | Max Color: 255; 255; 255 367 | Min Color: 0; 0; 0 368 | Name: PointCloud2 369 | Position Transformer: XYZ 370 | Queue Size: 10 371 | Selectable: true 372 | Size (Pixels): 3 373 | Size (m): 0.10000000149011612 374 | Style: Flat Squares 375 | Topic: /ground_grid 376 | Unreliable: false 377 | Use Fixed Frame: true 378 | Use rainbow: true 379 | Value: false 380 | - Alpha: 1 381 | Autocompute Intensity Bounds: true 382 | Autocompute Value Bounds: 383 | Max Value: 10 384 | Min Value: -10 385 | Value: true 386 | Axis: Z 387 | Channel Name: intensity 388 | Class: rviz/PointCloud2 389 | Color: 255; 255; 255 390 | Color Transformer: RGB8 391 | Decay Time: 0 392 | Enabled: false 393 | Invert Rainbow: false 394 | Max Color: 255; 255; 255 395 | Min Color: 0; 0; 0 396 | Name: PointCloud2 397 | Position Transformer: XYZ 398 | Queue Size: 10 399 | Selectable: true 400 | Size (Pixels): 3 401 | Size (m): 0.07999999821186066 402 | Style: Flat Squares 403 | Topic: /pfilter_localization_node/cloud_no_ground 404 | Unreliable: false 405 | Use Fixed Frame: true 406 | Use rainbow: true 407 | Value: false 408 | - Alpha: 1 409 | Class: rviz/RobotModel 410 | Collision Enabled: false 411 | Enabled: false 412 | Links: 413 | All Links Enabled: true 414 | Expand Joint Details: false 415 | Expand Link Details: false 416 | Expand Tree: false 417 | Link Tree Style: Links in Alphabetic Order 418 | Name: RobotModel 419 | Robot Description: robot_description 420 | TF Prefix: "" 421 | Update Interval: 0 422 | Value: false 423 | Visual Enabled: true 424 | - Alpha: 1 425 | Autocompute Intensity Bounds: true 426 | Autocompute Value Bounds: 427 | Max Value: 10 428 | Min Value: -10 429 | Value: true 430 | Axis: Z 431 | Channel Name: intensity 432 | Class: rviz/PointCloud2 433 | Color: 255; 255; 255 434 | Color Transformer: RGB8 435 | Decay Time: 0 436 | Enabled: false 437 | Invert Rainbow: false 438 | Max Color: 255; 255; 255 439 | Min Color: 0; 0; 0 440 | Name: PointCloud2 441 | Position Transformer: XYZ 442 | Queue Size: 10 443 | Selectable: true 444 | Size (Pixels): 3 445 | Size (m): 0.10000000149011612 446 | Style: Flat Squares 447 | Topic: /pfilter_localization_node/cloud_ground 448 | Unreliable: false 449 | Use Fixed Frame: true 450 | Use rainbow: true 451 | Value: false 452 | - Alpha: 1 453 | Autocompute Intensity Bounds: true 454 | Autocompute Value Bounds: 455 | Max Value: 10 456 | Min Value: -10 457 | Value: true 458 | Axis: Z 459 | Channel Name: intensity 460 | Class: rviz/PointCloud2 461 | Color: 252; 233; 79 462 | Color Transformer: FlatColor 463 | Decay Time: 0 464 | Enabled: false 465 | Invert Rainbow: false 466 | Max Color: 255; 255; 255 467 | Min Color: 0; 0; 0 468 | Name: PointCloud2 469 | Position Transformer: XYZ 470 | Queue Size: 10 471 | Selectable: true 472 | Size (Pixels): 3 473 | Size (m): 0.5 474 | Style: Flat Squares 475 | Topic: /height_cloud 476 | Unreliable: false 477 | Use Fixed Frame: true 478 | Use rainbow: true 479 | Value: false 480 | - Alpha: 1 481 | Autocompute Intensity Bounds: true 482 | Autocompute Value Bounds: 483 | Max Value: 10 484 | Min Value: -10 485 | Value: true 486 | Axis: Z 487 | Channel Name: intensity 488 | Class: rviz/PointCloud2 489 | Color: 255; 255; 255 490 | Color Transformer: "" 491 | Decay Time: 0 492 | Enabled: false 493 | Invert Rainbow: false 494 | Max Color: 255; 255; 255 495 | Min Color: 0; 0; 0 496 | Name: PointCloud2 497 | Position Transformer: "" 498 | Queue Size: 10 499 | Selectable: true 500 | Size (Pixels): 3 501 | Size (m): 0.5 502 | Style: Flat Squares 503 | Topic: /height_map_low_cloud 504 | Unreliable: false 505 | Use Fixed Frame: true 506 | Use rainbow: true 507 | Value: false 508 | - Alpha: 1 509 | Autocompute Intensity Bounds: true 510 | Autocompute Value Bounds: 511 | Max Value: 10 512 | Min Value: -10 513 | Value: true 514 | Axis: Z 515 | Channel Name: intensity 516 | Class: rviz/PointCloud2 517 | Color: 255; 255; 255 518 | Color Transformer: RGB8 519 | Decay Time: 0 520 | Enabled: true 521 | Invert Rainbow: false 522 | Max Color: 255; 255; 255 523 | Min Color: 0; 0; 0 524 | Name: segmented_trees 525 | Position Transformer: XYZ 526 | Queue Size: 10 527 | Selectable: true 528 | Size (Pixels): 3 529 | Size (m): 0.30000001192092896 530 | Style: Flat Squares 531 | Topic: /trees_clusters_segments 532 | Unreliable: false 533 | Use Fixed Frame: true 534 | Use rainbow: true 535 | Value: true 536 | - Alpha: 1 537 | Autocompute Intensity Bounds: true 538 | Autocompute Value Bounds: 539 | Max Value: 10 540 | Min Value: -10 541 | Value: true 542 | Axis: Z 543 | Channel Name: intensity 544 | Class: rviz/PointCloud2 545 | Color: 255; 255; 255 546 | Color Transformer: Intensity 547 | Decay Time: 0 548 | Enabled: true 549 | Invert Rainbow: false 550 | Max Color: 255; 255; 255 551 | Min Color: 0; 0; 0 552 | Name: sweep 553 | Position Transformer: XYZ 554 | Queue Size: 10 555 | Selectable: true 556 | Size (Pixels): 3 557 | Size (m): 0.10000000149011612 558 | Style: Flat Squares 559 | Topic: /downsampled_sweep 560 | Unreliable: false 561 | Use Fixed Frame: true 562 | Use rainbow: true 563 | Value: true 564 | - Class: rviz/MarkerArray 565 | Enabled: true 566 | Marker Topic: /trees_positions_markers 567 | Name: trees_axis 568 | Namespaces: 569 | trunks_extracted: true 570 | Queue Size: 100 571 | Value: true 572 | - Alpha: 1 573 | Class: rviz/Axes 574 | Enabled: true 575 | Length: 1 576 | Name: Axes 577 | Radius: 0.10000000149011612 578 | Reference Frame: 579 | Show Trail: false 580 | Value: true 581 | - Alpha: 1 582 | Autocompute Intensity Bounds: true 583 | Autocompute Value Bounds: 584 | Max Value: 10 585 | Min Value: -10 586 | Value: true 587 | Axis: Z 588 | Channel Name: intensity 589 | Class: rviz/PointCloud2 590 | Color: 255; 255; 255 591 | Color Transformer: Intensity 592 | Decay Time: 0 593 | Enabled: false 594 | Invert Rainbow: false 595 | Max Color: 255; 255; 255 596 | Min Color: 0; 0; 0 597 | Name: PointCloud2 598 | Position Transformer: XYZ 599 | Queue Size: 10 600 | Selectable: true 601 | Size (Pixels): 3 602 | Size (m): 0.20000000298023224 603 | Style: Flat Squares 604 | Topic: /cloud 605 | Unreliable: false 606 | Use Fixed Frame: true 607 | Use rainbow: true 608 | Value: false 609 | Enabled: true 610 | Global Options: 611 | Background Color: 0; 0; 0 612 | Default Light: true 613 | Fixed Frame: base_link 614 | Frame Rate: 30 615 | Name: root 616 | Tools: 617 | - Class: rviz/Interact 618 | Hide Inactive Objects: true 619 | - Class: rviz/MoveCamera 620 | - Class: rviz/Select 621 | - Class: rviz/FocusCamera 622 | - Class: rviz/Measure 623 | - Class: rviz/SetInitialPose 624 | Theta std deviation: 0.2617993950843811 625 | Topic: /initialpose 626 | X std deviation: 0.5 627 | Y std deviation: 0.5 628 | - Class: rviz/SetGoal 629 | Topic: /move_base_simple/goal 630 | - Class: rviz/PublishPoint 631 | Single click: true 632 | Topic: /clicked_point 633 | Value: true 634 | Views: 635 | Current: 636 | Class: rviz/XYOrbit 637 | Distance: 30.9471435546875 638 | Enable Stereo Rendering: 639 | Stereo Eye Separation: 0.05999999865889549 640 | Stereo Focal Distance: 1 641 | Swap Stereo Eyes: false 642 | Value: false 643 | Field of View: 0.7853981852531433 644 | Focal Point: 645 | X: -10.239376068115234 646 | Y: -3.683556079864502 647 | Z: 3.083961200900376e-05 648 | Focal Shape Fixed Size: true 649 | Focal Shape Size: 0.05000000074505806 650 | Invert Z Axis: false 651 | Name: Current View 652 | Near Clip Distance: 0.009999999776482582 653 | Pitch: 0.489784836769104 654 | Target Frame: 655 | Yaw: 3.7199387550354004 656 | Saved: ~ 657 | Window Geometry: 658 | Displays: 659 | collapsed: false 660 | Height: 889 661 | Hide Left Dock: true 662 | Hide Right Dock: true 663 | QMainWindow State: 000000ff00000000fd00000004000000000000034000000252fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c00610079007300000003d600000144000000c900ffffff0000000100000198000004ddfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004dd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a90000003efc0100000002fb0000000800540069006d00650100000000000004a9000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004a9000002db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 664 | Selection: 665 | collapsed: false 666 | Time: 667 | collapsed: false 668 | Tool Properties: 669 | collapsed: false 670 | Views: 671 | collapsed: true 672 | Width: 1193 673 | X: 153 674 | Y: 57 675 | -------------------------------------------------------------------------------- /src/config_server.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _CONFIG_SERVER_H 10 | #define _CONFIG_SERVER_H 11 | 12 | #include 13 | 14 | #include 15 | 16 | class ConfigServer 17 | { 18 | ros::NodeHandle node_; 19 | ros::NodeHandle private_node_; 20 | std::stringstream ss_config_; 21 | 22 | 23 | 24 | public: 25 | 26 | ConfigServer() 27 | { 28 | 29 | } 30 | 31 | ConfigServer(ros::NodeHandle node, ros::NodeHandle private_node): node_(node), private_node_(private_node) 32 | { 33 | getRosParam(private_node_,"root_folder", root_folder_, ""); 34 | getRosParam(private_node_,"save_folder",save_folder_, "~/Documents/"); 35 | getRosParam(private_node_,"dataset_name", dataset_name_, ""); 36 | 37 | getRosParam(private_node_,"traj_search_window", traj_search_window_, 10); 38 | getRosParam(private_node_,"groundtruth_trajectory_name", groundtruth_trajectory_name_, ""); 39 | 40 | getRosParam(private_node_,"ground_z_down_threshold", ground_z_down_threshold_, 1.3); 41 | getRosParam(private_node_,"ground_z_up_threshold", ground_z_up_threshold_, 4.5); 42 | getRosParam(private_node_,"tree_height", tree_height_, 3.0); 43 | getRosParam(private_node_,"tolerance", tolerance_, 0.45); 44 | getRosParam(private_node_,"min_cluster_size", min_cluster_size_, 50); 45 | getRosParam(private_node_,"max_cluster_size", max_cluster_size_, 20000); 46 | getRosParam(private_node_,"downsample_voxel_size", downsample_voxel_size_, 0.25); 47 | getRosParam(private_node_,"base_frame", base_frame_, "base_link"); 48 | getRosParam(private_node_,"map_frame", map_frame_, "map"); 49 | 50 | getRosParam(private_node_,"imu_topic", imu_topic_, "imu"); 51 | getRosParam(private_node_,"gator_data_topic", gator_data_topic_, "gator_info"); 52 | 53 | getRosParam(private_node_,"distance_threshold_stopped", distance_thresh_, 0.01); 54 | getRosParam(private_node_,"angle_threshold_stopped", angle_thresh_, 5*M_PI/180); 55 | getRosParam(private_node_,"velocity_threshold_stopped", avg_velocity_thresh_, 0.3); 56 | 57 | getRosParam(private_node_,"number_particles", numParticles_, 1000); 58 | 59 | getRosParam(private_node_,"odom_alpha1", alpha1_, 0.2); 60 | getRosParam(private_node_,"odom_alpha2", alpha2_, 0.2); 61 | getRosParam(private_node_,"odom_alpha3", alpha3_, 0.3); 62 | getRosParam(private_node_,"odom_alpha4", alpha4_, 0.2); 63 | 64 | std::vector init_pose = {0, 0, 0}; 65 | getRosParam(private_node_,"initial_pose", initial_pose_, init_pose); 66 | 67 | std::vector empty; 68 | getRosParam(private_node_, "trajectory_rotation", trajectory_rotation_, empty); 69 | getRosParam(private_node_, "trajectory_translation", trajectory_translation_, empty); 70 | 71 | getRosParam(private_node_,"laser_accumulate_time", laser_accum_time_, 1.5); 72 | getRosParam(private_node_,"compensate_cloud", compensate_cloud_, false); 73 | 74 | getRosParam(private_node_,"particle_range_x", particle_range_x_, 1.0); 75 | getRosParam(private_node_,"particle_range_y", particle_range_y_, 1.0); 76 | 77 | getRosParam(private_node_,"min_landmarks_threshold",min_landmarks_threshold_, 2); 78 | 79 | getRosParam(private_node_,"map_file_name",map_file_name_, ""); 80 | getRosParam(private_node_,"map_image_grid_size",grid_size_, 0.25); 81 | getRosParam(private_node_,"map_image_max_height",map_max_height_, 34.28); 82 | getRosParam(private_node_,"map_image_frame_x",map_image_frame_x_, -228.84); 83 | getRosParam(private_node_,"map_image_frame_y",map_image_frame_y_, 168.46); 84 | 85 | getRosParam(private_node_,"ssd_beta",ssd_beta_, 1.0); 86 | getRosParam(private_node_,"crown_radius",crown_radius_, 2.0); 87 | 88 | getRosParam(private_node_,"wheels_axis_distance",wheels_axis_distance_, 1.96); 89 | 90 | getRosParam(private_node_,"number_of_threads",num_threads_, 4); 91 | 92 | std::vector map_display_window_default; 93 | getRosParam(private_node_,"map_display_window",map_display_window_, map_display_window_default); 94 | 95 | std::vector default_color = {255,0,0}; 96 | getRosParam(private_node_,"trunks_color",trunks_color_, default_color); 97 | 98 | getRosParam(private_node_,"using_lidarodom_model",using_lidarodom_model_, false); 99 | getRosParam(private_node_,"lidarodom_model",lidarodom_model_, ""); 100 | getRosParam(private_node_,"input_lidar_topic",input_lidar_topic_, "cloud"); 101 | 102 | } 103 | 104 | template void getRosParam(ros::NodeHandle nh, std::string param_field, T& variable, T default_value) 105 | { 106 | nh.param(param_field, variable, default_value); 107 | std::cout << param_field << ": " << variable <<"\n"; 108 | ss_config_< void getRosParam(ros::NodeHandle nh, std::string param_field, std::vector& variable, std::vector default_value) 112 | { 113 | nh.param>(param_field, variable, default_value); 114 | 115 | std::cout << param_field << ": ["; 116 | ss_config_< initial_pose_; 164 | 165 | std::vector trajectory_rotation_; 166 | std::vector trajectory_translation_; 167 | 168 | std::string root_folder_; 169 | std::string dataset_name_; 170 | std::string groundtruth_trajectory_name_; 171 | int traj_search_window_; 172 | 173 | bool compensate_cloud_; 174 | double laser_accum_time_; 175 | double particle_range_x_; 176 | double particle_range_y_; 177 | 178 | std::string save_folder_; 179 | 180 | int min_landmarks_threshold_; 181 | 182 | //ACM map param 183 | std::string map_file_name_; 184 | double grid_size_; 185 | double map_max_height_; 186 | double map_image_frame_x_; 187 | double map_image_frame_y_; 188 | 189 | double ssd_beta_; 190 | double crown_radius_; 191 | 192 | int num_threads_; 193 | std::vector map_display_window_; 194 | std::vector trunks_color_; 195 | 196 | bool using_lidarodom_model_; 197 | std::string lidarodom_model_; 198 | 199 | 200 | }; 201 | #endif -------------------------------------------------------------------------------- /src/gator_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "forest_localisation/gator_data.h" 2 | #include "gator1hal/gator_feedback.h" 3 | 4 | #include 5 | 6 | static ros::Publisher publish_gator_data_; 7 | 8 | static void gatorReceivedCall(const gator1hal::gator_feedbackConstPtr &feedback_msg) 9 | { 10 | forest_localisation::gator_data gator_msg; 11 | gator_msg.header.frame_id = "gator_base_link"; 12 | gator_msg.header.seq = feedback_msg->header.seq; 13 | gator_msg.header.stamp = feedback_msg->header.stamp; 14 | gator_msg.inst_velocity = feedback_msg->inst_velocity; 15 | gator_msg.steering_angle = feedback_msg->steer_angle; 16 | publish_gator_data_.publish(gator_msg); 17 | } 18 | 19 | int 20 | main(int argc, char** argv) 21 | { 22 | ros::init(argc, argv, "gator_handler_node"); 23 | ros::NodeHandle n; 24 | 25 | ros::NodeHandle private_node = ros::NodeHandle ("~"); 26 | 27 | publish_gator_data_ = n.advertise("/gator_info",1); 28 | ros::Subscriber gator_feedback_sub_ = n.subscribe("gator1HAL/feedback",30, gatorReceivedCall); 29 | 30 | ros::spin(); 31 | 32 | 33 | 34 | 35 | return(0); 36 | } 37 | 38 | -------------------------------------------------------------------------------- /src/ground_extraction.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include "ground_extraction.h" 10 | 11 | 12 | GroundExtraction::GroundExtraction(ros::NodeHandle& nh, ros::NodeHandle& nh_private) 13 | { 14 | //Code partially extracted from the linefit_ground_segmentation package (https://github.com/lorenwel/linefit_ground_segmentation) 15 | nh_private.param("ground_output_topic", ground_topic_, ""); 16 | nh_private.param("obstacle_output_topic", obstacle_topic_, ""); 17 | nh_private.param("latch", latch_, false); 18 | 19 | ground_pub_ = nh_private.advertise>(ground_topic_, 1, latch_); 20 | obstacle_pub_ = nh_private.advertise>(obstacle_topic_, 1, latch_); 21 | 22 | //linefit_ground_segmentation params 23 | nh_private.param("visualize", params_.visualize, params_.visualize); 24 | nh_private.param("n_bins", params_.n_bins, params_.n_bins); 25 | nh_private.param("n_segments", params_.n_segments, params_.n_segments); 26 | nh_private.param("max_dist_to_line", params_.max_dist_to_line, params_.max_dist_to_line); 27 | nh_private.param("max_slope", params_.max_slope, params_.max_slope); 28 | nh_private.param("min_slope", params_.min_slope, params_.min_slope); 29 | nh_private.param("long_threshold", params_.long_threshold, params_.long_threshold); 30 | nh_private.param("max_long_height", params_.max_long_height, params_.max_long_height); 31 | nh_private.param("max_start_height", params_.max_start_height, params_.max_start_height); 32 | nh_private.param("sensor_height", params_.sensor_height, params_.sensor_height); 33 | nh_private.param("line_search_angle", params_.line_search_angle, params_.line_search_angle); 34 | nh_private.param("n_threads", params_.n_threads, params_.n_threads); 35 | 36 | double r_min, r_max, max_fit_error; 37 | if (nh_private.getParam("r_min", r_min)) { 38 | params_.r_min_square = r_min*r_min; 39 | } 40 | if (nh_private.getParam("r_max", r_max)) { 41 | params_.r_max_square = r_max*r_max; 42 | } 43 | if (nh_private.getParam("max_fit_error", max_fit_error)) { 44 | params_.max_error_square = max_fit_error * max_fit_error; 45 | } 46 | 47 | } 48 | 49 | void GroundExtraction::extractGround(const pcl::PointCloud& cloud) { 50 | 51 | if(ground_cloud_.points.size()>0) 52 | ground_cloud_.points.clear(); 53 | if(no_ground_cloud_.points.size()>0) 54 | no_ground_cloud_.clear(); 55 | 56 | GroundSegmentation segmenter(params_); 57 | pcl::PointCloud cloud_transformed; 58 | 59 | std::vector labels; 60 | 61 | const pcl::PointCloud& cloud_proc = cloud; 62 | 63 | segmenter.segment(cloud, &labels); 64 | 65 | ground_cloud_.header = cloud.header; 66 | no_ground_cloud_.header = cloud.header; 67 | 68 | for (size_t i = 0; i < cloud.size(); ++i) { 69 | if (labels[i] == 1) ground_cloud_.push_back(cloud[i]); 70 | else no_ground_cloud_.push_back(cloud[i]); 71 | } 72 | 73 | std::cout<<"Number of ground points: "< &cloud, pcl::PointCloud &cloud_out, double z_min, double z_max) 80 | { 81 | 82 | if(cloud_ground_grid_.points.size()==0) 83 | { 84 | ROS_WARN("Ground not detected: returning same pointcloud from split function"); 85 | pcl::copyPointCloud(cloud, cloud_out); 86 | return; 87 | } 88 | 89 | cloud_out.points.reserve(cloud.points.size()); 90 | 91 | 92 | for(int k=0;k=0 && j>=0 && i z_min && height_diff < z_max) 108 | cloud_out.points.push_back(point); 109 | } 110 | } 111 | 112 | //If there is no ground point to compare get the cloud point anyway 113 | if(!has_ground) 114 | cloud_out.points.push_back(point); 115 | 116 | } 117 | 118 | cloud_out.height = 1; 119 | cloud_out.width = cloud_out.points.size(); 120 | 121 | } 122 | 123 | 124 | 125 | void GroundExtraction::getLowGroundCloud() 126 | { 127 | //Get a grid of points (extracted from the ground segmentation) with lower heights 128 | 129 | if(ground_cloud_.points.size() < 10) 130 | { 131 | ROS_WARN("Ground points returned less than 10 points. Low cloud cannot be computed"); 132 | cloud_ground_grid_.clear(); 133 | return; 134 | } 135 | 136 | pcl::PointXYZ minPt, maxPt; 137 | pcl::getMinMax3D(ground_cloud_,minPt,maxPt); 138 | 139 | //Bottom left point used as reference to index the grid (cloud_ground_grid_) 140 | local_frame_[0] = minPt.x; 141 | local_frame_[1] = minPt.y; 142 | 143 | double grid_max_x_ = maxPt.x; 144 | double grid_min_x_ = minPt.x; 145 | double grid_max_y_ = maxPt.y; 146 | double grid_min_y_ = minPt.y; 147 | 148 | grid_size_ = 2.0; 149 | height_rows_ = (int)ceil((grid_max_y_-grid_min_y_)/grid_size_); 150 | height_cols_ = (int)ceil((grid_max_x_-grid_min_x_)/grid_size_); 151 | 152 | if(height_rows_<2 || height_cols_<2) 153 | { 154 | ROS_WARN("Ground points returned grid has dimensions lower than 2: Low cloud not conputed"); 155 | cloud_ground_grid_.clear(); 156 | return; 157 | } 158 | 159 | cloud_ground_grid_ = pcl::PointCloud(height_cols_,height_rows_,pcl::PointXYZ(0,0,1e10)); 160 | 161 | for(auto &point: ground_cloud_.points) 162 | { 163 | 164 | int i = int((point.y - local_frame_[1])/grid_size_); 165 | int j = int((point.x - local_frame_[0])/grid_size_); 166 | 167 | if(i<0 || j<0 || i>=height_rows_ || j>=height_cols_) 168 | { 169 | printf("i , j (%i, %i) \n", i, j); 170 | printf("rows , cols (%i, %i) \n", height_rows_, height_cols_); 171 | printf("corners minx %0.2f, maxx %0.2f, miny %0.2f, maxy %0.2f \n", grid_min_x_,grid_max_x_, grid_min_y_, grid_max_y_); 172 | // printf("highcloud minx %0.2f, maxx %0.2f, miny %0.2f, maxy %0.2f \n", min_x,max_x, min_y, max_y); 173 | } 174 | 175 | cloud_ground_grid_.at(j,i).z = std::min(cloud_ground_grid_.at(j,i).z,point.z); 176 | } 177 | 178 | for(int i=0; i 13 | #include 14 | #include 15 | 16 | #include "ground_segmentation/ground_segmentation.h" 17 | 18 | //Interface class to utilise the linefit_ground_segmentation package 19 | class GroundExtraction { 20 | 21 | ros::Publisher ground_pub_; 22 | ros::Publisher obstacle_pub_; 23 | GroundSegmentationParams params_; 24 | 25 | public: 26 | GroundExtraction(ros::NodeHandle& nh, ros::NodeHandle& nh_private); 27 | 28 | void extractGround(const pcl::PointCloud& cloud); 29 | void getLowGroundCloud(); 30 | void splitCloud(const pcl::PointCloud &cloud, pcl::PointCloud &cloud_out, double z_min, double z_max); 31 | 32 | 33 | std::string ground_topic_, obstacle_topic_; 34 | bool latch_; 35 | pcl::PointCloud ground_cloud_; 36 | pcl::PointCloud no_ground_cloud_; 37 | 38 | //Ground grid parameters 39 | pcl::PointCloud cloud_ground_grid_; 40 | Eigen::Vector2d local_frame_; 41 | int height_rows_; 42 | int height_cols_; 43 | double grid_size_; 44 | 45 | }; 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /src/imu_interface.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _IMU_INTERFACE_H 10 | #define _IMU_INTERFACE_H 11 | 12 | #include 13 | #include 14 | #include "config_server.h" 15 | 16 | class ImuInterface 17 | { 18 | public: 19 | 20 | ros::NodeHandle node_; 21 | ros::NodeHandle private_node_; 22 | std::shared_ptr config_; 23 | 24 | ros::Subscriber sub_imu; 25 | 26 | sensor_msgs::Imu last_imu_; 27 | sensor_msgs::Imu current_imu_; 28 | 29 | ros::Time current_time_; 30 | ros::Time last_time_; 31 | 32 | ImuInterface(ros::NodeHandle node, ros::NodeHandle private_node, std::shared_ptr &config): node_(node), private_node_(private_node) 33 | { 34 | config_ = config; 35 | sub_imu = node_.subscribe (config_->imu_topic_, 2000, &ImuInterface::ImuInterfaceSub, this, ros::TransportHints().tcpNoDelay()); 36 | } 37 | 38 | 39 | void ImuInterfaceSub(const sensor_msgs::Imu::ConstPtr& imu_raw) 40 | { 41 | last_imu_ = current_imu_; 42 | last_time_ = current_time_; 43 | 44 | current_imu_ = *imu_raw; 45 | current_time_ = current_imu_.header.stamp; 46 | } 47 | 48 | 49 | }; 50 | 51 | 52 | #endif -------------------------------------------------------------------------------- /src/localization_node.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include 10 | #include "pfilter_localization.h" 11 | 12 | 13 | int 14 | main(int argc, char** argv) 15 | { 16 | ros::init(argc, argv, "pfilter_localization_node"); 17 | ros::NodeHandle n; 18 | 19 | ros::NodeHandle private_node = ros::NodeHandle ("~"); 20 | 21 | PfilterLocalization pf_localization(n, private_node); 22 | 23 | while(ros::ok()) 24 | { 25 | ros::Duration(0.1).sleep(); 26 | ros::spinOnce(); 27 | 28 | } 29 | 30 | pf_localization.saveData(); 31 | 32 | 33 | 34 | return(0); 35 | } 36 | -------------------------------------------------------------------------------- /src/map.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include "map.h" 10 | 11 | ImageMap::ImageMap() 12 | { 13 | 14 | } 15 | 16 | ImageMap::~ImageMap() 17 | { 18 | 19 | } 20 | 21 | bool ImageMap::loadMapImage(const string& file_name, double image_ref_x, double image_ref_y, double grid_size, double max_height) 22 | { 23 | //Load the ACM image map and the respective global reference frame (x,y) coordinates 24 | image_ = cv::imread(file_name,cv::IMREAD_GRAYSCALE); 25 | if(image_.empty()) 26 | { 27 | std::cout << "Could not read the image: " << file_name << std::endl; 28 | return false; 29 | } 30 | 31 | image_frame_x_ = image_ref_x; 32 | image_frame_y_ = image_ref_y; 33 | grid_size_ = grid_size; 34 | max_height_ = max_height; 35 | 36 | ROS_INFO("ACM Map data loaded"); 37 | 38 | return true; 39 | } 40 | 41 | -------------------------------------------------------------------------------- /src/map.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _MAP_H 10 | #define _MAP_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include "opencv2/imgcodecs.hpp" 16 | 17 | #include 18 | using namespace std; 19 | 20 | class ImageMap 21 | { 22 | public: 23 | ImageMap(); 24 | ~ImageMap(); 25 | 26 | bool loadMapImage(const string& file_name, double image_ref_x, double image_ref_y, double grid_size, double max_height); 27 | 28 | cv::Mat image_; 29 | double min_x_,min_y_,max_x_,max_y_; 30 | double grid_size_; 31 | double image_frame_x_; 32 | double image_frame_y_; 33 | double max_height_; 34 | 35 | 36 | }; 37 | 38 | #endif -------------------------------------------------------------------------------- /src/pfilter_localization.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include "pfilter_localization.h" 10 | 11 | PfilterLocalization::PfilterLocalization(ros::NodeHandle node, ros::NodeHandle private_node) 12 | { 13 | 14 | node_ = node; 15 | private_node_ = private_node; 16 | 17 | config_.reset(new ConfigServer(node_,private_node_)); 18 | visualisation_.reset(new Visualisation(node_,private_node_,config_)); 19 | imu_interface_.reset(new ImuInterface(node_, private_node_,config_)); 20 | tree_segmentation_.reset(new TreeSegmentation(node_, private_node_, config_)); 21 | 22 | car_model = CarLikeModel(config_->wheels_axis_distance_); 23 | 24 | map_frame_ = config_->map_frame_; 25 | base_frame_ = config_->base_frame_; 26 | 27 | 28 | if(config_->num_threads_ > 0) 29 | pfilter_threads_.resize(config_->num_threads_); 30 | 31 | 32 | ucm_images_buffer_.resize(config_->numParticles_); 33 | local_frames_buffer_.resize(config_->numParticles_); 34 | 35 | last_odom_pose_.x = 0.0; 36 | last_odom_pose_.y = 0.0; 37 | last_odom_pose_.theta = 0.0; 38 | 39 | odom_pose_.x = 0.0; 40 | odom_pose_.y = 0.0; 41 | odom_pose_.theta = 0.0; 42 | 43 | robot_pose_.x = 0; 44 | robot_pose_.y = 0; 45 | robot_pose_.theta = 0; 46 | 47 | groundtruth_pose_.x = 0; 48 | groundtruth_pose_.y = 0; 49 | groundtruth_pose_.theta = 0; 50 | 51 | set_start_position_ = false; 52 | first_scan = true; 53 | 54 | random_gen.seed(random_dv()); 55 | 56 | robot_moved_ = false; 57 | 58 | current_sweep_time_ = 0; 59 | 60 | first_gator_fb = true; 61 | 62 | 63 | string groundtruth_trajectory_file; 64 | groundtruth_trajectory_file = config_->root_folder_ + config_->dataset_name_ + "/" + config_->groundtruth_trajectory_name_; 65 | 66 | if(groundtruth_trajectory_file.size()>0) 67 | { 68 | TrajectoryData::readTrajectoryFile(groundtruth_trajectory_file,config_->trajectory_rotation_,config_->trajectory_translation_, groundtruth_trajectory_); 69 | } 70 | else 71 | ROS_WARN("Groundtruth Trajectory not loaded: trajectory_file param not provided"); 72 | 73 | 74 | laser_cloud_sub_ = node_.subscribe(config_->input_lidar_topic_, 100, &PfilterLocalization::laserCloudReceived, this); 75 | gator_info_sub_ = node_.subscribe(config_->gator_data_topic_,30, &PfilterLocalization::gatorInfoWheelOdom, this); 76 | 77 | publish_pose_ = node_.advertise("/pf/robot_pose",1); 78 | publish_gt_pose_ = node_.advertise("/pf/groundtruth_pose",1); 79 | publish_odom_pose_ = node_.advertise("/pf/odom",1); 80 | publish_max_weight_poses_ = node_.advertise("/pf/particles/max_weight",1); 81 | 82 | publish_pose_path_ = node_.advertise("/pf/robot/path",1); 83 | publish_groundtruth_path_ = node_.advertise("/pf/groundtruth/path",1); 84 | publish_odom_pose_path_ = node_.advertise("/pf/odom/path",1); 85 | 86 | publish_particles_update_ = node_.advertise("/particles_update",1); 87 | publish_particles_resample_ = node_.advertise("/particles_resample",1); 88 | 89 | if(config_->map_file_name_.size()>0) 90 | { 91 | std::string acm_file = config_->root_folder_ + config_->dataset_name_ + "/" + config_->map_file_name_; 92 | image_map_.loadMapImage(acm_file, config_->map_image_frame_x_, config_->map_image_frame_y_, config_->grid_size_, config_->map_max_height_); 93 | } 94 | else 95 | ROS_WARN("ACM Map image not provided"); 96 | 97 | ROS_INFO("Pfilter node initialized"); 98 | 99 | } 100 | 101 | PfilterLocalization::~PfilterLocalization() 102 | { 103 | 104 | } 105 | 106 | void PfilterLocalization::setInitialPoses() 107 | { 108 | if(set_start_position_) 109 | { 110 | pose2D_t initial_pose(config_->initial_pose_[0], config_->initial_pose_[1], config_->initial_pose_[2]); 111 | 112 | odom_pose_.x = initial_pose.x; 113 | odom_pose_.y = initial_pose.y; 114 | odom_pose_.theta = initial_pose.theta; 115 | last_odom_pose_.x = initial_pose.x; 116 | last_odom_pose_.y = initial_pose.y; 117 | last_odom_pose_.theta = initial_pose.theta; 118 | 119 | printf("pose 0 = %.2f, %.2f \n", initial_pose.x, initial_pose.y); 120 | 121 | robot_pose_.x = initial_pose.x; 122 | robot_pose_.y = initial_pose.y; 123 | robot_pose_.theta = initial_pose.theta; 124 | 125 | InitParticles(initial_pose); 126 | 127 | set_start_position_ = false; 128 | 129 | ROS_INFO("Setting robot initial position"); 130 | 131 | } 132 | } 133 | 134 | void PfilterLocalization::laserCloudReceived(const sensor_msgs::PointCloud2ConstPtr& laser_scan) 135 | { 136 | bool generated_sweep = false; 137 | generated_sweep = accumulateLidarScansWheel(laser_scan); 138 | 139 | if(generated_sweep) 140 | { 141 | extractTreeTrunks(); 142 | 143 | updateGroundTruthPose(); 144 | 145 | //This is set only once 146 | setInitialPoses(); 147 | 148 | //Check if robot has moved, if it has not, skip resampling step 149 | pose2D_t pose_delta; 150 | pose_delta.x = odom_pose_.x - last_odom_pose_.x; 151 | pose_delta.y = odom_pose_.y - last_odom_pose_.y; 152 | pose_delta.theta = angle_diff(odom_pose_.theta,last_odom_pose_.theta); 153 | 154 | robot_moved_ = false; 155 | robot_moved_ = fabs(pose_delta.x) > config_->distance_thresh_ || 156 | fabs(pose_delta.y) > config_->distance_thresh_ || 157 | fabs(pose_delta.theta) > config_->angle_thresh_; 158 | 159 | PfilterAlgorithm(); 160 | 161 | publishMsgs(); 162 | 163 | last_odom_pose_.x = odom_pose_.x; 164 | last_odom_pose_.y = odom_pose_.y; 165 | last_odom_pose_.theta = odom_pose_.theta; 166 | 167 | } 168 | 169 | 170 | } 171 | 172 | bool PfilterLocalization::accumulateLidarScansWheel(const sensor_msgs::PointCloud2ConstPtr& laser_scan) 173 | { 174 | std::string laser_scan_frame_id = laser_scan->header.frame_id; 175 | current_laser_stamp_ = laser_scan->header.stamp; 176 | 177 | if(groundtruth_trajectory_.last_id>=groundtruth_trajectory_.transforms.size()) 178 | { 179 | ROS_WARN("Trajectory vector reached end"); 180 | return false; 181 | } 182 | 183 | tf2::Transform wheel_odom_transform; 184 | tf2::Transform final_transform; 185 | 186 | pcl::PointCloud cloud_out_pcl; 187 | pcl::PointCloud laser_scan_pcl; 188 | pcl::fromROSMsg(*laser_scan,laser_scan_pcl); 189 | 190 | tf2::Quaternion wheel_quat; 191 | wheel_quat.setRPY(0,0,odom_pose_.theta); 192 | 193 | wheel_odom_transform.setRotation(wheel_quat); 194 | wheel_odom_transform.setOrigin(tf2::Vector3(odom_pose_.x,odom_pose_.y,0)); 195 | 196 | tf2::Transform gravity_transform; 197 | gravity_transform.setIdentity(); 198 | 199 | if(config_->compensate_cloud_) 200 | { 201 | sensor_msgs::Imu imu_data = imu_interface_->current_imu_; 202 | 203 | tf2::Quaternion imu_orient_q(imu_data.orientation.x,imu_data.orientation.y,imu_data.orientation.z,imu_data.orientation.w); 204 | tf2::Matrix3x3 imu_rot(imu_orient_q); 205 | tf2::Vector3 z_L(imu_rot[0].z(),imu_rot[1].z(),imu_rot[2].z()); 206 | 207 | tf2Scalar roll, pitch, yaw; 208 | imu_rot.getRPY(roll, pitch, yaw); 209 | 210 | //printf("Roll, pitch, yaw: %0.2f, %0.2f, %0.2f \n", roll*180/M_PI, pitch*180/M_PI, yaw*180/M_PI); 211 | 212 | tf2::Matrix3x3 rot_rp; 213 | rot_rp.setRPY(roll,pitch,0); 214 | 215 | tf2::Quaternion rot_quat; 216 | rot_rp.getRotation(rot_quat); 217 | 218 | gravity_transform.setOrigin(tf2::Vector3(0,0,0)); 219 | gravity_transform.setRotation(rot_quat); 220 | 221 | } 222 | 223 | //Thansform input laser scans according to wheel odometry and gravity alignment (imu) 224 | final_transform = wheel_odom_transform*gravity_transform; 225 | geometry_msgs::Transform final_transform_msg; 226 | pcl_ros::transformPointCloud(laser_scan_pcl, cloud_out_pcl, tf2::toMsg(final_transform)); 227 | 228 | current_sweep_time_ = laser_scan->header.stamp.toSec(); 229 | if(first_scan) 230 | { 231 | start_sweep_time_ = current_sweep_time_; 232 | first_scan = false; 233 | set_start_position_ = true; 234 | } 235 | 236 | //Accumulate point cloud data in a given window time = (laser_accum_time_) 237 | if(current_sweep_time_ - start_sweep_time_ <= config_->laser_accum_time_) 238 | { 239 | accumulated_cloud_pcl_+= cloud_out_pcl; 240 | 241 | return false; 242 | } 243 | else{ 244 | 245 | pcl_ros::transformPointCloud(accumulated_cloud_pcl_,sweep_cloud_pcl_,tf2::toMsg(wheel_odom_transform.inverse())); 246 | 247 | //reset sweep time counter 248 | start_sweep_time_ = laser_scan->header.stamp.toSec(); 249 | current_sweep_time_ = start_sweep_time_; 250 | std::cout<<"Lidar accumulated points = "<inst_velocity >= config_->avg_velocity_thresh_) 263 | { 264 | if(first_gator_fb) 265 | { 266 | car_model.time = gator_msg->header.stamp.toSec(); 267 | last_odom_pose_.x = 0.0; 268 | last_odom_pose_.y = 0.0; 269 | last_odom_pose_.theta = 0.0; 270 | odom_pose_.x = 0.0; 271 | odom_pose_.y = 0.0; 272 | odom_pose_.theta = 0.0; 273 | 274 | first_gator_fb = false; 275 | 276 | } 277 | 278 | 279 | car_model.v_linear = gator_msg->inst_velocity; //linear velocity in m/s 280 | car_model.steering_pos = -gator_msg->steering_angle; 281 | car_model.computeVelocities(odom_pose_.theta); 282 | 283 | car_model.updateOdometryPose(odom_pose_, gator_msg->header.stamp.toSec()); 284 | } 285 | 286 | car_model.time = gator_msg->header.stamp.toSec(); 287 | 288 | geometry_msgs::PoseStamped odom_pose_msg; 289 | preparePoseMsg(odom_pose_,odom_pose_msg,map_frame_,gator_msg->header.stamp); 290 | publish_odom_pose_.publish(odom_pose_msg); 291 | 292 | robot_odom_path_.header.frame_id = map_frame_; 293 | 294 | robot_odom_path_.header.stamp = gator_msg->header.stamp; 295 | robot_odom_path_.poses.push_back(odom_pose_msg); 296 | publish_odom_pose_path_.publish(robot_odom_path_); 297 | 298 | 299 | } 300 | 301 | void PfilterLocalization::PfilterAlgorithm() 302 | { 303 | particle_state particle_state_temp; 304 | vector particles_temp; 305 | particles_temp.resize(config_->numParticles_); 306 | 307 | //Sampling from the robot motion model 308 | for(int j = 0; j < config_->numParticles_; j++) 309 | { 310 | particle_state_temp = sampleCarMotionModel(particles_[j]); 311 | 312 | particles_temp[j] = particle_state_temp; 313 | } 314 | particles_.clear(); 315 | particles_= particles_temp; 316 | 317 | total_weight_ = 0; 318 | 319 | new_weights_.clear(); 320 | new_weights_.resize(config_->numParticles_); 321 | 322 | std::vector weights_copy; 323 | weights_copy.resize(config_->numParticles_); 324 | 325 | if(ucm_images_buffer_.size()!=0 && local_frames_buffer_.size()!=0) 326 | { 327 | ucm_images_buffer_.clear(); 328 | local_frames_buffer_.clear(); 329 | ucm_images_buffer_.resize(config_->numParticles_); 330 | local_frames_buffer_.resize(config_->numParticles_); 331 | } 332 | 333 | //If enough trees are detected construct UCM and perform Map Matching for each particle 334 | if(tree_segmentation_->extracted_trunks_.size()>=config_->min_landmarks_threshold_) 335 | { 336 | pfilter_threads_.clear(); 337 | pfilter_threads_.resize(config_->num_threads_); 338 | int n_particles_thread = config_->numParticles_/config_->num_threads_; 339 | 340 | for (int id_thread = 0; id_threadnum_threads_; id_thread++) 341 | { 342 | int init_id = n_particles_thread*id_thread; 343 | int end_id = std::min(n_particles_thread*id_thread + n_particles_thread-1, config_->numParticles_-1); 344 | 345 | pfilter_threads_[id_thread] = std::thread(&PfilterLocalization::mapMatchingThread,this, init_id, end_id); 346 | 347 | } 348 | 349 | for (auto it = pfilter_threads_.begin(); it != pfilter_threads_.end(); ++it) 350 | it->join(); 351 | 352 | } 353 | else 354 | { 355 | //In case few trees are detected (less than 2), weights from last measurement are kept, new weights will be multiplied by factor 1 356 | for(int j = 0; j < config_->numParticles_; j++) 357 | new_weights_[j] = 1; 358 | 359 | total_weight_ = config_->numParticles_; 360 | 361 | ROS_INFO("Few trees detected - no resampling / keeping last particles weights"); 362 | } 363 | 364 | 365 | for(int j = 0; j < config_->numParticles_; j++) 366 | { 367 | weights_copy[j] = new_weights_[j]/total_weight_; 368 | } 369 | 370 | double has_nan = false; 371 | 372 | //Perform resampling of particles only if robot has moved and enough trees were detected 373 | if(robot_moved_ && tree_segmentation_->extracted_trunks_.size()>= config_->min_landmarks_threshold_) 374 | { 375 | 376 | for(int j = 0; j < config_->numParticles_; j++) 377 | { 378 | particles_[j].weight = new_weights_[j]/total_weight_; 379 | if(std::isnan(particles_[j].weight)) 380 | has_nan = true; 381 | } 382 | 383 | LowVarianceSampler(); 384 | total_weight_ = 1.0; 385 | 386 | ROS_INFO("Resampling occurred"); 387 | } 388 | else 389 | { 390 | total_weight_ = 0; 391 | for(int j = 0; j < config_->numParticles_; j++) 392 | { 393 | float new_weight = particles_[j].weight*new_weights_[j]; 394 | total_weight_+= new_weight; 395 | particles_[j].weight = new_weight; 396 | } 397 | 398 | for(int j = 0; j < config_->numParticles_; j++) 399 | { 400 | particles_[j].weight /= total_weight_; 401 | weights_copy[j] = particles_[j].weight; 402 | if(std::isnan(particles_[j].weight)) 403 | has_nan = true; 404 | 405 | } 406 | 407 | } 408 | 409 | //Get the 3 particles with highest weight (for visualisation purposes only) 410 | particle_state highest_weight_particle; 411 | int high_particle_id; 412 | maximumParticleWeights(highest_weight_particle, high_particle_id, weights_copy); 413 | publishParticles(particles_temp, particles_); 414 | 415 | double x,y,theta, c_theta, s_theta; 416 | x = 0; 417 | y = 0; 418 | c_theta = 0; 419 | s_theta = 0; 420 | total_weight_ = 0.0; 421 | 422 | //Compute estimated robot position using all particles (weighted average) 423 | for(int j = 0; j < config_->numParticles_; j++) 424 | { 425 | if(has_nan) 426 | { 427 | particles_[j].weight = 1.0/config_->numParticles_; 428 | } 429 | 430 | x = x + particles_[j].x * particles_[j].weight; 431 | y = y + particles_[j].y * particles_[j].weight; 432 | 433 | c_theta = c_theta + cos(particles_[j].theta)*particles_[j].weight; 434 | s_theta = s_theta + sin(particles_[j].theta)*particles_[j].weight; 435 | total_weight_+=particles_[j].weight; 436 | } 437 | 438 | robot_pose_.x = x/(total_weight_); 439 | robot_pose_.y = y/(total_weight_); 440 | robot_pose_.theta = atan2(s_theta/total_weight_,c_theta/total_weight_); 441 | 442 | //Save robot position in the ACM map image frame (for visualisation purposes) 443 | Eigen::Vector2i robot_image_pos((robot_pose_.x - image_map_.image_frame_x_)/image_map_.grid_size_,(image_map_.image_frame_y_ - robot_pose_.y)/image_map_.grid_size_); 444 | robot_image_pos_.push_back(robot_image_pos); 445 | 446 | //Publish UCM image superimposed in ACM map image, but UCM image is translated and rotated according to particle position with highest weight 447 | if(tree_segmentation_->extracted_trunks_.size()>=config_->min_landmarks_threshold_) 448 | { 449 | visualisation_->publishMapAndBeliefImages(pose2D_t(highest_weight_particle.x,highest_weight_particle.y, highest_weight_particle.theta), 450 | local_frames_buffer_[high_particle_id], 451 | ucm_images_buffer_[high_particle_id], 452 | image_map_, 453 | robot_image_pos_ 454 | ); 455 | } 456 | 457 | } 458 | 459 | 460 | void PfilterLocalization::extractTreeTrunks() 461 | { 462 | if(sweep_cloud_pcl_.points.size() > 0) 463 | { 464 | tree_segmentation_->extractTrunks(sweep_cloud_pcl_); 465 | 466 | if(tree_segmentation_->extracted_trunks_.size()==0) 467 | { 468 | ROS_WARN("No trunks extracted"); 469 | return; 470 | } 471 | 472 | ROS_INFO("Number of Extracted trunks: %d", int(tree_segmentation_->extracted_trunks_.size())); 473 | 474 | } 475 | else{ 476 | ROS_WARN("Trunk extraction callback didn't receive points"); 477 | } 478 | } 479 | 480 | 481 | void PfilterLocalization::mapMatchingThread(int init_id, int end_id) 482 | { 483 | for(int j = init_id; j < end_id+1; j++) 484 | { 485 | double weight = 0; 486 | 487 | pose2D_t particle_pose; 488 | particle_pose.x = particles_[j].x; 489 | particle_pose.y = particles_[j].y; 490 | particle_pose.theta = particles_[j].theta; 491 | 492 | weight = mapMatchingParticle(particle_pose, j); 493 | 494 | data_assoc_mutex_.lock(); 495 | new_weights_[j] = weight; 496 | total_weight_ += weight; 497 | data_assoc_mutex_.unlock(); 498 | } 499 | } 500 | 501 | 502 | double PfilterLocalization::mapMatchingParticle(pose2D_t pose_particle, int particle_id) 503 | { 504 | 505 | if(tree_segmentation_->extracted_trunks_.size()==0) 506 | return false; 507 | 508 | 509 | tf2::Quaternion temp_pose_q; 510 | temp_pose_q.setRPY(0, 0, pose_particle.theta); 511 | geometry_msgs::TransformStamped temp_pose_T; 512 | 513 | 514 | temp_pose_T.transform.translation.x = pose_particle.x; 515 | temp_pose_T.transform.translation.y = pose_particle.y; 516 | temp_pose_T.transform.translation.z = 0.0; 517 | temp_pose_T.transform.rotation.x = temp_pose_q.getX(); 518 | temp_pose_T.transform.rotation.y = temp_pose_q.getY(); 519 | temp_pose_T.transform.rotation.z = temp_pose_q.getZ(); 520 | temp_pose_T.transform.rotation.w = temp_pose_q.getW(); 521 | temp_pose_T.header.frame_id = map_frame_; 522 | temp_pose_T.child_frame_id = "base_link"; 523 | 524 | 525 | std::vector extracted_trunks_transformed; 526 | 527 | extracted_trunks_transformed.resize(tree_segmentation_->extracted_trunks_.size()); 528 | 529 | int i = 0; 530 | 531 | for(const auto& trunk: tree_segmentation_->extracted_trunks_) { 532 | 533 | geometry_msgs::PointStamped trunk_pos_in; 534 | trunk_pos_in.point.x = trunk.position.x(); 535 | trunk_pos_in.point.y = trunk.position.y(); 536 | trunk_pos_in.point.z = trunk.position.z(); 537 | geometry_msgs::PointStamped trunk_pos_out; 538 | 539 | tf2::doTransform(trunk_pos_in,trunk_pos_out,temp_pose_T); 540 | 541 | Tree trunk_transformed; 542 | trunk_transformed.position[0] = trunk_pos_out.point.x; 543 | trunk_transformed.position[1] = trunk_pos_out.point.y; 544 | trunk_transformed.radius = trunk.radius; 545 | trunk_transformed.max_height = trunk.max_height; 546 | 547 | extracted_trunks_transformed[i] = trunk_transformed; 548 | i++; 549 | } 550 | 551 | cv::Mat ucm_image; 552 | 553 | Eigen::Vector2d local_frame; 554 | 555 | tree_segmentation_->generateUCM(extracted_trunks_transformed, local_frame, ucm_image, image_map_.max_height_); 556 | 557 | data_assoc_mutex_.lock(); 558 | ucm_images_buffer_[particle_id] = ucm_image.clone(); 559 | local_frames_buffer_[particle_id] = local_frame; 560 | data_assoc_mutex_.unlock(); 561 | 562 | double grid_size = image_map_.grid_size_; 563 | double ssd_score = 0; 564 | 565 | cv::Point last_pos_px; 566 | last_pos_px.x = (pose_particle.x - image_map_.image_frame_x_)/grid_size; 567 | last_pos_px.y = (image_map_.image_frame_y_ - pose_particle.y)/grid_size; 568 | 569 | cv::Point last_pos_local_px; 570 | last_pos_local_px.x = (pose_particle.x - local_frame[0])/grid_size; 571 | last_pos_local_px.y = (local_frame[1] - pose_particle.y)/grid_size; 572 | 573 | if(last_pos_local_px.x < 0 || last_pos_local_px.y <0) 574 | { 575 | ROS_WARN("PARTICLE OUTSIDE IMAGE RANGE"); 576 | return 0; 577 | } 578 | 579 | similarityMeasure(ucm_image, image_map_.image_, ssd_score, last_pos_px, last_pos_local_px, local_frame); 580 | 581 | Eigen::Vector2d map_image_frame(image_map_.image_frame_x_,image_map_.image_frame_y_); 582 | 583 | return ssd_score; 584 | 585 | } 586 | 587 | void PfilterLocalization::similarityMeasure(cv::Mat &local_image, cv::Mat &map_image, double &score, cv::Point particle_pos_pxl, cv::Point particle_pos_local_pxl, Eigen::Vector2d local_frame) 588 | { 589 | 590 | cv::Point template_corner; 591 | template_corner.x = std::max(particle_pos_pxl.x - particle_pos_local_pxl.x, 0); 592 | template_corner.y = std::max(particle_pos_pxl.y - particle_pos_local_pxl.y, 0); 593 | 594 | int diff_x = (particle_pos_pxl.x - particle_pos_local_pxl.x >= 0)? 0: (particle_pos_pxl.x - particle_pos_local_pxl.x); 595 | int diff_y = (particle_pos_pxl.y - particle_pos_local_pxl.y >= 0)? 0: (particle_pos_pxl.y - particle_pos_local_pxl.y); 596 | int dx = local_image.cols - diff_x; 597 | int dy = local_image.rows - diff_y; 598 | 599 | score = 0; 600 | double ssd_beta = config_->ssd_beta_; 601 | 602 | 603 | double cost = 0; 604 | 605 | int n_overlap = 0; 606 | float scale = image_map_.max_height_/255.0; 607 | for (int i = 0; i< dy ; i++) 608 | { 609 | for (int j=0; j< dx ; j++) 610 | { 611 | if((float)local_image.at(i,j) > 0) 612 | { 613 | float diff = ((float)local_image.at(i,j) - (float)map_image.at(template_corner.y + i,template_corner.x + j))*scale; 614 | cost+= diff*diff; 615 | n_overlap++; 616 | } 617 | } 618 | 619 | } 620 | 621 | cost = cost/(2*n_overlap); 622 | 623 | score = exp(-cost/ssd_beta); 624 | 625 | } 626 | 627 | void PfilterLocalization::updateGroundTruthPose() 628 | { 629 | int groundtruth_id_min = TrajectoryData::findNearestTransformScan(groundtruth_trajectory_, config_->traj_search_window_, current_laser_stamp_, 0.01); 630 | tf2::fromMsg(groundtruth_trajectory_.transforms[groundtruth_id_min].transform, groundtruth_current_transform_); 631 | 632 | //Get real position (ground truth) from wildcat trajectory (.csv file) 633 | tf2::Vector3 gt_pos_vec = groundtruth_current_transform_.getOrigin(); 634 | groundtruth_pose_.x = gt_pos_vec.x(); 635 | groundtruth_pose_.y = gt_pos_vec.y(); 636 | tf2::Matrix3x3 rot_groundtruth_pose(groundtruth_current_transform_.getRotation()); 637 | tf2Scalar roll, pitch, yaw; 638 | rot_groundtruth_pose.getRPY(roll, pitch, yaw); 639 | groundtruth_pose_.theta = yaw; 640 | 641 | } 642 | 643 | void PfilterLocalization::InitParticles(pose2D_t initial_pose) 644 | { 645 | printf("Initializing particles \n"); 646 | for (int i=0; i<=config_->numParticles_; i++) 647 | { 648 | particle_state particle_temp; 649 | 650 | particle_temp.x = rand() / (float)RAND_MAX * config_->particle_range_x_ + initial_pose.x - config_->particle_range_x_/2.0; 651 | particle_temp.y = rand() / (float)RAND_MAX * config_->particle_range_y_ + initial_pose.y - config_->particle_range_y_/2.0; 652 | 653 | 654 | 655 | particle_temp.theta = initial_pose.theta; 656 | 657 | particle_temp.weight = 1.0/config_->numParticles_; 658 | 659 | //printf("x: %4.2f y: %4.2f \n", particle_temp.x, particle_temp.y); 660 | particles_.push_back(particle_temp); 661 | } 662 | } 663 | 664 | particle_state PfilterLocalization::sampleCarMotionModel(particle_state particle) 665 | { 666 | 667 | float delta_trans1 = sqrt(pow((odom_pose_.x - last_odom_pose_.x),2) + pow((odom_pose_.y - last_odom_pose_.y),2)); 668 | float delta_rot1 = 0.0; 669 | 670 | //Compute rotation angle_1 only if the robot has moved a min distance (avoid inconsistence in atan2 calculation) 671 | if(delta_trans1>0.01) 672 | delta_rot1 = angle_diff(atan2(odom_pose_.y - last_odom_pose_.y,odom_pose_.x - last_odom_pose_.x), last_odom_pose_.theta); 673 | 674 | float delta_rot2 = angle_diff(angle_diff(odom_pose_.theta, last_odom_pose_.theta), delta_rot1); 675 | 676 | float delta_rot1_hat = angle_diff(delta_rot1, sampleNormalDistribution(config_->alpha1_*abs(delta_rot1) + config_->alpha2_*abs(delta_trans1))); 677 | float delta_trans1_hat = delta_trans1 - sampleNormalDistribution(config_->alpha3_*delta_trans1 + config_->alpha4_*(abs(delta_rot1) + abs(delta_rot2))); 678 | float delta_rot2_hat = angle_diff(delta_rot2, sampleNormalDistribution(config_->alpha1_*abs(delta_rot2) + config_->alpha2_*delta_trans1)); 679 | 680 | particle_state particle_temp; 681 | 682 | particle_temp.x = particle.x + (delta_trans1_hat * cos(particle.theta + delta_rot1_hat)); 683 | particle_temp.y = particle.y + (delta_trans1_hat * sin(particle.theta + delta_rot1_hat)); 684 | particle_temp.theta = particle.theta + delta_rot1_hat + delta_rot2_hat; 685 | particle_temp.weight = particle.weight; 686 | 687 | return particle_temp; 688 | 689 | } 690 | 691 | double PfilterLocalization::sampleNormalDistribution(double std_dev) 692 | { 693 | 694 | std::normal_distribution distribution(0.0,std_dev); 695 | return distribution(random_gen); 696 | 697 | } 698 | 699 | void PfilterLocalization::LowVarianceSampler() 700 | { 701 | vector particles_temp = particles_; 702 | float r = (rand() / (float)RAND_MAX) * (1.0 / (float)config_->numParticles_); 703 | float c = particles_[0].weight; 704 | int i = 0; 705 | 706 | for (int m = 0;m < config_->numParticles_; m++) 707 | { 708 | float u = r + (float) m/ config_->numParticles_; 709 | while (u > c && i < config_->numParticles_ - 1) 710 | { 711 | i+=1; 712 | c += particles_temp[i].weight; 713 | } 714 | particles_[m] = particles_temp[i]; 715 | particles_[m].weight = 1.0 / config_->numParticles_; 716 | 717 | } 718 | } 719 | 720 | double PfilterLocalization::ProbNormal(double x, double std_dev) 721 | { 722 | return (1/(std_dev*sqrt(2*M_PI)))*exp(-x*x/(2*std_dev*std_dev)); 723 | } 724 | 725 | void PfilterLocalization::preparePoseMsg(pose2D_t pose, geometry_msgs::PoseStamped &msg, string frame, ros::Time stamp) 726 | { 727 | tf2::Quaternion q; 728 | q.setRPY(0,0,pose.theta); 729 | msg.pose.position.x = pose.x; 730 | msg.pose.position.y = pose.y; 731 | msg.pose.position.z = 0.0; 732 | msg.pose.orientation.x = q.getX(); 733 | msg.pose.orientation.y = q.getY(); 734 | msg.pose.orientation.z = q.getZ(); 735 | msg.pose.orientation.w = q.getW(); 736 | msg.header.frame_id = frame; 737 | msg.header.stamp = stamp; 738 | 739 | } 740 | 741 | void PfilterLocalization::preparePoseMsg(pose2D_t pose, geometry_msgs::Pose &msg, string frame, ros::Time stamp) 742 | { 743 | tf2::Quaternion q; 744 | q.setRPY(0,0,pose.theta); 745 | msg.position.x = pose.x; 746 | msg.position.y = pose.y; 747 | msg.position.z = 0.0; 748 | msg.orientation.x = q.getX(); 749 | msg.orientation.y = q.getY(); 750 | msg.orientation.z = q.getZ(); 751 | msg.orientation.w = q.getW(); 752 | 753 | } 754 | 755 | 756 | void PfilterLocalization::publishMsgs() 757 | { 758 | 759 | ros::Time time_now = ros::Time::now(); 760 | 761 | visualisation_->publishDownsampledSweep(tree_segmentation_->filtered_cloud_,time_now); 762 | visualisation_->publishClustersSegments(tree_segmentation_->extracted_trunks_,time_now); 763 | visualisation_->publishTrunksExtracted(tree_segmentation_->extracted_trunks_,time_now); 764 | 765 | ros::Time stamp = groundtruth_trajectory_.transforms[groundtruth_trajectory_.last_id].header.stamp; 766 | 767 | preparePoseMsg(robot_pose_,robot_pos_msg_,map_frame_,stamp); 768 | publish_pose_.publish(robot_pos_msg_); 769 | 770 | robot_pos_path_.header.frame_id = map_frame_; 771 | //robot_pos_path_.header.stamp = ros::Time::now(); 772 | robot_pos_path_.header.stamp = stamp; 773 | robot_pos_path_.poses.push_back(robot_pos_msg_); 774 | publish_pose_path_.publish(robot_pos_path_); 775 | 776 | // Publish transform from robot frame to world frame 777 | static tf2_ros::TransformBroadcaster br; 778 | geometry_msgs::TransformStamped transformStamped; 779 | transformStamped.header.stamp = ros::Time::now(); 780 | //transformStamped.header.stamp = stamp ; 781 | transformStamped.header.frame_id = map_frame_; 782 | transformStamped.child_frame_id = config_->base_frame_; 783 | transformStamped.transform.translation.x = robot_pose_.x; 784 | transformStamped.transform.translation.y = robot_pose_.y; 785 | transformStamped.transform.translation.z = 0.0; 786 | tf2::Quaternion q_b; 787 | q_b.setRPY(0, 0, robot_pose_.theta); 788 | transformStamped.transform.rotation.x = q_b.x(); 789 | transformStamped.transform.rotation.y = q_b.y(); 790 | transformStamped.transform.rotation.z = q_b.z(); 791 | transformStamped.transform.rotation.w = q_b.w(); 792 | 793 | br.sendTransform(transformStamped); 794 | 795 | geometry_msgs::PoseStamped groundtruth_pose_msg; 796 | preparePoseMsg(groundtruth_pose_,groundtruth_pose_msg,map_frame_,stamp); 797 | publish_gt_pose_.publish(groundtruth_pose_msg); 798 | 799 | groundtruth_path_.header.frame_id = map_frame_; 800 | groundtruth_path_.header.stamp = stamp; 801 | groundtruth_path_.poses.push_back(groundtruth_pose_msg); 802 | publish_groundtruth_path_.publish(groundtruth_path_); 803 | 804 | geometry_msgs::PoseStamped odom_pose_msg; 805 | preparePoseMsg(odom_pose_,odom_pose_msg,map_frame_, stamp); 806 | publish_odom_pose_.publish(odom_pose_msg); 807 | 808 | robot_odom_path_.header.frame_id = map_frame_; 809 | robot_odom_path_.header.stamp = stamp; 810 | robot_odom_path_.poses.push_back(odom_pose_msg); 811 | publish_odom_pose_path_.publish(robot_odom_path_); 812 | 813 | 814 | } 815 | 816 | bool PfilterLocalization::saveData() 817 | { 818 | string save_answer = "y"; 819 | 820 | cout << "Would you like to save experiment data in files: yes (y), no (n)"; 821 | 822 | cin >> save_answer; 823 | 824 | 825 | if(!save_answer.compare("y")) 826 | { 827 | 828 | 829 | cout<<"Starting to save data"<tm_year + 1900; 835 | int month = ltm->tm_mon + 1; 836 | int day = ltm->tm_mday; 837 | int hour = ltm->tm_hour; 838 | int min = ltm->tm_min; 839 | int sec = ltm->tm_sec; 840 | 841 | root_name<< year; 842 | 843 | if(month<10) 844 | root_name<<"0"; 845 | root_name<save_folder_ + "similarity_experiment_data.txt"; 865 | string time_files_names_file = config_->save_folder_ + "runtime_files.txt"; 866 | 867 | config_->save_folder_ = config_->save_folder_ + "/" + root_name.str(); 868 | if (mkdir(config_->save_folder_.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1) 869 | { 870 | cerr << "Not possible to save files - Error when creating folder: " << strerror(errno) << endl; 871 | return false; 872 | } 873 | config_->save_folder_= config_->save_folder_ + "/"; 874 | 875 | 876 | string odom_file = config_->save_folder_ + root_name.str() + "_odom_pos.txt"; 877 | string robot_file = config_->save_folder_ + root_name.str() + "_robot_pos.txt"; 878 | string groundtruth_file = config_->save_folder_ + root_name.str() + "_groundtruth_pos.txt"; 879 | 880 | string odom_py_file = config_->save_folder_ + root_name.str() + "_odom_pos_py.txt"; 881 | 882 | string particles_file = config_->save_folder_ + root_name.str() + "_particles.txt"; 883 | 884 | string info_file = config_->save_folder_ + root_name.str() + "_INFO.txt"; 885 | 886 | string time_info_file = config_->save_folder_ + "runtime_info.txt"; 887 | 888 | std::ofstream myFile(odom_file); 889 | 890 | if(!myFile.is_open()) 891 | { 892 | cout<<"Error: Data not saved in files."<getAllParamsString()<<"\n"; 1012 | 1013 | myFile.close(); 1014 | 1015 | std::cout<<"Finished save files"< particle_weights) 1025 | { 1026 | vector weights_copy = particle_weights; 1027 | int max_weights_index [3]; 1028 | max_weights_index[0] = (int)(max_element(weights_copy.begin(), weights_copy.end()) - weights_copy.begin()); 1029 | weights_copy[ max_weights_index[0]] = 0.0; 1030 | max_weights_index[1] = (int)(max_element(weights_copy.begin(), weights_copy.end()) - weights_copy.begin()); 1031 | weights_copy[ max_weights_index[1]] = 0.0; 1032 | max_weights_index[2] = (int)(max_element(weights_copy.begin(), weights_copy.end()) - weights_copy.begin()); 1033 | 1034 | geometry_msgs::PoseArray particles_max_weights_msg; 1035 | 1036 | particles_max_weights_msg.header.frame_id = map_frame_; 1037 | particles_max_weights_msg.header.stamp = ros::Time::now(); 1038 | particles_max_weights_msg.poses.clear(); 1039 | geometry_msgs::Pose max_weight_pose; 1040 | 1041 | particle_state temp_particle = particles_[max_weights_index[0]]; 1042 | preparePoseMsg(pose2D_t(temp_particle.x,temp_particle.y, temp_particle.theta),max_weight_pose,map_frame_,ros::Time::now()); 1043 | particles_max_weights_msg.poses.push_back(max_weight_pose); 1044 | 1045 | highest_w_particle = temp_particle; 1046 | particle_id = max_weights_index[0]; 1047 | 1048 | temp_particle = particles_[max_weights_index[1]]; 1049 | preparePoseMsg(pose2D_t(temp_particle.x,temp_particle.y, temp_particle.theta),max_weight_pose,map_frame_,ros::Time::now()); 1050 | particles_max_weights_msg.poses.push_back(max_weight_pose); 1051 | 1052 | temp_particle = particles_[max_weights_index[2]]; 1053 | preparePoseMsg(pose2D_t(temp_particle.x,temp_particle.y, temp_particle.theta),max_weight_pose,map_frame_,ros::Time::now()); 1054 | particles_max_weights_msg.poses.push_back(max_weight_pose); 1055 | 1056 | publish_max_weight_poses_.publish(particles_max_weights_msg); 1057 | 1058 | 1059 | } 1060 | 1061 | 1062 | void PfilterLocalization::publishParticles(vector particles, vector resampled_particles) 1063 | { 1064 | 1065 | particles_resample_msg_.poses.clear(); 1066 | particles_update_msg_.poses.clear(); 1067 | 1068 | int divisor = 50; 1069 | 1070 | for(int j = 0; j < config_->numParticles_; j+=int(config_->numParticles_/divisor)) 1071 | { 1072 | 1073 | //prepare particle position messages 1074 | geometry_msgs::Pose pose_temp; 1075 | tf2::Quaternion q; 1076 | q.setRPY(0.0,0.0,particles[j].theta); 1077 | pose_temp.position.x = particles[j].x; 1078 | pose_temp.position.y = particles[j].y; 1079 | pose_temp.position.z = 0.0; 1080 | pose_temp.orientation.x = q.x(); 1081 | pose_temp.orientation.y = q.y(); 1082 | pose_temp.orientation.z = q.z(); 1083 | pose_temp.orientation.w = q.w(); 1084 | 1085 | particles_update_msg_.header.frame_id = map_frame_; 1086 | particles_update_msg_.header.stamp = ros::Time::now(); 1087 | particles_update_msg_.poses.push_back(pose_temp); 1088 | 1089 | //prepare particle position messages 1090 | geometry_msgs::Pose pose_temp2; 1091 | tf2::Quaternion q2; 1092 | q2.setRPY(0.0,0.0,resampled_particles[j].theta); 1093 | pose_temp2.position.x = resampled_particles[j].x; 1094 | pose_temp2.position.y = resampled_particles[j].y; 1095 | pose_temp2.position.z = 0.0; 1096 | pose_temp2.orientation.x = q2.x(); 1097 | pose_temp2.orientation.y = q2.y(); 1098 | pose_temp2.orientation.z = q2.z(); 1099 | pose_temp2.orientation.w = q2.w(); 1100 | 1101 | particles_resample_msg_.header.frame_id = map_frame_; 1102 | particles_resample_msg_.header.stamp = ros::Time::now(); 1103 | particles_resample_msg_.poses.push_back(pose_temp2); 1104 | //particles_update_msg_.poses.push_back(pose_temp2); 1105 | } 1106 | 1107 | publish_particles_resample_.publish(particles_resample_msg_); 1108 | publish_particles_update_.publish(particles_update_msg_); 1109 | 1110 | 1111 | 1112 | } 1113 | 1114 | -------------------------------------------------------------------------------- /src/pfilter_localization.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _PFILTER_LOCALIZATION_H 10 | #define _PFILTER_LOCALIZATION_H 11 | 12 | #include "map.h" 13 | #include "utils.h" 14 | #include "config_server.h" 15 | #include "tree_segmentation.h" 16 | #include "csv.h" 17 | #include "visualisation.h" 18 | #include "imu_interface.h" 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "ros/ros.h" 25 | 26 | #include "forest_localisation/gator_data.h" 27 | #include "sensor_msgs/PointCloud2.h" 28 | #include "geometry_msgs/PointStamped.h" 29 | #include "geometry_msgs/PoseArray.h" 30 | #include 31 | #include "nav_msgs/Path.h" 32 | #include 33 | 34 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 35 | #include "tf2/utils.h" 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | using namespace std; 48 | 49 | struct particle_state 50 | { 51 | particle_state(){ 52 | } 53 | 54 | particle_state(float x_in, float y_in, float theta_in, float weight_in): x(x_in), y(y_in), theta(theta_in), weight(weight_in){ 55 | } 56 | 57 | float x; 58 | float y; 59 | float theta; 60 | float weight; 61 | }; 62 | 63 | 64 | struct CarLikeModel{ 65 | CarLikeModel(double distance_wheels_in = 0): distance_wheels(distance_wheels_in) 66 | { 67 | steering_pos=0; 68 | time = 0; 69 | } 70 | 71 | double steering_pos; 72 | double distance_wheels; 73 | double dx,dy,dtheta, v_linear; 74 | double time; 75 | 76 | 77 | void computeVelocities(double theta) 78 | { 79 | dtheta = tan(steering_pos)*v_linear/distance_wheels; 80 | dx = cos(theta)*v_linear; 81 | dy = sin(theta)*v_linear; 82 | }; 83 | 84 | void updateOdometryPose(pose2D_t &odom_pose, double current_time) 85 | { 86 | double dt = current_time - time; 87 | odom_pose.x = odom_pose.x + dt*dx; 88 | odom_pose.y = odom_pose.y + dt*dy; 89 | odom_pose.theta = odom_pose.theta + dt*dtheta; 90 | 91 | time = current_time; 92 | } 93 | 94 | }; 95 | 96 | 97 | class PfilterLocalization 98 | { 99 | 100 | public: 101 | PfilterLocalization(ros::NodeHandle node, ros::NodeHandle private_node); 102 | ~PfilterLocalization(); 103 | void InitParticles(pose2D_t initial_pose); 104 | void PfilterAlgorithm(); 105 | 106 | void laserCloudReceived(const sensor_msgs::PointCloud2ConstPtr& laser_scan); 107 | void gatorInfoWheelOdom(const forest_localisation::gator_dataConstPtr &gator_msg); 108 | bool accumulateLidarScansWheel(const sensor_msgs::PointCloud2ConstPtr& laser_scan); 109 | void updateGroundTruthPose(); 110 | void extractTreeTrunks(); 111 | 112 | void setInitialPoses(); 113 | 114 | void mapMatchingThread(int init_id, int end_id); 115 | double mapMatchingParticle(pose2D_t pose_particle, int particle_id); 116 | void similarityMeasure(cv::Mat &local_image, cv::Mat &map_image, double &score, cv::Point particle_pos_pxl, cv::Point particle_pos_local_pxl, Eigen::Vector2d local_frame); 117 | 118 | void publishMsgs(); 119 | bool saveData(); 120 | 121 | void publishParticles(vector particles, vector resampled_particles); 122 | void maximumParticleWeights(particle_state & highest_w_particle, int &particle_id, vector particle_weights); 123 | 124 | std::shared_ptr config_; 125 | 126 | private: 127 | particle_state sampleCarMotionModel(particle_state particle); 128 | 129 | double sampleNormalDistribution(double std_dev); 130 | void LowVarianceSampler(); 131 | double ProbNormal(double x, double std_dev); 132 | void preparePoseMsg(pose2D_t pose, geometry_msgs::PoseStamped &msg, string frame, ros::Time stamp); 133 | void preparePoseMsg(pose2D_t pose, geometry_msgs::Pose &msg, string frame, ros::Time stamp); 134 | 135 | ros::NodeHandle node_; 136 | ros::NodeHandle private_node_; 137 | std::shared_ptr visualisation_; 138 | std::unique_ptr imu_interface_; 139 | std::unique_ptr tree_segmentation_; 140 | 141 | std::shared_ptr tf_; 142 | 143 | TrajectoryData groundtruth_trajectory_; 144 | tf2::Transform groundtruth_current_transform_; 145 | pose2D_t groundtruth_pose_; 146 | 147 | ros::Subscriber laser_cloud_sub_; 148 | ros::Subscriber gator_info_sub_; 149 | 150 | std::random_device random_dv{}; 151 | std::mt19937 random_gen; 152 | 153 | std::string base_frame_; 154 | std::string laser_frame_; 155 | std::string map_frame_; 156 | std::string odom_frame_; 157 | 158 | ros::Publisher publish_particles_; 159 | ros::Publisher publish_pose_; 160 | ros::Publisher publish_gt_pose_; 161 | ros::Publisher publish_odom_pose_; 162 | 163 | ros::Publisher publish_particles_update_; 164 | ros::Publisher publish_particles_resample_; 165 | geometry_msgs::PoseArray particles_update_msg_; 166 | geometry_msgs::PoseArray particles_resample_msg_; 167 | 168 | ros::Publisher publish_pose_path_; 169 | ros::Publisher publish_groundtruth_path_; 170 | ros::Publisher publish_odom_pose_path_; 171 | 172 | ros::Publisher publish_max_weight_poses_; 173 | 174 | geometry_msgs::PoseStamped robot_pos_msg_; 175 | geometry_msgs::PoseStamped odom_pose_msg_; 176 | 177 | nav_msgs::Path robot_pos_path_; 178 | nav_msgs::Path robot_odom_path_; 179 | nav_msgs::Path groundtruth_path_; 180 | 181 | vector robot_image_pos_; 182 | 183 | ros::Publisher scan_pub_; 184 | 185 | vector particles_; 186 | std::vector new_weights_; 187 | double total_weight_; 188 | pose2D_t robot_pose_; 189 | 190 | bool robot_moved_; 191 | 192 | ImageMap image_map_; 193 | 194 | vector ucm_images_buffer_; 195 | vector local_frames_buffer_; 196 | 197 | pose2D_t last_odom_pose_; 198 | pose2D_t odom_pose_; 199 | 200 | pcl::PointCloud sweep_cloud_pcl_; 201 | pcl::PointCloud accumulated_cloud_pcl_; 202 | 203 | double current_sweep_time_; 204 | double start_sweep_time_; 205 | ros::Time current_laser_stamp_; 206 | 207 | bool first_scan; 208 | bool set_start_position_; 209 | 210 | CarLikeModel car_model; 211 | 212 | bool first_gator_fb; 213 | 214 | std::vector pfilter_threads_; 215 | std::mutex data_assoc_mutex_; 216 | 217 | 218 | }; 219 | 220 | #endif 221 | 222 | 223 | 224 | -------------------------------------------------------------------------------- /src/tree.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef TREE_HPP 10 | #define TREE_HPP 11 | 12 | #include "ros/ros.h" 13 | #include "pcl/point_cloud.h" 14 | #include 15 | 16 | class Tree 17 | { 18 | public: 19 | Tree(){ }; 20 | ~Tree(){}; 21 | 22 | int id; 23 | Eigen::Vector3d position; 24 | double radius; 25 | double max_height; 26 | double min_height; 27 | pcl::PointXYZ min_point; 28 | int min_point_idx; 29 | 30 | pcl::PointCloud upper_cloud; 31 | pcl::PointCloud cloud; 32 | pcl::PointCloud crown_points; 33 | 34 | 35 | }; 36 | 37 | #endif -------------------------------------------------------------------------------- /src/tree_segmentation.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include "tree_segmentation.h" 10 | 11 | TreeSegmentation::TreeSegmentation(ros::NodeHandle node, ros::NodeHandle private_node, std::shared_ptr& config_ptr) 12 | { 13 | node_ = node; 14 | private_node_ = private_node; 15 | config_ = config_ptr; 16 | ground_extraction_.reset(new GroundExtraction(node_, private_node_)); 17 | kdtree_.reset(new pcl::search::KdTree); 18 | 19 | euclidean_cluster_.setClusterTolerance(config_->tolerance_); 20 | euclidean_cluster_.setMinClusterSize(config_->min_cluster_size_); 21 | euclidean_cluster_.setMaxClusterSize(config_->max_cluster_size_); 22 | euclidean_cluster_.setSearchMethod(kdtree_); 23 | voxel_size_ = config_->downsample_voxel_size_; 24 | } 25 | 26 | TreeSegmentation::~TreeSegmentation() 27 | { 28 | 29 | } 30 | 31 | void TreeSegmentation::downSizeCloud(PointCloudXYZ &cloud, PointCloudXYZ &filtered_cloud) 32 | { 33 | pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); 34 | *cloud_ptr = cloud; 35 | pcl::VoxelGrid vg; 36 | vg.setInputCloud (cloud_ptr); 37 | vg.setLeafSize (voxel_size_, voxel_size_, voxel_size_); 38 | vg.filter (filtered_cloud); 39 | ROS_INFO("PointCloud after filtering (voxelisation) has: %d data points." , filtered_cloud.size ()); 40 | } 41 | 42 | bool TreeSegmentation::extractTrunks(PointCloudXYZ &cloud) 43 | { 44 | ROS_INFO("Extracting trunks"); 45 | 46 | pcl::PointCloud cloud_cropped; 47 | 48 | // std::cout << "Received " << cloud.width * cloud.height << " points" << std::endl; 49 | extracted_trunks_.clear(); 50 | 51 | downSizeCloud(cloud, filtered_cloud_); 52 | splitCloudByHeights(filtered_cloud_, cloud_cropped); 53 | euclideanSegmentation(cloud_cropped); 54 | 55 | int n_trees = extracted_trunks_.size(); 56 | ROS_INFO("Number of trees found: %d",n_trees); 57 | 58 | if(n_trees<=0) 59 | return false; 60 | 61 | findMaximumTreesHeights(cloud); 62 | 63 | return true; 64 | 65 | } 66 | 67 | void TreeSegmentation::splitCloudByHeights(PointCloudXYZ &cloud, PointCloudXYZ &cloud_cropped) 68 | { 69 | //Get points with heights between min and max thresholds 70 | 71 | pcl::PointCloud::Ptr cloud_sor_out(new pcl::PointCloud); 72 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 73 | 74 | ground_extraction_->extractGround(cloud); 75 | ground_extraction_->getLowGroundCloud(); 76 | ground_extraction_->splitCloud(cloud,cloud_cropped, config_->ground_z_down_threshold_, config_->ground_z_up_threshold_); 77 | } 78 | 79 | 80 | void TreeSegmentation::euclideanSegmentation(const pcl::PointCloud& cloud) 81 | { 82 | pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); 83 | *cloud_ptr = cloud; 84 | std::vector cluster_indices; 85 | 86 | kdtree_->setInputCloud (cloud_ptr); 87 | euclidean_cluster_.setInputCloud(cloud_ptr); 88 | euclidean_cluster_.extract(cluster_indices); 89 | std::cout<<"Euclidean clustering..."<::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) 93 | { 94 | pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 95 | 96 | for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) 97 | { 98 | pcl::PointXYZ point; 99 | point.x = cloud_ptr->points[*pit].x; 100 | point.y = cloud_ptr->points[*pit].y; 101 | point.z = cloud_ptr->points[*pit].z; 102 | 103 | cloud_cluster->push_back(point); 104 | 105 | } 106 | cloud_cluster->width = cloud_cluster->points.size (); 107 | cloud_cluster->height = 1; 108 | cloud_cluster->is_dense = true; 109 | 110 | Tree tree_object; 111 | 112 | tree_object.cloud = *cloud_cluster; 113 | tree_object.position = getCloudCentroid(*cloud_cluster); 114 | extracted_trunks_.push_back(tree_object); 115 | } 116 | 117 | //Verify if clusters are trees by cheking the height of clusters 118 | for(int i=0; i config_->tree_height_; 123 | if(is_tree) 124 | continue; 125 | else 126 | { 127 | // short clusters are removed from the list 128 | extracted_trunks_.erase(extracted_trunks_.begin()+i); 129 | i--; 130 | } 131 | } 132 | 133 | 134 | 135 | 136 | } 137 | 138 | Eigen::Vector3d TreeSegmentation::getCloudCentroid(const pcl::PointCloud& cloud) 139 | { 140 | Eigen::Vector3d centroid; 141 | int cloud_size = cloud.points.size(); 142 | float x = 0; 143 | float y = 0; 144 | float z = 0; 145 | 146 | for(auto &point: cloud.points) 147 | { 148 | x += point.x; 149 | y += point.y; 150 | z += point.z; 151 | } 152 | 153 | centroid[0] = x / cloud_size; 154 | centroid[1] = y / cloud_size; 155 | centroid[2] = z / cloud_size; 156 | 157 | return centroid; 158 | } 159 | 160 | void TreeSegmentation::getClusterHeight(Tree& tree) 161 | { 162 | if(!tree.cloud.size()){ 163 | return; 164 | } 165 | 166 | // Calculate height 167 | double max_height = tree.cloud.points[0].z; 168 | double min_height = tree.cloud.points[0].z; 169 | int cloud_size = tree.cloud.height * tree.cloud.width; 170 | for(int i = 0; i< cloud_size; i++){ 171 | // find the min and max height 172 | if( tree.cloud.points[i].z > max_height) max_height = tree.cloud.points[i].z; 173 | else if( tree.cloud.points[i].z < min_height) min_height = tree.cloud.points[i].z; 174 | } 175 | tree.min_height = min_height; 176 | tree.max_height = max_height; 177 | } 178 | 179 | void TreeSegmentation::findMaximumTreesHeights(const pcl::PointCloud& cloud) 180 | { 181 | if(extracted_trunks_.size()==0) 182 | return; 183 | 184 | 185 | pcl::PointCloud::Ptr cloud_projected (new pcl::PointCloud); 186 | int i=0; 187 | cloud_projected->resize(cloud.points.size()); 188 | for(auto &point: cloud.points) 189 | { 190 | (*cloud_projected)[i].x = point.x; 191 | (*cloud_projected)[i].y = point.y; 192 | (*cloud_projected)[i].z = 0.0; 193 | i++; 194 | } 195 | 196 | pcl::KdTreeFLANN kdtree; 197 | 198 | kdtree.setInputCloud (cloud_projected); 199 | 200 | int j = 0; 201 | int n_points = cloud.points.size(); 202 | for( auto & trunk: extracted_trunks_) 203 | { 204 | std::vector pointIdxRadiusSearch; 205 | std::vector pointRadiusSquaredDistance; 206 | pcl::PointXYZ search_point; 207 | search_point.x = trunk.position[0]; 208 | search_point.y = trunk.position[1]; 209 | search_point.z = 0; 210 | 211 | float max_height = -1e10; 212 | float min_height = 1e10; 213 | float radius_search = 4.0; 214 | if ( kdtree.radiusSearch (search_point, radius_search, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) 215 | { 216 | for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i) 217 | { 218 | double dx = cloud[pointIdxRadiusSearch[i]].x - search_point.x; 219 | double dy = cloud[pointIdxRadiusSearch[i]].y - search_point.y; 220 | 221 | double dist_point = sqrt(dx*dx + dy+dy); 222 | if(dist_point<=1.0) 223 | max_height = std::max(cloud[pointIdxRadiusSearch[i]].z, max_height); 224 | 225 | if(dist_point<=1.5) 226 | min_height = std::min(cloud[pointIdxRadiusSearch[i]].z, min_height); 227 | } 228 | 229 | } 230 | 231 | trunk.max_height = max_height - min_height; 232 | trunk.position[2] = min_height; 233 | 234 | 235 | j=j+1; 236 | 237 | } 238 | 239 | } 240 | 241 | 242 | void TreeSegmentation::generateUCM(std::vector extracted_trunks_transformed, Eigen::Vector2d &local_frame, cv::Mat &ucm_image, double map_max_height) 243 | { 244 | double grid_size = config_->grid_size_; 245 | double crown_radius = config_->crown_radius_; 246 | 247 | double x_min = 1000000; 248 | double y_min = 1000000; 249 | double x_max = -1000000; 250 | double y_max = -1000000; 251 | double space_offset = 5; 252 | 253 | for(auto & trunk: extracted_trunks_transformed) 254 | { 255 | x_min = (trunk.position[0] < x_min) ? trunk.position[0]: x_min; 256 | x_max = (trunk.position[0] > x_max) ? trunk.position[0]: x_max; 257 | y_min = (trunk.position[1] < y_min) ? trunk.position[1]: y_min; 258 | y_max = (trunk.position[1] > y_max) ? trunk.position[1]: y_max; 259 | } 260 | 261 | x_min-=space_offset; 262 | x_max+=space_offset; 263 | y_min-=space_offset; 264 | y_max+=space_offset; 265 | 266 | //Origin frame of images are on top left corner 267 | local_frame[0] = x_min; 268 | local_frame[1] = y_max; 269 | 270 | int image_width = (int)((x_max - x_min)/grid_size); 271 | int image_height = (int)((y_max - y_min)/grid_size); 272 | 273 | cv::Mat image(image_height,image_width, CV_8U, cv::Scalar(0)); 274 | ucm_image = image.clone(); 275 | 276 | 277 | for(auto &trunk: extracted_trunks_transformed) 278 | { 279 | 280 | double center_x = trunk.position[0]; 281 | double center_y = trunk.position[1]; 282 | 283 | int center_point_px_i = (int)((y_max - center_y)/grid_size); 284 | int center_point_px_j = (int)((center_x - x_min)/grid_size); 285 | 286 | cv::Point center(center_point_px_j,center_point_px_i); 287 | 288 | double point_x_min = center_x - crown_radius; 289 | double point_y_max = center_y + crown_radius; 290 | int point_px_i_min = (int)((y_max - point_y_max)/grid_size); 291 | int point_px_j_min = (int)((point_x_min - x_min)/grid_size); 292 | 293 | double point_x_max = center_x + crown_radius; 294 | double point_y_min = center_y - crown_radius; 295 | int point_px_i_max = (int)((y_max - point_y_min)/grid_size); 296 | int point_px_j_max = (int)((point_x_max - x_min)/grid_size); 297 | 298 | for(int i = point_px_i_min-5; i=0 && i=0 && jmap_max_height) 319 | trunk.max_height = map_max_height; 320 | 321 | if(dist>=0 && dist<=crown_radius) 322 | { 323 | value = (int)(255*trunk.max_height/map_max_height); 324 | } 325 | 326 | 327 | } 328 | 329 | ucm_image.at(i,j) = (ucm_image.at(i,j) >= value)? ucm_image.at(i,j): value; 330 | } 331 | 332 | } 333 | 334 | } 335 | 336 | } 337 | 338 | } -------------------------------------------------------------------------------- /src/tree_segmentation.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _TREE_SEGMENTATION_H 10 | #define _TREE_SEGMENTATION_H 11 | 12 | #include "ros/ros.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include "opencv2/imgcodecs.hpp" 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "tree.h" 30 | #include "ground_extraction.h" 31 | #include "config_server.h" 32 | 33 | typedef pcl::PointCloud PointCloudXYZ; 34 | 35 | class TreeSegmentation { 36 | 37 | public: 38 | TreeSegmentation(ros::NodeHandle node, ros::NodeHandle private_node, std::shared_ptr& config_ptr); 39 | ~TreeSegmentation(); 40 | void downSizeCloud(PointCloudXYZ &cloud, PointCloudXYZ &filtered_cloud); 41 | bool extractTrunks(PointCloudXYZ &cloud); 42 | void splitCloudByHeights(PointCloudXYZ &cloud, PointCloudXYZ &cloud_cropped); 43 | void euclideanSegmentation(const pcl::PointCloud& cloud); 44 | void findMaximumTreesHeights(const pcl::PointCloud& cloud); 45 | Eigen::Vector3d getCloudCentroid(const pcl::PointCloud& cloud); 46 | void getClusterHeight(Tree& tree); 47 | 48 | void generateUCM(std::vector extracted_trunks_transformed, Eigen::Vector2d &local_frame, cv::Mat &ucm_image, double map_max_height); 49 | 50 | 51 | ros::NodeHandle node_; 52 | ros::NodeHandle private_node_; 53 | std::shared_ptr config_; 54 | double voxel_size_; 55 | pcl::search::KdTree::Ptr kdtree_; 56 | pcl::EuclideanClusterExtraction euclidean_cluster_; 57 | std::unique_ptr ground_extraction_; 58 | pcl::PointCloud filtered_cloud_; 59 | 60 | std::vector extracted_trunks_; 61 | }; 62 | 63 | #endif -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (c) 2023 3 | // Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 | // ABN 41 687 119 230 5 | // 6 | // Author: Lucas C. Lima 7 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef _PFILTER_UTILS_H 10 | #define _PFILTER_UTILS_H 11 | 12 | #include 13 | #include "ros/ros.h" 14 | #include 15 | #include 16 | #include "geometry_msgs/TransformStamped.h" 17 | #include "csv.h" 18 | #include "tf2/utils.h" 19 | 20 | #define SMALL_ANGLE 0.0002 21 | 22 | struct pose2D_t 23 | { 24 | pose2D_t(double x_in=0.0, double y_in=0.0, double theta_in=0.0){ 25 | x = x_in; 26 | y = y_in; 27 | theta = theta_in; 28 | } 29 | double x; 30 | double y; 31 | double theta; 32 | }; 33 | 34 | static double 35 | normalize(double z) 36 | { 37 | return atan2(sin(z),cos(z)); 38 | } 39 | static double 40 | angle_diff(double a, double b) 41 | { 42 | double d1, d2; 43 | a = normalize(a); 44 | b = normalize(b); 45 | d1 = a-b; 46 | d2 = 2*M_PI - fabs(d1); 47 | if(d1 > 0) 48 | d2 *= -1.0; 49 | if(fabs(d1) < fabs(d2)) 50 | return(d1); 51 | else 52 | return(d2); 53 | } 54 | 55 | inline 56 | std::string stripSlash(const std::string& in) 57 | { 58 | std::string out = in; 59 | if ( ( !in.empty() ) && (in[0] == '/') ) 60 | out.erase(0,1); 61 | return out; 62 | } 63 | 64 | static double positiveAngleDiff(double angle2, double angle1) 65 | { 66 | if (angle1<0) 67 | angle1 = angle1 + 2*M_PI; 68 | 69 | if (angle2<0) 70 | angle2 = angle2 + 2*M_PI; 71 | 72 | double diff = angle2 - angle1; 73 | if(diff<0) 74 | { 75 | diff = diff + 2*M_PI; 76 | } 77 | return diff; 78 | } 79 | 80 | template static void addParamToSstream(std::stringstream &ss, std::string name, T value) 81 | { 82 | ss< transforms; 92 | int last_id; 93 | 94 | static bool readTrajectoryFile(const std::string traj_file, std::vector init_rotation, std::vector init_translation, TrajectoryData &trajectory) 95 | { 96 | io::CSVReader<8> in_gt(traj_file); 97 | // int time_s, time_ns; 98 | double time; 99 | 100 | double x, y, z, qx, qy, qz, qw; 101 | 102 | 103 | in_gt.read_header(io::ignore_extra_column, "time","x","y","z","qw","qx","qy","qz"); 104 | geometry_msgs::TransformStamped transform; 105 | 106 | transform.header.frame_id = "map"; 107 | transform.child_frame_id = "base_link"; 108 | 109 | ros::Rate r(10); 110 | bool init = true; 111 | while(in_gt.read_row(time, x, y, z, qw, qx, qy, qz)) 112 | { 113 | if(init) 114 | { 115 | trajectory.start_time = time; 116 | init = false; 117 | } 118 | 119 | tf2::Transform trajectoryTransform; 120 | trajectoryTransform.setOrigin(tf2::Vector3(x,y,z)); 121 | trajectoryTransform.setRotation(tf2::Quaternion(qx,qy,qz,qw)); 122 | 123 | if(init_rotation.size() > 0 && init_translation.size() > 0) 124 | { 125 | tf2::Transform initTransform; 126 | initTransform.setOrigin(tf2::Vector3 (init_translation[0], init_translation[1], init_translation[2])); 127 | 128 | tf2::Matrix3x3 init_rot_matrix(init_rotation[0],init_rotation[1], init_rotation[2], 129 | init_rotation[3],init_rotation[4], init_rotation[5], 130 | init_rotation[6],init_rotation[7], init_rotation[8]); 131 | 132 | tf2::Quaternion init_quat; 133 | init_rot_matrix.getRotation(init_quat); 134 | 135 | initTransform.setRotation(init_quat); 136 | 137 | trajectoryTransform = initTransform*trajectoryTransform; 138 | } 139 | 140 | transform.transform.translation.x = trajectoryTransform.getOrigin()[0]; 141 | transform.transform.translation.y = trajectoryTransform.getOrigin()[1]; 142 | transform.transform.translation.z = trajectoryTransform.getOrigin()[2]; 143 | 144 | transform.transform.rotation.w = trajectoryTransform.getRotation().getW(); 145 | transform.transform.rotation.x = trajectoryTransform.getRotation().getX(); 146 | transform.transform.rotation.y = trajectoryTransform.getRotation().getY(); 147 | transform.transform.rotation.z = trajectoryTransform.getRotation().getZ(); 148 | 149 | ros::Time pose_time; 150 | transform.header.stamp = pose_time.fromSec(time); 151 | 152 | trajectory.transforms.push_back(transform); 153 | 154 | trajectory.end_time = time; 155 | 156 | } 157 | 158 | trajectory.transforms.shrink_to_fit(); 159 | trajectory.last_id = 0; 160 | return true; 161 | } 162 | 163 | static int findNearestTransformScan(TrajectoryData & trajectory, int search_window, ros::Time laser_stamp, double min_diff) 164 | { 165 | int start_indice = 0; 166 | 167 | for(int i=trajectory.last_id;i 0) 180 | first = start_indice - search_window; 181 | 182 | int id_min_diff = 0; 183 | double min_diff_time = 10.0; 184 | int end = first + 2*search_window; 185 | if(first + 2*search_window > trajectory.transforms.size()) 186 | end = trajectory.transforms.size() -1; 187 | 188 | for(int i=first; i<=end; i++){ 189 | auto traj_transform = trajectory.transforms[i]; 190 | ros::Duration diff=traj_transform.header.stamp - laser_stamp; 191 | if(abs(diff.toSec())<=min_diff_time) 192 | { 193 | min_diff_time = abs(diff.toSec()); 194 | id_min_diff = i; 195 | } 196 | } 197 | if(id_min_diff==0) 198 | std::cout<<"ID before crash = "<& config_ptr) 13 | { 14 | node_ = node; 15 | private_node_ = private_node; 16 | config_ = config_ptr; 17 | 18 | clusters_segment_pub_ = node_.advertise("/trees_clusters_segments", 1,true); 19 | sweep_cloud_ = node_.advertise("/sweep_cloud",1, true); 20 | tree_pos_pub_ = node_.advertise("/trees_positions_markers",1, true); 21 | filtered_sweep_pub_ = node_.advertise("/downsampled_sweep",1, true); 22 | // ground_cloud_pub_ = node_.advertise("/ground_grid",1, true); 23 | 24 | publish_matched_images_ = node_.advertise("/map_local_images",1); 25 | 26 | } 27 | 28 | Visualisation::~Visualisation() 29 | { 30 | 31 | } 32 | 33 | void Visualisation::publishClustersSegments(std::vector trees, ros::Time timestamp) 34 | { 35 | int n_clusters = trees.size(); 36 | 37 | sensor_msgs::PointCloud2 clusters_msg; 38 | pcl::PointCloud clusters_clouds; 39 | 40 | int i = 0; 41 | 42 | //Creates n random colours 43 | std::vector colors_rgb_vec; 44 | colors_rgb_vec = rgb_rand_color(n_clusters); 45 | 46 | for( auto &tree_obj: trees) 47 | { 48 | pcl::PointCloud cloud_colour; 49 | pcl::copyPointCloud(tree_obj.cloud, cloud_colour); 50 | colorCloud(cloud_colour,colors_rgb_vec[i]); 51 | i++; 52 | clusters_clouds = clusters_clouds + cloud_colour; 53 | } 54 | 55 | pcl::toROSMsg(clusters_clouds, clusters_msg); 56 | clusters_msg.header.frame_id = config_->base_frame_; 57 | clusters_msg.header.stamp = timestamp; 58 | 59 | clusters_segment_pub_.publish(clusters_msg); 60 | 61 | 62 | } 63 | 64 | void Visualisation::colorCloud(pcl::PointCloud &cloud, RGB_color colour) 65 | { 66 | int n_points = cloud.size(); 67 | for(int i=0; i< n_points; i++) 68 | { 69 | cloud[i].r = uint8_t(colour.r*255); 70 | cloud[i].g = uint8_t(colour.g*255);; 71 | cloud[i].b = uint8_t(colour.b*255);; 72 | } 73 | } 74 | 75 | std::vector Visualisation::rgb_rand_color( int n_colors) 76 | { 77 | std::vector colours_vec; 78 | 79 | if(n_colors<=0) 80 | { 81 | std::cout<<"No cluster found"<&pcloud, ros::Time timestamp) 99 | { 100 | sensor_msgs::PointCloud2 pcloud_msg; 101 | pcl::toROSMsg(pcloud,pcloud_msg); 102 | pcloud_msg.header.frame_id = config_->base_frame_; 103 | pcloud_msg.header.stamp = timestamp; 104 | sweep_cloud_.publish(pcloud_msg); 105 | 106 | } 107 | 108 | void Visualisation::publishDownsampledSweep(pcl::PointCloud&cloud, ros::Time timestamp) 109 | { 110 | sensor_msgs::PointCloud2 pcloud_msg; 111 | pcl::toROSMsg(cloud,pcloud_msg); 112 | pcloud_msg.header.frame_id = config_->base_frame_; 113 | pcloud_msg.header.stamp = timestamp; 114 | filtered_sweep_pub_.publish(pcloud_msg); 115 | 116 | } 117 | 118 | void Visualisation::publishTrunksExtracted(std::vector trees, ros::Time timestamp) 119 | { 120 | 121 | if(tree_pos_msg_.markers.size()>0) 122 | { 123 | for(int k=0; kbase_frame_; 142 | tree_pos_msg_.markers[j].ns ="trunks_extracted"; 143 | tree_pos_msg_.markers[j].type = visualization_msgs::Marker::CYLINDER; 144 | tree_pos_msg_.markers[j].action = visualization_msgs::Marker::ADD; 145 | tree_pos_msg_.markers[j].pose.position.x = trunk.position[0]; 146 | tree_pos_msg_.markers[j].pose.position.y = trunk.position[1]; 147 | // double height = (trunk.max_height>0)? trunk.max_height: 1.0; 148 | tree_pos_msg_.markers[j].pose.position.z = (trunk.max_height>0) ? (trunk.position(2) + trunk.max_height/2) : trunk.position(2); 149 | tree_pos_msg_.markers[j].pose.orientation.x = 0.0; 150 | tree_pos_msg_.markers[j].pose.orientation.y = 0.0; 151 | tree_pos_msg_.markers[j].pose.orientation.z = 0.0; 152 | tree_pos_msg_.markers[j].pose.orientation.w = 1.0; 153 | tree_pos_msg_.markers[j].scale.x = 0.2; 154 | tree_pos_msg_.markers[j].scale.y = 0.2; 155 | tree_pos_msg_.markers[j].scale.z = (trunk.max_height>0) ? trunk.max_height : 1.0; //height; 156 | 157 | tree_pos_msg_.markers[j].color.r = 255; 158 | tree_pos_msg_.markers[j].color.g = 255; 159 | tree_pos_msg_.markers[j].color.b = 0; 160 | 161 | 162 | tree_pos_msg_.markers[j].color.a = 1.0; 163 | 164 | tree_pos_msg_.markers[j].lifetime = ros::Duration(); 165 | j+=1; 166 | } 167 | tree_pos_pub_.publish(tree_pos_msg_); 168 | 169 | } 170 | 171 | 172 | void Visualisation::publishGroundCloud(pcl::PointCloud&pcloud, ros::Time timestamp) 173 | { 174 | pcl::PointCloud cloud_ground_color(pcloud.points.size(),1,pcl::PointXYZRGB(255,0,0)); 175 | pcl::copyPointCloud(pcloud,cloud_ground_color); 176 | sensor_msgs::PointCloud2 cloud_msg; 177 | pcl::toROSMsg(cloud_ground_color,cloud_msg); 178 | cloud_msg.header.frame_id = "base_link"; 179 | ground_cloud_pub_.publish(cloud_msg); 180 | } 181 | 182 | void Visualisation::publishMapAndBeliefImages(pose2D_t pose_particle, Eigen::Vector2d local_frame, cv::Mat ucm_image, ImageMap acm_map, const std::vector &robot_poses_pixel) 183 | { 184 | 185 | double grid_size = acm_map.grid_size_; 186 | cv::Mat map_image_brg; 187 | cvtColor(acm_map.image_.clone(),map_image_brg,cv::COLOR_GRAY2BGR); 188 | 189 | cv::Mat ucm_image_color; 190 | 191 | // Apply the colormap: 192 | applyColorMap(ucm_image, ucm_image_color, cv::COLORMAP_AUTUMN); 193 | 194 | cv::Point particle_pos_pxl; 195 | particle_pos_pxl.x = (pose_particle.x - acm_map.image_frame_x_)/grid_size; 196 | particle_pos_pxl.y = (acm_map.image_frame_y_ - pose_particle.y)/grid_size; 197 | 198 | cv::Point particle_pos_local_pxl; 199 | particle_pos_local_pxl.x = (pose_particle.x - local_frame[0])/grid_size; 200 | particle_pos_local_pxl.y = (local_frame[1] - pose_particle.y)/grid_size; 201 | 202 | cv::Point template_corner; 203 | template_corner.x = std::max(particle_pos_pxl.x - particle_pos_local_pxl.x, 0); 204 | template_corner.y = std::max(particle_pos_pxl.y - particle_pos_local_pxl.y, 0); 205 | 206 | int diff_x = (particle_pos_pxl.x - particle_pos_local_pxl.x >= 0)? 0: (particle_pos_pxl.x - particle_pos_local_pxl.x); 207 | int diff_y = (particle_pos_pxl.y - particle_pos_local_pxl.y >= 0)? 0: (particle_pos_pxl.y - particle_pos_local_pxl.y); 208 | int dx = ucm_image.cols - diff_x; 209 | int dy = ucm_image.rows - diff_y; 210 | 211 | float cost = 0; 212 | float max_value_map = 0; 213 | 214 | 215 | cv::Mat_ map_image_brg_temp = map_image_brg; 216 | cv::Mat_ ucm_image_color_temp = ucm_image_color; 217 | for (int i = 0; i< dy ; i++) 218 | { 219 | for (int j=0; j< dx ; j++) 220 | { 221 | 222 | if(ucm_image.at(i,j)>0) 223 | { 224 | map_image_brg_temp(template_corner.y + i,template_corner.x + j)[0] = ucm_image_color_temp(i,j)[0]; 225 | map_image_brg_temp(template_corner.y + i,template_corner.x + j)[1] = ucm_image_color_temp(i,j)[1]; 226 | map_image_brg_temp(template_corner.y + i,template_corner.x + j)[2] = ucm_image_color_temp(i,j)[2]; 227 | } 228 | 229 | } 230 | 231 | } 232 | 233 | std::vector pattern = {0,0,0,1,1,1}; 234 | lineDot(map_image_brg_temp,template_corner, cv::Point(template_corner.x + dx, template_corner.y),cv::Scalar(0,128,255), pattern); 235 | lineDot(map_image_brg_temp,cv::Point(template_corner.x + dx, template_corner.y), cv::Point(template_corner.x + dx, template_corner.y + dy),cv::Scalar(0,128,255), pattern); 236 | lineDot(map_image_brg_temp,cv::Point(template_corner.x + dx, template_corner.y + dy), cv::Point(template_corner.x, template_corner.y + dy),cv::Scalar(0,128,255), pattern); 237 | lineDot(map_image_brg_temp,cv::Point(template_corner.x, template_corner.y + dy), template_corner,cv::Scalar(0,128,255), pattern); 238 | 239 | 240 | //Draw robot estimated position 241 | for(auto & robot_image_pos: robot_poses_pixel) 242 | { 243 | cv::Point r_img_pos; 244 | r_img_pos.x = robot_image_pos[0]; 245 | r_img_pos.y = robot_image_pos[1]; 246 | cv::circle(map_image_brg_temp, r_img_pos,2, cv::Scalar(0,255,255),cv::FILLED); //Scalar(0,255,128) 247 | } 248 | 249 | map_image_brg = map_image_brg_temp; 250 | 251 | if(config_->map_display_window_.size()>0) 252 | { 253 | if(config_->map_display_window_[0]map_display_window_[2]map_display_window_[1]map_display_window_[3]map_display_window_[1],config_->map_display_window_[3]),cv::Range(config_->map_display_window_[0],config_->map_display_window_[2])); 255 | } 256 | 257 | sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", map_image_brg).toImageMsg(); 258 | 259 | publish_matched_images_.publish(image_msg); 260 | } 261 | 262 | void Visualisation::lineDot(cv::OutputArray img, const cv::Point& pt1, const cv::Point& pt2, const cv::Scalar& color, const vector& pattern) 263 | { 264 | cv::LineIterator it(img.getMat(), pt1, pt2, cv::LINE_8); 265 | for(auto i=0; i 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "tree.h" 28 | #include "config_server.h" 29 | #include "utils.h" 30 | #include "map.h" 31 | 32 | 33 | struct RGB_color{ 34 | float r; 35 | float g; 36 | float b; 37 | }; 38 | 39 | class Visualisation 40 | { 41 | 42 | 43 | public: 44 | Visualisation(ros::NodeHandle node, ros::NodeHandle private_node, std::shared_ptr& config_ptr); 45 | ~Visualisation(); 46 | 47 | void publishClustersSegments(std::vector trees, ros::Time timestamp); 48 | void publishCloudInput(pcl::PointCloud&pcloud, ros::Time timestamp); 49 | void publishDownsampledSweep(pcl::PointCloud&cloud, ros::Time timestamp); 50 | void publishTrunksExtracted(std::vector trees, ros::Time timestamp); 51 | void publishGroundCloud(pcl::PointCloud&pcloud, ros::Time timestamp); 52 | void colorCloud(pcl::PointCloud &cloud, RGB_color colour); 53 | void publishMapAndBeliefImages(pose2D_t pose_particle, Eigen::Vector2d local_frame, cv::Mat ucm_image, ImageMap acm_map, const std::vector &robot_poses_pixel); 54 | void lineDot(cv::OutputArray img, const cv::Point& pt1, const cv::Point& pt2, const cv::Scalar& color, const vector& pattern); 55 | 56 | //void publishTrackedTrees(std::vector tracked_trees, ros::Time timestamp); 57 | std::vector rgb_rand_color( int n_colors); 58 | 59 | private: 60 | 61 | 62 | 63 | ros::NodeHandle node_; 64 | ros::NodeHandle private_node_; //for ros parameters 65 | std::shared_ptr config_; 66 | 67 | //Trunks markers in base link frame 68 | ros::Publisher tree_pos_pub_; 69 | visualization_msgs::MarkerArray tree_pos_msg_; 70 | 71 | //Publish clustered tree points 72 | ros::Publisher clusters_segment_pub_; 73 | visualization_msgs::Marker clusters_segment_msg_; 74 | 75 | ros::Publisher publish_matched_images_; 76 | 77 | ros::Publisher sweep_cloud_; 78 | ros::Publisher filtered_sweep_pub_; 79 | ros::Publisher ground_cloud_pub_; 80 | 81 | 82 | 83 | 84 | }; 85 | 86 | 87 | #endif --------------------------------------------------------------------------------