# Using LIDAR for path planning to move Stretch around obstacles **URL:** \ **Category:** Discuss\ **Created:** [March 24, 2021, 6:48pm UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136 "2021-03-24T18:48:30Z")\ **Posts on this page:** 6\ **Page:** 1
**Author:** ![rishi](https://avatars.discourse-cdn.com/v4/letter/r/90db22/32.png) [@rishi](https://forum.hello-robot.com/u/rishi)\ **Post date:** [March 24, 2021, 6:48pm UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/1 "2021-03-24T18:48:30Z")
Hi, I was thinking if it’s possible to use the LIDAR sensor in Stretch for path planning, and obstacle avoidance when Stretch moves around objects. I was looking at some path planing repos like these - [GitHub - AtsushiSakai/PythonRobotics: Python sample codes for robotics algorithms.](https://github.com/AtsushiSakai/PythonRobotics#path-planning) and was wondering if something like this could be used out of the box for Stretch? ---
**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\ **Post date:** [March 25, 2021, 4:40am UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/2 "2021-03-25T04:40:53Z")
Hi @rishi, it is possible to use the 2D Lidar sensor for path planning and obstacle avoidance. We use it within [FUNMAP](https://github.com/hello-robot/stretch_ros/blob/master/stretch_funmap/README.md), which is a ROS package we’ve developed for mapping, path planning, and navigation with obstacle avoidance. At the Python level, it should be possible to use the library you’ve linked with some effort. The 2D Lidar sensor has a nice [Python library](https://github.com/Roboticia/RPLidar) for fetching data and it looks like PythonRobotics library has a nice algorithm on [forming a grid map](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Mapping/lidar_to_grid_map/lidar_to_grid_map.py) from Lidar data. Let me know if you’d like to see an example or had another approach in mind. ---
**Author:** ![rishi](https://avatars.discourse-cdn.com/v4/letter/r/90db22/32.png) [@rishi](https://forum.hello-robot.com/u/rishi)\ **Post date:** [March 31, 2021, 4:43am UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/3 "2021-03-31T04:43:17Z")
Yes! It would be amazing to see an example. Thanks a lot! ---
**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\ **Post date:** [April 1, 2021, 5:46am UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/4 "2021-04-01T05:46:45Z")
Great! I will put one together over the weekend. Expect an update here on Monday. ---
**Author:** ![rishi](https://avatars.discourse-cdn.com/v4/letter/r/90db22/32.png) [@rishi](https://forum.hello-robot.com/u/rishi)\ **Post date:** [April 7, 2021, 3:14pm UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/5 "2021-04-07T15:14:37Z")
Hi @bshah, Just wanted to follow up on this. ---
**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\ **Post date:** [April 9, 2021, 6:38am UTC](https://forum.hello-robot.com/t/using-lidar-for-path-planning-to-move-stretch-around-obstacles/136/6 "2021-04-09T06:38:40Z")
Hi @rishi, thanks for the ping. I’ve written two demos for your reference and committed them to the following branch: [feature/rplidar\_tool](https://github.com/hello-robot/stretch_body/tree/feature/rplidar_tool). These scripts use the rplidar-roboticia library to interface with the 2D Lidar sensor. They are: - [stretch\_rplidar\_job.py](https://github.com/hello-robot/stretch_body/blob/feature/rplidar_tool/tools/bin/stretch_rplidar_jog.py) - Demonstrates the API used to interface with the sensor. Additionally, this script can be used to save Lidar scans into a dataset. This makes it easier to develop algorithms offline. - [stretch\_rplidar\_mapping.py](https://github.com/hello-robot/stretch_body/blob/feature/rplidar_tool/tools/bin/stretch_rplidar_mapping.py) - Demonstrates how to generate grid maps from Lidar scans (live or from a saved dataset). This script utilizes PythonRobotics’ [lidar\_to\_grid\_map](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Mapping/lidar_to_grid_map/lidar_to_grid_map.py) module to generate the grid maps. At the moment, the mapping script will only accept previously captured Lidar scans. There seems to be a bug with how Matplotlib visualizes the scans captured by the RPLidar library (I can provide more details if interested). I will push an update when resolved. To make the demos accessible, we plan to ship them with Stretch\_body. Until then, you may try them out using the following setup steps: ```bash mkdir ~/repos cd ~/repos git clone https://github.com/hello-robot/stretch_body.git --branch feature/rplidar_tool cd stretch_body/tools/bin curl https://raw.githubusercontent.com/AtsushiSakai/PythonRobotics/master/Mapping/lidar_to_grid_map/lidar_to_grid_map.py -o lidar_to_grid_map.py ``` Next, you may run the demos and visualize the grid maps (with a monitor plugged into the robot) using: ```bash python stretch_rplidar_jog.py --read_measurements ~/saved_scans.pickle [Ctrl-C to close the previous script and save the dataset] python stretch_rplidar_mapping.py --lidar_file ~/saved_scans.pickle ``` The grid maps will be visualized in a Matplotlib window, showing the open space as seen by the robot’s Lidar sensor. Generally, the next step in mapping is to use an algorithm called Iterative Closest Point (ICP) to align subsequent scans onto previous ones as the robot moves around. This yields a cloud of points that can be turned into a grid map that [builds up as the robot moves](https://youtu.be/dlB5rjIFGjs). With your map, you can use any common grid map based planning algorithm to plan paths around obstacles in the environment. Let me know if you have any questions, or come across any issues running these demos.