def hover_up(self, data):
"""give commands to hover at 1m above the starting position"""
if self.hover_counter == 0:
self.hover_start_time = data.header.stamp.to_sec()
#print 'i m here--', self.hover_start_time
self.trajectory_time = data.header.stamp.to_sec(
) - self.hover_start_time
#print "hover start time and trajectory time is", self.hover_start_time, self.trajectory_time
if self.hover_counter == 0:
waypoint_specified = True
waypoint_bc = False
path = [[
self.initial_position[0], self.initial_position[1],
self.initial_position[2]
],
[
self.initial_position[0], self.initial_position[1],
self.hovering_height
]]
path = zip(*path)
vel = np.array([[0, 0, 0]])
acc = np.array([[0, 0, 0]])
jerk = np.array([[0, 0, 0]])
planning_counter = -1
velocity = self.velocity_during_hover_land
T, p1, p2, p3 = Minimum_snap_trajetory(
planning_counter, velocity, path, waypoint_specified,
waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = self.N
for i in range(len(p1)):
msg.poly_x.append(p1[i])
msg.poly_y.append(p2[i])
msg.poly_z.append(p3[i])
msg.number_of_segments = 1
msg.planning_start_time = str(self.hover_start_time)
for j in T:
msg.segment_end_times.append(j)
msg.desired_direction.x = self.desired_direction[0]
msg.desired_direction.y = self.desired_direction[1]
msg.desired_direction.z = self.desired_direction[2]
msg.trajectory_mode = 'hover'
self.hover_traj_msg = msg
#print self.hover_traj_msg
self.traj_polycoeff.publish(self.hover_traj_msg)
self.hover_counter += 1
def generate_trajectory_from_waypoints(self, data):
"""generates trajecory from the way points"""
self.trajectory_time = data.header.stamp.to_sec(
) - self.hover_start_time
if self.waypoint_counter == 0 and self.reached_goal == False:
# -------change this segment in order to construct a new trajectory---------------
start = self.Pglobal
end = self.end_point
no_of_segments = 8
path = []
for j in range(no_of_segments + 1):
path.append(list(start + j * (end - start) / no_of_segments))
path = zip(*path)
# -------change this segment in order to construct a new trajectory---------------
waypoint_specified = True
waypoint_bc = False
velocity = self.uav_velocity
planning_counter = 0
vel = np.array([[0, 0, 0]])
acc = np.array([[0, 0, 0]])
jerk = np.array([[0, 0, 0]])
T, _p1, _p2, _p3 = Minimum_snap_trajetory(
planning_counter, velocity, path, waypoint_specified,
waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = self.N
for i in range(len(_p1)):
msg.poly_x.append(_p1[i])
msg.poly_y.append(_p2[i])
msg.poly_z.append(_p3[i])
msg.number_of_segments = no_of_segments
msg.planning_start_time = str(data.header.stamp.to_sec())
for j in T[:no_of_segments + 1]:
msg.segment_end_times.append(j)
msg.desired_direction.x = self.desired_direction[0]
msg.desired_direction.y = self.desired_direction[1]
msg.desired_direction.z = self.desired_direction[2]
msg.trajectory_mode = 'planning_from_waypoints'
self.traj_msg = msg
self.trajectory_end_in_global_time = self.trajectory_time + T[-1]
self.traj_polycoeff.publish(self.traj_msg)
# check if the goal is reached
if self.trajectory_time >= self.trajectory_end_in_global_time + 5.0: # 5 sec is the time for which the quad will hover before coming down
self.reached_goal = True
self.waypoint_counter += 1
def hover_the_quad(self, data):
"""give commands to hover at 1m above the starting position"""
a = time.time()
if self.hover_counter == 0:
self.hover_start_time = data.header.stamp.to_sec()
self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time
waypoint_specified = True
waypoint_bc = False
path = [[
self.initial_position[0], self.initial_position[1],
self.initial_position[2]
],
[
self.initial_position[0], self.initial_position[1],
self.hovering_height
]]
path = zip(*path)
vel = np.array([[0, 0, 0]])
acc = np.array([[0, 0, 0]])
jerk = np.array([[0, 0, 0]])
planning_counter = -1
velocity = 0.5
T, p1, p2, p3 = Minimum_snap_trajetory(
planning_counter, velocity, path, waypoint_specified, waypoint_bc,
vel, acc, jerk).construct_polynomial_trajectory()
print 'time from start is:', self.RHP_time
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = 7
for i in range(len(p1)):
msg.poly_x.append(p1[i])
msg.poly_y.append(p2[i])
msg.poly_z.append(p3[i])
msg.number_of_segments = 1
msg.planning_start_time = self.hover_start_time
for j in T:
msg.segment_end_times.append(j)
msg.desired_direction.x = self.initial_orientation[0]
msg.desired_direction.y = self.initial_orientation[1]
msg.desired_direction.z = self.initial_orientation[2]
#msg.desired_direction.x = 0; msg.desired_direction.y = -1; msg.desired_direction.z = 0
#msg.desired_direction.x = 1; msg.desired_direction.y = 0; msg.desired_direction.z = 0
msg.trajectory_mode = 'hover'
self.traj_polycoeff.publish(msg)
self.initial_dist_to_goal = np.linalg.norm(self.end_point -
self.Pglobal)
f1 = open('total_time_taken.txt', 'a')
f1.write("%s,\n" % (time.time() - a))
self.hover_counter += 1
def land(self, data):
"""generate trajectory for landing"""
if self.land_counter == 0:
self.land_start_time = data.header.stamp.to_sec()
self.land_start = [
self.Pglobal[0], self.Pglobal[1], self.Pglobal[2]
]
if self.land_counter == 0:
waypoint_specified = True
waypoint_bc = False
path = [[
self.land_start[0], self.land_start[1], self.land_start[2]
], [self.land_start[0], self.land_start[1], 0.12]]
path = zip(*path)
vel = np.array([[0, 0, 0]])
acc = np.array([[0, 0, 0]])
jerk = np.array([[0, 0, 0]])
planning_counter = -1
velocity = self.velocity_during_hover_land
T, p1, p2, p3 = Minimum_snap_trajetory(
planning_counter, velocity, path, waypoint_specified,
waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = self.N
for i in range(len(p1)):
msg.poly_x.append(p1[i])
msg.poly_y.append(p2[i])
msg.poly_z.append(p3[i])
msg.number_of_segments = 1
msg.planning_start_time = str(self.land_start_time)
for j in T:
msg.segment_end_times.append(j)
msg.desired_direction.x = self.desired_direction[0]
msg.desired_direction.y = self.desired_direction[1]
msg.desired_direction.z = self.desired_direction[2]
msg.trajectory_mode = 'land'
self.land_traj_msg = msg
self.traj_polycoeff.publish(self.land_traj_msg)
self.land_counter += 1
def hover_up(self, data):
"""give commands to hover at 1m above the starting position"""
if self.hover_counter == 0:
self.hover_start_time = data.header.stamp.to_sec()
self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time
if self.hover_counter == 0:
path = [[
self.initial_position[0], self.initial_position[1],
self.initial_position[2]
],
[
self.initial_position[0], self.initial_position[1],
self.hovering_height
]]
vel = np.array([0, 0, 0])
acc = np.array([0, 0, 0])
jerk = np.array([0, 0, 0])
counter = 0
velocity = self.vel_hl
T, p1, p2, p3 = Minimum_jerk_trajetory(
counter, velocity, path, vel, acc,
jerk).construct_trajectory()
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'map'
msg.header = header
msg.polynomial_order = 7
for i in range(len(p1)):
msg.poly_x.append(p1[i])
msg.poly_y.append(p2[i])
msg.poly_z.append(p3[i])
msg.number_of_segments = 1
msg.planning_start_time = str(self.hover_start_time)
for j in T:
msg.segment_end_times.append(j)
msg.desired_direction.x = self.initial_orientation[0]
msg.desired_direction.y = self.initial_orientation[1]
msg.desired_direction.z = self.initial_orientation[2]
msg.execution_time_horizon = str(0) # dummy value
msg.trajectory_mode = 'hover'
self.traj_polycoeff.publish(msg)
self.hover_counter += 1
def trajectory_in_camera_fov(self, data):
"""generates trajecory in camera workspace"""
r = 10
self.end_point = np.array([r * np.sin((self.RHP_time-5)/10), -r * np.cos((self.RHP_time-5)/10), 1.5])
#self.end_point = self.end_point + np.array([self.target_velocity*0.05, 0, 0])
start = time.time()
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.end_point[0]
odom_msg.pose.pose.position.y = self.end_point[1]
odom_msg.pose.pose.position.z = self.end_point[2]
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'world'
self.target_odom.publish(odom_msg)
t1 = time.time()-start
start_point = np.zeros((0,3))
if self.traj_gen_counter != 0:
start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe
else:
start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity
#start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
points_in_cone = self.generate_regular_pyramid_grid()
occupied_points = ros_numpy.numpify(data)
if len(occupied_points) != 0:
points_in_cone = self.generate_final_grid(points_in_cone, occupied_points)
p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1]
#p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], x) for x in p1]
points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2]
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
#kdt_points_in_cone = cKDTree(points_in_cone)
#if len(occupied_points) == 0:
#print start_point, len(points_in_cone), len(occupied_points)
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = data.header.stamp#rospy.Time.now()
header.frame_id = 'world'
p = pc2.create_cloud_xyz32(header, points_in_cone)
self.pcl_pub.publish(p)
t3 = time.time()-start
kdt_points_in_cone = cKDTree(points_in_cone)
closest_to_end = kdt_points_in_cone.query(self.end_point, 1)
#closest_to_end_index = points_in_cone[closest_to_end[1]]
#closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2])
end_point_index = closest_to_end[1]
planning_horizon = 4 # planning horizon
execution_horizon = 1 # execution horizon
dist_to_goal = np.linalg.norm(self.end_point-self.Pglobal)
ttt = self.RHP_time-5.0
timefactor = 1/(1+np.exp(-1*(ttt)))
distancefactor = erf(0.4*(dist_to_goal-self.resolution))
smoothing_factor = timefactor * distancefactor
if self.traj_gen_counter == 0 or self.RHP_time > self.tracking_in_global_time-0.06:
#one dimension simplicial complex which is basically a graph
rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution)
f = rips.create_simplex_tree(max_dimension = 1)
# make graph
G = nx.Graph()
G.add_nodes_from(range(f.num_vertices()))
edge_list = [(simplex[0][0], simplex[0][1], {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
edge_list = [k for k in edge_list if k is not None]
G.add_edges_from(edge_list)
try:
shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra')
path = np.array([points_in_cone[j] for j in shortest_path])
t4 = time.time()-start
#dist_to_goal = np.linalg.norm(self.end_point-path[-1]) # works with the velocity profile
if dist_to_goal < self.resolution:
self.reached_goal = True
# define replanning strategy
if self.reached_goal == False:
if len(path)-1 >= planning_horizon:
no_of_segments = planning_horizon
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments+1]
elif execution_horizon <= len(path)-1 < planning_horizon:
no_of_segments = len(path)-1
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments+1]
elif len(path)-1 < execution_horizon:
no_of_segments = len(path)-1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments+1]
else:
if self.static_goal == True:
no_of_segments = len(path)-1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments+1]
else:
if len(path)-1 >= planning_horizon:
no_of_segments = planning_horizon
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments+1]
elif execution_horizon <= len(path)-1 < planning_horizon:
no_of_segments = len(path)-1
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments+1]
elif len(path)-1 < execution_horizon:
no_of_segments = len(path)-1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments+1]
#print '-----len of path is---', len(path)-1
path = zip(*path)
#f0 = open('path.txt', 'a')
#f0.write('%s\n' %(path))
# now construct the minimum snap trajectory on the minimum path
waypoint_specified = True
waypoint_bc = False
#erf1 = erf(1000*dist_to_goal)
#erf2 = erf(-1000*(dist_to_goal-self.initial_dist_to_goal))
velocity = self.uav_velocity*smoothing_factor
#print '----------average velocity is--------', velocity
#f1 = open('velocity.txt', 'a')
#f1.write('%s, %s, %s, %s, %s, %s\n' %(self.initial_dist_to_goal, dist_to_goal, ttt, timefactor, distancefactor, velocity))
T, _p1, _p2, _p3 = Minimum_snap_trajetory(self.replanning_started, velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory()
t5 = time.time()-start
#self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
N = 8
_pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
[i.reverse() for i in _pp1]
_pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
[i.reverse() for i in _pp2]
_pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
[i.reverse() for i in _pp3]
xx = np.poly1d(_pp1[no_of_segments_to_track-1])
vx = np.polyder(xx, 1); accx = np.polyder(xx, 2)
jx = np.polyder(xx, 3); sx = np.polyder(xx, 4)
yy = np.poly1d(_pp2[no_of_segments_to_track-1])
vy = np.polyder(yy, 1); accy = np.polyder(yy, 2)
jy = np.polyder(yy, 3); sy = np.polyder(yy, 4)
zz = np.poly1d(_pp3[no_of_segments_to_track-1])
vz = np.polyder(zz, 1); accz = np.polyder(zz, 2)
jz = np.polyder(zz, 3); sz = np.polyder(zz, 4)
xdes = xx(T[no_of_segments_to_track]); ydes = yy(T[no_of_segments_to_track]); zdes = zz(T[no_of_segments_to_track])
vxdes = vx(T[no_of_segments_to_track]); vydes = vy(T[no_of_segments_to_track]); vzdes = vz(T[no_of_segments_to_track])
axdes = accx(T[no_of_segments_to_track]); aydes = accy(T[no_of_segments_to_track]); azdes = accz(T[no_of_segments_to_track])
jxdes = jx(T[no_of_segments_to_track]); jydes = jy(T[no_of_segments_to_track]); jzdes = jz(T[no_of_segments_to_track])
self.p_eoe = np.array([[xdes, ydes, zdes]])
self.v_eoe = np.array([[vxdes, vydes, vzdes]])
self.a_eoe = np.array([[axdes, aydes, azdes]])
self.j_eoe = np.array([[jxdes, jydes, jzdes]])
eoe_msg = Odometry()
eoe_msg.pose.pose.position.x = self.p_eoe[0][0]
eoe_msg.pose.pose.position.y = self.p_eoe[0][1]
eoe_msg.pose.pose.position.z = self.p_eoe[0][2]
eoe_msg.header.stamp = rospy.Time.now()
eoe_msg.header.frame_id = 'world'
self.eoe_odom.publish(eoe_msg)
#f1 = open('eoe_points.txt', 'a')
#f1.write('%s, %s\n' %(self.Pglobal, self.p_eoe))
vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#vector = np.array([self.end_point[0]-self.p_eoe[0][0], self.end_point[1]-self.p_eoe[0][1], self.end_point[2]-self.p_eoe[0][2]])
#vector = self.v_eoe
direction = vector/np.linalg.norm(vector)
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp#rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = N-1
for i in range(len(_p1)):
msg.poly_x.append(_p1[i]); msg.poly_y.append(_p2[i]); msg.poly_z.append(_p3[i])
msg.number_of_segments = no_of_segments_to_track
msg.planning_start_time = data.header.stamp.to_sec()
for j in T[:no_of_segments_to_track+1]:
msg.segment_end_times.append(j)
msg.desired_direction.x = direction[0]; msg.desired_direction.y = direction[1]; msg.desired_direction.z = direction[2]
#msg.desired_direction.x = direction[0][0]; msg.desired_direction.y = direction[0][1]; msg.desired_direction.z = direction[0][2]
msg.trajectory_mode = 'planning_in_camera_fov'
self.previous_coefficients = msg
self.traj_polycoeff.publish(msg)
self.republish_trajectory = True
except:
print 'No path between start and end'
#vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#direction = vector/np.linalg.norm(vector)
#self.previous_coefficients.desired_direction.x = direction[0]
#self.previous_coefficients.desired_direction.y = direction[1]
#self.previous_coefficients.desired_direction.z = direction[1]
self.traj_polycoeff.publish(self.previous_coefficients)
self.republish_trajectory = False
else: #elif self.RHP_time <= self.tracking_in_global_time - 0.1:
self.traj_polycoeff.publish(self.previous_coefficients)
self.republish_trajectory = False
time_taken = time.time()-start
#time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
self.RHP_time = data.header.stamp.to_sec()-self.hover_start_time
#print 'total time taken to execute the callbacak is:', time_taken
f1 = open('total_time_taken.txt', 'a')
f1.write("%s\n" % (time_taken))
self.traj_gen_counter += 1
def trajectory_in_camera_fov(self, data):
"""generates trajecory in camera workspace"""
start = time.time()
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.end_point[0]
odom_msg.pose.pose.position.y = self.end_point[1]
odom_msg.pose.pose.position.z = self.end_point[2]
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'world'
self.target_odom.publish(odom_msg)
t1 = time.time() - start
start_point = np.zeros((0, 3))
if self.traj_gen_counter != 0:
start_point = np.concatenate((start_point, self.p_eoe),
0) # was self.p_eoe
else:
start_point = np.concatenate(
(start_point, self.Pglobal[np.newaxis]),
0) # new axis was needed because you dont maintain uniformity
#start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
points_in_cone = self.generate_regular_pyramid_grid()
occupied_points = ros_numpy.numpify(data)
if len(occupied_points) != 0:
points_in_cone = self.generate_final_grid(points_in_cone,
start_point,
occupied_points)
p1 = [
np.dot(self.Rcdoc_cdl[0:3, 0:3], x[np.newaxis].T)
for x in points_in_cone
]
p2 = [
self.Rcg_vibase[0:3, 3:4] +
np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1
]
points_in_cone = [
self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2
]
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
#if len(occupied_points) == 0:
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
p = pc2.create_cloud_xyz32(header, points_in_cone)
self.pcl_pub.publish(p)
t3 = time.time() - start
kdt_points_in_cone = cKDTree(points_in_cone)
closest_to_end = kdt_points_in_cone.query(self.end_point, 1)
#closest_to_end_index = points_in_cone[closest_to_end[1]]
#closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2])
end_point_index = closest_to_end[1]
#one dimension simplicial complex which is basically a graph
rips = gudhi.RipsComplex(points=points_in_cone,
max_edge_length=1.5 * self.resolution)
f = rips.create_simplex_tree(max_dimension=1)
# make graph
G = nx.Graph()
G.add_nodes_from(range(f.num_vertices()))
edge_list = [(simplex[0][0], simplex[0][1], {
'weight': simplex[1]
}) if len(simplex[0]) == 2 else None for simplex in f.get_skeleton(1)]
edge_list = [k for k in edge_list if k is not None]
G.add_edges_from(edge_list)
try:
shortest_path = nx.shortest_path(G,
source=0,
target=end_point_index,
weight='weight',
method='dijkstra')
#length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
#print 'length of the path is', length
path = np.array([points_in_cone[j] for j in shortest_path])
except:
print 'No path between start and end'
#if self.traj_gen_counter == 0 :
# for simplex in f.get_skeleton(1):
# print simplex
#length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
# planning horizon is a fixed (5 for now) segments, trajectory will be planned only for these segments,
# execution horizon will be just 3 segments
# at current resolution of 0.25m, that means a trajecotory of approximately 1.25m will be planned and 0.75m will be executed
# at each time the quad plans its motion, depending on how fast it can replan, these values would be changed
t4 = time.time() - start
no_of_segments = 4
no_of_segments_to_track = 1
path1 = path[:no_of_segments +
1] # n segments means n+1 points in the path
#f1 = open('path.txt', 'a')
#f1.write('%s\n' %(path))
path = zip(*path1)
# now construct the minimum snap trajectory on the minimum path
waypoint_specified = True
waypoint_bc = False
T, _p1, _p2, _p3 = Minimum_snap_trajetory(
self.traj_gen_counter, self.uav_velocity, path, waypoint_specified,
waypoint_bc, self.v_eoe, self.a_eoe,
self.j_eoe).construct_polynomial_trajectory()
t5 = time.time() - start
#self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
N = 8
_pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
[i.reverse() for i in _pp1]
_pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
[i.reverse() for i in _pp2]
_pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
[i.reverse() for i in _pp3]
xx = np.poly1d(_pp1[no_of_segments_to_track - 1])
vx = np.polyder(xx, 1)
accx = np.polyder(xx, 2)
jx = np.polyder(xx, 3)
sx = np.polyder(xx, 4)
yy = np.poly1d(_pp2[no_of_segments_to_track - 1])
vy = np.polyder(yy, 1)
accy = np.polyder(yy, 2)
jy = np.polyder(yy, 3)
sy = np.polyder(yy, 4)
zz = np.poly1d(_pp3[no_of_segments_to_track - 1])
vz = np.polyder(zz, 1)
accz = np.polyder(zz, 2)
jz = np.polyder(zz, 3)
sz = np.polyder(zz, 4)
xdes = xx(T[no_of_segments_to_track])
ydes = yy(T[no_of_segments_to_track])
zdes = zz(T[no_of_segments_to_track])
vxdes = vx(T[no_of_segments_to_track])
vydes = vy(T[no_of_segments_to_track])
vzdes = vz(T[no_of_segments_to_track])
axdes = accx(T[no_of_segments_to_track])
aydes = accy(T[no_of_segments_to_track])
azdes = accz(T[no_of_segments_to_track])
jxdes = jx(T[no_of_segments_to_track])
jydes = jy(T[no_of_segments_to_track])
jzdes = jz(T[no_of_segments_to_track])
"""
t = []; xx = []; yy = []; zz = []
vx = []; vy = []; vz = []; accx = []; accy = []; accz = []
jx = []; jy = []; jz = []
traj_frequency = 100
for ii in range(no_of_segments_to_track):
#t.append(np.linspace(T[ii], T[ii+1], int((T[ii+1]-T[ii])*traj_frequency)))
t.append(np.linspace(T[ii], T[ii+1], 101))
xx.append(np.poly1d(p1[ii]))
vx.append(np.polyder(xx[-1], 1)); accx.append(np.polyder(xx[-1], 2))
jx.append(np.polyder(xx[-1], 3))#; sx.append(np.polyder(xx[-1], 4))
yy.append(np.poly1d(p2[ii]))
vy.append(np.polyder(yy[-1], 1)); accy.append(np.polyder(yy[-1], 2))
jy.append(np.polyder(yy[-1], 3))#; sy.append(np.polyder(yy[-1], 4))
zz.append(np.poly1d(p3[ii]))
vz.append(np.polyder(zz[-1], 1)); accz.append(np.polyder(zz[-1], 2))
jz.append(np.polyder(zz[-1], 3))#; sz.append(np.polyder(zz[-1], 4))
t6 = time.time()-start
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp#rospy.Time.now()
header.frame_id = 'world'
traj.header = header
traj.joint_names = 'firefly/base_link' # testing for now
trajectory_start_time = self.RHP_time
for i in range(no_of_segments_to_track): # "changed to no_of_segments_to_track" instead of "no_of_segments"
for j in t[i]:
self.RHP_time = j + trajectory_start_time
xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
jxdes = jx[i](j); jydes = jy[i](j); jzdes = jz[i](j)
f4 = open('trajectory_generated.txt','a')
f4.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (self.RHP_time, xdes, ydes, zdes, vxdes, vydes, vzdes, axdes, aydes, azdes))
# for now angular acceleration Point msg of MultiDOFJointTrajectory() msg is used to specify the desired direction
#vector = np.array([self.end_point[0]-xdes, self.end_point[1]-ydes, self.end_point[2]-zdes])
#vector = np.array([self.p_eoe[0][0]-xdes, self.p_eoe[0][1]-ydes, self.p_eoe[0][2]-zdes])
vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
direction = vector/np.linalg.norm(vector)
transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
#accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time))
traj.points.append(point)
self.uav_traj_pub.publish(traj)
traj.points.pop(0)
#time.sleep(1.0/300)
"""
self.p_eoe = np.array([[xdes, ydes, zdes]])
self.v_eoe = np.array([[vxdes, vydes, vzdes]])
self.a_eoe = np.array([[axdes, aydes, azdes]])
self.j_eoe = np.array([[jxdes, jydes, jzdes]])
eoe_msg = Odometry()
eoe_msg.pose.pose.position.x = self.p_eoe[0][0]
eoe_msg.pose.pose.position.y = self.p_eoe[0][1]
eoe_msg.pose.pose.position.z = self.p_eoe[0][2]
eoe_msg.header.stamp = rospy.Time.now()
eoe_msg.header.frame_id = 'world'
self.eoe_odom.publish(eoe_msg)
f1 = open('eoe_points.txt', 'a')
f1.write('%s, %s\n' % (self.p_eoe, path1[0]))
#self.uav_traj_pub.publish(traj)
#vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
vector = np.array([
self.end_point[0] - self.p_eoe[0][0],
self.end_point[1] - self.p_eoe[0][1],
self.end_point[2] - self.p_eoe[0][2]
])
direction = vector / np.linalg.norm(vector)
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = N - 1
for i in range(len(_p1)):
msg.poly_x.append(_p1[i])
msg.poly_y.append(_p2[i])
msg.poly_z.append(_p3[i])
msg.number_of_segments = no_of_segments_to_track
msg.planning_start_time = data.header.stamp.to_sec()
for j in T[:no_of_segments_to_track + 1]:
msg.segment_end_times.append(j)
msg.desired_direction.x = direction[0]
msg.desired_direction.y = direction[1]
msg.desired_direction.z = direction[2]
msg.trajectory_mode = 'planning_in_camera_fov'
if self.traj_gen_counter == 0:
self.traj_polycoeff.publish(msg)
self.republish_trajectory = True
if self.RHP_time <= self.tracking_in_global_time - 0.1:
self.traj_polycoeff.publish(self.previous_coefficients)
self.republish_trajectory = False
else:
self.previous_coefficients = msg
self.traj_polycoeff.publish(msg)
self.republish_trajectory = True
time_taken = time.time() - start
#time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time
print 'total time taken to execute the callbacak is:', time_taken
f1 = open('total_time_taken.txt', 'a')
f1.write(
"%s, %s, %s, %s, %s, %s, %s\n" %
(T[no_of_segments_to_track], start, t1, t3, t4, t5, time_taken))
self.traj_gen_counter += 1
def plan_in_camera_fov(self, pc_data):
a = time.time()
self.RHP_time = pc_data.header.stamp.to_sec() - self.hover_start_time
distance_to_goal = np.linalg.norm(self.goal - self.Pglobal)
if self.replanning_counter == 1:
ps = self.Pglobal
direction = self.initial_orientation
self.replanning_start_time = time.time()
currenttime = 0.1
else:
ps = self.Pglobal #self.p_eoe
vector = self.goal - self.Pglobal
direction = vector / np.linalg.norm(vector)
currenttime = time.time() - self.replanning_start_time
if self.goal_moving == False and distance_to_goal > self.sensor_range:
goal_vector = self.goal - ps
self.lgoal = ps + goal_vector * self.sensor_range / np.linalg.norm(
goal_vector)
r = min(distance_to_goal, self.sensor_range)
end_points = self.generate_regular_pyramid_grid2(r)
p11 = np.dot(self.Rcdoc_cdl[0:3, 0:3], end_points.T).T
p22 = self.Rvibase_cl[0:3, 3:4].T + p11
p33 = self.Rcg_vibase[0:3, 3:4].T + np.dot(
np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), p22.T).T
end_points = ps[np.newaxis] + np.dot(self.Rglobal, p33.T).T
occupied_points = ros_numpy.numpify(pc_data)
if len(occupied_points) != 0:
transformed_cloud = self.transform_cloud(ps, occupied_points)
if self.replanning_counter == 100:
for i in transformed_cloud:
f3 = open(self.path + 'occ_points.txt', 'a')
f3.write('%s, %s\n' %
(pc_data.header.stamp.to_sec(), i))
if len(transformed_cloud) == 0:
final_points = end_points
Wcollision = [1] * len(final_points)
Wdist2goal = np.linalg.norm(self.lgoal - final_points,
axis=1)
Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
Wdist2goal)
else:
final_points, Wdist2goal, Wcollision = self.check_collision_get_cost(
ps, end_points, transformed_cloud, self.lgoal)
Wcollision = map(lambda i: i * 1.0 / max(Wcollision),
Wcollision)
Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
Wdist2goal)
else:
final_points = end_points
Wcollision = [1] * len(final_points)
Wdist2goal = np.linalg.norm(self.lgoal - final_points, axis=1)
Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
Wdist2goal)
#print 'length of end points and occupied points' , len(Wdist2goal), len(Wcollision), len(final_points)
Wtotal = map(lambda i, j: 3 * i + 1.0 / j, Wdist2goal, Wcollision)
#print 'wtotal len', len(Wtotal)
local_goal = final_points[np.argmin(Wtotal)]
n = self.no_of_segments
#self.N = 8
path = zip(np.linspace(ps[0], local_goal[0], n + 1),
np.linspace(ps[1], local_goal[1], n + 1),
np.linspace(ps[2], local_goal[2], n + 1))
vel, f1, f2 = self.get_velocity(currenttime, distance_to_goal)
f = open(self.path + 'velocities.txt', 'a')
f.write('%s, %s, %s, %s, %s\n' %
(currenttime, distance_to_goal, f1, f2, vel))
T, _p1, _p2, _p3 = Minimum_jerk_trajetory(
self.replanning_counter, vel, path, self.v_eoe, self.a_eoe,
self.j_eoe).construct_trajectory()
_pp1 = [_p1[i:i + self.N] for i in range(0, len(_p1), self.N)]
[i.reverse() for i in _pp1]
_pp2 = [_p2[i:i + self.N] for i in range(0, len(_p2), self.N)]
[i.reverse() for i in _pp2]
_pp3 = [_p3[i:i + self.N] for i in range(0, len(_p3), self.N)]
[i.reverse() for i in _pp3]
#t = map(lambda i: np.linspace(T[i], T[i+1], 201), range(1))
tt = self.execution_time
index = len(np.where(np.array(T) < tt)[0]) - 1
xx = np.poly1d(_pp1[index])
yy = np.poly1d(_pp2[index])
zz = np.poly1d(_pp3[index])
vx = np.polyder(xx, 1)
vy = np.polyder(yy, 1)
vz = np.polyder(zz, 1)
accx = np.polyder(xx, 2)
accy = np.polyder(yy, 2)
accz = np.polyder(zz, 2)
jx = np.polyder(xx, 3)
jy = np.polyder(yy, 3)
jz = np.polyder(zz, 3)
#index = np.argmin(np.abs(np.array(t[0])-tt))
self.p_eoe = np.array([xx(tt), yy(tt), zz(tt)])
self.v_eoe = np.array([vx(tt), vy(tt), vz(tt)])
self.a_eoe = np.array([accx(tt), accy(tt), accz(tt)])
self.j_eoe = np.array([jx(tt), jy(tt), jz(tt)])
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = pc_data.header.stamp
header.frame_id = 'map'
msg.header = header
msg.polynomial_order = self.N - 1
for i in range(len(_p1)):
msg.poly_x.append(_p1[i])
msg.poly_y.append(_p2[i])
msg.poly_z.append(_p3[i])
msg.number_of_segments = n
#msg.sensor_update_segment = N_sensor_update
msg.planning_start_time = str(self.odom_time)
for j in T:
msg.segment_end_times.append(j)
msg.desired_direction.x = direction[0]
msg.desired_direction.y = direction[1]
msg.desired_direction.z = direction[2]
msg.trajectory_mode = 'planning_in_camera_fov'
msg.execution_time_horizon = str(self.execution_time)
self.previous_msg = msg
self.traj_polycoeff.publish(msg)
self.trajectory_endtime = T[-1]
#print '1', time.time()-a
"""
#****************** only for rviz visualization will be deleted eventually **************************************************
#eoe_msg = Odometry()
#eoe_msg.pose.pose.position.x = self.p_eoe[0]
#eoe_msg.pose.pose.position.y = self.p_eoe[1]
#eoe_msg.pose.pose.position.z = self.p_eoe[2]
#eoe_msg.header.stamp = rospy.Time.now()
#eoe_msg.header.frame_id = 'map'
#self.eoe_odom.publish(eoe_msg)
t = np.linspace(T[:n], T[1:n+1], 20, axis=1)
xx = map(lambda i: np.poly1d(_pp1[i]), range(n))
yy = map(lambda i: np.poly1d(_pp2[i]), range(n))
zz = map(lambda i: np.poly1d(_pp3[i]), range(n))
vx = map(lambda i: np.polyder(xx[i], 1), range(n))
vy = map(lambda i: np.polyder(yy[i], 1), range(n))
vz = map(lambda i: np.polyder(zz[i], 1), range(n))
accx = map(lambda i: np.polyder(xx[i], 2), range(n))
accy = map(lambda i: np.polyder(yy[i], 2), range(n))
accz = map(lambda i: np.polyder(zz[i], 2), range(n))
jx = map(lambda i: np.polyder(xx[i], 3), range(n))
jy = map(lambda i: np.polyder(yy[i], 3), range(n))
jz = map(lambda i: np.polyder(zz[i], 3), range(n))
#print '12', time.time()-a
#f = open(self.path+'end_points.txt', 'a')
#f.write('%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n' %(time.time()-a, vel, f1, f2, local_goal, tt, T[1], currenttime, self.RHP_time, pc_data.header.stamp.to_sec(), self.hover_start_time))
self.traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
self.traj.header = header
self.traj.joint_names = 'mav_cog' # testing for now
self.pathh = Path()
self.pathh.header.stamp = rospy.Time.now()
self.pathh.header.frame_id = 'map'
start_time =self.odom_time
for i in range(n): # "changed to no_of_segments_to_track" instead of "no_of_segments"
for j in t[i]:
pose = PoseStamped()
pose.header = pc_data.header
ttt = j + start_time
xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
vector = np.array([self.goal[0]-self.Pglobal[0], self.goal[1]-self.Pglobal[1], self.goal[2]-self.Pglobal[2]])
#vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
direction = vector/np.linalg.norm(vector)
transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
#accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(ttt))
self.traj.points.append(point)
pose.pose.position.x = xdes; pose.pose.position.y = ydes; pose.pose.position.z = zdes
pose.pose.orientation.x = 0; pose.pose.orientation.y = 0; pose.pose.orientation.z = 0; pose.pose.orientation.w = 1
self.pathh.poses.append(pose)
self.uav_traj_pub.publish(self.traj)
self.path_pub.publish(self.pathh)
#publishing
f1 = open(self.path+'goals.txt', 'a')
f1.write('%s, %s, %s, %s, %s\n' % (time.time()-a, index, T[1], self.Pglobal, ps))
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = pc_data.header.stamp
header.frame_id = 'map'#, 'firefly1/vi_sensor/camera_depth_optical_center_link'
p = pc2.create_cloud_xyz32(header, transformed_cloud)
self.pcl_pub.publish(p)
# publish local_goal
header = std_msgs.msg.Header()
header.stamp = pc_data.header.stamp
header.frame_id = 'map'
p = pc2.create_cloud_xyz32(header, final_points)
self.pcl_pub2.publish(p)
#****************** will be deleted eventually **********************************************************************************
"""
print '2', time.time() - a
else:
if RHPendcounter == 0:
self.goal_in_fov = self.RHP_time
self.previous_msg.execution_time_horizon = str(
self.trajectory_endtime)
self.traj_polycoeff.publish(self.previous_msg)
self.RHPendcounter += 1
if self.RHP_time > self.goal_in_fov + self.trajectory_endtime + 10:
self.reached_goal = True
print time.time() - a
self.replanning_counter += 1