class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def to_degrees(self, rad):
angle_degrees = rad * 180 / math.pi
return angle_degrees
def is_valid_distance(self, d):
"""
Discard Inf distances
"""
if (np.isnan(d) or np.isinf(d)):
return False
else:
return True
def is_within_range(self, i):
"""
Only consider -90 to 90 degrees
"""
#return -math.pi/4 < i < math.pi/4
return math.pi / 2 < abs(i) # LIDAR rotated backwards
def process_measurements(self):
"""
Basic code to run autonomously
"""
# Check samples from https://github.com/Arkapravo/laser_obstacle_avoidance_pr2/blob/master/src/obstacle_avoidance_pr2.cpp
scan = self.ros_interface.get_scan()
angular_z = 0
linear_x = 0
k = 0.4 # 0.3
min_linear = 0.09
rotate_fast_value = 1. # 5.8 # rad/s
linear_k = 0.15
go_forward = 0.5 # default value
action = "None"
if (scan == None):
return
#print "i:%.5f"%angle+"(%.5f"%self.to_degrees(angle)+"degrees) x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational
min_range = scan.ranges[0]
min_range_angle = 0
min_angle = 0
for j in range(0, 360): #increment by one degree
real_dist = scan.ranges[j]
angle = scan.angle_min + j * scan.angle_increment
if self.is_within_range(angle) and self.is_valid_distance(
real_dist):
if (real_dist < min_range):
min_range = real_dist
min_angle = angle
min_range_angle = j / 2
#print "minimum range is [%.3f]"%min_range+" at an angle of [%.3f]"%min_range_angle
if (
min_range <= 0.5
): # min_range<=0.5 gave box pushing like behaviour, min_range<=1.2 gave obstacle avoidance
if (min_range_angle < 90):
angular_z = rotate_fast_value
linear_x = 0
action = "left "
else:
angular_z = -rotate_fast_value
linear_x = 0
action = "right"
else:
angular_z = 0
linear_x = go_forward
action = "straight"
print "action:" + action + " min distance[%.2f]" % min_range + " angle [%.2f]" % min_range_angle + " (%.1f)" % angle + " min angle %.1f" % min_angle
self.ros_interface.command_velocity(linear_x, angular_z)
return
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing,
y_spacing, t_cam_to_body, min_lin_x, max_lin_x,
rotate_fast_value, linear_k, k, min_linear_vel):
"""
Initialize the class
"""
self.min_lin_x = min_lin_x
self.max_lin_x = max_lin_x
self.rotate_fast_value = rotate_fast_value
self.linear_k = linear_k
self.k = k
self.min_linear_vel = min_linear_vel
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def to_degrees(self, rad):
angle_degrees = rad * 180 / math.pi
return angle_degrees
def is_valid_distance(self, d):
"""
Discard Inf distances
"""
if (np.isnan(d) or np.isinf(d)):
return False
else:
return True
def is_within_range(self, i):
"""
Only consider -90 to 90 degrees
"""
#return -math.pi/4 < i < math.pi/4
return math.pi / 2 < abs(i) # LIDAR rotated backwards
def process_measurements(self):
"""
Basic code to run autonomously
"""
# Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py
scan = self.ros_interface.get_scan()
linear = 0
rotational = 0
rcount = 0
hack = False
msg = ""
if (scan == None):
return
rcount = 0
for i, real_dist in enumerate(scan.ranges):
angle = scan.angle_min + i * scan.angle_increment
denom = 1. + real_dist * real_dist
if self.is_within_range(angle) and self.is_valid_distance(
real_dist):
linear += math.sin(angle) / denom * 1.
rotational += math.cos(angle) / denom * 1.
rcount += 1
#print "%.5f"%self.to_degrees(angle)+"degrees x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational
try:
linear /= rcount
rotational /= rcount
except Exception as e:
pass
"""
if (linear > self.k):
linear = self.k
elif (linear < -self.k):
linear = -self.k
"""
linear_x = linear
angular_z = rotational
linear_x = linear # check this
print "linear_x:%.5f" % linear_x + " angular_z:%.1f" % self.to_degrees(
angular_z) + "(%.2f)" % angular_z + " " + msg
#self.ros_interface.command_velocity(linear_x, angular_z)
return
class RobotControl(object):
"""
Robot interface class. In charge of getting sensor measurements (ROS subscribers),
and transforms them into the 2D plane, and publishes velocity commands.
"""
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
def publish_action(self, speed, turn):
self.ros_interface.command_velocity(speed, turn)
def apply_avoidance_rules(self, distance_from_center, angle):
p_angle = util.proportional_rotation(angle)
action = "None"
if self.inside_too_close_box(distance_from_center):
# dangeorous distance, too close to turn, just try to go back
action = "Back"
rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center))
self.react_to_inside_too_close_box(p_angle)
elif util.is_forward_left(angle):
# obstacle to left, go right
action = "go forward right"
self.publish_action(default_speed, p_angle)
elif util.is_forward_right(angle):
# obstacle to right, go left
action = "go forward left"
self.publish_action(default_speed, p_angle)
elif util.is_backwards_left(angle):
action = "obstacle backwards left"
elif util.is_backwards_right(angle):
action = "obstacle backwards right"
else:
action = "unknown"
#rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center))
def react_to_inside_too_close_box(self,angle):
# go back and turn 180 degrees
self.publish_action(-default_speed*2, 0)
rate = rospy.Rate(avoid_check_rate)
rate.sleep()
if util.is_forward_right(angle):
sign = -1
else:
sign = 1
z = math.pi*1.6*sign
rospy.loginfo('Back angle:'+str(angle)+' new angle:'+str(z))
self.publish_action(0, z) # around 5 rad/s
rate.sleep()
def inside_too_close_box(self, distance_from_center):
forward_box_length, forward_box_width = self.forward_box()
return distance_from_center < forward_box_width
def get_closest_obstacle(self, scan):
# returns closest point to obstacle inside a vehicle's 'forward box'
# forward box is a rectangle ahead of the vehicle that if there is an obstacle
# inside it it's necessary to do an action in order to avoid it
forward_box_length, forward_box_width = self.forward_box()
is_inside = False
min_distance = max_distance
min_i = 0
min_angle = 0
for i, distance in enumerate(scan.ranges):
angle = scan.angle_min + i * scan.angle_increment
if util.is_within_range(angle) and util.is_valid_distance(distance):
#print i, distance, to_degrees(angle), angle
if distance < min_distance:
if point_is_inside_box(i, distance, scan, forward_box_length, forward_box_width, util.to_degrees(angle)):
is_inside = True
min_distance = distance
min_i = i
min_angle = angle
#angle = angle_from_scan(min_i, scan)
#angle_degrees = angle*180/math.pi
#print min_i, angle_degrees, min_distance
return is_inside, min_distance, min_angle
def forward_box(self):
# returns the Vehicle 'forward box'
# to do: in future versions forward box should depend on vehicle's speed
# more speed -> larger forward box
length = Behaviour.body_width*look_ahead_factor
width = clearance_safety_factor*Behaviour.body_width/2
return length, width
def check_action():
pass
def process_measurements(self):
"""
Basic code to run autonomously
"""
scan = self.ros_interface.get_scan()
if(scan == None):
return
action = "None"
angular_z = 0
linear_x = 0.3
is_inside, distance_from_center, angle = self.get_closest_obstacle(scan)
# only do something if obstacle is inside forward box
if is_inside:
#print 'point inside ', distance_from_center, angle
self.apply_avoidance_rules(distance_from_center, angle)
else:
self.publish_action(linear_x, angular_z)
return
