1 of 16

CISC849 RN&A:�Costmaps &

Discrete Motion Planning

Instructor: Christopher Rasmussen (cer@cis.udel.edu)

​

​

Course page: http://nameless.cis.udel.edu/class_wiki/index.php/CISC849_F2023

​

September 26, 2023 ❖ Class 9

Quiz #1

2 of 16

Sep. 26 robot status check

blinky

sonic

qbert

zelda

yoshi

splinter

3 of 16

HW #3: Wall-following (link)

  • Drive forward while keeping a "wall" a fixed distance to the left or right

​

​

​

4 of 16

Workspace Representations

  • A standard way to represent obstacles, whether given as polyhedra or point clouds, is to rasterize them to a 2-D or 3-D grid
  • This grid is often called an occupancy grid or costmap
  • Parameters
    • Size: w x h meters
    • Resolution: How large shall each grid cell be?
    • Frame: Will the costmap be fixed or move with the robot? If the latter, what offset to use for the robot?
  • Projection of robot R(q) to costmap is called its footprint

5 of 16

What Goes into a Grid Cell?

  • Most basic idea:
    • 0 if no obstacle definitely intersects this cell (aka "free space")
    • 1 if an obstacle even partially intersects this cell (aka "obstacle space")
  • Issues
    • Should inflate obstacles by robot radius to give better sense of where it can go
    • As with computer graphics, limited resolution produces aliasing
    • This aliasing effectively causes over-estimation of obstacle (and robot) size

​

6 of 16

From 3-D models to 2-D costmaps -- Example

shamelessly stolen from https://www.youtube.com/watch?v=xc8d6vZskV4

​

3-D environment in Gazebo

Costmap for particular "slice" projected to ground plane in rviz2

Costmap inflated by approximate robot footprint

7 of 16

Motion Planning with Costmaps

  • Now the planning problem is just grid search, right?

​

8 of 16

Motion Planning with Costmaps

  • Now the planning problem is just grid search, right?

​

9 of 16

Motion Planning with Costmaps

  • Now the planning problem is just grid search, right?
    • Hooray for Dijkstra's shortest-path algorithm!
    • Cells along way to goal can be used as "subgoals" for trajectory-following controller

​

10 of 16

Motion Planning with Costmaps

  • Now the planning problem is just grid search, right?
    • Hooray for Dijkstra's shortest-path algorithm!
    • Cells along way to goal can be used as "subgoals" for trajectory-following controller
  • But wait...
    • The obstacle inflation step is only realistic for approximately circular robots
    • We can't assume nonholonomic robots can move between any two adjacent cells
    • May still need obstacle avoidance while following trajectory because of aliasing

11 of 16

Costmaps and Lidar sensors

  • AND: Rasterizing a map seems straightforward, but what about when we just have sensor observations (i.e., lidar scans)?

Sample scan from RPLIDAR

12 of 16

Costmaps and Lidar sensors

  • Obstacle space, free space, and....unknown space

​

​

​

​

​

  • Each laser "beam" may both mark obstacle grid cells and clear free space grid cells
  • Quantifying confidence: Even with ground truth poses, sensor noise & multiple observations means the probability p(occupied) of each cell is not just 0, 1, or 0.5
  • Spoiler: slam_toolbox will do this for us

from Thrun et al.'s Probabilistic Robotics

13 of 16

Costmaps and Lidar sensors

14 of 16

Firing up the lidar

  • Plug in robot's Pi machine, plug in blue lidar USB cable
  • ssh to Pi (see posted table of IP addresses) with student account
    • ros2 run rplidar_ros rplidar_composition /scan:=/yoshi/scan --ros-args -p frame_id:=rplidar_link
  • UDBOT IS NOT NECESSARY (AND HAS SOME PROBLEMS), SO SKIP RED INSTRUCTIONS AND NEXT SLIDE Back on your laptop (after you get UDBot package for rviz2)
    • Verify that lidar scans are being published with ros2 topic echo /yoshi/scan, for example
    • ros2 run udbot_odom2tf udbot_odom2tf --remap odom:=/yoshi/odom
    • ros2 launch udbot_viz view_model.launch.py
    • Add LaserScan type in rviz2 "By topic name", set Fixed Frame to "rplidar_link"

15 of 16

HW #3

prep

  • In rviz2:
    • Set fixed_frame to odom
    • Check/add Odometry topic
    • Make sure topic name matches robot
    • History Policy = Keep All
    • Reliability Policy = Best Effort
    • Shorten arrow length for clearer display

​

​

​

ros2 run udbot_odom2tf udbot_odom2tf --remap odom:=/<robot name>/odom

ros2 launch udbot_viz view_model.launch.py

Clone these into your workspace src directory and colcon build:

https://github.com/iRobotEducation/create3_sim (Humble branch, need ros-humble-xacro package installed already)

https://github.com/UDRobotics/udbot

16 of 16

NVIDIA Isaac simulator

After sim install and udbot_isaac download (see course page), change USD path and run:

PYTHON_PATH beta_udbot_standalone.py