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