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.
Note: Remember that to move a robot you have to run a safety system application, select and unlock the robot you want to control.
#!/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()