34761 Robot Autonomy

Notes and commands taken from lectures and exercises.

Table of Contents

Set up VNC Container
Exercises
ROS2 Concepts

Exercise Log

Week 2 Intro to ROS2
- ROS tutorial with turtlesim actions & services
Week 3 More on ROS2
- Spawn 10 turtles moving in circles
- Subscribe to LIDAR in turtlebot
- Make turtle2 follow turtle1 using tf2 broadcaster
Week 4 Localization
- ICP with stationary robot
- KISS ICP with constant velocity model
Week 5 Mapping
- Publish a map with random rectangle obstacles
- Create 2D map from LIDAR on a stationary robot
- Create 2D map from LIDAR on a moving robot
Week 6 Localization wrt maps
- Implement your own particle filter
Week 7 SLAM
- Create map using SLAM
Week 8 Navigation
- Implement Dijkstra in Notebook
- Implement Probabilistic Roadmap (PRM) in ROS
- Query the PRM with to get path from start to end
Week 9 Exploration
- Build PRM of partial map
- Compute information gain for each node in PRM
- Execute path with the highest information gain

Set up VNC Container

cd docker-ros2-desktop-vnc/humble
docker build -t tiryoh/ros2-desktop-vnc:humble .
docker run -p 6080:80 --security-opt seccomp=unconfined --shm-size=512m tiryoh/ros2-desktop-vnc:humble

Go to localhost:6080, password: ubuntu

Exercises

If there are errors encountered during colcon build, I think it requires the following package versions.

pip install setuptools_scm == 6.0
pip install setuptools == 58.2

sudo apt install ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-turtlebot3-gazebo ros-$ROS_DISTRO-turtlebot3*

For exercises with turtlebot, run these commands to set up the simulation environment.

cd autonomy_ws
source install/setup.bash
export ROS_DOMAIN_ID=11
export TURTLEBOT3_MODEL=burger
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:`ros2 pkg prefix my_turtlebot`/share/my_turtlebot/models
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(ros2 pkg prefix turtlebot3_gazebo)/share/turtlebot3_gazebo/models

Run teleoperation node for turtlebot.

ros2 run turtlebot3_teleop teleop_keyboard

Week 2 Exercise

ros2 run turtlesim turtlesim_node

ros2 service call /kill turtlesim/srv/Kill "{name: turtle1}"
ros2 service call /spawn turtlesim/srv/Spawn "{x: 6, y: 3, theta: 0., name: 'T63'}"
ros2 run turtlesim turtle_teleop_key --ros-args --remap turtle1/cmd_vel:=T63/cmd_vel
ros2 bag -o bag1 record /T63/cmd_vel
ros2 bag play bag1
ros2 action send_goal /T63/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 0.5}"

Week 3 Exercise

Task 1: ROS node to spawn 10 turtles and move them in circles. (see turtle_circles.py)

ros2 run turtlesim turtlesim_node
ros2 run my_turtlebot turtle_circles

Task 2: Launch burger turtlebot simulation, use RVIZ, obtain LIDAR scan. (see lidar_sub.py)

ros2 launch my_turtlebot turtlebot_simulation.launch.py
# In RViz, select '2D Pose Estimate', 
# click on the robot and drag  towards +x (red line)

ros2 run my_turtlebot lidar_sub

Task 3: TF2 broadcaster tutorial from here (see scripts inside tf_broadcaster package)

sudo apt-get install ros-humble-rviz2 ros-humble-turtle-tf2-py ros-humble-tf2-ros ros-humble-tf2-tools ros-humble-turtlesim

ros2 pkg create --build-type ament_python tf_broadcaster --dependencies tf2_ros rclpy
pip3 install scipy

# Set up simulation, spawn turtle1
ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

# Broadtcast turtle1
ros2 run tf_broadcaster broadcaster turtle1  
ros2 run tf2_ros tf2_echo turtle1 world

# Spawn and broadcast turtle2
ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: "turtle2"}"
ros2 run tf_broadcaster broadcaster turtle2

# Make turtle2 follow turtle1
ros2 run tf_broadcaster listener turtle1 turtle2

Week 4 Exercise

Task 1: Create a localization ROS node for your turtlebot (not complete)

Use the LIDAR scanner
Assume a constant velocity model
Publish a TF with the result of your odometry
Compare with the one provided by ROS

LIDAR Localization with ICP (see lidar_icp.py)

ros2 launch my_turtlebot turtlebot_simulation.launch.py
# In RViz, select '2D Pose Estimate and click towards +x (red line)'

ros2 run my_turtlebot lidar_icp
ros2 run my_turtlebot lidar_icp --ros-args --log-level debug

