ROS2 Labs

This repo a ROS2 workspace. Each folder in src/ is a ROS2 package

• src/kalman_filter - Kalman Filter Localization
• src/planner - Motion planner and P-controller
• src/particle_filter - Particle Filter Localization

Setup

To setup the packages, build and source this workspace:

rosdep install --from-paths src --ignore-src -r -y
pip install scikit-image numpy matplotlib
colcon build --symlink-install
source install/setup.bash

Notes on ROS2

Kalman Filter

This kalman_filter package implements a linear Kalman Filter algorithm for estimating the pose of a 2D mobile robot. It uses IMU measurements as control inputs, and wheel encoders for sensor measurements.

See my whitepaper report for a complete explanation.

Click here for a Youtube video of the Kalman Filter in action!

In below image, the green path is ground truth, while red is the Kalamn Filter's estimate.

Running

• ros2 launch kalman_filter kalman_filter.launch.py
• ros2 run turtlebot3_teleop teleop_keyboard (when not using a rosbag)
• In RViz, click on Publish Point to calculate MSE and view plots

Output on linear path

A fairly linear path, provided by a precorded rosbag.

In below image, the green path is ground truth, while red is the Kalamn Filter's estimate.

See output folder for more plots and stats

Output on nonlinear path (teleop)

Testing with a rather non-linear path. Evidently, a linear Kalman Filter is not suitable for such a path.

In below image, the green path is ground truth, while red is the Kalamn Filter's estimate.

See output folder for more plots and stats

ROS Graph

Links/Sources:

• Turtlebot3 datasheet
• Kalman filter - Wikipedia
• kalmanfilter.net
• fjp - ROS Kalman Filter for Sensor Fusion
• fjp - Kalman Filter Math Explanation, Simple English
• ROS sensor fusion tutorial with Kalman Filter - Q Matrix

Nav2 Demo

This tests out ROS2's Navigation Nav2 stack which generates a dynamic costmap, runs the AMCL node for localization, as well as some behavioural planners.

Click here for a demo video of the ROS2 navigation stack.

• ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
• ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=./src/planner/map/office_map/map.yaml
• Now use the 2D Pose Estimate and Nav2Goal functionality in RViz, and watch bevaviour of robot.

Links/Sources:

• nav_msgs/msg/OccupancyGrid.msg
• Published costmap topics
• Using RViz for ROS' Navigation stack
• ROS Map format
• Setting Up Lifecycle and Composition Nodes
• ROS2 Navigation stack intro:
  • ROS 2 Navigation
  • Navigating with a Physical Turtlebot 3
  • Getting Started

Lab 3 - Path planning and control

By Abhinav Agrahari, Ayush Ghosh, and Nick Shaju

Original repo

This planner package implements an A* Planner based on a given map, and a P-controller to autonomously guide the TurtleBot along the determined path.

Click here for a Youtube video of the path planner in action!

Pink/Purple/Blue is the inflated costmap. Green is the path the robot autonomously follows

Running

• ros2 launch planner planner.launch.py
• For defining goals in RViz:
  • ros2 run planner navigation_client.py
  • Then in RViz, click Publish Point on the goal point
• For pre-defining a goal from terminal (modify x and y as needed):
  • ros2 run planner navigation_client.py --ros-args -p predefined_goal:=True -p goal_x:=4.0 -p goal_y:=0.8

The Nav2 package provides an inflated static costmap of the environment on the topic /global_costmap/costmap of type nav_msgs/msg/OccupancyGrid. We can use this in A* planning.

See Lab3_extra_notes for more

preLab 3

Drive around in the turtlebot3 house and visualize lidar scans:

Click here for a demo video

• ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
• ros2 run turtlebot3_teleop teleop_keyboard
• ros2 run rviz2 rviz2 -d rviz_configs/prelab3_LaserScan.rviz

Lab 2 - Particle Filter Localization

By Abhinav Agrahari, Ayush Ghosh, and Nick Shaju

Original repo

This particle_filter package implements a Particle Filter for localizing a stationary 2D mobile robot using LiDAR data against a provided map, with a likelihood field model.

Click here for a Youtube video of the particle filter in action!

Yellow arrows are the Particles, while red is the super-imposed LiDAR data with the origin at average pose of the particle filter.

Running

• ros2 launch particle_filter particle_filter_launch.py. If you get a Substituion Failure, make sure the exectuable listed in the error is marked as executable on your file system
• Wait for the launch file to finish loading. If the map does not appear, try re-running the launch file.
• ros2 bag play src/particle_filter/bag_files/point2 or ros2 bag play src/particle_filter/bag_files/point5