├── CMakeLists.txt ├── LICENSE ├── README.md ├── action ├── GPPointCloud.action └── PathPlanning.action ├── config ├── diff_drive │ └── diff_drive.yaml ├── gp_module │ └── gp_module.yaml └── path_planning │ ├── husky_env_a.yaml │ ├── husky_env_b.yaml │ └── jackal_env_a.yaml ├── launch ├── gp_navigation_husky_env_a.launch ├── gp_navigation_husky_env_b.launch ├── gp_navigation_jackal_env_a.launch ├── husky_gazebo.launch └── jackal_gazebo.launch ├── media ├── env_a_gazebo.png ├── env_b_gazebo.png ├── husky.jpeg ├── overview_diagram.png └── results.png ├── package.xml ├── requirements.txt ├── rviz_config └── gp.rviz ├── scripts ├── gp_mapping_module.py ├── path_planning_module_client.py ├── path_planning_module_server.py └── transform_node.py └── worlds ├── env_a.world └── env_b.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(gp_navigation) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | gazebo_ros 6 | rospy 7 | std_msgs 8 | nav_msgs 9 | visualization_msgs 10 | geometry_msgs 11 | actionlib_msgs 12 | roslaunch 13 | ) 14 | 15 | ################################################ 16 | ## Declare ROS messages, services and actions ## 17 | ################################################ 18 | 19 | ## Generate actions in the 'action' folder 20 | add_action_files( 21 | FILES 22 | PathPlanning.action 23 | GPPointCloud.action 24 | ) 25 | 26 | ## Generate added messages and services with any dependencies listed here 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | nav_msgs 31 | sensor_msgs 32 | actionlib_msgs 33 | ) 34 | 35 | ################################################ 36 | ## Declare ROS dynamic reconfigure parameters ## 37 | ################################################ 38 | 39 | catkin_package( 40 | LIBRARIES gp_navigation 41 | CATKIN_DEPENDS nav_msgs 42 | ) 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | include_directories( 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | ############# 53 | ## Install ## 54 | ############# 55 | 56 | install(DIRECTORY worlds models launch 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 58 | ) 59 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024 Abe Leininger 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. 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. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Gaussian Process-based Traversability Analysis for Terrain Mapless Navigation 2 | 3 | ![Algorithm Overview Diagram](media/overview_diagram.png) 4 | 5 | - Related Paper: [arXiv Preprint](https://arxiv.org/abs/2403.19010) 6 | - Video Demo: [YouTube](https://www.youtube.com/watch?v=rYURYf1w7f8) 7 | 8 | This work has been accepted for publication at 2024 IEEE International Conference on Robotics and Automation. [(ICRA 2024)](https://2024.ieee-icra.org/) 9 | 10 | ## Table of Contents 11 | 12 | - [Installation](#installation) 13 | - [Simulation Setup](#simulation-setup) 14 | - [Running](#running) 15 | - [Module Parameters](#module-parameters) 16 | - [Results](#results) 17 | - [Citation](#citation) 18 | 19 | ## Installation 20 | 21 | The code was tested on Ubuntu 20.04, using ROS Noetic, and Python 3.8.10. 22 | 23 | 1. Clone GitHub Repositories: 24 | 25 | ``` 26 | cd ~/catkin_ws/src 27 | git clone https://github.com/abeleinin/gp-navigation.git 28 | git clone https://github.com/merose/diff_drive.git 29 | ``` 30 | 31 | Clone the current repository and [merose/diff_drive](https://github.com/merose/diff_drive), which provides a Python differential-drive controller API. 32 | 33 | 2. Install Python and ROS dependencies: 34 | 35 | ``` 36 | cd gp-navigation 37 | pip install -r requirements.txt 38 | sudo apt install ros-noetic-tf2-sensor-msgs 39 | ``` 40 | 41 | 3. Download the model files for simulation environments a and b from this [OneDrive link](https://indiana-my.sharepoint.com/:f:/g/personal/aleinin_iu_edu/Esf6rV39RNtPrzw9a7iT24cBjYHK_rlYWmthE26xXv_ZOw?e=mCygab) into the `gp-navigation` package directory. 42 | 43 | 4. Build workspace: 44 | 45 | ``` 46 | cd ../.. 47 | catkin_make 48 | source devel/setup.bash 49 | ``` 50 | 51 | ## Simulation Setup 52 | 53 | ### Environments 54 | 55 |

56 | Environment A 57 | Environment B 58 |

