Hello Team,
I hope everyone is doing well.
I am currently facing an issue in the Real Robot Lab, specifically in the course “ROS2 BASICS IN 5 DAYS (PYTHON).” My challenge arises in Part 2, under the ‘services’ section. I was tasked with creating a service server that aligns the robot’s right side to the wall. This alignment should then trigger the wall-following behavior.
While my code executes flawlessly in simulation, it doesn’t seem to function as intended in the Real Robot Lab. I’m unsure where the discrepancy lies. I will provide my code for your reference:
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from custom_interfaces.srv import FindWall
import rclpy
from rclpy.node import Node
import time
import math
class WallFinderServer(Node):
def __init__(self):
super().__init__('wall_finder_server')
self.group = ReentrantCallbackGroup()
self.wall_aligned = False # Flag to indicate if the wall is aligned
self.wall_finder_srv = self.create_service(
FindWall,
'find_wall',
self.find_wall_callback)
self.twist_publisher = self.create_publisher(
Twist,
'/cmd_vel',
10)
self.sub_laser = self.create_subscription(
LaserScan,
"/scan",
self.laser_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT),
callback_group=self.group
)
self.initial_configuration = False
self.turn_sign = +1
self.laser_min_idx = 0
self.laser_min = -1.0
self.angle_to_rotate = 0.0
self.laser_front = 0.0
self.n_ranges = 0
self.front_laser_idx = 0
self.right_laser_idx = 0
self.dist_tol = 0.03
self.laser_idx_tol = 6
self.vel_angular_z = 0.2
self.vel_linear_x = 0.05
self.first_turn_finished = False
self.second_turn_finished = False
def laser_callback(self, msg):
if not self.initial_configuration:
self.n_ranges = len(msg.ranges)
self.front_laser_idx = int(self.n_ranges / 2)
# Calculate the right laser index based on the front laser index
self.right_laser_idx = (self.front_laser_idx + int(self.n_ranges / 4)) % self.n_ranges
self.initial_configuration = True
self.get_logger().info('Front laser idx: "%s"' % str(self.front_laser_idx))
self.get_logger().info('Right laser idx: "%s"' % str(self.right_laser_idx))
self.laser_min = min(msg.ranges)
self.laser_min_idx = msg.ranges.index(self.laser_min)
min_range_angle = (2 * math.pi * (self.laser_min_idx + 1)) / self.n_ranges
if not self.first_turn_finished:
self.angle_to_rotate = min_range_angle - math.pi
if self.angle_to_rotate < 0:
self.turn_sign = -1
self.get_logger().info('Angle to rotate: "%s"' % str(self.angle_to_rotate * 180 / math.pi))
elif not self.second_turn_finished:
self.angle_to_rotate = min_range_angle - (3 * math.pi / 2)
self.get_logger().info('Angle to rotate 2: "%s"' % str(self.angle_to_rotate * 180 / math.pi))
self.laser_front = msg.ranges[self.front_laser_idx]
def move_robot(self, linear_x, angular_z, duration):
# Move the robot with the given velocities for the specified duration
msg = Twist()
msg.linear.x = linear_x
msg.angular.z = angular_z
start_time = time.time()
while time.time() - start_time < duration:
self.twist_publisher.publish(msg)
time.sleep(0.1)
msg.linear.x = 0.0
msg.angular.z = 0.0
self.twist_publisher.publish(msg)
def find_wall_callback(self, request, response):
while self.laser_min < 0:
self.get_logger().info('WAITING FOR IDENTIFYING THE NEAREST WALL')
time.sleep(0.5)
while abs(self.laser_min_idx - self.front_laser_idx) > self.laser_idx_tol:
turn_sign = self.turn_sign
self.move_robot(0.0, turn_sign * self.vel_angular_z, 0.5)
self.get_logger().info('FIRST TURN')
self.first_turn_finished = True
self.move_robot(0.0, 0.0, 0.5)
self.get_logger().info('First turn finished, the robot is looking at the nearest wall')
linear_x_sign = 1 if self.laser_front < 0.3 else -1
while abs(self.laser_front - 0.3) > self.dist_tol:
self.move_robot(linear_x_sign * self.vel_linear_x, 0.0, 0.5)
self.get_logger().info('LINEAR MOTION')
self.move_robot(0.0, 0.0, 0.5)
self.get_logger().info('The robot is 30 cm away from the wall')
while abs(self.laser_min_idx - self.right_laser_idx) > self.laser_idx_tol:
self.move_robot(0.0, self.vel_angular_z, 0.5)
self.get_logger().info('SECOND TURN')
self.second_turn_finished = True
self.move_robot(0.0, 0.0, 0.5)
self.get_logger().info('The robot is aligned with the right wall')
self.wall_aligned = True
response.wallfound = True
return response
def main(args=None):
# initialize the ROS communication
rclpy.init(args=args)
wall_finder_node = WallFinderServer()
# Create a MultiThreadedExecutor with 3 threads
executor = MultiThreadedExecutor(num_threads=3)
# Add the node to the executor
executor.add_node(wall_finder_node)
try:
# Spin the executor
executor.spin()
finally:
# Shutdown the executor
executor.shutdown()
wall_finder_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
I would greatly appreciate any assistance you can offer.
I hope you can help me as fast as you can 
.
?