def __init__(self):
# velocity publisher
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# lidar listener
self.scan_listener = LidarListener()
# angular and linear velocity in rescue mode
self.angular_vel = 0.2 #rad/s
self.linear_vel = 1.0 #m/s
# threshold showing there exists obstacles
self.stop_threshold = 1.5 # if any thing within this range emerge in the laser scan, stop robot
self.move_threshold = 4.0 # if all points in the front are farther then this value, go straight forward
# parameter storing the width of the robot
self.robot_width = 1.5
self.open_area_pts = 40 # at least 40 points exist in an "open area"
#self.out_of_boundary_threshold = 5 # threshold for tolerating small number of points within move_threshold
# paramter storing the current mode of the robot
self.mode = "turning" # valid values: "turning", "forward"
# define the variable to have velocity
self.twist = Twist()
# store the previous turning state
self.previous_turn = None
self.cumulative_turn_angle = 0.0 # store the cumulative turning angle
self.view_coef = 0.5 # coefficient to determine the view for navigation detection
self.view_coef_reset = 0.5 # backup copy of the coefficient to determine the view for navigation detection
self.view_decay_rate = 0.5 # gradually narrow the view
self.straight_count = 0
self.restore_view_threshold = 10 # move straight 5 steps and then recover the view
def __init__(self):
self.roi_size = 450
self.darken_gradient = (30, 30, 30, 30)
self.lighten_gradient = (70, 70, 70, 70)
self.mm_per_pixel = 15.0
self.lock = False
self.__q = Queue.Queue()
self.__listener = LidarListener(self.__q)
self.__m = cv.GetImage(cv.fromarray(255 * numpy.ones((4000, 4000, 4), numpy.uint8)))
self.__innermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
self.__outermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
import struct
import socket
import string
import threading, Queue
import cv, numpy, time
import math, signal
import sys
from lidar_listener import LidarListener
q = Queue.Queue()
listener = LidarListener(q)
b = True
x = 4000
y = 4000
heading = 0
start_index = 0
# invent a destination
dest_x = 4000
dest_y = 3980
dest_conv = 10
d_x = 4000
d_y = 4000
# Build the map, dummy
print "Building the map..."
m = cv.GetImage(cv.fromarray(255 * numpy.ones( (9000,9000,3), numpy.uint8 ) ))
cv.NamedWindow("map")
class Mapper:
def __init__(self):
self.roi_size = 450
self.darken_gradient = (30, 30, 30, 30)
self.lighten_gradient = (70, 70, 70, 70)
self.mm_per_pixel = 15.0
self.lock = False
self.__q = Queue.Queue()
self.__listener = LidarListener(self.__q)
self.__m = cv.GetImage(cv.fromarray(255 * numpy.ones((4000, 4000, 4), numpy.uint8)))
self.__innermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
self.__outermask = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
def start(self):
try:
self.__listener.daemon = True
self.__listener.start()
except Exception as ex:
raise ex
def shutdown(self):
try:
self.__listener.kill = True
except Exception as ex:
raise ex
def obstaclePresent(self, abs_center, radius):
roi = (abs_center[0] - radius, abs_center[1] - radius, 2 * radius, 2 * radius)
cv.SetImageROI(self.__m, roi)
result = cv.CreateMat(2 * radius, 2 * radius, cv.CV_8UC4)
cv.AndS(self.__m, (255, 255, 255), result)
result_sum = (255 * 4 * radius * radius) - cv.Sum(result)[0]
"""
conflicts = 0
for i in range(2*radius):
for j in range(2*radius):
if (result[i,j][0] < 255):
conflicts = conflicts + 1
result_sum = conflicts
"""
cv.ResetImageROI(self.__m)
return result_sum > 0.0
# Could be improved, not focused on it right now
def obstaclePresentMask(self, mask, mask_roi):
cv.SetImageROI(self.__m, mask_roi)
result = cv.CreateMat(mask.height, mask.width, cv.CV_8UC4)
cv.AndS(self.__m, (255, 255, 255), result, mask)
conflicts = 0
for i in range(mask.height):
for j in range(mask.width):
if result[i, j][0] < 255 and mask[i, j] != 0:
conflicts = conflicts + 1
# result_sum = ((255*mask.height*mask.width) - cv.Sum(result)[0])
cv.ResetImageROI(self.__m)
return conflicts > 0
def safeLine(self, waypoint, thickness):
cv.ResetImageROI(self.__m)
conflicts = 0
angle = math.radians(cv.FastArctan(waypoint[0][1] - waypoint[1][1], waypoint[0][0] - waypoint[1][0]))
for i in range(
cv.Floor(
cv.Sqrt(math.pow(waypoint[0][0] - waypoint[1][0], 2) + math.pow(waypoint[0][1] - waypoint[1][1], 2))
)
):
check_x = cv.Floor(waypoint[0][0] - i * math.cos(angle))
check_y = cv.Floor(waypoint[0][1] - i * math.sin(angle))
if self.obstaclePresent((check_x, check_y), thickness / 2):
conflicts = conflicts + 1
return conflicts == 0
def mapImage(self):
mapCopy = cv.GetImage(cv.fromarray(numpy.zeros((self.roi_size, self.roi_size), numpy.uint8)))
cv.SetImageROI(self.__m, (x - (self.roi_size / 2), y - (self.roi_size / 2), self.roi_size, self.roi_size))
cv.Copy(self.__m, mapCopy)
cv.ResetImageROI(self.__m)
return mapCopy
def mapCopy(self, roi):
mapCopy = cv.GetImage(cv.fromarray(numpy.zeros((roi[2], roi[3]), numpy.uint8)))
cv.SetImageROI(self.__m, roi)
cv.Copy(self.__m, mapCopy)
cv.ResetImageROI(self.__m)
return mapCopy
def mapPointer(self):
return self.__m
def updateFromROS(self, newMap):
self.__m = newMap
cv.ResetImageROI(self.__m)
def scan(self):
x = 2000
y = 2000
heading = 0
try:
data = self.__q.get(False)
if data:
cv.ResetImageROI(self.__m)
cv.SetZero(self.__innermask)
cv.SetZero(self.__outermask)
inner_poly = []
for i in range(len(data)):
ping = data[i] / self.mm_per_pixel
# This needs to be replaced by an error code
valid_ping = True
if ping == 0:
ping = 220
valid_ping = False
# This needs to be replaced with a navdata structure
angle = (i * 0.0062832) + heading - 2.09 + 1.61
ping_x = int(math.floor((self.roi_size / 2) + ping * math.cos(angle)))
ping_y = int(math.floor((self.roi_size / 2) - ping * math.sin(angle)))
# Build the contours of the scanning masks
inner_poly.append((ping_x, ping_y))
if valid_ping == True:
cv.Circle(self.__outermask, (ping_x, ping_y), 3, 255, -1)
# Append the origin to close the polygon's contour
inner_poly.append((x, y))
# Finally create the inner scan mask
cv.FillPoly(self.__innermask, [inner_poly], 255)
# Apply the masks to the map
cv.SetImageROI(
self.__m, (x - (self.roi_size / 2), y - (self.roi_size / 2), self.roi_size, self.roi_size)
)
cv.SubS(self.__m, self.darken_gradient, self.__m, self.__outermask)
cv.AddS(self.__m, self.lighten_gradient, self.__m, self.__innermask)
cv.ResetImageROI(self.__m)
except Exception as ex:
cv.ResetImageROI(self.__m)
pass
class SelfRescueMode(object):
def __init__(self):
# velocity publisher
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# lidar listener
self.scan_listener = LidarListener()
# angular and linear velocity in rescue mode
self.angular_vel = 0.2 #rad/s
self.linear_vel = 1.0 #m/s
# threshold showing there exists obstacles
self.stop_threshold = 1.5 # if any thing within this range emerge in the laser scan, stop robot
self.move_threshold = 4.0 # if all points in the front are farther then this value, go straight forward
# parameter storing the width of the robot
self.robot_width = 1.5
self.open_area_pts = 40 # at least 40 points exist in an "open area"
#self.out_of_boundary_threshold = 5 # threshold for tolerating small number of points within move_threshold
# paramter storing the current mode of the robot
self.mode = "turning" # valid values: "turning", "forward"
# define the variable to have velocity
self.twist = Twist()
# store the previous turning state
self.previous_turn = None
self.cumulative_turn_angle = 0.0 # store the cumulative turning angle
self.view_coef = 0.5 # coefficient to determine the view for navigation detection
self.view_coef_reset = 0.5 # backup copy of the coefficient to determine the view for navigation detection
self.view_decay_rate = 0.5 # gradually narrow the view
self.straight_count = 0
self.restore_view_threshold = 10 # move straight 5 steps and then recover the view
def do_nothing(self):
print("why we are here?")
def do_nothing1(self):
print("why we are here?")
def move_straight_forward(self, linear_speed=0.0):
'''
Move the vehicle straight forward under constant speed
---
Parameter
linear_speed : float
the linear speed of the vehicle
'''
#twist = Twist()
self.twist.linear.x = linear_speed
self.twist.angular.z = 0.0
self.pub.publish(self.twist)
time.sleep(0.05)
def counter_clockwise_turn(self, angle=5.0):
'''
turn the robot counter clockwise
with a small angle (only roughly since
neither does the robot have sensors like
imu in use, nor is the robot localized
---
Parameter
angle : float
the small angle we are going to turn. Unit is in degree,
angle can also be negative, which will lead to clockwise turning
'''
self.twist.linear.x = 0.0
if angle != 0.0:
self.twist.angular.z = angle / abs(
angle) * self.angular_vel #self.angular_vel # 0.2 rad/s
else:
self.twist.angular.z = 0.0
angle_rad = angle / 180.0 * np.pi
twist_time = abs(angle_rad / self.angular_vel)
#print("angle_rad = %f, twist_time = %f" % (angle_rad,twist_time))
# turn counter-clock wise
self.pub.publish(self.twist)
# sleep for sometime, so as to turn the robot
time.sleep(twist_time)
# stop the vehicle
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
self.pub.publish(self.twist)
time.sleep(0.1)
def rescue(self):
'''
one step for rescueing the robot
logic:
check whether there exists any obstacle in the front
of the robot
if no obstacle exists:
go straight forward
else:
if there exists open area to the left:
turn counter-clock wise
elif there exists open area to the right:
turn clock-wise
else:
turn counter-clockwise
'''
# get the latest scan data
latest_scan = self.scan_listener.get_latest_scan()
# check whether there exists any obstacle in the front
middle_count = int(len(latest_scan.ranges) / 2)
angle_min = latest_scan.angle_min
angle_max = latest_scan.angle_max
angle_increment = latest_scan.angle_increment
no_obstacle = True
front_distance_list = []
front_distance_index_list = []
for ii in range(int(middle_count * (1 - self.view_coef)),
int(middle_count * (1 + self.view_coef))):
x_distance = abs(latest_scan.ranges[ii] *
np.cos(angle_min + angle_increment * ii))
y_distance = abs(latest_scan.ranges[ii] *
np.sin(angle_min + angle_increment * ii))
if y_distance <= self.robot_width / 2: # check whether the point is in the front
front_distance_list.append(x_distance)
front_distance_index_list.append(ii)
if self.mode == "turning":
print("length == ",
len(np.array(front_distance_list) < self.move_threshold))
if min(front_distance_list) > self.move_threshold:
# we have find an open area, go straight forward
self.mode = "forward"
self.move_straight_forward(self.linear_vel)
self.previous_turn = None # going straight forward, clear turning
self.cumulative_turn_angle = 0.0
self.straight_count = 0
print("going straight forward!")
return
else:
if self.cumulative_turn_angle > 360.0:
# turned around without finding open place, weaken the constraint on view
print("robot captured, weaken constraint")
self.view_coef *= self.view_decay_rate
self.cumulative_turn_angle = 0.0
# self.counter_clockwise_turn() # slowly turn counter-clockwise, try to find an open area
# return
elif self.mode == "forward":
self.previous_turn = None # going straight forward, clear turning
if min(front_distance_list) > self.stop_threshold:
# we are moving in an open area, keep going
self.mode = "forward"
self.move_straight_forward(self.linear_vel)
self.straight_count += 1
if self.straight_count > self.restore_view_threshold:
print("reset view")
self.view_coef = self.view_coef_reset # reset the view coefficient
return
else:
# there exists obstacle in the front, stop
self.mode = "turning"
self.straight_count = 0
self.cumulative_turn_angle = 0.0
self.move_straight_forward(0.0) # stop robot
# robot meets obstacle if coming here
# check which area is occupied and don't turn into this direction
front_distance_list = np.array(front_distance_list)
front_distance_index_list = np.array(front_distance_index_list)
small_distance_index = front_distance_index_list[
front_distance_list <= self.stop_threshold]
right_occupied = False
left_occupied = False
if len(small_distance_index) != 0:
if min(small_distance_index) < middle_count * (1 -
self.view_coef / 2):
right_occupied = True
if max(small_distance_index) > middle_count / (1 -
self.view_coef / 2):
left_occupied = True
if right_occupied and not left_occupied:
# turn left
if not self.previous_turn == "right":
print("right occupied, turn left")
self.previous_turn = "left"
self.counter_clockwise_turn(20.0)
# update the cumulative turning angle
self.cumulative_turn_angle += 20.0
return
if left_occupied and not right_occupied:
# turn right
if not self.previous_turn == "left": # avoid oscillating between turning left and right
print("left occupied, turn right")
self.previous_turn = "right"
self.counter_clockwise_turn(-20.0)
# update the cumulative turning angle
self.cumulative_turn_angle -= 20.0
return
'''
# check whether there exists an open area
exist_open_area = False
center_of_open_area = None
for ii in range(0,len(latest_scan.ranges) - self.open_area_pts):
if min(latest_scan.ranges[ii:ii+self.open_area_pts]) > self.move_threshold + 1.0 and (ii < middle_count - 10 or ii > middle_count + 10):
exist_open_area = True
center_of_open_area = ii + self.open_area_pts / 2
break
if exist_open_area:
print("Find open area, turn")
self.counter_clockwise_turn(center_of_open_area * 0.25 - 90.0)
self.mode = "turning"
return
else:
# no open area exist, turn counter-clockwise for self-rescue
print("Turn to find open area")
self.mode = "turning"
self.counter_clockwise_turn(45.0) # turn 45 degrees
return
'''
# both left and right occupied, turn to find open area
print("Turn to find open area")
self.mode = "turning"
if self.previous_turn != "right":
#self.counter_clockwise_turn(15.0)
self.counter_clockwise_turn(5.0)
# update the cumulative turning angle
self.cumulative_turn_angle += 5.0
else:
self.counter_clockwise_turn(45.0)
# update the cumulative turning angle
self.cumulative_turn_angle += 45.0
self.previous_turn = "left"
return
