def pub_data(event): if(goal_x == 0 and goal_y == 0): return else: wp_list = WaypointList() wp_list.header.stamp = rospy.Time.now() wp_list.header.frame_id = 'odom' wp = Waypoint() wp.x = goal_x wp.y = goal_y wp_list.list.append(wp) pub_wp_list.publish(wp_list)
def _rrt_process(self):
while (np.linalg.norm(self.q_goal - self.q_near) > self.epsilon):
# Generate q_rand
#self.q_rand = np.array([np.random.uniform(-1,1)*self.X_DIM, np.random.uniform(-1,1)*self.Y_DIM])
self.q_rand = np.array([np.random.normal(self.X_DIM, self.sigma),
np.random.normal(self.Y_DIM, self.sigma)])
self.q_near, index = self._near()
if(self.q_near[0] > self.X_DIM):
self.q_near[0] = self.X_DIM
if(self.q_near[0] < 0):
self.q_near[0] = 0
if(self.q_near[1] > self.Y_DIM):
self.q_near[1] = self.Y_DIM
if(self.q_near[1] < 0):
self.q_near[1] = 0
if(self._is_hit_constrain(self.q_list[index], self.q_near) == 0):
self.q_list = np.append(self.q_list, [[0, 0, 0]], axis = 0)
self.q_list[self.q_size][0:2] = self.q_near
self.q_list[self.q_size][2] = int(index+1)
self.q_size = self.q_size + 1
# end while
# Reverse tracking q_list
waypoint_index_list = [self.q_size]
child_index = int(self.q_size)
parent_index = int(self.q_list[self.q_size-1][2])
while (child_index != 1):
self.waypoint_size = self.waypoint_size + 1
waypoint_index_list.append(parent_index)
temp = parent_index
parent_index = int(self.q_list[temp-1][2])
child_index = temp
# end while
self.waypoint_list.size = self.waypoint_size
for i in range(self.waypoint_size, 0, -1):
waypoint = Waypoint()
waypoint.x = self.q_list[waypoint_index_list[i-1]-1][0]
waypoint.y = self.q_list[waypoint_index_list[i-1]-1][1]
print waypoint.x, waypoint.y
self.waypoint_list.list.append(waypoint)
self.waypoint_list.header.stamp = rospy.Time.now()
self.pub_waypointList.publish(self.waypoint_list)
#self.published = True
print "q_list", self.q_list
print "waypoint index", waypoint_index_list
print "waypoint list"
for i in range(0, self.waypoint_size):
print self.waypoint_list.list[i].x, self.waypoint_list.list[i].y
rospy.loginfo("[%s] RRT path planning finished" %(self.node_name))
self.rviz()
self.lock = False
def move_down(self, move, timeout=10):
''' Move down in the given amount of time '''
to_send = Waypoint()
to_send.pose.position.z = self.current_pos.pose.position.z - move
# send goal to server to complete within timeout
move_result = self.send_goal(to_send, timeout)
return move_result
def move_forward(self, move, timeout=10):
''' Move forward in the given amount of time '''
to_send = Waypoint()
to_send.pose.position.x = self.current_pos.pose.position.x + move
move_result = self.send_goal(to_send, timeout)
return move_result
def __init__(self, testing):
self.current_pos = Waypoint()
self.client = actionlib.SimpleActionClient(
'goal', robotx_mission_planner.msg.current_goalAction)
self.client.wait_for_server()
self.testing = testing
def relative_move(self, x=0, y=0, z=0, tx=0, ty=0, tz=0, timeout=10):
'''
Move to a position relative to the current position
The user should specify position with
class_instance.relative_goal and then call this function
'''
to_send = Waypoint()
# Add move onto current position
to_send.pose.position.x = self.current_pos.pose.position.x + x
to_send.pose.position.y = self.current_pos.pose.position.y + y
to_send.pose.position.z = self.current_pos.pose.position.z + z
# This section adds the desired axis rotation to current position
# convert from quaternion to numpy array
cur_rotation = gh.quat_to_euler(
gh.quat_to_np(self.current_pos.pose.orientation))
new_rotation = cur_rotation
new_rotation[0] = cur_rotation[0] + tx
new_rotation[1] = cur_rotation[1] + ty
new_rotation[2] = cur_rotation[2] + tz
# convert from rotvec back into ROS quaternion message
quat = gh.euler_to_quat(new_rotation)
# set quaternion to send as the new quaternion
to_send.pose.orientation = mh.numpy_to_quat(quat)
# send goal to server to complete within timeout
move_result = self.send_goal(to_send, timeout)
return move_result
def rviz_debug(self):
self.waypoint_list = WaypointList()
self.waypoint_list.header.frame_id = self.frame_id
self.waypoint_size = 0
for i in self.wp_list:
self.waypoint_size = self.waypoint_size + 1
waypoint = Waypoint()
waypoint.x = i[0]
waypoint.y = i[1]
waypoint.z = 0
#print waypoint.x, waypoint.y
self.waypoint_list.list.append(waypoint)
self.waypoint_list.size = self.waypoint_size
marker = Marker()
marker.header.frame_id = self.frame_id
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 1.0
marker.color.b = 0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.points = []
for i in range(self.waypoint_size):
p = Point()
#print i
#print self.waypoint_size, i, self.waypoint_list.list[i].x
p.x = self.waypoint_list.list[i].x
p.y = self.waypoint_list.list[i].y
p.z = self.waypoint_list.list[i].z
marker.points.append(p)
self.pub_rviz.publish(marker)
def absolute_move(self,
x=None,
y=None,
z=None,
qx=None,
qy=None,
qz=None,
qw=None,
timeout=10):
'''
Move to an absolute /map position
The user should specify position with
class_instance.absolute_goal and then call this function
'''
if x is None:
x = self.current_pos.pose.x
if y is None:
y = self.current_pos.pose.y
if z is None:
z = self.current_pos.pose.z
if qx is None:
qx = self.current_pos.pose.orientation.x
if qy is None:
qy = self.current_pos.pose.orientation.y
if qz is None:
qz = self.current_pos.pose.orientation.z
if qw is None:
qw = self.current_pos.pose.orientation.w
to_send = Waypoint()
# Set variables from built position
to_send.pose.position.x = x
to_send.pose.position.y = y
to_send.pose.position.z = z
to_send.pose.orientation.x = qx
to_send.pose.orientation.y = qy
to_send.pose.orientation.z = qz
to_send.pose.orientation.w = qw
# send goal to server to complete within timeout
move_result = self.send_goal(to_send, timeout)
return move_result
def _process(self):
self.iteration = self.iteration + 1
if self.iteration > 50:
self.plan_done = True
return
#print self.p_now, self.p_next
for obs_index in range(self.obstacle_list.size):
collision = False
intersect_count = 0
intersect_p1 = None
intersect_p2 = None
may_be_p1 = None
may_be_p2 = None
p_new = []
p1 = [
self.obstacle_list.list[obs_index].x_min_x,
self.obstacle_list.list[obs_index].x_min_y
]
p2 = [
self.obstacle_list.list[obs_index].y_min_x,
self.obstacle_list.list[obs_index].y_min_y
]
p3 = [
self.obstacle_list.list[obs_index].x_max_x,
self.obstacle_list.list[obs_index].x_max_y
]
p4 = [
self.obstacle_list.list[obs_index].y_max_x,
self.obstacle_list.list[obs_index].y_max_y
]
obs_vertex = [p1]
######## UNDO: deal with the situation that the line only intersect a single vertex ########
if not p2 in obs_vertex:
obs_vertex.append(p2)
if not p3 in obs_vertex:
obs_vertex.append(p3)
if not p4 in obs_vertex:
obs_vertex.append(p4)
for i, j in zip(obs_vertex[0::], obs_vertex[1::]):
check_line_intersect = self.check_intersect(
i, j, self.p_now, self.p_next, False)
if check_line_intersect == True:
collision = True
intersect_count = intersect_count + 1
intersect_p1 = i
intersect_p2 = j
if check_line_intersect is None:
may_be_p1 = i
may_be_p2 = j
check_line_intersect = self.check_intersect(
obs_vertex[-1], obs_vertex[0], self.p_now, self.p_next, False)
if check_line_intersect == True:
collision = True
intersect_count = intersect_count + 1
intersect_p1 = i
intersect_p2 = j
if check_line_intersect is None:
may_be_p1 = obs_vertex[-1]
may_be_p2 = obs_vertex[0]
#############
'''
if self.now in obstacle:
self.plan_done = True
return
'''
# if self.p_next is in the obstacle!!!
if intersect_count == 1 and len(obs_vertex) > 2:
if self.check_avoid_point:
#print "oh no"
self.wp_list.remove(self.p_next)
self.p_next = self.wp_list[self.wp_index + 1]
self.change_side = True
return
#print "F**K"
p_intersect = self.check_intersect(may_be_p1, may_be_p2,
self.p_now, self.p_next,
True)
d = self.dis_point2line(intersect_p1, intersect_p2,
p_intersect) + self.safe_dis
self.p_next = self.find_shift_point(intersect_p1, intersect_p2,
d, self.p_next)
self.wp_list[self.wp_index + 1] = self.p_next
#print "DAMMM"
return
self.check_avoid_point = False
if collision:
obs_angle_list = []
angle_list_positive = []
angle_list_negetive = []
second_angle = None
for i in range(len(obs_vertex)):
obs_angle_list.append([
obs_vertex[i],
self.get_angle(self.p_now, self.p_next, obs_vertex[i])
])
if obs_angle_list[i][1] >= 0:
angle_list_positive.append(obs_angle_list[i][1])
else:
angle_list_negetive.append(obs_angle_list[i][1])
angle_list_positive.sort()
angle_list_negetive.sort()
if abs(angle_list_positive[-1]) < abs(angle_list_negetive[0]):
if self.change_side:
second_angle = angle_list_negetive[0]
print "change"
self.change_side = False
else:
second_angle = angle_list_positive[-1]
else:
if self.change_side:
second_angle = angle_list_positive[-1]
print "change"
self.change_side = False
else:
second_angle = angle_list_negetive[0]
#print "sec,", second_angle
#print angle_list
#print angle_list[-2]
for i in range(len(obs_vertex)):
if obs_angle_list[i][1] == second_angle:
p_new = obs_angle_list[i][0]
#print "p_new", p_new
break
p_new = self.find_shift_point(self.p_now, self.p_next,
self.safe_dis + 0.1, p_new)
#print "A"
self.wp_list.insert(self.wp_index + 1, p_new)
self.p_next = p_new
self.plan_done = False
self.check_avoid_point = True
return
min_dis = 1e6
closest_p = None
for i in obs_vertex:
dis_P2L = self.dis_point2line(
self.p_now, self.p_next,
i) # distance from a point to the line
if dis_P2L < min_dis:
min_dis = dis_P2L
closest_p = i
if min_dis < self.safe_dis:
#print "------", min_dis, self.safe_dis
if abs(self.get_angle(
self.p_now, self.p_next, closest_p)) < 90 and abs(
self.get_angle(self.p_next, self.p_now,
closest_p)) < 90:
# shift to another side
#print self.get_angle(self.p_now, self.p_next, i), self.get_angle(self.p_next, self.p_now, i)
#print "----", closest_p
#print self.p_now, self.p_next
self.check_avoid_point = True
p_new = self.find_shift_point(self.p_now, self.p_next,
-self.safe_dis - 0.1,
closest_p)
#print "B"
self.wp_list.insert(self.wp_index + 1, p_new)
self.p_next = p_new
#print "it,s", self.p_next
self.plan_done = False
return
if self.p_next != self.wp_list[-1]:
self.wp_index = self.wp_index + 1
self.p_now = self.wp_list[self.wp_index]
self.p_next = self.wp_list[self.wp_index + 1]
self.plan_done = False
#print "ok"
return
for i in self.wp_list:
self.waypoint_size = self.waypoint_size + 1
waypoint = Waypoint()
waypoint.x = i[0]
waypoint.y = i[1]
waypoint.z = 0
#print waypoint.x, waypoint.y
self.waypoint_list.list.append(waypoint)
self.waypoint_list.size = self.waypoint_size
self.waypoint_list.header.frame_id = self.frame_id
self.waypoint_list.header.stamp = rospy.Time.now()
self.pub_waypointList.publish(self.waypoint_list)
self.plan_done = True
self.rviz()
print "Finish path planning"