FOSSBot4AI - Template

1. Activity Identity

2. Learning Objectives and Competences

3. Prerequisites

• Basic knowledge of Python programming

• Basic terminal and ROS2 command-line tools knowledge

• Basic understanding of ROS2 concepts (nodes, services)

• Basic understanding of 2D coordinate geometry (X, Y coordinates, grid resolution, map origin)

• Ability to capture evidence: screenshots, logs or observation tables

4. Required Material and Setup

5. Safety, Ethics and Accessibility Notes

If using a physical robot: When teleoperating the physical robot for mapping, follow the robot to ensure it does not bump into anything or fall down. Ensure batteries are safely charged.

6. Scenario and Problem Statement

Autonomous mobile robots need to navigate through environments that may contain obstacles. To do this, they often rely on occupancy grid maps, which represent the environment as a grid of cells, each indicating the probability of it being free or occupied, or in a case it wasn’t seen, unknown.

In this lab, you will use a simulated robot to create an occupancy grid map of a simple environment. You will then implement a ROS2 service that checks the occupancy of specific points in the map and extends it to check if a straight-line segment between two points intersects with any occupied or unknown cells.

7. Lab Workflow

8. Step-by-Step Instructions

Step 1 - Environment preparation

0. Check if you have the FOSSBot ros2 environment set up (if not, install it following the instructions available in the repository).

1. Start the FOSSBotEduSim container.

bash start_container.sh

or if you are on a machine without an NVIDIA GPU, use:

bash start_container_no_gpus.sh

2. [Only if you’re not using the Docker container] Open a terminal and source the ROS2 setup file:

source /opt/ros/jazzy/setup.bash

The Docker image added the ROS2 setup file to your .bashrc, so if you are using the container, you don’t need to run this command.

Step 2 - Simulator configuration and mapping

1. Launch the FOSSBot simulator with the single wall world:

ros2 launch fossbot_educational_description single.launch.py world:=sample_rooms.sdf

2. Ensure that in Gazebo you can see the robot, the blue walls and that RViz is running.

3. Open a new terminal and enter the docker container. Launch the SLAM toolbox to start mapping the environment:

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True

Note: More about SLAM will be explained in the Lab 12. For now, it is enough to know that the robot will use its position and sensors to create a map of the environment.

4. Open another new terminal and enter the docker container. Start teleoperation of the robot using the keyboard:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

5. Drive the robot around to create a map of the environment. Make sure that all areas are covered (everything inside the blue walls should be white on the map).

6. Save the map to a file:

ros2 run nav2_map_server map_saver_cli -f /fossbot_ros2/ws_fossbot/sample_rooms_map

❗❗IMPORTANT:❗❗ The map will be needed for the next lab, so make sure to save it in a safe place.

Expected result:

The complete map should look like this:

Step 3 - Occupancy checking and ROS2 services

❗❗IMPORTANT:❗❗: Do not close the terminal with the simulator and the SLAM toolbox. It will be needed in the next steps.

1. In ROS2 services allow to send a request and receive a response from a node (in this case the node will receive coordinates of a point and will return the occupancy value stored in the map for that point).

Let’s start with defining a new service type. Open a new terminal and enter the docker container. Create a new ROS2 package for the service definition:

cd /fossbot_ros2/ws_fossbot/src

ros2 pkg create --build-type ament_cmake map_occupancy_interfaces

mkdir -p /fossbot_ros2/ws_fossbot/src/map_occupancy_interfaces/srv

touch /fossbot_ros2/ws_fossbot/src/map_occupancy_interfaces/srv/CheckOccupancy.srv

Paste the following content into the CheckOccupancy.srv file:

geometry_msgs/Point point
---
bool occupied
string status

In the above service definition, the request contains a geometry_msgs/Point message with the coordinates of the point to check, and the response contains a boolean indicating whether the point is occupied and a string with a status message.

