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