# To move turtlebot and run lidar_localization,
ros2 launch my_turtlebot turtlebot_localization.launch.py

Week 5 Exercise

Task 1: Publishing a map (see map_random.py)

Create a 2D occupancy grid using a 2D matrix
Fill random cells in the matrix with the value 100 and the rest with 0
Create an OccupancyGrid message and fill in the information (along with your map)
Publish the map on the topic /map with a frequency of 0.5Hz
Remember to add a transform that the map can live in (either static or dynamic) (what does this mean?)

# In one terminal,
ros2 launch my_turtlebot turtlebot_rviz.launch.py 

# In another terminal,
ros2 run my_turtlebot map_random

Task 2: Overlaying laser scans (see map_lidar_static.py)

Create an empty 2D map
Subscribe to the LIDAR topic and convert the LIDAR scan to Euclidean coordinates
Add them to your internal map representation
Publish the updated map

# In one terminal,
ros2 launch my_turtlebot turtlebot_simulation.launch.py
# In RViz, select '2D Pose Estimate', 
# click on the robot and drag  towards +x (red line)

# In another terminal,
ros2 run my_turtlebot map_lidar_static

Task 3: Moving the laser scan around in the map

Use the odometry you developed last time to move the pointcloud as the robot moves

Week 6 Exercise

Task 1: Using your own localization and accumulate a map when you drive around in the environment (see map_lidar.py)

Use a counter to define if a cell is free or occupied

# In one terminal,
ros2 launch my_turtlebot turtlebot_simulation.launch.py rviz_config_file:=/home/yufan/autonomy_ws/src/RobotAutonomy/rviz/nav2_yufan_view.rviz
# In RViz, select '2D Pose Estimate', 
# click on the robot and drag  towards +x (red line)

# In another terminal,
ros2 run my_turtlebot map_lidar

Task 2: The simulation we are using already uses a particle filter to perform localization [not done]

Set an initial starting point of the robot in rviz using the “2D pose estimate” button
Create a ROS2 node that
- Prints the number of particles used
- Computes the expected position of the robot based on the particles
Change the number of particles when you launch the simulation

# In one terminal,
ros2 launch my_turtlebot turtlebot_simulation.launch.py
# In RViz, select '2D Pose Estimate', 
# click on the robot and drag  towards +x (red line)

# In another terminal,
ros2 run my_turtlebot particle_sub

Task 3: Implement your own particle filter for localization in a map

Use the cmd_topic to estimate your motion model
Subscribe to the LIDAR topic and compute features
For each particle compute the error for each feature
Update your importance weights based on the error

Week 7 Exercise

Task 1: Create your own map in the simulation using SLAM

This task follows this tutorial
Launch simulation with SLAM

ros2 launch my_turtlebot turtlebot_simulation.launch.py slam:=True

ros2 run turtlebot3_teleop teleop_keyboard

Make sure that the fixed frame in RVIz is set to map.
Move the robot through the environment using teleoperation or RViz Nav2Goal.
Save the map.

ros2 run nav2_map_server map_saver_cli -f map
ros2 run nav2_map_server map_saver_cli -f sparse_map

Launch simulation without SLAM.

ros2 launch my_turtlebot turtlebot_simulation.launch.py map:=/home/yufan/autonomy_ws/src/RobotAutonomy/maps/yf_map.yaml

2 files map.pgm and map.yaml will be generated. Put them in the map directory of the my_turtlebot package and launch the simulation again without the slam argument

GIF below is sped up 4x.

Task 2: Use the odometry topic provided by the simulation to accumulate a map (if you don’t already have your own localization/odometry)

Task 3: Use a counter to define if a cell is free or occupied

Task 4: Continue on your own localization and map integration

Using your own localization, accumulate a map when you drive around in the environment

Week 8 Exercise

Task 1: Implement a probailistic roadmap method. (see planner_prm.py)

In ROS2, subscribe to the map topic and use that map for collision checks.
Assume constant cost value for all nodes.
Use any nearest neighbor implementation.
Create a function that takes as input a number of randomly samped node positions and returns the graph G.

ros2 launch my_turtlebot turtlebot_simulation.launch.py map:=/home/yufan/autonomy_ws/src/RobotAutonomy/maps/yf_map.yaml

ros2 run my_turtlebot planner_prm

Note

connect_k_nearest_neighbors()
Find K-nearest neighbours, create edge if the path is collision-free. This results in each node having at most K edges created, but usually having less than K.
create_k_nearest_edges()
Add K shortest edges that are collision free. This ensures that more nodes have K edges created, but the edges will not be to the K-nearest neighbours