Now modify the package.xml file to add the necessary dependencies (paste this before tag):

  <depend>geometry_msgs</depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

Now open the CMakeLists.txt file and add the following lines (below find_package(ament_cmake REQUIRED)):

find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/CheckOccupancy.srv"
  DEPENDENCIES geometry_msgs
)

The rosidl_generate_interfaces macro generates the necessary code for the service based on the .srv file.

2. Now it’s time to implement the node that will check the occupancy of the map. Create a new ROS2 package for the occupancy checker node:

cd /fossbot_ros2/ws_fossbot/src
ros2 pkg create --build-type ament_python map_occupancy_checker

Add a new Python file where you will implement the node that will check the occupancy of the map:

touch /fossbot_ros2/ws_fossbot/src/map_occupancy_checker/map_occupancy_checker/occupancy_checker.py

Copy the following code snippet into the occupancy_checker.py file:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
# Import the service definition that we created earlier
from map_occupancy_interfaces.srv import CheckOccupancy

class OccupancyChecker(Node):
    def __init__(self):
        super().__init__('occupancy_checker')
        # Create a subscription to the /map topic to receive occupancy grid messages
        self.subscription = self.create_subscription(
            OccupancyGrid,
            '/map',
            self.map_callback,
            10)
        self.subscription
        # A variable to store the occupancy grid data
        self._map = None
        # A variable to store the map info data
        self._map_info = None

    def map_callback(self, msg):
        self._map = msg.data
        if self._map_info is None:
            self._map_info = msg.info

def main(args=None):
    rclpy.init(args=args)
    occupancy_checker = OccupancyChecker()
    rclpy.spin(occupancy_checker)
    occupancy_checker.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Update the setup.py file to include the new node in the entry points section:

    entry_points={
        'console_scripts': [
            'occupancy_checker = map_occupancy_checker.occupancy_checker:main'
        ],
    },

Modify the package.xml file to add the necessary dependencies:

  <exec_depend>rclpy</exec_depend>
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>map_occupancy_interfaces</exec_depend>

3. Add a service server to the occupancy_checker.py node that will handle requests to check the occupancy of a point in the map. Update the OccupancyChecker class as follows:

• in the __init__ method, add the following line to create a service server:

        self.service_server = self.create_service(CheckOccupancy, 
                                                  'check_occupancy', 
                                                  self.check_occupancy_callback)

• below the map_callback method, add the following method to handle service requests:

    def check_occupancy_callback(self, request, response):
        if self._map is None or self._map_info is None:
            response.occupied = True
            response.status = "Map data not available yet."
            return response

        # REMOVE THIS WHEN YOU IMPLEMENT THE LOGIC TO CHECK IF THE POINT IS OCCUPIED
        else:
            response.occupied = True
            response.status = "The occupancy check logic is not implemented yet."
            return response

4. Implement the occupancy checking logic in the check_occupancy_callback method. You will need to convert the point coordinates from world frame to map frame and then check the occupancy value in the occupancy grid data. Take the pseudocode below as a reference:

• calculate the map coordinates relative to the map origin:

map_x = request_x - map_origin_x
map_y = request_y - map_origin_y

• calculate the grid cell indices:

grid_x = int(map_x / map_resolution)
grid_y = int(map_y / map_resolution)

• check if the indices are within the bounds of the occupancy grid:

out_of_bounds = (grid_x < 0 or grid_x >= map_width or grid_y < 0 or grid_y >= map_height)

• if the indices are out of bounds, return a proper status message and set response.occupied to True. Otherwise, calculate the index in the occupancy grid data. The data is stored in a 1D array, so you will need to convert the 2D indices to a 1D index. Take a look at the following visualization of the occupancy grid data:

Index of the orange cell in the occupancy grid data can be calculated as follows:

index_orange = 0 * 5 + 1 = 1,

for the green cell:

index_green = 2 * 5 + 0 = 10,

and for the blue cell:

