Skip to content

Commit b9ea660

Browse files
author
Fan Yang
committed
code release V1.0
0 parents  commit b9ea660

40 files changed

+4068
-0
lines changed

‎.gitignore

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
build
2+
3+
devel
4+
5+
install
6+
7+
logs
8+
9+
models/
10+
11+
.catkin_workspace
12+
13+
.catkin_tools
14+
15+
.vscode
16+
17+
*.png
18+
19+
*.ply
20+
21+
*.out
22+
23+
*.pt
24+
25+
__pycache__
26+
27+
data/
28+
29+
*~
30+
31+
*.bag
32+
33+
.DS_Store
34+
35+
wandb/

‎CMakeLists.txt

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(iplanner_node)
3+
4+
## Compile as C++11, supported in ROS Kinetic and newer
5+
# add_compile_options(-std=c++11)
6+
# set(CMAKE_CXX_STANDARD 11)
7+
# list(APPEND CMAKE_PREFIX_PATH "/usr/lib/libtorch")
8+
9+
## Find catkin macros and libraries
10+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
11+
## is used, also find other catkin packages
12+
find_package(catkin REQUIRED COMPONENTS
13+
roscpp
14+
rospy
15+
std_msgs
16+
image_transport
17+
sensor_msgs
18+
geometry_msgs
19+
message_generation
20+
)
21+
22+
23+
# find_package(Torch REQUIRED)
24+
# find_package(OpenCV REQUIRED)
25+
26+
## System dependencies are found with CMake's conventions
27+
find_package(Boost REQUIRED COMPONENTS system)
28+
29+
30+
31+
## Generate messages in the 'msg' folder
32+
# add_message_files(
33+
# FILES
34+
# Msg1.msg
35+
# Msg2.msg
36+
# )
37+
38+
## Generate services in the 'srv' folder
39+
# add_service_files(
40+
# FILES
41+
# Service1.srv
42+
# Service2.srv
43+
# )
44+
45+
## Generate actions in the 'action' folder
46+
# add_action_files(
47+
# FILES
48+
# Action1.action
49+
# Action2.action
50+
# )
51+
52+
## Generate added messages and services with any dependencies listed here
53+
# generate_messages(
54+
# DEPENDENCIES
55+
# geometry_msgs
56+
# std_msgs
57+
# )
58+
59+
catkin_package(
60+
# INCLUDE_DIRS include
61+
CATKIN_DEPENDS
62+
roscpp
63+
rospy
64+
std_msgs
65+
geometry_msgs
66+
# DEPENDS system_lib
67+
)
68+
69+
catkin_install_python(PROGRAMS src/data_collect_node.py
70+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
71+
)
72+
73+
catkin_install_python(PROGRAMS src/iplanner_node.py
74+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
75+
)
76+
77+
catkin_install_python(PROGRAMS src/iplanner_viz.py
78+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
79+
)

‎LICENSE

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
MIT License
2+
3+
Copyright (c) 2023 Fan Yang
4+
Robotic Systems Lab, ETH Zurich
5+
6+
Permission is hereby granted, free of charge, to any person obtaining a copy
7+
of this software and associated documentation files (the "Software"), to deal
8+
in the Software without restriction, including without limitation the rights
9+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10+
copies of the Software, and to permit persons to whom the Software is
11+
furnished to do so, subject to the following conditions:
12+
13+
The above copyright notice and this permission notice shall be included in all
14+
copies or substantial portions of the Software.
15+
16+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22+
SOFTWARE.

‎README.md