Task 2: Query the plan from the previous exercise.

By searching for the list of vertices in the graph that connects the start and the goal.
Create a function that takes as input a start, goal, and a graph, and returns a list of nodes on the queried path.

Week 9 Exercise

Next-best-view exploration (see explore_nbv.py)

# Ignore these when running scripts
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose '{pose: {header: {frame_id: "map"}, pose: {position: {x: 0.0, y: 0.0, z:0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}'

ros2 launch my_turtlebot turtlebot_simulation.launch.py map:=/home/yufan/autonomy_ws/src/RobotAutonomy/maps/sparse_map.yaml

ros2 launch my_turtlebot turtlebot_simulation.launch.py slam:=True

ros2 run my_turtlebot map_lidar

ros2 run turtlebot3_teleop teleop_keyboard

ros2 run my_turtlebot explore_nbv

blue: known free cells	gray: unknown cells

Question:

when I load the partial map, it is being read as binary with {0,100} instead of trinary. how do I avoid this?
why does it make more sense when I set the max range to be $3.5 \div 2$ m? Is the max range half of that?

ROS2 Concepts

ros nodes

ros2 run <package_name> <executable_name>
ros2 node list
ros2 run turtlesim turtlesim_node --ros-args --remap __node:=my_turtle
ros2 node info <node_name>

remap: reassign default node properties

ros topics

continuous data streams, published/subscribed at any time regardless of senders/receivers
many to many connections

rqt_graph             # visualize nodes & topics
ros2 topic list
ros2 topic list -t    # append topic type
ros2 topic echo <topic_name>
ros2 topic info <topic_name> --verbose  # shows QOS profile
ros2 topic info <topic_name>
ros2 topic hz <topic_name>     # pub rate
ros2 interface show <msg_name>

ros2 topic pub <topic_name> <msg_type> '<args>'
ros2 topic pub --rate 1 /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}"

ros services

call-and-response model. cannot be cancelled preemptively.
useful for trigger behaviour
client sends a request message to the service server, receives a response message from server
msg structure: request, response
many service client to 1 service server

ros2 service list
ros2 service type <service_name>
ros2 service find <type_name>
ros2 interface show <type_name>
ros2 service call <service_name> <service_type> <arguments>
ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: ''}"

ros actions

for long running tasks, discrete behaviour, can be cancelled. provides feedback during execution.
msg structure: goal, result, intermediate result/feedback
action server (accepts/rejects requests) , action client

ros2 action list
ros2 action info <action_name>
ros2 interface show <action_name>
ros2 action send_goal <action_name> <action_type> <values>

ros parameters

Parameter: configuration value of a node (bool, float, str, list)

ros2 param list
ros2 param get <node_name> <parameter_name>
ros2 param set <node_name> <parameter_name> <value>
ros2 param dump <node_name>
ros2 param dump /turtlesim > turtlesim.yaml
ros2 param load <node_name> <param_file>
ros2 run <package_name> <executable_name> --ros-args --params-file <file_name>

rqt_console

GUI tool to introspect log messages, collect them over time in an organized manner

ros2 run rqt_console rqt_console

Fatal messages indicate the system is going to terminate to try to protect itself from detriment.
Error messages indicate significant issues that won’t necessarily damage the system, but are preventing it from functioning properly.
Warn messages indicate unexpected activity or non-ideal results that might represent a deeper issue, but don’t harm functionality outright.
Info messages indicate event and status updates that serve as a visual verification that the system is running as expected.
Debug messages detail the entire step-by-step process of the system execution.

# launch & bag
ros2 launch <package> <launch_file>
ros2 bag record <topic_name>
# choose name of rosbag
ros2 bag record -o <bag_name> /turtle1/cmd_vel /turtle1/pose
ros2 bag play <bag_name>

ros packages

package: organization unit of ROS2 code, uses ament as build system, colcon as build tool, create package using Python or CMake
add python executable to the build system by modifying setup.py

ros2 pkg create --build-type ament_python <pkg_name>
colcon build --packages-select <pkg_name> --symlink-install
source install/local_setup.bash

ros workspace

space to store entire project (source, build, resources)
- build: temporary build files
- log: store generated logs
- install: final binaries/libraries are stored, modified by build system
- src: modified by user, directory to place code
- ```
ros2_ws
├── build
├── install
│   ├── setup.bash
├── log
├── src
│   ├── my_package1 
│   ├── my_package2
```
~/ros2_ws$ source install/setup.bash
source /opt/ros/humble/setup.bash

yufanana/autonomy_ws