index_blue = 4 * 5 + 2 = 22.

• check the occupancy value at the calculated index in the occupancy grid data. If the value is -1, it means that the cell is unknown. If the value is 0, it means that the cell is free. If the value is 100, it means that the cell is definitely occupied. Values between 0 and 100 represent the probability of occupancy. Set response.occupied to True if the cell is occupied (with at least 50% probability) or unknown, and set it to False if the cell is free. Set a proper status message in response.status.

5. Build the packages and source the setup file:

cd /fossbot_ros2/ws_fossbot

colcon build --symlink-install --packages-select map_occupancy_interfaces map_occupancy_checker && source install/setup.bash

6. Launch the occupancy checker node:

ros2 run map_occupancy_checker occupancy_checker

7. Open a new terminal and enter the docker container. Check if the service is available:

ros2 service list

You should see the /check_occupancy service in the list.

8. Call the service to check the occupancy of the map at a specific point:

ros2 service call /check_occupancy map_occupancy_interfaces/srv/CheckOccupancy "{point: {x: 0.0, y: 0.0, z: 0.0}}"

Check the response to see if the point is occupied or free. Try calling the service with different points and observe the results. Save the results in an observation table.

Tip: In RViz you can visualize axes of the global frame. Set the Fixed Frame to map and add the Axes display type. This will help you to determine the coordinates of the points you want to check. By default, the length of the axes is 1 meter, so you can use the grid in RViz to estimate the coordinates of the points. Red axis is the X axis, green axis is the Y axis and blue axis is the Z axis.

Expected result: You should be able to call the service and receive a response indicating whether the point is occupied or free. The response should also include a status message to clarify the result.

Step 4 - Grid occupancy checking in motion planning

In the previous step, you implemented a service that checks the occupancy of a point in the map. Now you will modify the code to enable collision checking for a segment. We assume that the segment is a straight line between two points and that we want to check the path for a set of points not a robot (a robot has a size, so we would have to consider its dimensions). The service should: - receive two points as input (start and end of the segment), - check the occupancy of those points, - check if the segment between those points intersects with any occupied or unknown cells in the occupancy grid, - return a boolean indicating whether the segment is free or not, and one of the following status messages: - “Map data not available yet.” if the map data was not received yet, - “The segment is free.” if the segment is fully free, - “One of the starting points is occupied or unknown.” if one of the starting points is occupied or unknown, - “The segment intersects with an occupied or unknown cell.” if the segment intersects with an occupied or unknown cell.

Tip: To check if the segment intersects with any occupied or unknown cells, you should generate a set of points along the segment and check the occupancy of each point. You can simply sample the segment every grid_resolution/10 or use Bresenham’s line algorithm.

Expected result: You should be able to call the service with two points and receive a response indicating whether the segment is free or not, along with an appropriate status message.

9. Analysis Questions

1. When is it better to use a service instead of a topic in ROS2?

2. What could happen if you sample the segment too sparsely when checking for occupancy? How could this affect the results?

3. As mentioned in the step 4, the service is used to check if a segment of points is free or not. How could you modify the service or the map itself to account for the size of a robot when checking for collisions along a path?

10. Submission Requirements

• Completed source code for the ROS2 package from Step 4.

• Observation table for the occupancy checking service from Step 3.

• Short answer to analysis questions

11. References and Open Licence

[Add references, datasets, libraries, repositories, and licensing information. Mention original authors where applicable.] - Writing a Simple Py Service and Client in ROS2: https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Service-And-Client.html, - Nav2 Documentation: https://nav2.org/.

The Creative Commons Attribution 4.0 International (CC BY 4.0) license allows users to share, copy, distribute, and adapt the work, even for commercial purposes, as long as proper credit is given to the original creator.

EU funding disclaimer

Funded by the European Union. Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Education and Culture Executive Agency (EACEA). Neither the European Union nor EACEA can be held responsible for them.