def debugging(self):
self.robot_pose = np.array([10, 10])
self.robot_target = np.array([100, 190])
self.positionmap = [0, 0]
self.img = imread(MAP_IMG, mode="L")
kernel = np.ones((2, 2), np.uint8)
self.img = cv2.dilate(self.img, kernel, iterations=1)
self.img = cv2.erode(self.img, kernel, iterations=2)
size = (np.shape(self.img))
self.rrt_star.env.img = self.img
self.rrt_star.utils.img = self.img
self.rrt_star.plotting.img = self.img
self.rrt_star.x_range = (0, size[1])
self.rrt_star.y_range = (0, size[0])
self.rrt_star.env.x_range = self.rrt_star.x_range
self.rrt_star.env.y_range = self.rrt_star.y_range
robposex = 35.0
robposey = 28.0
start_time = time.time()
tarpose = self.getTargetRandomPose()
print("Tiempo: ", time.time() - start_time)
print("target pose")
print(tarpose.x, tarpose.y)
tarposex = tarpose.x
tarposey = tarpose.y
self.robot_pose = np.array(
[robposex - self.positionmap[0], robposey - self.positionmap[1]])
self.robot_target = np.array(
[tarposex - self.positionmap[0], tarposey - self.positionmap[1]])
self.x_start = Node(self.robot_pose) # Starting node
self.x_goal = Node(self.robot_target)
self.rrt_star.selectStartGoalPoints(self.x_start, self.x_goal)
path = self.rrt_star.planning()
#self.rrt.selectStartGoalPoints(Node(x_start), Node(x_goal) )
if path is None:
print("Cannot find path")
while True:
start_time = time.time()
tarpose = self.getTargetRandomPose()
print("Tiempo: ", time.time() - start_time)
pass
else:
print("found path!!")
return path
def __init__(self):
self.Odometry = False
self.rate = rospy.Rate(1)
self.rate = rospy.Rate(1)
self.bridge = CvBridge()
self.rrt_star = RrtStar((0,0), (0,0), 1500, 0.1, 1000, 3000)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.positionmap = [0,0]
self.object_target = Node([7/0.05,-5/0.05])
self.x_start = []
self.x_goal = []
self.real_start = []
self.real_goal = []
self.path = []
self.path_final = []
self.img = []
self.img2 = []
self.img_debug = []
self.path_pub = rospy.Publisher("/rrt/path_drone",Path, queue_size=1)
self.path_image_pub = rospy.Publisher("/debug/image_path_drone",Image, queue_size=1)
#self.image_sub = rospy.Subscriber("/rtabmap/grid_map",OccupancyGrid, self.callback, queue_size=1, buff_size=1)
self.image_sub = rospy.Subscriber("/bebop/image_map",Image, self.callback_image, queue_size=1, buff_size=1)
self.map_start_sub = rospy.Subscriber("/bebop/start_map", Point,self.callback_map_start, queue_size=1, buff_size=1)
self.map_goal_sub= rospy.Subscriber('/bebop/goal_map', Point, self.callback_map_goal, queue_size=1, buff_size=1)
self.start_sub = rospy.Subscriber("/bebop/start_map", Point,self.callback_start, queue_size=1, buff_size=1)
self.goal_sub= rospy.Subscriber('/bebop/goal', Point, self.callback_goal, queue_size=1, buff_size=1)
def getWaypointRandomPose(self):
delta = 10
min_dist = sys.maxint
node_dist_min = []
point = self.object_target
zonex = int(point.y)
zoney = int(point.x)
zonex = self.width
zoney = self.height
for k in range(1000):
# node_x = np.random.randint(zonex - delta, zonex + delta)
# if node_x >= self.width:
# node_x = self.width-1
# if node_x < 0:
# node_x=0
#
# node_y = np.random.randint(zoney - delta, zoney + delta)
# if node_y >= self.height:
# node_y = self.height-1
# if node_y < 0:
# node_y=0
node_x = np.random.randint(0, zonex)
node_y = np.random.randint(0, zoney)
node = Node((node_x, node_y))
if (self.img[int(node.y)][int(node.x)] >= 200):
my_list1 = np.array(self.neighbors(2, node.y, node.x))
#print(my_list1)
if ((my_list1 <= 100).any()): #if black
#print("continue")
continue
if ((my_list1 > 100).any()
and (my_list1 <= 200).any()): #if gray
robotToNode = self.rrt_star.utils.get_dist(
self.x_start, node)
nodeToObject = self.rrt_star.utils.get_dist(
node, self.object_target)
if robotToNode < delta * 2 or robotToNode >= delta * 500:
#print("node ", node.y, node.x)
#print("to short", robotToNode)
continue
if (robotToNode + nodeToObject < min_dist):
min_dist = robotToNode + nodeToObject #if gray
print("shortest", min_dist)
node_dist_min = node
if node_dist_min != []:
print("shortest_ node ", node_dist_min.y, node_dist_min.x)
#print("shortest",self.rrt_star.utils.get_dist(self.x_start,node_dist_min))
return node_dist_min
else:
print("fail no wayPoint avalible")
return node_dist_min
def callback_map_goal(self, data):
print("callback_map_goal")
self.x_goal = Node([data.x, data.y])
if self.img != []:
self.send_image_path(self.img_debug)
if self.path_final != []:
self.send(self.path_final[::-1])
print("SENDING MAP!!!!!!!!!!!!!!!!!!!!1")
def __init__(self):
self.Odometry = False
self.rate = rospy.Rate(0.05)
#########
#self.debug_image = imread(MAP_IMG,mode="L")
#self.img2 = cv2.cvtColor(self.debug_image,cv2.COLOR_RGB2BGR)
########
self.respix = 0.05
self.bridge = CvBridge()
self.rrt_star = RrtStar((0, 0), (0, 0), 150, 0.2, 20, 2000)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.positionmap = [0, 0]
self.object_target = Node([0, 0])
self.robot_pose = Node([0, 0])
self.x_start = Node([0, 0])
self.x_goal = Node([0, 0])
self.wayPoints = []
self.haveWayPoints = False
self.haveMap = False
self.path_pub = rospy.Publisher("/rrt/path", Path, queue_size=1)
self.path_image_pub = rospy.Publisher("/debug/ddr_path",
Image,
queue_size=1)
self.image_sub = rospy.Subscriber("/rtabmap/grid_map",
OccupancyGrid,
self.callback,
queue_size=1,
buff_size=1)
#self.image_drone_sub = rospy.Subscriber("/bebop/image_map",Image, self.callback_image_drone, queue_size=1, buff_size=1)
self.poseWhitCovariance_sub = rospy.Subscriber(
"/rtabmap/localization_pose", PoseWithCovarianceStamped,
self.poseWhitCovariance_callback)
self.odom_sub = rospy.Subscriber("/roboto_diff_drive_controller/odom",
Odometry, self.odometry_callback)
self.goal_pose_sub = rospy.Subscriber('/goal/pose', Point,
self.goal_pose_callback)
self.path_sub = rospy.Subscriber("/rrt/path_drone", Path,
self.callback_drone_path)
def work(self):
"""
:param: work_time - the working time step for the robot.
"""
work_time = 100
# make the initial plan
path = self.find_path()
robot_state = 1
while work_time != 0:
# try to change to new goal
self.new_goal = self.change_goal()
# if goal change
if operator.ne(self.cur_goal, self.new_goal):
# replan
self.planner.start = Node(self.cur_loc[0], self.cur_loc[1])
self.planner.cur_goal = Node(self.new_goal[0], self.new_goal[1])
#self.planner.path = [[self.new_goal[0], self.new_goal[1]]]
self.planner.reset()
print("the goal change from", self.cur_goal, "to", self.new_goal)
self.cur_goal = self.new_goal
path = self.find_path()
robot_state = 1
# draw the new path
self.draw_path(path=path)
plt.pause(0.1)
else:
# move the robot to the next point
if robot_state < len(path):
self.move(self.cur_loc, path[robot_state])
self.draw_path(path)
plt.pause(0.5)
robot_state += 1
else:
print("the robot reach the goal!:", self.cur_goal)
work_time -= 1
time.sleep(1)
def getTargetRandomPose(self):
delta = 5
min_dist = sys.maxint
node_dist_min = []
for k in range(10000):
node = Node((np.random.randint(self.rrt_star.x_range[0] + delta,
self.rrt_star.x_range[1] - delta),
np.random.randint(self.rrt_star.y_range[0] + delta,
self.rrt_star.y_range[1] - delta)))
if (self.img[int(node.y)][int(node.x)] >= 200):
my_list1 = np.array(self.neighbors(2, node.y, node.x))
#print(my_list1)
if ((my_list1 <= 100).any()): #if black
#print("continue")
continue
if ((my_list1 > 100).any() and (my_list1 <= 200).any()):
# if ((node.x >= (self.object_target.x - delta)) and (node.x < (self.object_target.x + delta))):
# if ((node.y >= (self.object_target.y - delta)) and (node.y < (self.object_target.y + delta))):
# return self.object_target
robotToNode = self.rrt_star.utils.get_dist(
self.x_start, node)
nodeToObject = self.rrt_star.utils.get_dist(
node, self.object_target)
if robotToNode < delta * 15 or robotToNode >= delta * 500:
print("node ", node.y, node.x)
print("to short", robotToNode)
continue
if (robotToNode + nodeToObject < min_dist):
min_dist = robotToNode + nodeToObject #if gray
#print(my_list1)
#print(node.x, node.y)
#print("shortest",min_dist)
node_dist_min = node
if node_dist_min != []:
#print("shortest_ node ",node_dist_min.y, node_dist_min.x)
#print("shortest",self.rrt_star.utils.get_dist(self.x_start,node_dist_min))
return node_dist_min
else:
print("fail no point avalible")
return node_dist_min
def callback_drone_path(self, data):
if not self.haveWayPoints:
self.wayPoints = []
for pose in data.poses:
point = Node([
(pose.pose.position.x - self.positionmap[0]) / self.respix,
-(pose.pose.position.y - self.positionmap[1]) / self.respix
])
self.wayPoints.append(point)
#print (self.wayPoints)
print("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA")
#print(data.poses)
self.haveWayPoints = True
def goal_pose_callback(self, data):
self.object_target = Node([data.x / self.respix, data.y / self.respix])
def pathPlanning(self, data):
if self.Odometry:
path = []
#self.example_function()
map = np.array(data.data)
self.width = data.info.width
self.height = data.info.height
size = [self.height, self.width]
self.positionmap = [
round(data.info.origin.position.x / self.respix),
round(data.info.origin.position.y / self.respix)
]
self.fig = None
self.z = 0
self.img = np.array(
np.uint8(np.resize(map, [self.height, self.width])))
self.img = np.where(self.img == 255, 102, self.img)
self.img = np.where(self.img == 0, 255, self.img)
self.img = np.where(self.img == 100, 0, self.img)
#print(self.img)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
self.img = cv2.erode(self.img, kernel, iterations=1)
#self.img = cv2.dilate(self.img,kernel,iterations = 1)
self.rrt_star.env.img = self.img
self.rrt_star.utils.img = self.img
self.rrt_star.plotting.img = self.img
self.rrt_star.x_range = (0, size[1])
self.rrt_star.y_range = (0, size[0])
self.rrt_star.env.x_range = self.rrt_star.x_range
self.rrt_star.env.y_range = self.rrt_star.y_range
#print(self.rrt_star.x_range)
#print(self.rrt_star.y_range)
start_time = time.time()
tarpose = self.getTargetRandomPose()
#print("Tiempo: ", time.time() - start_time)
#print("target pose")
#print(tarpose.x, tarpose.y)
if tarpose != []:
tarposex = tarpose.x
tarposey = tarpose.y
else:
return
#tarposex = 0
#tarposey = 0
#self.robot_pose = np.array([robposex - self.positionmap[0], robposey - self.positionmap[1]])
self.robot_target = np.array([tarposex, tarposey])
#print(self.robot_pose)
print(self.robot_target)
print(self.width)
print(self.height)
size = (np.shape(self.img))
self.rrt_star.env.img = self.img
self.rrt_star.utils.img = self.img
self.rrt_star.plotting.img = self.img
self.rrt_star.x_range = (0, size[1])
self.rrt_star.y_range = (0, size[0])
self.rrt_star.env.x_range = self.rrt_star.x_range
self.rrt_star.env.y_range = self.rrt_star.y_range
self.x_start = Node(self.robot_pose) # Starting node
self.x_goal = Node(self.robot_target)
#x_start = (18, 20) # Starting node
#x_goal = (190, 125)
self.rrt_star.selectStartGoalPoints(self.x_start, self.x_goal)
path = self.rrt_star.planning()
if path is None:
print("Cannot find path")
for i in range(50):
self.neighborsToBlack(5, tarposey, tarposex)
#plt.imshow(self.img, cmap=cm.Greys_r)
tarpose = self.getTargetRandomPose()
if tarpose != []:
tarposex = tarpose.x
tarposey = tarpose.y
else:
continue
self.robot_target = np.array([tarposex, tarposey])
self.x_goal = Node(self.robot_target)
self.rrt_star.selectStartGoalPoints(
self.x_start, self.x_goal)
path = self.rrt_star.planning()
if path is not None:
break
if path is not None:
print("found path!!")
odomcoor = np.array(path)
odomcoorx = np.array([odomcoor[:, 0] + self.positionmap[0]
]) * self.respix
odomcoory = np.array([odomcoor[:, 1] + self.positionmap[1]
]) * self.respix
odomcoor = np.append(odomcoorx, odomcoory, axis=0)
path = odomcoor.T
print 'Final path:', odomcoor.T
self.send(path[::-1])
rospy.signal_shutdown("End node")
img2 = np.fromstring(
self.rrt_star.plotting.fig.canvas.tostring_rgb(),
dtype=np.uint8,
sep='')
ncols, nrows = self.rrt_star.plotting.fig.canvas.get_width_height(
)
img2 = img2.reshape(self.rrt_star.plotting.fig.canvas.
get_width_height()[::-1] + (3, ))
self.img2 = cv2.cvtColor(img2, cv2.COLOR_RGB2BGR)
self.send_image_path(self.img2)
return path
pass
def getTargetRandomPose(self):
delta = 5
node = Node((750, 250))
return node
def callback_map_start(self, data):
self.x_start = Node([data.x, data.y])
def callback_start(self, data):
self.real_start = Node([data.x, data.y])
def callback_goal(self, data):
self.real_goal = Node([data.x, data.y])
def pathPlanning(self, data):
print("mapa recibido")
if self.Odometry and self.haveWayPoints:
self.haveMap = False
path = []
print("waypoints recibiodos")
self.rrt_star.env.x_range = self.rrt_star.x_range
self.rrt_star.env.y_range = self.rrt_star.y_range
wayPoint_x = self.wayPoints[-1].x - self.positionmap[0]
wayPoint_y = self.wayPoints[-1].y - self.positionmap[1]
if wayPoint_x >= self.width:
wayPoint_x = self.width - 1
if wayPoint_x < 0:
wayPoint_x = 0
if wayPoint_y >= self.height:
wayPoint_y = self.height - 1
if wayPoint_y < 0:
wayPoint_y = 0
self.object_target.x = wayPoint_x
self.object_target.y = wayPoint_y
start_time = time.time()
print(self.width, self.height)
print("waypoint targert")
print(self.wayPoints[-1].x, self.wayPoints[-1].y)
print(self.object_target.x, self.object_target.y)
#tarpose = self.getTargetRandomPose()
self.robot_start = np.array([
self.robot_pose[1] - self.positionmap[0],
self.robot_pose[0] - self.positionmap[1]
])
self.x_start = Node([self.robot_start[0],
self.robot_start[1]]) # Starting node
tarpose = self.getWaypointRandomPose()
#print("Tiempo: ", time.time() - start_time)
if tarpose != []:
tarposex = tarpose.x
tarposey = tarpose.y
else:
return
self.robot_start = np.array([
self.robot_pose[1] - self.positionmap[0],
self.robot_pose[0] - self.positionmap[1]
])
self.robot_target = np.array([tarposex, tarposey])
#self.robot_target = np.array([78 , 70])
print("desde:", self.robot_start)
print("hasta:", self.robot_target)
print("desde:", self.robot_start * self.respix)
print("hasta:", self.robot_target * self.respix)
size = (np.shape(self.img))
self.rrt_star.env.img = self.img
self.rrt_star.utils.img = self.img
self.rrt_star.plotting.img = self.img
self.rrt_star.x_range = (0, size[1])
self.rrt_star.y_range = (0, size[0])
self.rrt_star.env.x_range = self.rrt_star.x_range
self.rrt_star.env.y_range = self.rrt_star.y_range
self.x_goal = Node(self.robot_target)
#x_start = (18, 20) # Starting node
#x_goal = (190, 125)
print(self.robot_start[0], self.robot_start[1])
print(self.x_start.x, self.x_start.y)
self.neighborsToWhite(4, int(self.x_start.y), int(self.x_start.x))
#self.neighborsToBlack(2, int(self.object_target.x), int(self.object_target.y))
#self.img[int(self.object_target.y)][int(self.object_target.x)] = 180
self.img2 = cv2.cvtColor(self.img, cv2.COLOR_RGB2BGR)
self.rrt_star.selectStartGoalPoints(self.x_start, self.x_goal)
path = self.rrt_star.planning()
if path is None:
print("Cannot find path")
for i in range(50):
self.neighborsToBlack(5, tarposey, tarposex)
tarpose = self.getWaypointRandomPose()
if tarpose != []:
tarposex = tarpose.x
tarposey = tarpose.y
else:
continue
self.robot_target = np.array([tarposex, tarposey])
self.x_goal = Node(self.robot_target)
self.rrt_star.selectStartGoalPoints(
self.x_start, self.x_goal)
path = self.rrt_star.planning()
if path is not None:
break
if path is not None:
print("found path!!")
odomcoor = np.array(path)
odomcoorx = np.array([odomcoor[:, 0] + self.positionmap[0]
]) * self.respix
odomcoory = np.array([odomcoor[:, 1] + self.positionmap[1]
]) * self.respix
odomcoor = np.append(odomcoorx, odomcoory, axis=0)
path = odomcoor.T
print 'Final path:', odomcoor.T
self.send(path[::-1])
# # Blue color in BGR
# color = (255, 0, 0)
#
# # Line thickness of 2 px
# thickness = 2
#
# cv2.polylines(self.img2,path , False, color, thickness)
# plt.imshow(self.img2, cmap=cm.Greys_r)
# plt.pause(1)
self.send_image_path(self.img2)
return path
pass
else:
print("Esperando waypoints")