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 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