+180
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
# Imperative Path Planner (iPlanner)
2+
3+
## Overview
4+
5+
iPlanner is a local path planning package that uses front depth image and imperative learning.
6+
7+
**Keywords:** Navigation, Local Planning, Imperative Learning
8+
9+
### License
10+
11+
This code is released under the MIT License.
12+
13+
**Author: Fan Yang<br />
14+
Maintainer: Fan Yang, [email protected]**
15+
16+
The iPlanner package has been tested under ROS Noetic on Ubuntu 20.04. This is research code, and any fitness for a particular purpose is disclaimed.
17+
18+
<p align="center">
19+
<img src="img/example.jpg" alt="Method" width="70%"/>
20+
</p>
21+
22+
## Installation
23+
24+
#### Dependencies
25+
26+
To run iPlanner, you need to install [PyTorch](https://pytorch.org/). We recommend using [Anaconda](https://docs.anaconda.com/anaconda/install/index.html) for installation. Check the official website for installation instructions for Anaconda and PyTorch accordingly.
27+
28+
Then, please follow the instructions to install the necessary packages listed in the
29+
30+
requirements.txt
31+
32+
#### Simulation Environment Setup
33+
34+
Please refer to the Autonomous Exploration Development Environment repository for setting up the Gazebo simulation Environment: [Website](https://www.cmu-exploration.com/), switch to the branch **rgbd_camera**.
35+
36+
#### Building
37+
38+
To build the repository and set up the right Python version for running, use the command below:
39+
40+
catkin build iplanner_node -DPYTHON_EXECUTABLE=$(which python)
41+
42+
The Python3 should be the Python version you set up before with Torch and PyPose ready. If using the Anaconda environment, activate the conda env and check
43+
44+
which python
45+
46+
47+
## Training
48+
Go to the **iplanner** folder
49+
50+
cd <your_imperative_planenr_path>/iplanner
51+
52+
#### Pre-trained Network and Training Data
53+
Download the pre-trained network weights **plannernet.pt** [here](https://drive.google.com/file/d/1UD11sSlOZlZhzij2gG_OmxbBN4WxVsO_/view?usp=share_link) and put it into the **models** folder. Noted this pre-trained network has not been adapted to real-world data.
54+
55+
You can also collect data yourself either inside the simulation environment or in the real-world. Launch the **data_collect_node**
56+
57+
roslaunch iplanner_node data_collector.launch
58+
59+
Provide the information for the necessary topics listed in **config/data_params.yaml**. The collected data will be put into the folder **data/CollectedData**, and generate folders for different environments that you can specify in **config/data_params.yaml** under **env_name**.
60+
61+
For each of the environments, the data contains the structure of:
62+
63+
Environment Data
64+
├── camera
65+
| ├── camera.png
66+
│ └── split.pt
67+
├── camera_extrinsic.txt
68+
├── cloud.ply
69+
├── color_intrinsic.txt
70+
├── depth
71+
| ├── depth.png
72+
│ └── split.pt
73+
├── depth_intrinsic.txt
74+
├── maps
75+
│ ├── cloud
76+
│ │ └── tsdf1_cloud.txt
77+
│ ├── data
78+
│ │ ├── tsdf1
79+
├── data
80+
│ │ └── tsdf1_map.txt
81+
│ └── params
82+
│ └── tsdf1_param.txt
83+
└── odom_ground_truth.txt
84+
85+
You can download the example data we provided using the Google Drive link [here](https://drive.google.com/file/d/1bUN7NV7arMM8ASA2pTJ8hvdkc5N3qoJw/view?usp=sharing).
86+
87+
#### Generating Training Data
88+
89+
Navigate to the iplanner folder within your project using the following command:
90+
91+
cd <<YORU WORKSPACE>>/src/iPlanner/iplanner
92+
93+
Run the Python script to generate the training data. The environments for which data should be generated are specified in the file **collect_list.txt**. You can modify the data generation parameters in the **config/data_generation.json** file.
94+
95+
python data_generation.py
96+
97+
Once you have the training data ready, use the following command to start the training process. You can specify different training parameters in the **config/training_config.json** file.
98+
99+
python training_run.py
100+
101+
## Run iPlanner ROS node
102+
103+
Launch the simulation environment without the default local planner
104+
105+
roslaunch vehicle_simulator simulation_env.launch
106+
107+
Run the iPlanner ROS node without visualization:
108+
109+
roslaunch iplanner_node iplanner.launch
110+
111+
Or run the iPlanner ROS node with visualization:
112+
113+
roslaunch iplanner_node iplanner_viz.launch
114+
115+
### Path Following
116+
To ensure the planner executes the planned path correctly, you need to run an independent controller or path follower. Follow the steps below to set up the path follower using the provided launch file from the iplanner repository:
117+
118+
Download the default iplanner_path_follower into your workspace. Navigate to your workspace's source directory using the following command:
119+
120+
cd <<YOUR WORKSPACE>>/src
121+
122+
Then clone the repository:
123+
124+
git clone https://github.com/MichaelFYang/iplanner_path_follow.git
125+
126+
Compile the path follower using the following command:
127+
128+
catkin build iplanner_path_follow
129+
130+
Please note that this repository is a fork of the path following component from [CMU-Exploration](https://www.cmu-exploration.com/). You are welcome to explore and try different controllers or path followers suitable for your specific robot platform.
131+
132+
### Waypoint Navigation
133+
To send the waypoint through Rviz, please download the rviz waypoint plugin. Navigate to your workspace's source directory using the following command:
134+
135+
cd <<YOUR WORKSPACE>>/src
136+
137+
Then clone the repository:
138+
139+
git clone https://github.com/MichaelFYang/waypoint_rviz_plugin.git
140+
141+
Compile the waypoint rviz plugin using the following command:
142+
143+
catkin build waypoint_rviz_plugin
144+
145+
146+
### SmartJoystick
147+
148+
Press the **LB** button on the joystick, when seeing the output on the screen:
149+
150+
Switch to Smart Joystick mode ...
151+
152+
Now the smartjoystick feature is enabled. It takes the joystick command as motion intention and runs the iPlanner in the background for low-level obstacle avoidance.
153+
154+
## Config files
155+
156+
The params file **`data_params.yaml`** is for data collection
157+
158+
* **vehicle_sim.yaml** The config file contains:
159+
- **`main_freq`** The ROS node running frequency
160+
- **`odom_associate_id`** Depending on different SLAM setup, the odometry base may not be set under robot base frame
161+
162+
The params file **`vehicle_sim.yaml`** is for iPlanner ROS node
163+
164+
* **vehicle_sim.yaml** The config file contains:
165+
- **`main_freq`** The ROS node running frequency
166+
- **`image_flap`** Depending on the camera setup, it may require to flip the image upside down or not
167+
- **`crop_size`** The size to crop the incoming camera images
168+
- **`is_fear_act`** Using the predicted collision possibility value to stop
169+
- **`joyGoal_scale`** The max distance of goal sent by joystick in smart joystick model
170+
171+
172+
## Reference
173+
174+
If you utilize this codebase in your research, we kindly request you to reference our work. You can cite us as follows:
175+
176+
- Yang, F., Wang, C., Cadena, C., & Hutter, M. (2023). iPlanner: Imperative Path Planning. Robotics: Science and Systems Conference (RSS). Daegu, Republic of Korea, July 2023.
177+
178+
## Author
179+
180+
This codebase has been developed and maintained by [Fan Yang](https://github.com/MichaelFYang). Should you have any queries or require further assistance, you may reach out to him at [email protected]

‎bag/placeforrosbag.txt

Whitespace-only changes.

‎config/data_generation.json

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
{
2+
"folder_name": "CollectedData",
3+
"outfolder_name": "TrainingData",
4+
"voxel_size": 0.05,
5+
"robot_size": 0.3,
6+
"map_name": "tsdf1",
7+
"is_max_iter": true,
8+
"max_depth_range": 10.0,
9+
"is_flat_ground": true,
10+
"is_visualize": false
11+
}
12+

‎config/data_params.yaml

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
# images and odom topics
2+
main_freq: 2.5
3+
depth_topic: /rgbd_camera/depth/image
4+
color_topic: /rgbd_camera/color/image
5+
scan_topic: /velodyne_points
6+
odom_topic: /state_estimation
7+
8+
# camaera info topics
9+
depth_info_topic: /rgbd_camera/depth/camera_info
10+
color_info_topic: /rgbd_camera/color/camera_info
11+
camera_frame_id: rgbd_camera
12+
scan_frame_id: sensor
13+
base_frame_id: vehicle
14+
odom_associate_id: vehicle
15+
16+
# environment name
17+
env_name: test_env

‎config/training_config.json

+39
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
{
2+
"dataConfig": {
3+
"data-root": "data",
4+
"env-id": "training_list.txt",
5+
"env_type": "TrainingData",
6+
"crop-size": [360, 640],
7+
"max-camera-depth": 10.0
8+
},
9+
"modelConfig": {
10+
"model-save": "models/plannernet.pt",
11+
"resume": false,
12+
"in-channel": 16,
13+
"knodes": 5,
14+
"goal-step": 5,
15+
"max-episode-length": 25
16+
},
17+
"trainingConfig": {
18+
"training": true,
19+
"lr": 0.0001,
20+
"factor": 0.1,
21+
"min-lr": 0.000001,
22+
"patience": 4,
23+
"epochs": 50,
24+
"batch-size": 128,
25+
"w-decay": 0.001,
26+
"num-workers": 2,
27+
"gpu-id": 0
28+
},
29+
"logConfig": {
30+
"log-save": "models/log-",
31+
"test-env-id": 0,
32+
"visual-number": 10
33+
},
34+
"sensorConfig": {
35+
"camera-tilt": 0.2,
36+
"sensor-offsetX-ANYmal": 0.4,
37+
"fear-ahead-dist": 2.5
38+
}
39+
}

0 commit comments

Comments
 (0)