Skip to content

Commit

Permalink
Added readme
Browse files Browse the repository at this point in the history
  • Loading branch information
@patweatharva
patweatharva committed May 23, 2024
1 parent 413b843 commit f5b3d4a
Show file tree
Hide file tree
Showing 13 changed files with 22,585 additions and 9,631 deletions.
172 changes: 148 additions & 24 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,68 +1,192 @@
# turtlebot_graph_slam

This ROS package implements Graph SLAM (Simultaneous Localization and Mapping) for the Turtlebot robot.
This ROS package implements SLAM (Simultaneous Localization and Mapping) using Pose Graph Optimization with the help of Factor Graphs from GTSAM library for the Turtlebot2 robot. This package is designed in ROS noetic on ubuntu 20.04 machine. The package was desigen in [stonefish](https://github.com/patrykcieslak/stonefish) simulator using [Turtlebot2](https://bitbucket.org/udg_cirs/turtlebot_simulation/src/master/) Simulation Packages.

## Overview
Simultaneous Localization and Mapping (SLAM) is a critical challenge in robotics, where a robot concurrently builds or updates a map of an unknown environment and tracks its position within that environment. This work aims to implement a robust Simultaneous Localization and Mapping algorithm on a turtlebot platform using onboard sensors - 2D Lidar, Wheel Encoders, and Magnetometer compass. This study explores the implementation of online SLAM and full SLAM using factor graphs, specifically leveraging the Georgia Tech Smoothing and Mapping (GTSAM) library on a Turtlebot robot platform with the Robot Operating System (ROS). We describe the construction of the factor graph for SLAM, incorporating 2D Lidar scans and odometry data, and validate our approach through simulations and experiments in real world. Our study also incorporates the Closed form ICP covariance estimation method proposed in [here](https://ieeexplore.ieee.org/document/7153246). The Graph SLAM output, both with and without the proposed method, was recorded during real-world testing. Additionally, the impact of the keyframe triggering parameter was evaluated.

Factor graphs are a powerful tool for modeling and solving estimation problems. They are used in many applications, including SLAM, sensor fusion, and computer vision. In this project, we use factor graphs to model the SLAM problem. The factor graph consists of nodes and factors. The nodes represent the robot poses and the landmarks in the environment. The factors represent the constraints between the nodes. The constraints are derived from the sensor measurements, such as odometry and laser scans. The goal of the SLAM algorithm is to estimate the robot poses and the landmarks that best explain the sensor measurements.

<div align="center">
<img src="result_plots/factor_graph.png" alt="Factor Graph" width="300"/>
</div>


## Package Architecture
The turtlebot_graph_slam package has the following architecture:

<div align="center">
<img src="result_plots/package_architecture.png" alt="Factor Graph" width="300"/>
</div>

The architecture consists of several components:
- **EKF Odometry Node**: This node is responsible for performing the Odometry using Extended Kalman Filter on encoder readings obtained from the wheels.
- **Scan Matching Node**: This node is responsible for registering consecutive laser scans and estimating the robot's odometry using the Iterative Closest Point (ICP) algorithm.
- **Graph SLAM Node**: This node hoses the code for adding the Scan registration and Odometry factors to the graph and optimizing using the Levenberg-Marquardt algorithm or the ISAM2 algorithm, depending on the value of the `Use_ISAM2` parameter.
- **Configuration Files**: The package includes several configuration files that define the parameters for the SLAM algorithm, the EKF odometry, the ICP algorithm, and other components.

The package architecture is designed to provide a modular and flexible framework for SLAM on the Turtlebot2 robot. Each component can be customized or replaced to adapt to different requirements or sensor configurations as long as same data is added to the graph.

Following is the Factor Graph Structure for this package:

<div align="center">
<img src="result_plots/graph_struct.png" alt="Factor Graph" width="300"/>
</div>

Graph SLAM is a technique used in robotics to estimate the robot's trajectory and create a map of the environment simultaneously. This package utilizes the Turtlebot robot to perform Graph SLAM using sensors - 2D Lidar, Wheel Encoders, and Magnetometer Compass.

## Dependencies

This package has the following dependencies:

- ROS (Robot Operating System)
- Turtlebot packages
- GMapping package
- ROS Noetic
- Stonefish Simulator
- Turtlebot Simulation Packages
- GTSAM Library
- PCL Library
- Eigen Library
- Boost Library


## Installation

To install the turtlebot_graph_slam package, follow these steps:

1. Clone the repository into your ROS workspace:
### Install the dependencies:
1. ROS Noetic: Follow the instructions [here](http://wiki.ros.org/noetic/Installation/Ubuntu).
2. Stonefish Simulator: Follow the instructions [here](https://github.com/patrykcieslak/stonefish)
3. Turtlebot Simulation Packages: Follow the instructions [here](https://bitbucket.org/udg_cirs/turtlebot_simulation/src/master/)
4. GTSAM Library: Follow the instructions [here](https://gtsam.org/build/)
5. PCL Library: Follow the instructions [here](https://pointclouds.org/downloads/)
6. If you don't have Eigen and Boost libraries installed, you can install them using the following commands:
```
$ sudo apt install libeigen3-dev
$ sudo apt install libboost-all-dev
```
### Build the package:

To build the turtlebot_graph_slam package, follow these steps:
1. Clone the repository into your ROS catkin workspace:
```
$ git clone https://github.com/your-username/turtlebot_graph_slam.git
$ git clone https://github.com/patweatharva/turtlebot_graph_slam.git
```

2. Build the package:
```
$ cd ~/catkin_ws
$ catkin_make
$ catkin build
```
if you dont have catkin tools installed, you can install it from [here](https://catkin-tools.readthedocs.io/en/latest/installing.html)

3. Source the setup file:
```
$ source devel/setup.bash
```
You can add this line to your `.bashrc` file to run it automatically when you open a new terminal.

4. If while building the package can not find the GTSAM library, provide necessary path to the library in CMakeLists.txt file. Same goes for PCL, Eigen and Boost libraries.

## Usage

### IMPORTANT!!
1. Before launching the files change which mode (SIL/HIL) is in use from the [src/config.py](/src/config.py) file and launch respective file using folloing instructions.

* SIL - Software in Loop when working in Simulator.
* HIL - Hardware in Loop when working on real turtlebot robot.

To use the turtlebot_graph_slam package, follow these steps:

1. Launch the Turtlebot robot:
If you are using the stonefish simulator, you can launch the turtlebot_graph_slam package using the following command:
```
$ roslaunch turtlebot_graph_slam env_graph_slam_SIL.launch
```

This file is going to launch `turtlebot_integration.launch` file from turtlebot_simulation package. This file is going to launch the turtlebot in the stonefish simulator.


If you are using the real turtlebot robot, you can launch the turtlebot_graph_slam package using the following command:

```
$ roslaunch turtlebot_graph_slam env_graph_slam_HIL.launch
```
Once the simulation is running, you can visualize the SLAM output using RViz. The graph SLAM starts optmization process after atleast two keyframes are added thus start moving the robot using the teleop package. You can use the following command to run the teleop node:
```
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
```

If you don't have the teleop_twist_keyboard package installed, you can intall it from [here](http://wiki.ros.org/teleop_twist_keyboard)

## Parameters

The turtlebot_graph_slam package provides the following parameters:

- `saveGraph`: If set to true, the graph will be saved to a file in the directory [graph_viz](/graph_viz/) in .dot format.
- `saveWorldMap`: If set to true, the world map will be saved to a file in the directory [world_map](/pcl_viz/) in .pcd format.
-`Use_ISAM2`: If set to true, the ISAM2 algorithm will be used for optimization, otherwise the Levenberg-Marquardt algorithm will be used.

- The optmizer related parameters can be set in the [config/params.yaml](/config/params.yaml) file.

- The EKF Odometry related parameters can be set in the [src/config.py](/src/config.py) file.

- The ICP related parameters along with all scan registration node related parameter can be found in [include/scan_matching_node.hpp](/include/scan_match_node.hpp) file.

- The keyframe triggering parameter can be set in the [ScanHandler arguments](/src/scan_match_node.cpp).


## Launch Files

The turtlebot_graph_slam package provides the following launch files:

- `env_graph_slam_SIL.launch`: Launches the turtlebot_graph_slam package in the stonefish simulator.
- `env_graph_slam_HIL.launch`: Launches the turtlebot_graph_slam package in the real turtlebot robot.

## Resources
1. In the directory pcl_viz, you can find the world map generated by the SLAM algorithm in .pcd format along with few python scripts to visualize the point cloud data. These python scripts use pypcd4 and matplotlib libraries to visualize the point cloud data. You can install these libraries using the following commands:
```
$ roslaunch turtlebot_bringup minimal.launch
$ pip install pypcd4
$ pip install matplotlib
```
2. In the directory graph_viz, you can find the graph generated by the SLAM algorithm in .dot format. You can visualize this graph using the Graphviz software. You can install Graphviz using the following command:

2. Launch the GMapping package:
```
$ roslaunch turtlebot_navigation gmapping.launch
$ sudo apt install graphviz
```
3. To convert .dot format to pdf format use the following command:

3. Run the turtlebot_graph_slam node:
```
$ rosrun turtlebot_graph_slam turtlebot_graph_slam_node
$ dot -Tpdf <input_file>.dot -o <output_file>.pdf
```

## Parameters
4. You can also download plotjuggler to visualize the data in real time. You can download it from [here](https://github.com/facontidavide/PlotJuggler)

The turtlebot_graph_slam package provides the following parameters:
5. In the directory result_plots, you can find the plots generated by the SLAM algorithm. These plots include the ICP alignment before and after, the simulation testing results, and the real-world testing results.

- `scan_topic` (string): The topic name for the laser scan data. Default: `/scan`
- `odom_topic` (string): The topic name for the odometry data. Default: `/odom`
- `map_frame` (string): The frame ID for the map. Default: `map`
- `base_frame` (string): The frame ID for the base link of the robot. Default: `base_link`

## Launch Files
## Results
1. ICP Alignment:
<div align="center">
<img src="result_plots/ICP_before_after.png" alt="Factor Graph" width="300"/>
</div>

The turtlebot_graph_slam package provides the following launch files:
2. Simulation Testing:

- `turtlebot_graph_slam.launch`: Launches the turtlebot_graph_slam node with default parameters.
<div align="center">
<img src="result_plots/test_stonefish.png" alt="Factor Graph" width="300"/><img src="result_plots/world_map_simulation.png" alt="Factor Graph" height = "330"/>
</div>

3. Real World Testing:

<div align="center">
<img src="result_plots/Untitled%20design.jpg" alt="Factor Graph" width="300"/><img src="result_plots/test_real_rviz_2.png" alt="Factor Graph" width="300"/>
</div>
## License

This project is licensed under the MIT License. See the [LICENSE](LICENSE) file for more details.
This project is licensed under the MIT License. See the [LICENSE](LICENSE) file for more details.


### Lastly

*Don't forget to touch some grass!!*

## Contact

If you have any questions or suggestions, feel free to open an issue or contact me by [Email](mailto:patweatharva@gmail.com).
10 changes: 5 additions & 5 deletions include/scan_match_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,7 @@ class ScanHandler

double yaw = tf::getYaw(q);

if (fabs(yaw) > M_PI / 6 || fabs(current_odom_.pose.pose.position.x) >= 0.5 || fabs(current_odom_.pose.pose.position.y) >= 0.5)
if (fabs(yaw) > M_PI / 6 || fabs(current_odom_.pose.pose.position.x) >= (thresholdOdometry_/2) || fabs(current_odom_.pose.pose.position.y) >= (thresholdOdometry_/2))
{
odom_trigger_ = true;
}
Expand Down Expand Up @@ -511,10 +511,10 @@ class ScanHandler
ROS_INFO("ICP Fitness score --- %f", meanSquaredDistance);

// For checking scan Matching
if (current_scan_index < 5)
{
savePointcloud(currentScan, hypothesis_[i], cloud_source_aligned , std::to_string(hypothesisIDs_[i]), std::to_string(current_scan_index));
};
// if (current_scan_index < 5)
// {
// savePointcloud(currentScan, hypothesis_[i], cloud_source_aligned , std::to_string(hypothesisIDs_[i]), std::to_string(current_scan_index));
// };

if (meanSquaredDistance <= 1.5)
{
Expand Down
Loading

0 comments on commit f5b3d4a

Please sign in to comment.