Mobile Robots – lab7

Introduction

The objective of the assignment is to implement a robot controller using the results of previous assignments.

The robot should receive a pose of the goal to go to and plan a path to reach the goal while avoiding obstacles.

As the robot moves, it should avoid obstacles, update the map according to current sensor readings and re-plan the path if necessary.

Tasks

Before class, prepare a proposal for the structure of the system. Analyze how to adapt the solutions of the previous assignments and what additional functions are needed.
Consider the following points and plan the implementation according to the proposals:
- How should the robot's movement be implemented?
- Is a separate obstacle avoidance behavior needed?
- How should the need for path replanning be decided?
Implement (or adapt from previous tasks) the behaviors and test the results on the robot.

Note: Remember that to move a robot you have to run a safety system application, select and unlock the robot you want to control.

Report

The report should be sent by email in pdf format and should contain

Description of the system architecture.
Comments on the results.
Attachment with implemented code.

Sample code

Velocity publisher (set the number of robot in pub variable)


#!/usr/bin/env python3

import rclpy
import time
from rclpy.node import Node
from geometry_msgs.msg import Twist

class VelocityPublisher(Node):
    def __init__(self):
        super().__init__('velocity_publisher')
        self.publisher_ = self.create_publisher(Twist, '/pioneer5/cmd_vel', 10)
        self.get_logger().info('Velocity Publisher has been started.')

    def publish_velocity(self,v,w):
        velocity_command = Twist()
        velocity_command.linear.x = v  # Replace this with your desired linear velocity
        velocity_command.angular.z = w  # Replace this with your desired angular velocity

        self.publisher_.publish(velocity_command)
        self.get_logger().info('Published velocity command: linear={}, angular={}'.format(
            velocity_command.linear.x, velocity_command.angular.z))

def main(args=None):
    rclpy.init(args=args)
    velocity_publisher = VelocityPublisher()
    time.sleep(8)
    velocity_publisher.get_logger().info('Start')
    try:
        velocity_publisher.publish_velocity(0.0,0.5)
        time.sleep(2)
        velocity_publisher.publish_velocity(0.0,-0.5)
        time.sleep(2)
        velocity_publisher.publish_velocity(0.0,0.0) 	
        rclpy.spin(velocity_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        velocity_publisher.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Update to the map example - get a row and column from mouse click

import matplotlib as mpl
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.cm import ScalarMappable

def on_click(event):
    if event.inaxes:
        # Get the coordinates of the click
        x, y = int(event.xdata + 0.5), int(event.ydata + 0.5)

        # Print the corresponding row and column
        print(f"Clicked on row {y}, column {x}")


f = np.array([[1,2,3], [4,5,6], [7,8,9]]);

fig, ax = plt.subplots()
im=ax.imshow(f, interpolation="nearest",cmap='Blues')


# Create a ScalarMappable for colorbar
sm = ScalarMappable(cmap='Blues')
sm.set_array([])  # Dummy array for mapping

plt.colorbar(sm)

fig.canvas.mpl_connect('button_press_event', on_click)

plt.show()