59 | 60 | We tested our code in 2 Gazebo environments shown above. Originally, environment A is Map 1 from the our baselines repo [(PUTN)](https://github.com/jianzhuozhuTHU/putn). Environment B was chosen as a more challenging terrain with less flat areas. 61 | 62 | ### Robot Platform 63 | 64 | Husky Robot 65 | 66 | The [Clearpath Robotics](https://clearpathrobotics.com/) Husky robot was used during testing. We also include launch files to run our method with the Jackal robot. The LiDAR sensor we use is the Velodyne VLP-16. 67 | 68 | ### Groundtruth localization in Gazebo 69 | To use the ground truth localization for Husky in Gazebo, add the `p3d_base_controller` plugin in the Husky xacro file `husky_description/urdf/husky.urdf.xacro` 70 | ``` 71 | 72 | 73 | true 74 | 60.0 75 | base_link 76 | ground_truth/state 77 | 0.01 78 | world 79 | 0 0 0 80 | 0 0 0 81 | 82 | 83 | ``` 84 | - Note: you need to add the `p3d_base_controller` plugin into `jackal.urdf.xacro` file if you are using Jackal instead of Husky. 85 | 86 | ### A-LOAM localization 87 | 88 | We also tested our method with [aloam](https://github.com/HKUST-Aerial-Robotics/A-LOAM) localization to make a fair comparison with our baseline method [(PUTN)](https://github.com/jianzhuozhuTHU/putn). 89 | Since, the repo was created prior to ROS Noetic's release, but we we're still able to make it work with the following dependency version. We found that [ceres solve version 2.0.0](https://github.com/ceres-solver/ceres-solver/releases/tag/2.0.0) works and using the following [installation guide](http://ceres-solver.org/installation.html#linux). 90 | 91 | ## Running 92 | 93 | Here is a demo of how you'd launch and run Task 3 from our paper: 94 | 95 | 1. Spawn Husky into Environment B at the starting location: 96 | 97 | ``` 98 | roslaunch gp_navigation husky_gazebo.launch env:=env_b x:=-7 y:=-7 z:=-5.2 99 | ``` 100 | 101 | Use the `env` argument to chose the environment (default: `env_a`). 102 | 103 | 2. Next, use the corresponding `gp_navigation_{robot}_{env}.launch` file to launch our method with the appropriate robot and environment configurations: 104 | 105 | ``` 106 | roslaunch gp_navigation gp_navigation_husky_env_b.launch 107 | ``` 108 | 109 | In RViz you can use the `2D Nav Goal` tool to select a goal. To send more precise goals you can publish a Pose to the topic `/plan_path_to_goal` as shown below: 110 | 111 | ``` 112 | rostopic pub --once /plan_path_to_goal geometry_msgs/PoseStamped "header: 113 | seq: 0 114 | stamp: 115 | secs: 0 116 | nsecs: 0 117 | frame_id: 'world' 118 | pose: 119 | position: 120 | x: 10.0 121 | y: 18.0 122 | z: 0.0 123 | orientation: 124 | x: 0.0 125 | y: 0.0 126 | z: 0.0 127 | w: 1.0" 128 | ``` 129 | 130 | ## Module Parameters 131 | 132 | All the parameters for our method is stored in the [config/](config/) directory. **Click to expand** for parameter descriptions: 133 | 134 |
135 | GP Mapping Module Parameters 136 | 137 | GP Mapping configs located in [config/gp_module/](config/gp_module/). 138 | 139 | | Name | Description | Default | 140 | |--------------|-------------|---------| 141 | | length_in_x | map size in the x-axis | 10.0 | 142 | | length_in_y | map size in the y-axis | 10.0 | 143 | | resolution | Pixel density of maps grids | 0.2 | 144 | | inducing_points | Subset of points to train GP model on | 500 | 145 | 146 |
147 | 148 |
149 | Path Planning Module Parameters 150 | 151 | Path Planning configs located in [config/path_planning/](config/path_planning/). 152 | 153 | | Name | Description | Default | 154 | |--------------|-------------|---------| 155 | | step_len | Length of RRT* branching | 0.5 | 156 | | iter_max | Maximum interations of RRT* | 1000 | 157 | | radius | Circle about the robot, usually half the map size | 5.0 | 158 | | trav_limit | Threshold for traversability safety | Determines how agressive to plan. | 159 | | replan_dist | Distance to sub-goal before initiating replanning | Map size and compute dependent. | 160 | | critical | Limitation on what the robot can safely traverse | Environment and robot dependent. | 161 | | trav_weights | Weighting coefficients to determine their impact on traversability calculation | Environment and robot dependent. Weights should sum to 1. | 162 | | sub_goal_weights | Weighting for sub-goal selection method | Environment and robot dependent. Weights should sum to 1. | 163 | 164 |
165 | 166 | ## Results 167 | 168 | ![Result Table](media/results.png) 169 | 170 | Our baseline comparison was [PUTN: A Plane-fitting based Uneven Terrain Navigation Framework](https://arxiv.org/abs/2203.04541). The authors released their code here on GitHub: [jianzhuozhuTHU/putn](https://github.com/jianzhuozhuTHU/putn). We we're able to out perform their method in Task 2, which involved traversing the more challenging Environment B. 171 | 172 | ## Citation 173 | 174 | If you find this code or our work valuable, please cite: 175 | 176 | ``` 177 | @article{leininger2024, 178 | title={Gaussian Process-based Traversability Analysis for Terrain Mapless Navigation}, 179 | author={Leininger, Abe and Ali, Mahmoud and Jardali, Hassan and Liu, Lantao}, 180 | journal={arXiv preprint arXiv:2403.19010}, 181 | year={2024} 182 | } 183 | ``` 184 | 185 | -------------------------------------------------------------------------------- /action/GPPointCloud.action: -------------------------------------------------------------------------------- 1 | # GPPointCloud.action 2 | # Request 3 | --- 4 | # Result 5 | sensor_msgs/PointCloud2 pc2_elevation 6 | sensor_msgs/PointCloud2 pc2_magnitude 7 | sensor_msgs/PointCloud2 pc2_uncertainty 8 | --- 9 | # Feedback -------------------------------------------------------------------------------- /action/PathPlanning.action: -------------------------------------------------------------------------------- 1 | # PathPlanning.action 2 | # Request 3 | nav_msgs/Path path 4 | --- 5 | # Result 6 | std_msgs/Bool achieved 7 | --- 8 | # Feedback 9 | std_msgs/Bool final -------------------------------------------------------------------------------- /config/diff_drive/diff_drive.yaml: -------------------------------------------------------------------------------- 1 | # control rate 2 | rate: 20 3 | 4 | # paramters/gains 5 | kP: 1.6 6 | kA: 8.4 7 | kB: -1.2 8 | 9 | # limits 10 | max_linear_speed: 0.6 11 | min_linear_speed: 0.05 12 | max_angular_speed: 2.0 13 | min_angular_speed: -1.0 14 | linear_tolerance: 0.25 15 | angular_tolerance: 4.0 16 | 17 | # mode 18 | forwardMovementOnly: false -------------------------------------------------------------------------------- /config/gp_module/gp_module.yaml: -------------------------------------------------------------------------------- 1 | gp_map: 2 | length_in_x: 10.0 3 | length_in_y: 10.0 4 | resolution: 0.2 5 | inducing_points: 500 6 | -------------------------------------------------------------------------------- /config/path_planning/husky_env_a.yaml: -------------------------------------------------------------------------------- 1 | rrt_star: 2 | step_len: 0.5 3 | iter_max: 1200 4 | radius: 5.0 5 | trav_limit: 0.4 6 | replan_dist: 1.8 7 | critical: 8 | slope: 0.5236 9 | flatness: 0.5236 10 | step_height: 0.3 11 | trav_weights: 12 | step_height: 0.4 13 | flatness: 0.4 14 | slope: 0.2 15 | sub_goal_weights: 16 | dist: 0.8 17 | edge: 0.2 18 | -------------------------------------------------------------------------------- /config/path_planning/husky_env_b.yaml: -------------------------------------------------------------------------------- 1 | rrt_star: 2 | step_len: 0.5 3 | iter_max: 1000 4 | radius: 5.0 5 | trav_limit: 0.3 6 | replan_dist: 1.4 7 | critical: 8 | slope: 0.436 9 | flatness: 0.436 10 | step_height: 0.3 11 | trav_weights: 12 | step_height: 0.4 13 | flatness: 0.4 14 | slope: 0.2 15 | sub_goal_weights: 16 | dist: 0.8 17 | edge: 0.2 18 | -------------------------------------------------------------------------------- /config/path_planning/jackal_env_a.yaml: -------------------------------------------------------------------------------- 1 | rrt_star: 2 | step_len: 0.5 3 | iter_max: 700 4 | radius: 5.0 5 | trav_limit: 0.3 6 | replan_dist: 1.2 7 | critical: 8 | slope: 0.2517 9 | flatness: 0.2517 10 | step_height: 0.2 11 | trav_weights: 12 | step_height: 0.4 13 | flatness: 0.4 14 | slope: 0.2 15 | sub_goal_weights: 16 | dist: 0.8 17 | edge: 0.2 18 | -------------------------------------------------------------------------------- /launch/gp_navigation_husky_env_a.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/gp_navigation_husky_env_b.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/gp_navigation_jackal_env_a.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/husky_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /launch/jackal_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /media/env_a_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abeleinin/gp-navigation/597bfb7dafac686248deae8a3b6897108dc2bd9c/media/env_a_gazebo.png -------------------------------------------------------------------------------- /media/env_b_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abeleinin/gp-navigation/597bfb7dafac686248deae8a3b6897108dc2bd9c/media/env_b_gazebo.png -------------------------------------------------------------------------------- /media/husky.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abeleinin/gp-navigation/597bfb7dafac686248deae8a3b6897108dc2bd9c/media/husky.jpeg -------------------------------------------------------------------------------- /media/overview_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abeleinin/gp-navigation/597bfb7dafac686248deae8a3b6897108dc2bd9c/media/overview_diagram.png -------------------------------------------------------------------------------- /media/results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abeleinin/gp-navigation/597bfb7dafac686248deae8a3b6897108dc2bd9c/media/results.png -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gp_navigation 4 | 0.0.0 5 | Gaussian Process-based traversability analysis for terrain mapless navigation 6 | 7 | Abe Leininger 8 | 9 | BSD 3-Clause 10 | 11 | catkin 12 | 13 | gazebo_ros 14 | rospy 15 | std_msgs 16 | nav_msgs 17 | actionlib 18 | actionlib_msgs 19 | 20 | gazebo_ros 21 | rospy 22 | std_msgs 23 | 24 | gazebo_ros 25 | rospy 26 | std_msgs 27 | nav_msgs 28 | gazebo 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | actionlib==1.14.0 2 | gpytorch==1.11 3 | numpy==1.17.4 4 | py3rosmsgs==1.18.2 5 | rosnumpy==0.0.5.2 6 | rospy==1.16.0 7 | sensor_msgs==1.13.1 8 | tf==1.13.2 9 | tf2_ros==0.7.7 10 | tf2_sensor_msgs==0.7.7 11 | torch==2.0.1 12 | -------------------------------------------------------------------------------- /rviz_config/gp.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 | Splitter Ratio: 0.5 10 | Tree Height: 363 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Nav Goal1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | - /Current View1/Focal Point1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Traversability Map 28 | - Class: rviz/Displays 29 | Help Height: 70 30 | Name: Displays 31 | Property Tree Widget: 32 | Expanded: 33 | - /Global Options1 34 | - /Start/Goal1/Status1 35 | Splitter Ratio: 0.7588235139846802 36 | Tree Height: 727 37 | - Class: rviz/Help 38 | Name: Help 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 0.5 47 | Cell Size: 1 48 | Class: rviz/Grid 49 | Color: 160; 160; 164 50 | Enabled: true 51 | Line Style: 52 | Line Width: 0.029999999329447746 53 | Value: Lines 54 | Name: Grid 55 | Normal Cell Count: 0 56 | Offset: 57 | X: 0 58 | Y: 0 59 | Z: 0 60 | Plane: XY 61 | Plane Cell Count: 20 62 | Reference Frame: 63 | Value: true 64 | - Alpha: 1 65 | Class: rviz/Axes 66 | Enabled: true 67 | Length: 1 68 | Name: Axes 69 | Radius: 0.10000000149011612 70 | Reference Frame: 71 | Show Trail: false 72 | Value: true 73 | - Alpha: 1 74 | Class: rviz/RobotModel 75 | Collision Enabled: false 76 | Enabled: true 77 | Links: 78 | All Links Enabled: true 79 | Expand Joint Details: false 80 | Expand Link Details: false 81 | Expand Tree: false 82 | Link Tree Style: Links in Alphabetic Order 83 | base_footprint: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | base_link: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | front_bumper_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | front_left_wheel_link: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | front_right_wheel_link: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | imu_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | inertial_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | rear_bumper_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | rear_left_wheel_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | rear_right_wheel_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | top_chassis_link: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | top_plate_front_link: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | top_plate_link: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | top_plate_rear_link: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | user_rail_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | velodyne: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | velodyne_base_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | vlp16_mount_base_link: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | vlp16_mount_left_support: 168 | Alpha: 1 169 | Show Axes: false 170 | Show Trail: false 171 | Value: true 172 | vlp16_mount_plate: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | Value: true 177 | vlp16_mount_right_support: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | Value: true 182 | Name: RobotModel 183 | Robot Description: robot_description 184 | TF Prefix: "" 185 | Update Interval: 0 186 | Value: true 187 | Visual Enabled: true 188 | - Class: rviz/Marker 189 | Enabled: true 190 | Marker Topic: /world/dest 191 | Name: Start/Goal 192 | Namespaces: 193 | "": true 194 | Queue Size: 100 195 | Value: true 196 | - Class: rviz/Marker 197 | Enabled: true 198 | Marker Topic: /world/tree 199 | Name: Tree 200 | Namespaces: 201 | Path: true 202 | Queue Size: 100 203 | Value: true 204 | - Class: rviz/Marker 205 | Enabled: true 206 | Marker Topic: /path_marker 207 | Name: Path 208 | Namespaces: 209 | line_list: true 210 | spheres: true 211 | Queue Size: 100 212 | Value: true 213 | - Alpha: 1 214 | Autocompute Intensity Bounds: true 215 | Autocompute Value Bounds: 216 | Max Value: 10 217 | Min Value: -10 218 | Value: true 219 | Axis: Z 220 | Channel Name: intensity 221 | Class: rviz/PointCloud2 222 | Color: 255; 255; 255 223 | Color Transformer: Intensity 224 | Decay Time: 0 225 | Enabled: true 226 | Invert Rainbow: true 227 | Max Color: 255; 255; 255 228 | Min Color: 0; 0; 0 229 | Name: Traversability Map 230 | Position Transformer: XYZ 231 | Queue Size: 10 232 | Selectable: true 233 | Size (Pixels): 10 234 | Size (m): 0.20000000298023224 235 | Style: Boxes 236 | Topic: /trav_map 237 | Unreliable: false 238 | Use Fixed Frame: true 239 | Use rainbow: true 240 | Value: true 241 | - Class: rviz/Marker 242 | Enabled: true 243 | Marker Topic: /edge_nodes 244 | Name: Edge Nodes 245 | Namespaces: 246 | Path: true 247 | Queue Size: 100 248 | Value: true 249 | - Class: rviz/Marker 250 | Enabled: true 251 | Marker Topic: /frontier_nodes 252 | Name: Frontier Nodes 253 | Namespaces: 254 | Path: true 255 | Queue Size: 100 256 | Value: true 257 | Enabled: true 258 | Global Options: 259 | Background Color: 255; 255; 255 260 | Default Light: true 261 | Fixed Frame: world 262 | Frame Rate: 30 263 | Name: root 264 | Tools: 265 | - Class: rviz/Interact 266 | Hide Inactive Objects: true 267 | - Class: rviz/MoveCamera 268 | - Class: rviz/Select 269 | - Class: rviz/FocusCamera 270 | - Class: rviz/Measure 271 | - Class: rviz/SetGoal 272 | Topic: /plan_path_to_goal 273 | Value: true 274 | Views: 275 | Current: 276 | Class: rviz/Orbit 277 | Distance: 27.7502384185791 278 | Enable Stereo Rendering: 279 | Stereo Eye Separation: 0.05999999865889549 280 | Stereo Focal Distance: 1 281 | Swap Stereo Eyes: false 282 | Value: false 283 | Field of View: 0.7853981852531433 284 | Focal Point: 285 | X: 0 286 | Y: 0 287 | Z: 0 288 | Focal Shape Fixed Size: false 289 | Focal Shape Size: 0.05000000074505806 290 | Invert Z Axis: false 291 | Name: Current View 292 | Near Clip Distance: 0.009999999776482582 293 | Pitch: 0.5647972822189331 294 | Target Frame: 295 | Yaw: 3.9495487213134766 296 | Saved: ~ 297 | Window Geometry: 298 | Displays: 299 | collapsed: false 300 | Height: 1016 301 | Help: 302 | collapsed: false 303 | Hide Left Dock: false 304 | Hide Right Dock: false 305 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc020000000afb0000001200530065006c0065006300740069006f006e000000003d0000033d0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000024a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000000800480065006c007000000002e2000000b50000006e00ffffff000000010000027c0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000035a00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 306 | Selection: 307 | collapsed: false 308 | Time: 309 | collapsed: false 310 | Tool Properties: 311 | collapsed: false 312 | Views: 313 | collapsed: false 314 | Width: 1848 315 | X: 72 316 | Y: 27 317 | -------------------------------------------------------------------------------- /scripts/gp_mapping_module.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | from time import time 5 | 6 | ### to disable GPU 7 | # import os 8 | # os.environ['CUDA_VISIBLE_DEVICES']='-1' 9 | 10 | import torch 11 | import gpytorch 12 | 13 | import warnings 14 | warnings.filterwarnings('ignore') 15 | 16 | import rospy 17 | import actionlib 18 | import ros_numpy 19 | 20 | from sensor_msgs import point_cloud2 21 | from std_msgs.msg import Header 22 | from sensor_msgs.msg import PointCloud2 23 | from sensor_msgs.msg import PointField 24 | from gp_navigation.msg import GPPointCloudAction, GPPointCloudResult 25 | 26 | from gpytorch.means import ConstantMean 27 | from gpytorch.kernels import ScaleKernel, InducingPointKernel, RQKernel 28 | from gpytorch.distributions import MultivariateNormal 29 | 30 | class SGPModel(gpytorch.models.ExactGP): 31 | def __init__(self, train_x, train_y, likelihood, inducing_points): 32 | super(SGPModel, self).__init__(train_x, train_y, likelihood) 33 | self.mean_module = ConstantMean() 34 | num_inducing_pts = inducing_points 35 | initial_pts = range(0, train_x.shape[0], int(train_x.shape[0]/num_inducing_pts) ) 36 | inducing_variable = train_x[[r for r in initial_pts], :] 37 | self.base_covar_module = ScaleKernel(RQKernel(lengthscale=torch.tensor([0.7, 0.7]), alpha=torch.tensor([10]))) 38 | self.covar_module = InducingPointKernel(self.base_covar_module, inducing_points=inducing_variable, likelihood=likelihood) 39 | self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') 40 | 41 | def forward(self, x): 42 | mean_x = self.mean_module(x) 43 | covar_x = self.covar_module(x) 44 | return MultivariateNormal(mean_x, covar_x) 45 | 46 | 47 | class GPMappingModule: 48 | def __init__(self): 49 | rospy.init_node('GPMappingModule') 50 | rospy.loginfo('GP Mapping Module Started...') 51 | self.gp_action_server = actionlib.SimpleActionServer('gp_mapping_module', GPPointCloudAction, self.handle_matrix_request, False) 52 | self.gp_action_server.start() 53 | self.get_map_params() 54 | self.smpld_pcl_pub = rospy.Publisher('/elevation_pcl', PointCloud2, queue_size=1) 55 | self.oc_srfc_pcl_pub = rospy.Publisher('/elevation_srfc', PointCloud2, queue_size=1) 56 | self.oc_var_pcl_pub = rospy.Publisher('/elevation_var', PointCloud2, queue_size=1) 57 | 58 | self.gradx_pub = rospy.Publisher('/x_grad', PointCloud2, queue_size=1) 59 | self.grady_pub = rospy.Publisher('/y_grad', PointCloud2, queue_size=1) 60 | self.magnitude_pub = rospy.Publisher('/magnitude', PointCloud2, queue_size=1) 61 | self.uncertainty_pcl_pub = rospy.Publisher('/uncertainty', PointCloud2, queue_size=1) 62 | 63 | self.X = None 64 | self.Y = None 65 | self.Z = None 66 | 67 | self.device = 'cuda' if torch.cuda.is_available() else 'cpu' 68 | 69 | self.gp_model = None 70 | self.kernel = None 71 | self.kernel1 = None 72 | self.kernel2 = None 73 | self.likelihood = None 74 | self.mean_func = None 75 | 76 | self.pc2 = None 77 | 78 | self.skip = 3 79 | self.gp_trng_t = None 80 | self.pcl_size = None 81 | self.induc_pts_size = None 82 | self.dwnsmpld_pcl_size = None 83 | 84 | 85 | self.header = Header() 86 | self.header.seq = 0 87 | self.header.frame_id = 'velodyne_horizontal' 88 | self.fields = [PointField('x', 0, PointField.FLOAT32, 1), 89 | PointField('y', 4, PointField.FLOAT32, 1), 90 | PointField('z', 8, PointField.FLOAT32, 1), 91 | PointField('intensity', 12, PointField.FLOAT32, 1)] 92 | 93 | self.trng_file = None 94 | 95 | def handle_matrix_request(self, goal): 96 | rospy.loginfo('GP Mapping Module: Request received...') 97 | self.sph_pcl_sub = rospy.Subscriber('mid/points', PointCloud2, self.elevation_cb, queue_size=1) 98 | # self.sph_pcl_sub = rospy.Subscriber('/velodyne_points', PointCloud2, self.elevation_cb, queue_size=1) 99 | 100 | rospy.loginfo('Wating for magnitude pcl') 101 | rospy.wait_for_message('/magnitude', PointCloud2) 102 | 103 | result = GPPointCloudResult() 104 | result.pc2_elevation = self.pc2 105 | result.pc2_magnitude = self.magnitude 106 | result.pc2_uncertainty = self.uncertainty 107 | 108 | self.gp_action_server.set_succeeded(result) 109 | 110 | def get_map_params(self): 111 | self.resolution = rospy.get_param('~gp_map/resolution', 0.2) 112 | self.x_length = rospy.get_param('~gp_map/length_in_x', 10.0) 113 | self.y_length = rospy.get_param('~gp_map/length_in_y', 10.0) 114 | self.inducing_points = rospy.get_param('~gp_map/inducing_points', 500) 115 | 116 | def elevation_cb(self, pcl_msg): 117 | print('\n\n******************** ', pcl_msg.header.seq, pcl_msg.header.stamp.to_sec(), ' ********************') 118 | msg_rcvd_time = time() 119 | 120 | ################### extract points from pcl for trainging ############### 121 | # self.pcl_arr = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pcl_msg, remove_nans=True) 122 | pcl_arr = ros_numpy.point_cloud2.pointcloud2_to_array(pcl_msg, squeeze = True) 123 | 124 | self.header.seq = pcl_msg.header.seq 125 | self.header.stamp = pcl_msg.header.stamp 126 | self.pcl_size = np.shape(pcl_arr)[0] 127 | pcl_arr = np.round( np.array(pcl_arr.tolist(), dtype='float'), 4) # np.asarray(self.pcl_arr[0]) 128 | pcl_arr 129 | print('pcl_arr shape: ', np.shape(pcl_arr) ) 130 | 131 | ### downsample_pcl 132 | self.downsample_pcl(pcl_arr) 133 | self.sampling_grid() 134 | 135 | ### vsgp input datat 136 | d_in = np.column_stack( (self.Xs, self.Ys) ) 137 | d_out = np.array(self.Zs, dtype='float').reshape(-1,1) 138 | data_in_tensor = torch.tensor(d_in, dtype=torch.float32, device=self.device) 139 | data_out_tensor = torch.tensor(d_out, dtype=torch.float32, device=self.device) 140 | data_out_tensor = torch.flatten(data_out_tensor) 141 | training_data = (data_in_tensor, data_out_tensor) 142 | 143 | ### inducing inputs 144 | initial_pts = range(0, self.Xs.shape[0], int(self.Xs.shape[0] / self.inducing_points) ) 145 | inducing_variable = d_in[[r for r in initial_pts], :] 146 | print('inducing_variables: ', np.shape(inducing_variable) ) 147 | 148 | likelihood = gpytorch.likelihoods.GaussianLikelihood(mean=0) 149 | self.sgp_model = SGPModel(data_in_tensor, data_out_tensor, likelihood, self.inducing_points) 150 | # self.sgp_model.covar_module.inducing_points = inducing_variable 151 | 152 | if torch.cuda.is_available(): 153 | self.sgp_model = self.sgp_model.cuda() 154 | likelihood = likelihood.cuda() 155 | 156 | training_iterations = 1 157 | 158 | # Find optimal model hyperparameters 159 | self.sgp_model.train() 160 | likelihood.train() 161 | 162 | # Use the adam optimizer 163 | optimizer = torch.optim.Adam(self.sgp_model.parameters(), lr=0.01) 164 | 165 | # 'Loss' for GPs - the marginal log likelihood 166 | mll = gpytorch.mlls.ExactMarginalLogLikelihood(likelihood, self.sgp_model) 167 | 168 | for _ in range(training_iterations): 169 | # Zero backprop gradients 170 | optimizer.zero_grad() 171 | # Get output from model 172 | output = self.sgp_model(data_in_tensor) 173 | # Calc loss and backprop derivatives 174 | loss = -mll(output, data_out_tensor).mean() 175 | loss.backward() 176 | # iterator.set_postfix(loss=loss.item()) 177 | optimizer.step() 178 | # torch.cuda.empty_cache() 179 | 180 | Xtest_tensor = torch.tensor(self.grid, requires_grad=True) 181 | # Xtest_tensor = torch.tensor(self.grid).permute(1, 0) 182 | Xtest_tensor = Xtest_tensor.to(self.sgp_model.device) 183 | self.sgp_model.eval() 184 | likelihood.eval() 185 | with torch.autograd.set_grad_enabled(True): 186 | preds = self.sgp_model.likelihood(self.sgp_model(Xtest_tensor)) 187 | 188 | # print(preds.mean) 189 | self.mean = preds.mean.detach().cpu().numpy() 190 | self.var = preds.variance.detach().cpu().numpy() 191 | # self.var = preds.var.numpy() 192 | 193 | grad_one_like = torch.ones_like(preds.mean) 194 | grad_mean_res = torch.autograd.grad(preds.mean, Xtest_tensor, grad_outputs=grad_one_like, retain_graph=True)[0] 195 | grad_var_res = torch.autograd.grad(preds.variance, Xtest_tensor, grad_outputs=grad_one_like, retain_graph=True)[0] 196 | 197 | self.grad_mean = grad_mean_res.cpu().numpy() 198 | self.grad_var = grad_var_res.cpu().numpy() 199 | 200 | self.uncertainty_pcl() 201 | self.smpld_pcl() 202 | self.magnitude_pcl() 203 | self.sph_pcl_sub.unregister() 204 | 205 | print('\n>>total time: ', time() - msg_rcvd_time) 206 | 207 | def smpld_pcl(self): 208 | # x, y, z = self.convert_spherical_2_cartesian(self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.rds_fun) 209 | intensity = np.array(self.var, dtype='float32').reshape(-1, 1) 210 | smpld_pcl = np.column_stack( (self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.mean - 0.2, intensity)) 211 | self.pc2 = point_cloud2.create_cloud(self.header, self.fields, smpld_pcl) 212 | self.smpld_pcl_pub.publish(self.pc2) 213 | 214 | def uncertainty_pcl(self): 215 | # x, y, z = self.convert_spherical_2_cartesian(self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.rds_fun) 216 | intensity = np.array(self.var, dtype='float32').reshape(-1, 1) 217 | # smpld_pcl = np.column_stack( (self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), np.ones(self.grid.T[:][0].reshape(-1, 1).shape) * 5, self.var) ) 218 | smpld_pcl = np.column_stack( (self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.mean - 0.2, self.var)) 219 | self.uncertainty = point_cloud2.create_cloud(self.header, self.fields, smpld_pcl) 220 | self.uncertainty_pcl_pub.publish(self.uncertainty) 221 | 222 | ### Model Params ### 223 | def select_likelihood(self): 224 | self.likelihood = gpytorch.likelihoods.GaussianLikelihood() 225 | 226 | def select_mean_function(self): 227 | self.select_mean_function = gpytorch.mean_functions.Constant(0) 228 | 229 | def select_kernel(self): 230 | self.kernel1 = 0 231 | 232 | ### Downsample_pcl ### 233 | def sampling_grid(self): 234 | ### sample uniformaly according to vlp16 azimuth & elevation resolution ### 235 | x_range = self.x_length / 2 236 | y_range = self.y_length / 2 237 | x_s = np.arange(-x_range, x_range, self.resolution, dtype='float32') 238 | y_s = np.arange(-y_range, y_range, self.resolution, dtype='float32') 239 | self.grid = np.array(np.meshgrid(x_s,y_s)).T.reshape(-1,2) 240 | # print('grid: ', np.shape(self.grid)) 241 | 242 | def magnitude_pcl(self): 243 | intensity = np.sqrt(np.square(self.grad_var.T[:][0]) + np.square(self.grad_var.T[:][1])) 244 | # intensity = self.grad_var.T[:][0] + self.grad_var.T[:][1] 245 | # Subtract offset between where velodyne sensor is on Jackal robot 246 | grad_mean = np.sqrt(np.square(self.grad_mean.T[:][0]) + np.square(self.grad_mean.T[:][1])) - 0.4 247 | # grad_mean = self.grad_mean.T[:][0] + self.grad_mean.T[:][1] 248 | smpld_pcl = np.column_stack( (self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.mean - 0.2, grad_mean) ) 249 | self.magnitude = point_cloud2.create_cloud(self.header, self.fields, smpld_pcl) 250 | self.magnitude_pub.publish(self.magnitude) 251 | 252 | def gradx_pcl(self): 253 | # x, y, z = self.convert_spherical_2_cartesian(self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.rds_fun) 254 | intensity = np.array(self.grad_var.T[:][0], dtype='float32').reshape(-1, 1) 255 | smpld_pcl = np.column_stack((self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.grad_mean.T[:][0], intensity)) 256 | pc2 = point_cloud2.create_cloud(self.header, self.fields, smpld_pcl) 257 | self.gradx_pub.publish(pc2) 258 | 259 | def grady_pcl(self): 260 | # x, y, z = self.convert_spherical_2_cartesian(self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.rds_fun) 261 | intensity = np.array(self.grad_var.T[:][1], dtype='float32').reshape(-1, 1) 262 | smpld_pcl = np.column_stack((self.grid.T[:][0].reshape(-1,1), self.grid.T[:][1].reshape(-1,1), self.grad_mean.T[:][1], intensity)) 263 | pc2 = point_cloud2.create_cloud(self.header, self.fields, smpld_pcl) 264 | self.grady_pub.publish(pc2) 265 | 266 | 267 | def downsample_pcl(self, pcl_arr): 268 | ## sort original pcl based on thetas 269 | pcl_arr = pcl_arr[np.argsort(pcl_arr[:, 0])] 270 | self.Xs = pcl_arr.transpose()[:][0].reshape(-1,1) 271 | self.Ys = pcl_arr.transpose()[:][1].reshape(-1,1) 272 | self.Zs = pcl_arr.transpose()[:][2].reshape(-1,1) 273 | self.Is = pcl_arr.transpose()[:][3].reshape(-1,1) 274 | dist = np.sqrt(np.square(self.Xs) + np.square(self.Ys) + np.square(self.Zs)) 275 | idx = np.where(dist > 5) 276 | print('Before Downsample : ', self.Xs.shape[0] ) 277 | self.Xs = np.delete(self.Xs, idx) 278 | self.Ys = np.delete(self.Ys, idx) 279 | self.Zs = np.delete(self.Zs, idx) 280 | print('Xs : ', self.Xs.shape[0] ) 281 | print('Ys : ', self.Ys.shape[0] ) 282 | print('Zs : ', self.Zs.shape[0] ) 283 | 284 | def spin(self): 285 | rospy.spin() 286 | 287 | if __name__ =='__main__': 288 | gpServer = GPMappingModule() 289 | gpServer.spin() -------------------------------------------------------------------------------- /scripts/path_planning_module_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import time 4 | 5 | import tf 6 | from tf.transformations import * 7 | 8 | import math 9 | import rospy 10 | import actionlib 11 | import ros_numpy 12 | import numpy as np 13 | 14 | from nav_msgs.msg import Path, Odometry 15 | from std_msgs.msg import ColorRGBA, Header 16 | from geometry_msgs.msg import Point, PointStamped, PoseStamped, Vector3, Quaternion 17 | from sensor_msgs import point_cloud2 18 | from sensor_msgs.msg import PointCloud2, PointField 19 | from visualization_msgs.msg import Marker 20 | 21 | from gp_navigation.msg import PathPlanningAction, PathPlanningGoal, GPPointCloudAction, GPPointCloudGoal 22 | 23 | import tf.transformations as tft 24 | 25 | class RobotOdometryData: 26 | def __init__(self): 27 | self.position = None 28 | self.orientation = None 29 | self.yaw = None 30 | self.robot_odom_sub = rospy.Subscriber('/ground_truth/state', Odometry, self.robot_odom_sub) 31 | rospy.wait_for_message('/ground_truth/state', Odometry) 32 | 33 | def robot_odom_sub(self, odom): 34 | self.position = odom.pose.pose.position 35 | self.position.x = round(self.position.x, 2) 36 | self.position.y = round(self.position.y, 2) 37 | self.position.z = round(self.position.z, 2) 38 | self.orientation = odom.pose.pose.orientation 39 | quaternion = [self.orientation.x, 40 | self.orientation.y, 41 | self.orientation.z, 42 | self.orientation.w] 43 | euler = tft.euler_from_quaternion(quaternion) 44 | self.yaw = round(math.degrees(euler[2])) 45 | 46 | 47 | class Node: 48 | def __init__(self, point, parent=None, relative_angle=0): 49 | self.point = point 50 | self.parent = parent 51 | self.relative_angle = relative_angle 52 | self.cost = 0 53 | 54 | 55 | class PathPlanningModuleClient: 56 | def __init__(self): 57 | rospy.init_node('PathPlanningModuleClient') 58 | rospy.loginfo('Path Planning Module Client started...') 59 | 60 | ### Services to retrieve map and send path to controller ### 61 | self.path_planning_client = actionlib.SimpleActionClient('path_planning_action', PathPlanningAction) 62 | self.gp_mapping_client = actionlib.SimpleActionClient('gp_mapping_module', GPPointCloudAction) 63 | self.path_planning_client.wait_for_server() 64 | self.gp_mapping_client.wait_for_server() 65 | 66 | self.driver = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) 67 | 68 | ### RRT Star Constants ### 69 | self.step_len = rospy.get_param('~rrt_star/step_len', 0.5) 70 | self.iter_max = rospy.get_param('~rrt_star/iter_max', 1000) 71 | self.limit = rospy.get_param('~rrt_star/trav_limit', 0.3) 72 | self.radius = rospy.get_param('~rrt_star/radius', 5) 73 | self.replan_distance = rospy.get_param('~rrt_star/replan_dist', 1.5) 74 | 75 | ### Map Constants ### 76 | self.resolution = rospy.get_param('~gp_map/resolution', 0.2) 77 | self.x_length = rospy.get_param('~gp_map/length_in_x', 10.0) 78 | self.y_length = rospy.get_param('~gp_map/length_in_y', 10.0) 79 | self.x_min, self.x_max = -self.x_length / 2, self.x_length / 2 80 | self.y_min, self.y_max = -self.y_length / 2, self.y_length / 2 81 | self.x_range = self.y_range = (self.x_length / self.resolution) 82 | 83 | self.path_pub = rospy.Publisher('/world/path', Path, queue_size=10) 84 | 85 | ### Visualization Publishers ### 86 | self.trav_map_pub = rospy.Publisher('/trav_map', PointCloud2, queue_size=10) 87 | 88 | self.path_marker = rospy.Publisher('/path_marker', Marker, queue_size=10) 89 | 90 | self.path_viz_pub = rospy.Publisher('/world/path_viz', Path, queue_size=10) 91 | self.marker_dest_pub = rospy.Publisher('/world/dest', Marker, queue_size=10) 92 | self.marker_tree_pub = rospy.Publisher('/world/tree', Marker, queue_size=10) 93 | 94 | 95 | self.marker_tree = Marker() 96 | self.marker_tree.header.stamp = rospy.Time.now() 97 | self.marker_tree.header.frame_id = 'world' 98 | self.marker_tree.type = Marker.LINE_LIST 99 | self.marker_tree.pose.orientation.w = 1.0 100 | self.marker_tree.scale = Vector3(0.03, 0.03, 0.03) 101 | self.marker_tree.color = ColorRGBA(1, 1, 1, 0.7) # white 102 | self.marker_tree.id = 0 103 | self.marker_tree.ns = "Path" 104 | 105 | self.marker_node = Marker() 106 | self.marker_node.header.stamp = rospy.Time.now() 107 | self.marker_node.header.frame_id = 'world' 108 | self.marker_node.type = Marker.SPHERE_LIST 109 | self.marker_node.ns = "Path" 110 | self.marker_node.id = 1 111 | self.marker_node.pose.orientation.w = 1.0 112 | self.marker_node.scale = Vector3(0.07, 0.07, 0.07) 113 | self.marker_node.color = ColorRGBA(1, 1, 0, 0.8) # yellow 114 | 115 | self.marker_frontier = Marker() 116 | self.marker_frontier.header.stamp = rospy.Time.now() 117 | self.marker_frontier.header.frame_id = 'world' 118 | self.marker_frontier.type = Marker.SPHERE_LIST 119 | self.marker_frontier.ns = "Path" 120 | self.marker_frontier.id = 1 121 | self.marker_frontier.pose.orientation.w = 1.0 122 | self.marker_frontier.scale = Vector3(0.2, 0.2, 0.2) 123 | self.marker_frontier.color = ColorRGBA(0, 1, 0, 1.0) # blue 124 | self.marker_frontier_pub = rospy.Publisher('/frontier_nodes', Marker, queue_size=10) 125 | 126 | self.marker_edge = Marker() 127 | self.marker_edge.header.stamp = rospy.Time.now() 128 | self.marker_edge.header.frame_id = 'world' 129 | self.marker_edge.type = Marker.SPHERE_LIST 130 | self.marker_edge.ns = "Path" 131 | self.marker_edge.id = 1 132 | self.marker_edge.pose.orientation.w = 1.0 133 | self.marker_edge.scale = Vector3(0.2, 0.2, 0.2) 134 | self.marker_edge.color = ColorRGBA(0, 0, 0, 0.8) # black 135 | self.marker_edge_pub = rospy.Publisher('/edge_nodes', Marker, queue_size=10) 136 | 137 | self.neighbor_radius = 3 138 | 139 | self.header = Header() 140 | self.header.seq = 0 141 | self.header.frame_id = 'velodyne_horizontal' 142 | self.fields = [PointField('x', 0, PointField.FLOAT32, 1), 143 | PointField('y', 4, PointField.FLOAT32, 1), 144 | PointField('z', 8, PointField.FLOAT32, 1), 145 | PointField('intensity', 12, PointField.FLOAT32, 1)] 146 | 147 | ### Await PoseStamped from RVIZ ### 148 | rospy.loginfo('PathPlanningModuleClient: Awaiting 2D Nav Goal') 149 | self.plan_path_sub = rospy.Subscriber('/plan_path_to_goal', PoseStamped, callback=self.plan_path_to_goal_callback) 150 | 151 | def init_params(self): 152 | rospy.loginfo('PathPlanningModuleClient: Initialize params...') 153 | self.robot = RobotOdometryData() 154 | 155 | self.tf_base_to_world = tf.TransformListener() 156 | self.tf_world_to_base = tf.TransformListener() 157 | # self.tf_base_to_world.waitForTransform('world', 'velodyne_horizontal', rospy.Time(), rospy.Duration(5.0)) 158 | # self.tf_world_to_base.waitForTransform('velodyne_horizontal', 'world', rospy.Time(), rospy.Duration(5.0)) 159 | 160 | self.gp_mapping_client.send_goal(GPPointCloudGoal()) 161 | self.gp_mapping_client.wait_for_result() 162 | rospy.loginfo('PathPlanningModuleClient: GPPointClouds received') 163 | gp_response = self.gp_mapping_client.get_result() 164 | self.elevation_matrix = self.pc2_to_matrix(gp_response.pc2_elevation, 2) 165 | self.slope_matrix = (np.array(self.pc2_to_matrix(gp_response.pc2_magnitude, 2)) / 1.57).tolist() 166 | self.uncertainty_matrix = self.pc2_to_matrix(gp_response.pc2_uncertainty, 3) 167 | 168 | self.np_elevation_matrix = np.array(self.elevation_matrix) 169 | self.height = np.array(self.elevation_matrix).flatten() 170 | 171 | self.step_height_matrix = self.create_step_height_matrix() 172 | self.flatness_matrix = self.create_flatness_matrix() 173 | self.traversability_matrix = self.create_trav_matrix() 174 | 175 | self.front_parent = self.start_node 176 | self.back_parent = self.start_node 177 | self.front_leaves = [] 178 | self.back_leaves = [] 179 | self.frontier = [] 180 | self.edge = [] 181 | self.path_msg = Path() 182 | self.nodes = [self.start_node] 183 | 184 | def pc2_to_matrix(self, pc2, index): 185 | array = ros_numpy.point_cloud2.pointcloud2_to_array(pc2) 186 | 187 | matrix = [[0] * int(self.y_range) for _ in range(int(self.x_range))] 188 | for point in array: 189 | i = self.scale(round(point[0].item(), 2), self.x_min, self.x_max, 0, self.x_range) 190 | j = self.scale(round(point[1].item(), 2), self.y_min, self.y_max, 0, self.x_range) 191 | height = round(point[index].item(), 2) 192 | matrix[int(i)][int(j)] = height 193 | 194 | return matrix 195 | 196 | # Step Height map 197 | def create_step_height_matrix(self): 198 | matrix = [[0] * int(self.y_range) for _ in range(int(self.x_range))] 199 | 200 | size = 5 201 | 202 | X, Y = [], [] 203 | 204 | for i in range(int(self.x_range)): 205 | for j in range(int(self.x_range)): 206 | x = self.scale(i, 0, self.x_range, self.x_min, self.x_max) 207 | y = self.scale(j, 0, self.x_range, self.y_min, self.y_max) 208 | max_height = np.max(np.abs(self.get_sub_matrix(i, j, size, self.np_elevation_matrix) + 0.8)) 209 | matrix[i][j] = max_height 210 | X.append(x), Y.append(y) 211 | 212 | return matrix 213 | 214 | # Flatness map 215 | def create_flatness_matrix(self): 216 | matrix = [[0] * int(self.y_range) for _ in range(int(self.x_range))] 217 | 218 | size = 5 219 | 220 | X, Y = [], [] 221 | 222 | for i in range(int(self.x_range)): 223 | for j in range(int(self.x_range)): 224 | x_p = self.scale(i, 0, self.x_range, self.x_min, self.x_max) 225 | y_p = self.scale(j, 0, self.x_range, self.y_min, self.y_max) 226 | z = self.get_sub_matrix(i, j, size, self.np_elevation_matrix) 227 | 228 | span = (size // 2) * self.resolution 229 | x, y = np.meshgrid(np.linspace(-span, span, z.shape[0]), np.linspace(-span, span, z.shape[1])) 230 | 231 | x = x.reshape(-1, 1) 232 | y = y.reshape(-1, 1) 233 | z = z.reshape(-1, 1) 234 | 235 | Stack = np.hstack([y, x, np.ones(x.shape)]) 236 | A, _, _, _ = np.linalg.lstsq(Stack, z, rcond=None) 237 | a, b, c = A 238 | normal_vector = np.array([a[0], b[0], -1]) 239 | z_axis_unit_vector = np.array([0, 0, -1]) 240 | cosine_theta = np.dot(normal_vector, z_axis_unit_vector) / (np.linalg.norm(normal_vector) * np.linalg.norm(z_axis_unit_vector)) 241 | theta = np.arccos(cosine_theta) 242 | matrix[i][j] = theta 243 | X.append(x_p), Y.append(y_p) 244 | 245 | np_matrix = np.array(matrix) 246 | scaled = np_matrix / 1.57 247 | matrix = scaled.tolist() 248 | 249 | return matrix 250 | 251 | # Traversability map 252 | def create_trav_matrix(self): 253 | matrix = [[0] * int(self.y_range) for _ in range(int(self.x_range))] 254 | X, Y = [], [] 255 | 256 | # Traversability weights 257 | step_height_weight = rospy.get_param('~trav_weights/step_height', 0.3) 258 | flatness_weight = rospy.get_param('~trav_weights/flatness', 0.5) 259 | slope_weight = rospy.get_param('~trav_weights/slope', 0.2) 260 | 261 | # Traversability critical values 262 | step_height_crit = rospy.get_param('~critical/step_height', 0.3) 263 | flatness_crit = rospy.get_param('~critical/flatness', 0.5236) 264 | slope_crit = rospy.get_param('~critical/slope', 0.5236) 265 | 266 | for i in range(int(self.x_range)): 267 | for j in range(int(self.x_range)): 268 | x = self.scale(i, 0, self.x_range, self.x_min, self.x_max) 269 | y = self.scale(j, 0, self.x_range, self.y_min, self.y_max) 270 | trav = (step_height_weight * (self.step_height_matrix[i][j] / step_height_crit)) + \ 271 | (flatness_weight * (self.flatness_matrix[i][j] / flatness_crit)) + \ 272 | (slope_weight * (self.slope_matrix[i][j] / slope_crit)) 273 | matrix[i][j] = trav 274 | X.append(x), Y.append(y) 275 | 276 | scaled = self.normalize(matrix) 277 | matrix = scaled.tolist() 278 | 279 | mean = np.mean(np.array(self.uncertainty_matrix)) 280 | 281 | for i in range(int(self.x_range)): 282 | for j in range(int(self.x_range)): 283 | h = 10 284 | if i - h <= 25 <= i + h and \ 285 | j - h <= 25 <= j + h: 286 | continue 287 | elif self.uncertainty_matrix[i][j] >= mean * 1.4: 288 | matrix[i][j] = 1 289 | 290 | scaled = np.array(matrix) 291 | trav_column = np.column_stack((X, Y, self.height, scaled.flatten())) 292 | trav_pcl = point_cloud2.create_cloud(self.header, self.fields, trav_column) 293 | self.trav_map_pub.publish(trav_pcl) 294 | return matrix 295 | 296 | # Normalize matrix values from [0, 1] 297 | def normalize(self, matrix): 298 | np_matrix = np.array(matrix) 299 | min_trav = np.min(np_matrix) 300 | max_trav = np.max(np_matrix) 301 | scaled = (np_matrix - min_trav) / (max_trav - min_trav) 302 | return scaled 303 | 304 | # Linear interpolation function 305 | def scale(self, val, x1, y1, x2, y2): 306 | output = (val - x1) * (y2 - x2) / (y1 - x1) + x2 307 | return round(output, 1) 308 | 309 | def new_state(self, node_start, node_goal): 310 | dist, theta = self.get_distance_and_angle(node_start, node_goal) 311 | 312 | dist = min(self.step_len, dist) 313 | node_new = Node(Point(node_start.point.x + dist * math.cos(theta), 314 | node_start.point.y + dist * math.sin(theta), 0.2)) 315 | 316 | node_new.parent = node_start 317 | 318 | return node_new 319 | 320 | @staticmethod 321 | def nearest_neighbor(node_list, n): 322 | return node_list[int(np.argmin([math.hypot(nd.point.x - n.point.x, nd.point.y - n.point.y) 323 | for nd in node_list]))] 324 | 325 | def planning(self): 326 | local_goal_point_stamped = self.tf_world_to_base.transformPoint('velodyne_horizontal', self.goal_point_stamped) 327 | local_goal_node = Node(Point(local_goal_point_stamped.point.x, local_goal_point_stamped.point.y, 0.5)) 328 | 329 | start_time = time.time() 330 | for i in range(self.iter_max): 331 | if i % 10 == 0: 332 | rospy.loginfo('Planning iteration: ' + str(i)) 333 | 334 | node_rand = Node(Point(np.random.uniform(-5, 5), np.random.uniform(-5, 5), 0.2)) 335 | node_near = self.nearest_neighbor(self.nodes, node_rand) 336 | node_new = self.new_state(node_near, node_rand) 337 | 338 | if node_new.point.x**2 + node_new.point.y**2 > self.radius**2: 339 | continue 340 | 341 | if node_new and not self.is_collision(node_near, node_new, True): 342 | neighbor_index = self.find_near_neighbor(node_new) 343 | self.nodes.append(node_new) 344 | dist, _ = self.get_distance_and_angle(node_new, local_goal_node) 345 | if dist <= self.step_len: 346 | self.goal_found = True 347 | local_goal_node.parent = node_new.parent 348 | return self.extract_path(local_goal_node) 349 | 350 | if neighbor_index: 351 | self.choose_parent(node_new, neighbor_index) 352 | self.rewire(node_new, neighbor_index) 353 | 354 | x = node_new.point.x + 0.5 * math.cos(math.radians(0)) 355 | y = node_new.point.y + 0.5 * math.sin(math.radians(0)) 356 | if x ** 2 + y ** 2 > self.radius**2: 357 | self.frontier.append(node_new) 358 | new_node_point_stamped = self.point_to_point_stamped(node_new.point, 'velodyne_horizontal', 'world') 359 | self.marker_frontier.points.append(new_node_point_stamped.point) 360 | 361 | for node in self.nodes: 362 | if node.parent: 363 | new_node_point_stamped = self.point_to_point_stamped(node.point, 'velodyne_horizontal', 'world') 364 | new_node_parent_point_stamped = self.point_to_point_stamped(node.parent.point, 'velodyne_horizontal', 'world') 365 | self.marker_tree.points.append(new_node_point_stamped.point) 366 | self.marker_tree.points.append(new_node_parent_point_stamped.point) 367 | self.marker_node.points.append(new_node_point_stamped.point) 368 | 369 | self.marker_tree_pub.publish(self.marker_tree) 370 | self.marker_tree_pub.publish(self.marker_node) 371 | self.marker_edge_pub.publish(self.marker_edge) 372 | self.marker_frontier_pub.publish(self.marker_frontier) 373 | 374 | end_time = time.time() 375 | elapsed_time = end_time - start_time 376 | rospy.loginfo(f"Elapsed time: {elapsed_time} seconds") 377 | 378 | # Local minimum found 379 | if len(self.frontier) == 0: 380 | rospy.logerr('No frontier') 381 | else: 382 | self.nearest = self.nearest_to_goal(self.frontier, local_goal_node) 383 | return self.extract_path(self.nearest) 384 | 385 | @staticmethod 386 | def cost(node_p): 387 | node = node_p 388 | cost = 0.0 389 | 390 | while node.parent: 391 | cost += math.hypot(node.point.x - node.parent.point.x, node.point.y - node.parent.point.y) 392 | node = node.parent 393 | 394 | return cost 395 | 396 | def get_new_cost(self, node_start, node_end): 397 | dist, _ = self.get_distance_and_angle(node_start, node_end) 398 | 399 | return self.cost(node_start) + dist 400 | 401 | def choose_parent(self, node_new, neighbor_index): 402 | cost = [self.get_new_cost(self.nodes[i], node_new) for i in neighbor_index] 403 | 404 | cost_min_index = neighbor_index[int(np.argmin(cost))] 405 | node_new.parent = self.nodes[cost_min_index] 406 | 407 | def rewire(self, node_new, neighbor_index): 408 | for i in neighbor_index: 409 | node_neighbor = self.nodes[i] 410 | 411 | if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor): 412 | node_neighbor.parent = node_new 413 | 414 | def find_near_neighbor(self, node_new): 415 | r = 1 416 | dist_table = [math.hypot(nd.point.x - node_new.point.x, nd.point.y - node_new.point.y) for nd in self.nodes] 417 | dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and 418 | not self.is_collision(node_new, self.nodes[ind], False)] 419 | 420 | return dist_table_index 421 | 422 | def point_to_point_stamped(self, curr_point, source_frame, target_frame): 423 | point_stamped = PointStamped() 424 | point_stamped.point = curr_point 425 | point_stamped.header.frame_id = source_frame 426 | return self.tf_world_to_base.transformPoint(target_frame, point_stamped) 427 | 428 | def two_node_coord_to_matrix(self, node1, node2): 429 | x1 = round(self.scale(node1.point.x, self.x_min, self.x_max, 0, self.x_range)) 430 | y1 = round(self.scale(node1.point.y, self.y_min, self.y_max, 0, self.x_range)) 431 | x2 = round(self.scale(node2.point.x, self.x_min, self.x_max, 0, self.x_range)) 432 | y2 = round(self.scale(node2.point.y, self.y_min, self.y_max, 0, self.x_range)) 433 | p1 = Point(x1, y1, 0) 434 | p2 = Point(x2, y2, 0) 435 | return p1, p2 436 | 437 | def is_collision(self, start, end, update_edge): 438 | start_point, end_point = self.two_node_coord_to_matrix(start, end) 439 | p_mid = Point(int((start_point.x + end_point.x) / 2), int((start_point.y + end_point.y) / 2), 0) 440 | 441 | size = 3 442 | np_trav_matrix = np.array(self.traversability_matrix) 443 | 444 | for e_i, e_j in self.edge: 445 | h = size // 2 446 | if e_i - h <= end_point.x <= e_i + h and \ 447 | e_j - h <= end_point.y <= e_j + h: 448 | return True 449 | 450 | mid_sub_matrix = self.get_sub_matrix(p_mid.x, p_mid.y, size, np_trav_matrix) 451 | end_point_sub_matrix = self.get_sub_matrix(end_point.x, end_point.y, size, np_trav_matrix) 452 | 453 | # sub matrices are not full 454 | if mid_sub_matrix.shape[0] != size or mid_sub_matrix.shape[1] != size: 455 | self.frontier.append(start) 456 | self.marker_frontier.points.append(self.point_to_point_stamped(start.point, 'velodyne_horizontal', 'world').point) 457 | return True 458 | if end_point_sub_matrix.shape[0] != size or end_point_sub_matrix.shape[1] != size: 459 | self.frontier.append(start) 460 | self.marker_frontier.points.append(self.point_to_point_stamped(start.point, 'velodyne_horizontal', 'world').point) 461 | return True 462 | 463 | num_collisions = (end_point_sub_matrix > self.limit).sum() 464 | 465 | if num_collisions >= size: 466 | if update_edge: 467 | self.edge.append((start_point.x, start_point.y)) 468 | self.marker_edge.points.append(self.point_to_point_stamped(start.point, 'velodyne_horizontal', 'world').point) 469 | return True 470 | elif num_collisions == 0: 471 | # Append to Frontier 472 | return False 473 | else: 474 | # 0 < num_collisions < size 475 | return True 476 | 477 | def get_sub_matrix(self, i, j, size, matrix): 478 | half_size = size // 2 479 | 480 | row_min = max(0, i - half_size) 481 | row_max = min(matrix.shape[0], i + half_size + 1) 482 | col_min = max(0, j - half_size) 483 | col_max = min(matrix.shape[1], j + half_size + 1) 484 | 485 | return matrix[row_min:row_max, col_min:col_max] 486 | 487 | def extract_path(self, curr_node): 488 | path = [self.start_node] 489 | 490 | while curr_node.parent.point is not None: 491 | curr_point = Point(round(curr_node.point.x, 1), round(curr_node.point.y, 1), round(curr_node.point.z, 1)) 492 | waypoint = Node(curr_point) 493 | path.insert(1, waypoint) 494 | if curr_node.parent.point == self.start_node.point: 495 | return path 496 | curr_node = curr_node.parent 497 | 498 | return path 499 | 500 | def get_distance_and_angle(self, node_start, node_end): 501 | dx = node_end.point.x - node_start.point.x 502 | dy = node_end.point.y - node_start.point.y 503 | return math.hypot(dx, dy), math.atan2(dy, dx) 504 | 505 | def distance_to_edge(self, x, y): 506 | distance_to_center = math.sqrt(x**2 + y**2) 507 | distance_to_edge = abs(distance_to_center - self.radius) 508 | return distance_to_edge 509 | 510 | # find nearest leaf node to the goal node 511 | def nearest_to_goal(self, leaves, goal_node): 512 | dist_array = [] 513 | edge_array = [] 514 | nodes_update = [] 515 | scores = [] 516 | dist_weight = rospy.get_param('~sub_goal_weights/dist', 0.8) 517 | edge_weight = rospy.get_param('~sub_goal_weights/edge', 0.2) 518 | 519 | for node in leaves: 520 | curr_dist, _ = self.get_distance_and_angle(node, goal_node) 521 | edge_dist = self.distance_to_edge(node.point.x, node.point.y) 522 | dist_array.append(curr_dist) 523 | edge_array.append(edge_dist) 524 | nodes_update.append((node, curr_dist, edge_dist)) 525 | 526 | normal_dist = self.normalize(dist_array) * dist_weight 527 | normal_edge = self.normalize(edge_array) * edge_weight 528 | 529 | for i in range(len(leaves)): 530 | scores.append((leaves[i], normal_dist[i] + normal_edge[i])) 531 | 532 | scores.sort(key=lambda x: x[1]) 533 | 534 | return scores[0][0] 535 | 536 | def path_vizualizer(self, nodes): 537 | points = [self.point_to_point_stamped(node.point, 'velodyne_horizontal', 'world').point for node in nodes] 538 | 539 | # Create Sphere markers 540 | sphere_marker = Marker() 541 | sphere_marker.header.frame_id = "world" 542 | sphere_marker.ns = "spheres" 543 | sphere_marker.type = Marker.SPHERE_LIST 544 | # sphere_marker.action = Marker.ADD 545 | sphere_marker.pose.orientation.w = 1.0 546 | sphere_marker.scale = Vector3(0.2, 0.2, 0.2) 547 | sphere_marker.color = ColorRGBA(1, 1, 1, 1) 548 | sphere_marker.points = points 549 | 550 | # Create line list marker 551 | line_marker = Marker() 552 | line_marker.header.frame_id = "world" 553 | line_marker.ns = "line_list" 554 | line_marker.type = Marker.LINE_LIST 555 | # line_marker.action = Marker.ADD 556 | line_marker.pose.orientation.w = 1.0 557 | line_marker.scale = Vector3(0.1, 0.1, 0) 558 | line_marker.color = ColorRGBA(0, 1, 0, 1) 559 | 560 | for i in range(len(points)-1): 561 | line_marker.points.append(points[i]) 562 | line_marker.points.append(points[i+1]) 563 | 564 | # Publish the markers 565 | self.path_marker.publish(sphere_marker) 566 | self.path_marker.publish(line_marker) 567 | 568 | def create_path(self, nodes): 569 | self.path_msg.header.frame_id = 'world' 570 | self.path_len = len(nodes) 571 | 572 | self.path_vizualizer(nodes) 573 | 574 | lst = [] 575 | for node in nodes: 576 | pose_stamped = PoseStamped() 577 | pose_stamped.header.frame_id = 'velodyne_horizontal' 578 | 579 | pose_stamped.pose.position.x = node.point.x 580 | pose_stamped.pose.position.y = node.point.y 581 | pose_stamped.pose.position.z = 0 582 | q = tft.quaternion_from_euler(0, 0, node.relative_angle) 583 | pose_stamped.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) 584 | 585 | world_pose_stamped = self.tf_base_to_world.transformPose('world', pose_stamped) 586 | lst.append(world_pose_stamped) 587 | self.path_msg.poses.append(world_pose_stamped) 588 | self.path_viz.poses.append(world_pose_stamped) 589 | 590 | self.path_pub.publish(self.path_msg) 591 | self.path_viz_pub.publish(self.path_viz) 592 | return self.path_msg 593 | # return lst 594 | 595 | def publish_marker_viz(self): 596 | # Origin and destination yellow markers 597 | self.marker_dest = Marker() 598 | self.marker_dest.header.frame_id = 'world' 599 | self.marker_dest.type = Marker.SPHERE_LIST 600 | self.marker_dest.action = Marker.ADD 601 | self.marker_dest.pose.orientation.w = 1.0 602 | self.marker_dest.scale = Vector3(0.3, 0.3, 0) 603 | self.marker_dest.color = ColorRGBA(0, 0, 1, 1) # yellow 604 | self.marker_dest.points.append(self.goal_point_stamped.point) 605 | self.marker_dest_pub.publish(self.marker_dest) 606 | 607 | self.path_viz = Path() 608 | self.path_viz.header.frame_id = 'world' 609 | 610 | def plan_path_to_goal_callback(self, goal_pose_stamped): 611 | self.goal_point_stamped = PointStamped() 612 | self.goal_point_stamped.point = goal_pose_stamped.pose.position 613 | self.goal_point_stamped.header.frame_id = 'world' 614 | self.goal_node = Node(goal_pose_stamped.pose.position) 615 | 616 | self.nearest = None 617 | self.goal_found = False 618 | self.start_point = Point(0, 0, 0.2) 619 | self.start_node = Node(self.start_point) 620 | self.marker_tree.points = [] 621 | self.init_params() 622 | 623 | self.publish_marker_viz() 624 | self.handle_path_planning() 625 | 626 | def distance_to_current_goal(self, data): 627 | currX = data.pose.pose.position.x 628 | currY = data.pose.pose.position.y 629 | 630 | dist = math.sqrt(math.pow(currX - self.current_goal.pose.position.x, 2) + math.pow(currY - self.current_goal.pose.position.y, 2)) 631 | 632 | if dist > self.replan_distance: 633 | # print("Distance to local goal:", dist) 634 | pass 635 | else: 636 | self.complete = True 637 | self.distance_to_local.unregister() 638 | return 639 | 640 | def handle_path_planning(self): 641 | try: 642 | self.complete = False 643 | req = PathPlanningGoal() 644 | while not self.goal_found: 645 | if not self.nearest: 646 | rospy.loginfo('Initial RRT Branching...') 647 | else: 648 | rospy.loginfo('RRT Re-branching...') 649 | self.init_params() 650 | self.path_planning_client.cancel_goal() 651 | 652 | # RRT branching iteration 653 | nodes = self.planning() 654 | path = self.create_path(nodes) 655 | self.current_goal = path.poses[-1] 656 | req.path = path 657 | 658 | self.path_planning_client.send_goal(req) 659 | 660 | self.distance_to_local = rospy.Subscriber('ground_truth/state', Odometry, self.distance_to_current_goal) 661 | while not self.complete: 662 | pass 663 | self.complete = False 664 | 665 | # Code will hang awaiting response from Path Planning Module Server 666 | rospy.loginfo('Awaiting response from Path Planning Module Server...') 667 | self.marker_edge.points = [] 668 | self.marker_frontier.points = [] 669 | self.marker_node.points = [] 670 | self.marker_tree.points = [] 671 | self.marker_tree_pub.publish(self.marker_tree) 672 | self.marker_tree_pub.publish(self.marker_node) 673 | self.marker_edge_pub.publish(self.marker_edge) 674 | self.marker_frontier_pub.publish(self.marker_frontier) 675 | 676 | rospy.loginfo('RRT Goal Achieved...') 677 | 678 | except rospy.ServiceException as e: 679 | rospy.logerr('Service call failed:', e) 680 | 681 | def spin(self): 682 | rospy.spin() 683 | 684 | if __name__ == '__main__': 685 | pathPlanningClient = PathPlanningModuleClient() 686 | pathPlanningClient.spin() -------------------------------------------------------------------------------- /scripts/path_planning_module_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import actionlib 5 | 6 | from geometry_msgs.msg import PoseStamped 7 | from std_msgs.msg import Float32, Bool 8 | from tf.transformations import * 9 | 10 | from gp_navigation.msg import PathPlanningAction, PathPlanningResult 11 | 12 | class PathPlanningModuleServer: 13 | def __init__(self): 14 | rospy.init_node('PathPlanningModuleServer') 15 | rospy.loginfo('Path Planning Module Server started...') 16 | self.path_planning_action_server = actionlib.SimpleActionServer('path_planning_action', PathPlanningAction, self.handle_path_planning, False) 17 | self.path_planning_action_server.start() 18 | 19 | self.path = None 20 | self.path_length = 0 21 | self.goal_reached = False 22 | self.i = 1 23 | 24 | self.driver = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) 25 | 26 | def handle_path_planning(self, req): 27 | received_path = req.path 28 | 29 | self.process_path(received_path) 30 | self.goal_achieved = rospy.Subscriber('goal_achieved', Bool, callback=self.goal_achieved_callback) 31 | 32 | while not self.goal_reached: 33 | if self.path_planning_action_server.is_preempt_requested(): 34 | rospy.loginfo("Goal Preempted") 35 | self.path_planning_action_server.set_preempted() 36 | reached = False 37 | break 38 | reached = True 39 | pass 40 | 41 | result = PathPlanningResult() 42 | 43 | self.goal_achieved.unregister() 44 | self.path = None 45 | self.path_length = 0 46 | self.goal_reached = False 47 | self.i = 1 48 | 49 | result.achieved = Bool(reached) 50 | self.path_planning_action_server.set_succeeded(result) 51 | 52 | 53 | def process_path(self, path): 54 | self.path = path 55 | for _ in path.poses: 56 | self.path_length += 1 57 | rospy.loginfo('Path of length ' + str(self.path_length - 1) + ' received') 58 | self.publish_next_goal() 59 | 60 | def publish_next_goal(self): 61 | goal = self.path.poses[self.i] 62 | point = 'Point(' + str(goal.pose.position.x) + ", " + str(goal.pose.position.y) + ") " 63 | rospy.loginfo(point + str(self.i) + ' of ' + str(self.path_length - 1)) 64 | self.driver.publish(goal) 65 | self.i += 1 66 | 67 | def distance_to_goal_callback(self, dist): 68 | if dist.data <= Float32(0.2).data: 69 | if self.i > (self.path_length - 1): 70 | self.goal_reached = True 71 | return 72 | self.publish_next_goal() 73 | 74 | def goal_achieved_callback(self, achieved): 75 | if achieved.data: 76 | if self.i > (self.path_length - 1): 77 | rospy.loginfo('Path Planning Module Server: goal_achieved unregistered...') 78 | self.goal_reached = True 79 | return 80 | self.publish_next_goal() 81 | 82 | def spin(self): 83 | rospy.spin() 84 | 85 | if __name__ == '__main__': 86 | pathPlanningModuleServer = PathPlanningModuleServer() 87 | pathPlanningModuleServer.spin() -------------------------------------------------------------------------------- /scripts/transform_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import tf2_ros 5 | import tf.transformations as tf_trans 6 | from nav_msgs.msg import Odometry 7 | from sensor_msgs.msg import PointCloud2 8 | from geometry_msgs.msg import TransformStamped, Quaternion 9 | from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud 10 | 11 | def pointcloud_callback(data): 12 | try: 13 | trans = tf_buffer.lookup_transform('world', 'velodyne', rospy.Time(0), rospy.Duration(1.0)) 14 | q = [ 15 | trans.transform.rotation.x, 16 | trans.transform.rotation.y, 17 | trans.transform.rotation.z, 18 | trans.transform.rotation.w 19 | ] 20 | 21 | euler_angles = tf_trans.euler_from_quaternion(q) 22 | roll = euler_angles[0] 23 | pitch = euler_angles[1] 24 | counter_rotation = tf_trans.quaternion_from_euler(-roll, -pitch, 0) 25 | 26 | counter_trans = TransformStamped() 27 | counter_trans.header.stamp = rospy.Time.now() 28 | counter_trans.header.frame_id = 'velodyne' 29 | counter_trans.child_frame_id = 'velodyne_horizontal' 30 | counter_trans.transform.rotation = Quaternion(*counter_rotation) 31 | 32 | broadcaster.sendTransform(counter_trans) 33 | trans1 = tf_buffer.lookup_transform('velodyne_horizontal', 'velodyne', rospy.Time(0), rospy.Duration(1.0)) 34 | 35 | cloud_out = do_transform_cloud(data, trans1) 36 | 37 | cloud_out.header.frame_id = 'velodyne_horizontal' 38 | 39 | velodyne_pub.publish(cloud_out) 40 | 41 | except (tf2_ros.LookupException, tf2_ros.ExtrapolationException, tf2_ros.ConnectivityException, tf2_ros.TransformException) as ex: 42 | rospy.logerr(ex) 43 | 44 | 45 | if __name__ == '__main__': 46 | rospy.init_node('velodyne_horizontal_node', anonymous=True) 47 | 48 | tf_buffer = tf2_ros.Buffer() 49 | tf_listener = tf2_ros.TransformListener(tf_buffer) 50 | 51 | broadcaster = tf2_ros.TransformBroadcaster() 52 | 53 | velodyne_pub = rospy.Publisher('velodyne_horizontal_points', PointCloud2, queue_size=10) 54 | 55 | rospy.Subscriber("mid/points", PointCloud2, pointcloud_callback) 56 | 57 | rospy.spin() 58 | -------------------------------------------------------------------------------- /worlds/env_a.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://env_a 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /worlds/env_b.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 6 | 0 0 10 0 -0 0 7 | 0.8 0.8 0.8 1 8 | 0.2 0.2 0.2 1 9 | 10 | 1000 11 | 0.9 12 | 0.01 13 | 0.001 14 | 15 | -0.5 0.1 -0.9 16 | 17 | 0 18 | 0 19 | 0 20 | 21 | 22 | 0 0 -9.8 23 | 6e-06 2.3e-05 -4.2e-05 24 | 25 | 26 | 0.001 27 | 1 28 | 1000 29 | 30 | 31 | 32 | 33 | 12 34 | 35 | 36 | 0 37 | 0.4 0.4 0.4 1 38 | 0.7 0.7 0.7 1 39 | 1 40 | 41 | 42 | 43 | EARTH_WGS84 44 | 0 45 | 0 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | 1 53 | 54 | 0.166667 55 | 0 56 | 0 57 | 0.166667 58 | 0 59 | 0.166667 60 | 61 | 62 | 0 0 0 0 -0 0 63 | 64 | 0 0 0 0 -0 0 65 | 66 | 67 | model://env_b/env_b_path.dae 68 | 0.5 0.5 0.5 69 | 70 | 71 | 72 | 77 | 78 | 0 79 | 1 80 | 81 | 82 | 1 83 | 10 84 | 0 0 0 0 -0 0 85 | 86 | 87 | model://env_b/env_b_path.dae 88 | 0.5 0.5 0.5 89 | 90 | 91 | 92 | 93 | 94 | 1 95 | 1 96 | 0 0 0 97 | 0 98 | 0 99 | 100 | 101 | 1 102 | 0 103 | 0 104 | 1 105 | 106 | 0 107 | 108 | 109 | 110 | 111 | 0 112 | 1e+06 113 | 114 | 115 | 0 116 | 1 117 | 1 118 | 119 | 0 120 | 0.2 121 | 1e+13 122 | 1 123 | 0.01 124 | 0 125 | 126 | 127 | 1 128 | -0.01 129 | 0 130 | 0.2 131 | 1e+13 132 | 1 133 | 134 | 135 | 136 | 137 | 0 138 | 0 139 | 0 140 | 141 | 1 142 | 1 143 | 0.84494 -2.00124 0 0 -0 0 144 | 145 | 146 | 13225 499000000 147 | 250 553945383 148 | 1676607622 636704399 149 | 250135 150 | 151 | 0 0 -5 0 -0 0 152 | 1 1 1 153 | 154 | 0 0 -5 0 -0 0 155 | 0 0 0 0 -0 0 156 | 0 0 0 0 -0 0 157 | 0 0 0 0 -0 0 158 | 159 | 160 | 161 | 0 0 10 0 -0 0 162 | 163 | 164 | 165 | 166 | -85.605 -2.0093 45.3903 0 0.4553 -0.045282 167 | orbit 168 | perspective 169 | 170 | 171 | 174 | 175 | 176 | --------------------------------------------------------------------------------