Hello, I was trying to figure out which laser index corresponds to the front, back, left and right and I was doing it by orienting the robot so that the front of the robot was close to the wall and reading the minimum distance measured and which laser index it corresponded to. I repeated this for left, right and back by orienting the robot such that the left, right and back are closest to the wall and seeing the laser index with the minimum distance. This worked in simulation, however, in trying this with the FastBot, I got the same laser index with minimum distance regardless of the orientation of t he robot which I believe should not be possible if different directions of the robot (and therefore different laser indices) are closest to a wall. I have attached my code below if it helps. I would appreciate any help regarding this.
#!/usr/bin/python3
# python imports
import time
import threading
import traceback
from math import inf
import statistics as stat
# ros2 imports
import rclpy
from rclpy.executors import MultiThreadedExecutor
# module imports
from robot_interface import RobotInterface
#~#~#~#~#~# start your class after this line #~#~#~#~#~#
class RobotControl:
def __init__(self, robot_interface):
# class constructor
# write your initialization codes here if any
self.robot_interface = robot_interface
self.initial_position = dict()
self.initial_orientation = dict()
self.current_position = dict()
self.current_orientation = dict()
self.total_distance = 0.0
self.start_time = time.time()
return None
def __del__(self):
# class destructor
# write your termination codes here if any
return None
#~#~#~#~#~# add your functions after this line #~#~#~#~#~#
def get_linear_and_angular_velocities(self):
return self.robot_interface.linear_velocity, self.robot_interface.angular_velocity
def stop_robot(self):
self.robot_interface.linear_velocity = 0.0
self.robot_interface.angular_velocity = 0.0
def move_robot_front(self, linear_speed):
self.robot_interface.angular_velocity = 0.0
self.robot_interface.linear_velocity = linear_speed
def move_robot_back(self, linear_speed):
self.robot_interface.angular_velocity = 0.0
self.robot_interface.linear_velocity = -1.0 * linear_speed
def turn_robot_left(self, angular_speed):
self.robot_interface.linear_velocity = 0.0
self.robot_interface.angular_velocity = angular_speed
def turn_robot_right(self, angular_speed):
self.robot_interface.linear_velocity = 0.0
self.robot_interface.angular_velocity = -1.0 * angular_speed
def timed_move_front(self, linear_speed, time_to_move):
self.move_robot_front(linear_speed)
time.sleep(0.3)
print("Moving forward at ", linear_speed, " m/s for ", time_to_move, " s")
time.sleep(time_to_move)
self.stop_robot()
def timed_move_back(self, linear_speed, time_to_move):
self.move_robot_back(linear_speed)
time.sleep(0.3)
print("Moving backward at ", linear_speed, " m/s for ", time_to_move, " s")
time.sleep(time_to_move)
self.stop_robot()
def timed_turn_left(self, angular_speed, time_to_move):
self.turn_robot_left(angular_speed)
time.sleep(0.3)
print("Turning left at ", angular_speed, " rad/s for ", time_to_move, " s")
time.sleep(time_to_move)
self.stop_robot()
def timed_turn_right(self, angular_speed, time_to_move):
self.turn_robot_right(angular_speed)
time.sleep(0.3)
print("Turning right at ", angular_speed, " rad/s for ", time_to_move, " s")
time.sleep(time_to_move)
self.stop_robot()
def move_distance_front(self, linear_speed, distance_to_move_cm):
distance_to_move_m = distance_to_move_cm / 100.0
time_to_move = distance_to_move_m / linear_speed
print("Moving forward by ", distance_to_move_cm, " cm at ", linear_speed, " m/s")
self.timed_move_front(linear_speed, time_to_move)
def move_distance_back(self, linear_speed, distance_to_move_cm):
distance_to_move_m = distance_to_move_cm / 100.0
time_to_move = distance_to_move_m / linear_speed
print("Moving backward by ", distance_to_move_cm, " cm at ", linear_speed, " m/s")
self.timed_move_back(linear_speed, time_to_move)
def turn_angle_left(self, angular_speed, angle_to_move):
time_to_move = angle_to_move / angular_speed
print("Turning left by ", angle_to_move, " rads at ", angular_speed, " rad/s")
self.timed_turn_left(angular_speed, time_to_move)
def turn_angle_right(self, angular_speed, angle_to_move):
time_to_move = angle_to_move / angular_speed
print("Turning left by ", angle_to_move, " rads at ", angular_speed, " rad/s")
self.timed_turn_right(angular_speed, time_to_move)
def get_minimum_scan_angle(self):
return self.robot_interface.scan_angle_min
def get_maximum_scan_angle(self):
return self.robot_interface.scan_angle_max
def get_angle_increment(self):
return self.robot_interface.scan_angle_increment
def get_minimum_scan_range(self):
return self.robot_interface.scan_range_min
def get_maximum_scan_range(self):
return self.robot_interface.scan_range_max
def get_all_scan_ranges(self):
return self.robot_interface.scan_ranges
def get_scan_range_by_index(self, index):
scan_ranges = self.get_all_scan_ranges()
return scan_ranges[index]
# TODO
def get_front_scan_range(self):
scan_ranges = self.get_all_scan_ranges()
front_scan_ranges = scan_ranges[248:413]
minimum_front_scan_range = inf
for front_scan_range in front_scan_ranges:
if front_scan_range == inf:
continue
if front_scan_range < minimum_front_scan_range:
minimum_front_scan_range = front_scan_range
return minimum_front_scan_range
# TODO
def get_back_scan_range(self):
scan_ranges = self.get_all_scan_ranges()
back_scan_ranges = scan_ranges[578:] + scan_ranges[0:83]
minimum_back_scan_range = inf
for back_scan_range in back_scan_ranges:
if back_scan_range == inf:
continue
if back_scan_range < minimum_back_scan_range:
minimum_back_scan_range = back_scan_range
return minimum_back_scan_range
# TODO
def get_left_scan_range(self):
scan_ranges = self.get_all_scan_ranges()
left_scan_ranges = scan_ranges[413:578]
minimum_left_scan_range = inf
for left_scan_range in left_scan_ranges:
if left_scan_range == inf:
continue
if left_scan_range < minimum_left_scan_range:
minimum_left_scan_range = left_scan_range
return minimum_left_scan_range
# TODO
def get_right_scan_range(self):
scan_ranges = self.get_all_scan_ranges()
right_scan_ranges = scan_ranges[83:248]
minimum_right_scan_range = inf
for right_scan_range in right_scan_ranges:
if right_scan_range == inf:
continue
if right_scan_range < minimum_right_scan_range:
minimum_right_scan_range = right_scan_range
return minimum_right_scan_range
def get_minimum_range_ne_infinity_with_index(self):
scan_ranges = self.get_all_scan_ranges()
index_of_minimum_range = -1
minimum_range = inf
for index, scan_range in enumerate(scan_ranges):
if scan_range == inf:
continue
if scan_range < minimum_range:
minimum_range = scan_range
index_of_minimum_range = index
return index_of_minimum_range, minimum_range
def get_maximum_range_ne_infinity_with_index(self):
scan_ranges = self.get_all_scan_ranges()
index_of_maximum_range = -1
maximum_range = -inf
for index, scan_range in enumerate(scan_ranges):
if scan_range == inf:
continue
if scan_range > maximum_range:
maximum_range = scan_range
index_of_maximum_range = index
return index_of_maximum_range, maximum_range
def get_current_position_x_y_z(self):
return {"x": self.robot_interface.odom_position_x, "y": self.robot_interface.odom_position_y, "z": self.robot_interface.odom_position_z}
def get_current_orientation_r_p_y(self):
return {"r": self.robot_interface.odom_orientation_r, "p": self.robot_interface.odom_orientation_p, "y": self.robot_interface.odom_orientation_y}
def get_distance_using_euclidean_formula(self, x1, y1, x2, y2):
distance_squared = (x2 - x1)**2 + (y2 - y1)**2
return distance_squared**0.5
def display_statistics(self):
lin_vel, ang_vel = self.get_linear_and_angular_velocities()
print("Linear Velocity: ", lin_vel, " m/s Angular Velocity: ", ang_vel, " rad/s")
print("Number of scan samples: ", len(self.get_all_scan_ranges()))
print("Minimum scan angle: ", self.get_minimum_scan_angle(), " rads")
print("Maximum scan angle: ", self.get_maximum_scan_angle(), " rads")
print("Scan angle increment: ", self.get_angle_increment(), " rads")
print("Minumum scan range: ", self.get_minimum_scan_range(), " m")
print("Maxumum scan range: ", self.get_maximum_scan_range(), " m")
print("Front scan range: ", self.get_front_scan_range(), " m")
print("Back scan range: ", self.get_back_scan_range(), " m")
print("Left scan range: ", self.get_left_scan_range(), " m")
print("Right scan range: ", self.get_right_scan_range(), " m")
min_range_index, min_range = self.get_minimum_range_ne_infinity_with_index()
print("Minimum range: ", min_range, " m Index: ", min_range_index)
max_range_index, max_range = self.get_maximum_range_ne_infinity_with_index()
print("Maximum range: ", max_range, " m Index: ", max_range_index)
print("Position (m): ", self.get_current_position_x_y_z())
print("Orientation (rads): ", self.get_current_orientation_r_p_y())
def obstacle_prediction(self, scan_ranges):
front_scan_ranges = scan_ranges[248:413]
front_left_scan_ray_index = 412
front_right_scan_ray_index = 248
front_left_scan_ranges = scan_ranges[248:330]
front_right_scan_ranges = scan_ranges[330:413]
filtered_front_scan_ranges = []
for front_scan_range in front_scan_ranges:
if front_scan_range == inf:
continue
filtered_front_scan_ranges.append(front_scan_range)
threshold = 0.300
if min(filtered_front_scan_ranges) > threshold:
print("None")
else:
if len(stat.multimode(filtered_front_scan_ranges)) > 1:
print("Obstacle")
else:
print("Wall")
def direction_tracking(self, current_orientation):
yaw = current_orientation["y"]
PI = 3.14159
tolerance_rads = 0.1
north_rads = 0.0
north_west_rads = PI / 4
north_east_rads = -1.0 * PI / 4
west_rads = PI / 2
south_west_rads = (PI / 2) + (PI / 4)
south_plus_rads = PI
south_minus_rads = -1.0 * PI
east_rads = -1.0 * PI / 2
south_east_rads = -1.0 * ((PI / 2) + (PI / 4))
direction_string = ""
if yaw <= (north_rads + tolerance_rads) and yaw >= (north_rads - tolerance_rads):
direction_string = "-N-"
elif yaw > (north_rads + tolerance_rads) and yaw < (north_west_rads - tolerance_rads):
direction_string = "NNW"
elif yaw <= (north_west_rads + tolerance_rads) and yaw >= (north_west_rads - tolerance_rads):
direction_string = "N-W"
elif yaw > (north_west_rads + tolerance_rads) and yaw < (west_rads - tolerance_rads):
direction_string = "WNW"
elif yaw <= (west_rads + tolerance_rads) and yaw >= (west_rads - tolerance_rads):
direction_string = "-W-"
elif yaw > (west_rads + tolerance_rads) and yaw < (south_west_rads - tolerance_rads):
direction_string = "WSW"
elif yaw <= (south_west_rads + tolerance_rads) and yaw >= (south_west_rads - tolerance_rads):
direction_string = "S-W"
elif yaw > (south_west_rads + tolerance_rads) and yaw < (south_plus_rads - tolerance_rads):
direction_string = "SSW"
elif yaw >= (south_plus_rads - tolerance_rads) or yaw <= (south_minus_rads + tolerance_rads):
direction_string = "-S-"
elif yaw <= (north_east_rads + tolerance_rads) and yaw >= (north_east_rads - tolerance_rads):
direction_string = "N-E"
elif yaw > (north_east_rads + tolerance_rads) and yaw < (north_rads - tolerance_rads):
direction_string = "NNE"
elif yaw > (east_rads + tolerance_rads) and yaw < (north_east_rads - tolerance_rads):
direction_string = "ENE"
elif yaw <= (east_rads + tolerance_rads) and yaw >= (east_rads - tolerance_rads):
direction_string = "-E-"
elif yaw > (south_east_rads + tolerance_rads) and yaw < (east_rads - tolerance_rads):
direction_string = "ESE"
elif yaw <= (south_east_rads + tolerance_rads) and yaw >= (south_east_rads - tolerance_rads):
direction_string = "S-E"
elif yaw > (south_minus_rads + tolerance_rads) and yaw < (south_east_rads - tolerance_rads):
direction_string = "SSE"
else:
direction_string = "N/A"
print("Current direction: ", direction_string)
def initialise_position_and_orientation(self):
self.initial_orientation = self.get_current_orientation_r_p_y()
self.initial_position = self.get_current_position_x_y_z()
def remove_inf_from_list(self, input_list):
list_to_return = list()
for var in input_list:
if var == inf:
continue
list_to_return.append(var)
return list_to_return
def execute(self):
if len(self.initial_orientation) == 3 and len(self.initial_position) == 3:
# TODO
scan_ranges = self.get_all_scan_ranges()
number_of_lasers = len(scan_ranges)
front_centre = scan_ranges[289:371]
back_centre = scan_ranges[:41] + scan_ranges[618:]
right_centre = scan_ranges[124:206]
left_centre = scan_ranges[454:536]
front_right = scan_ranges[206:289]
front_left = scan_ranges[371:454]
back_left = scan_ranges[536:618]
back_right = scan_ranges[41:124]
threshold = 0.300
front_centre_blocked = min(self.remove_inf_from_list(front_centre)) < threshold
front_left_blocked = min(self.remove_inf_from_list(front_left)) < threshold
front_right_blocked = min(self.remove_inf_from_list(front_right)) < threshold
current_time = time.time()
if current_time - self.start_time > 1:
print("There are ", number_of_lasers, " lasers")
self.current_position = self.get_current_position_x_y_z()
print("Current Position: ", self.current_position)
self.direction_tracking(self.get_current_orientation_r_p_y())
self.obstacle_prediction(scan_ranges)
self.start_time = current_time
if not front_centre_blocked:
if front_left_blocked:
print("Front left blocked. Turning right")
self.stop_robot()
self.turn_angle_right(0.1, 0.5)
print("Moving forward now")
self.move_robot_front(0.1)
elif front_right_blocked:
print("Front right blocked. Turning left")
self.stop_robot()
self.turn_angle_left(0.1, 0.5)
print("Moving forward now")
self.move_robot_front(0.1)
else:
self.move_robot_front(0.1)
else:
if min(self.remove_inf_from_list(left_centre)) > min(self.remove_inf_from_list(right_centre)):
print("Forward blocked. More space on the left. Turning left")
self.stop_robot()
self.turn_angle_left(0.1, 0.5)
print("Moving forward now")
self.move_robot_front(0.1)
else:
print("Forward blocked. More space on the right. Turning right")
self.stop_robot()
self.turn_angle_right(0.1, 0.5)
print("Moving forward now")
self.move_robot_front(0.1)
#~#~#~#~#~# finish your class before this line #~#~#~#~#~#
def spin_node():
"""
make the robot interface program to run in a separate thread
NOTE: THE ROBOT WILL NOT WORK IF THIS FUNCTION IS REMOVED !!!
"""
global executor
executor.spin()
return None
if __name__ == "__main__":
# initialize ros2 with python
rclpy.init(args=None)
# instantiate robot interface program module
robot_interface = RobotInterface()
# start robot interface program execution
executor = MultiThreadedExecutor(num_threads=6)
executor.add_node(robot_interface)
# run robot interface program in a separate thread
threading.Thread(target=spin_node).start()
# wait for a few seconds for program to initialize
print("Getting Ready in 5 Seconds...")
time.sleep(5.0)
print("READY !!!")
try:
#~#~#~#~#~# start your program after this line #~#~#~#~#~#
########## |~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~| ##########
########## | ! YOU ARE ON YOUR OWN FROM HERE ! | ##########
########## |~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~| ##########
#~#~#~#~#~# write code here to run only once #~#~#~#~#~#
rc = RobotControl(robot_interface)
rc.initialise_position_and_orientation()
#~#~#~#~#~# write code here to run continuously #~#~#~#~#~#
while True:
# threshold = 0.5
# front_range = rc.get_front_scan_range()
# right_range = rc.get_right_scan_range()
# left_range = rc.get_left_scan_range()
# back_range = rc.get_back_scan_range()
# if front_range > threshold:
# rc.move_robot_front(0.1)
# else:
# rc.stop_robot()
# if right_range > left_range:
# rc.turn_angle_right(0.5, 0.4)
# else:
# rc.turn_angle_left(0.5, 0.4)
# rc.display_statistics()
# time.sleep(0.5)
# rc.turn_angle_left(0.1, 0.1)
# rc.direction_tracking()
# rc.execute()
# minimum_range_index, minimum_range = rc.get_minimum_range_ne_infinity_with_index()
# print("Minimum range index: ", minimum_range_index, " Minimum range: ", minimum_range, " m")
# time.sleep(1)
# rc.move_distance_front(0.1, 100)
# rc.move_robot_front(0.1)
print(rc.get_minimum_range_ne_infinity_with_index())
# print(rc.get_all_scan_ranges()[10])
time.sleep(1)
#~#~#~#~#~# finish your program before this line #~#~#~#~#~#
except Exception as error:
# report exception
print("~~~~~~~~~~~ ERROR: ~~~~~~~~~~~")
print(traceback.print_exception(error))
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
# clean up before shutdown
executor.shutdown()
robot_interface.destroy_node()
finally:
# shutdown ros2
rclpy.shutdown()
# End of Code
