def fastPlannerTrajCallback(self, msg): # position and yaw pose = Transform() pose.translation.x = msg.position.x pose.translation.y = msg.position.y pose.translation.z = msg.position.z q = quaternion_from_euler(0, 0, msg.yaw) # RPY pose.rotation.x = q[0] pose.rotation.y = q[1] pose.rotation.z = q[2] pose.rotation.w = q[3] # velocity vel = Twist() vel.linear = msg.velocity # TODO: set vel.angular to msg.yaw_dot # acceleration acc = Twist() acc.linear = msg.acceleration traj_point = MultiDOFJointTrajectoryPoint() traj_point.transforms.append(pose) traj_point.velocities.append(vel) traj_point.accelerations.append(acc) traj_msg = MultiDOFJointTrajectory() traj_msg.header = msg.header traj_msg.points.append(traj_point) self.traj_pub.publish(traj_msg)
def emergency_response(self, action): pub_action = Point() # IN FUTURE CHANGE TO RELATIVE if abs(action.y - self.pose_y) < 0.05: pub_action.x = 5 pub_action.y = 0 pub_action.z = 1.5 else: pub_action = action wpt = MultiDOFJointTrajectory() header = Header() header.stamp = rospy.Time() header.frame_id = 'frame' wpt.joint_names.append('base_link') wpt.header = header quat = quaternion.from_euler_angles(0, 0, math.radians(-45)) # # if self.active == 'astar': # transforms = Transform(translation=self.astar, rotation=quat) # elif self.active == 'dnn': # transforms = Transform(translation=self.dnn, rotation=quat) # else: # print('No Choice Made!') # transforms = Transform() print('Selected {}'.format(self.active)) transforms = Transform(translation=pub_action, rotation=quat) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(0)) wpt.points.append(point) self.waypoint_pub.publish(wpt)
def hover_the_quad(self): """give commands to hover at 1m above the starting position""" self.RHP_time = time.time()-self.hovering_time #custom_traj = PolynomialTrajectory() #custom_traj.header.stamp = rospy.Time.now() #custom_traj.pdes.x = -6.5; custom_traj.pdes.y = -4.0; custom_traj.pdes.z = 2.0 #custom_traj.vdes.x = 0; custom_traj.vdes.y = 0; custom_traj.vdes.z = 0 #custom_traj.ades.x = 0; custom_traj.ades.y = 0; custom_traj.ades.z = 0 #custom_traj.ddes.x = 1.0; custom_traj.ddes.y = 0.0; custom_traj.ddes.z = 0.0 #custom_traj.controller = 0; custom_traj.time = self.RHP_time #self.custom_traj_pub.publish(custom_traj) # for now angular acceleration Point is used to specify the direction traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'frame' traj.header = header traj.joint_names = 'nothing' #print self.initial_position, self.initial_orientation, self.hovering_height transforms = Transform(translation = Point(self.initial_position[0], self.initial_position[1], self.hovering_height), rotation = Quaternion(0,0,0,1)) velocities = Twist(linear = Point(0, 0, 0), angular = Point(0,0,0)) accelerations = Twist(linear = Point(0, 0, 0), angular = Point(self.initial_orientation[0],self.initial_orientation[1],self.initial_orientation[2])) point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time)) traj.points.append(point) self.uav_traj_pub.publish(traj)
def publish_pose_speed_command(self, x, y, z, rx, ry, rz, speed_vector): print('Command pose: '+self._namespace, x, y, z, rx, ry, rz, speed_vector ) quaternion = tf.transformations.quaternion_from_euler(rx, ry, rz) rotation = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]) location = Point(x, y, z) transforms = Transform(translation=location, rotation=rotation) velocities = Twist() velocities.linear = speed_vector accelerations = Twist() traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(0)) traj.points.append(point) self.command_pub.publish(traj)
def publish_command(position,velocity): rospy.init_node('goto_poition',anonymous=True) firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory',MultiDOFJointTrajectory,queue_size=10) desired_yaw = -10 desired_x = position[0] desired_y = position[1] desired_z = position[2] quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3])) velocities = Twist() velocities.linear.x = velocity[0] velocities.linear.y = velocity[1] velocities.linear.z = velocity[2] accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2)) traj.points.append(point) time.sleep(3) firefly_command_publisher.publish(traj) rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def hover_the_quad(self): """give commands to hover at 1m above the starting position""" self.RHP_time = time.time() - self.hovering_time # for now angular acceleration Point is used to specify the direction traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' traj.header = header traj.joint_names = 'firefly/base_link' #print self.initial_position, self.initial_orientation, self.hovering_height transforms = Transform(translation=Point(self.initial_position[0], self.initial_position[1], self.hovering_height), rotation=Quaternion(0, 0, 0, 1)) velocities = Twist(linear=Point(0, 0, 0), angular=Point(0, 0, 0)) accelerations = Twist(linear=Point(0, 0, 0), angular=Point(self.initial_orientation[0], self.initial_orientation[1], self.initial_orientation[2])) point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Duration(self.RHP_time)) traj.points.append(point) self.uav_traj_pub.publish(traj)
def generate_trajectory(self): traj_to_execute = MultiDOFJointTrajectory() traj_header = std_msgs.msg.Header() traj_velocities = Twist() traj_accelerations = Twist() traj_to_execute.joint_names = ["virtual_joint"] for i in range(0, 5): traj_header.stamp = self.get_clock().now().to_msg( ) #rclpy.time.Time().msg() traj_to_execute.header = traj_header #traj_quaternion = tf2.transformations.quaternion_from_euler(roll,pitch,yaw) traj_quaternion = [0.0, 0.0, 0.0, 0.0] traj_transforms = Transform() traj_transforms.translation.x = 1.0 * i traj_transforms.translation.y = 2.0 * i traj_transforms.translation.z = 3.0 * i traj_transforms.rotation = Quaternion() traj_transforms.rotation.x = traj_quaternion[0] traj_transforms.rotation.y = traj_quaternion[1] traj_transforms.rotation.z = traj_quaternion[2] traj_transforms.rotation.w = traj_quaternion[3] point_i = MultiDOFJointTrajectoryPoint() point_i.transforms.append(traj_transforms) point_i.velocities.append(traj_velocities) point_i.accelerations.append(traj_accelerations) duration_i = Duration(seconds=1.0).to_msg() point_i.time_from_start = duration_i # self.get_clock().now().to_msg()+Duration(seconds=1.0) traj_to_execute.points.append(point_i) return traj_to_execute
def callback(data): rospy.loginfo("Current position:%s,%s,%s", data.point.x,data.point.y,data.point.z) R1.location_x, R1.location_y, R1.location_z = position[0], position[1], position[2] currrent_x, currrent_y = R1.location_x, R1.location_y target_x, target_y = R1.target_x, R2.target_y position = [round(data.point.x) + 1, round(data.point.y) + 1, round(data.point.z)+1] velocity = [0,0,0] desired_yaw = -10 desired_x = position[0] desired_y = position[1] desired_z = position[2] quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3])) velocities = Twist() velocities.linear.x = velocity[0] velocities.linear.y = velocity[1] velocities.linear.z = velocity[2] accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2)) traj.points.append(point) firefly_command_publisher.publish(traj) rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def odom_callback(odom_msg): global waypoint_count global error global x, y, z global wpt x_truth = float(odom_msg.pose.pose.position.x) y_truth = float(odom_msg.pose.pose.position.y) z_truth = float(odom_msg.pose.pose.position.z) wpt = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' wpt.joint_names.append('base_link') wpt.header = header if error < 0.1: print('Waypoint ' + str(waypoint_count) + ' complete') waypoint_count += 1 if waypoint_count >= np.size(waypoints, axis=0) and error < 0.1: print('Waypoint list complete!\n' 'Staying at last waypoint: ' + str(x) + ' ' + str(y) + ' ' + str(z) + '\n') waypoint_count = 4 x = waypoints[waypoint_count][0] y = waypoints[waypoint_count][1] z = waypoints[waypoint_count][2] theta = waypoints[waypoint_count][3] error = math.sqrt((x_truth - x)**2 + (y_truth - y)**2 + (z_truth - z)**2) quat = quaternion.from_euler_angles(0, 0, math.radians(theta)) # quaternion = Quaternion() transforms = Transform(translation=Point(x, y, z), rotation=quat) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(2)) wpt.points.append(point) wp_publisher.publish(wpt)
def callback_odom(self, msg_odom): self.pose_x = msg_odom.pose.pose.position.x self.pose_y = msg_odom.pose.pose.position.y self.pose_z = msg_odom.pose.pose.position.z self.got_odom = True if self.restarting: self.astar = Point(0, 0, 1.5) self.dnn = Point(0, 0, 1.5) if abs(self.pose_x - 0) < 0.01: self.got_odom = False self.got_astar = False self.got_dnn = False print('Returned to start position') print('Waiting to reset Astar and DNN') rospy.sleep(3) self.restarting = False wpt = MultiDOFJointTrajectory() header = Header() header.stamp = rospy.Time() header.frame_id = 'frame' wpt.joint_names.append('base_link') wpt.header = header if self.namespace == "DJI": direction_theta = -45 else: direction_theta = 0 quat = quaternion.from_euler_angles(0, 0, math.radians(direction_theta)) transforms = Transform(translation=Point(0, 0, 1.5), rotation=quat) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(2)) wpt.points.append(point) self.waypoint_pub.publish(wpt) if abs(self.pose_x - 5) < 0.1 and self.training: self.restarting = True print('Made it to goal location')
def pose_cb(self, msg): # Fill temp_traj = MultiDOFJointTrajectory() temp_traj.header = msg.header temp_traj.points = [] temp_traj.points.append(MultiDOFJointTrajectoryPoint()) temp_traj.points[0].transforms = [] temp_traj.points[0].transforms.append(Transform()) temp_traj.points[0].transforms[0].translation.x = msg.pose.position.x temp_traj.points[0].transforms[0].translation.y = msg.pose.position.y temp_traj.points[0].transforms[0].translation.z = msg.pose.position.z temp_traj.points[0].transforms[0].rotation.x = msg.pose.orientation.x temp_traj.points[0].transforms[0].rotation.y = msg.pose.orientation.y temp_traj.points[0].transforms[0].rotation.z = msg.pose.orientation.z temp_traj.points[0].transforms[0].rotation.w = msg.pose.orientation.w # Call trajectory callback with one point self.trajectory_cb(temp_traj)
def publisher(self, trajectory_parameters): """ Generates and publishes a MultiDOFJointTrajectory message from inputs to publisher topic for simulator. Args: trajectory_parameters(dict): A dictionary containing all data fields required to build a Trajectory message. """ trajectory = MultiDOFJointTrajectory() trajectory.header = Header() trajectory.header.stamp = rospy.Time() trajectory.header.frame_id = '' trajectory.joint_names = ["base_link"] # Create the message expected by the simulator. point = MultiDOFJointTrajectoryPoint([ self.create_point(trajectory_parameters["x"], trajectory_parameters["y"], trajectory_parameters["z"], trajectory_parameters["rotation_x"], trajectory_parameters["rotation_y"], trajectory_parameters["rotation_z"], trajectory_parameters["rotation_w"]) ], [ self.create_velocity(trajectory_parameters["velocity_x"], trajectory_parameters["velocity_y"], trajectory_parameters["velocity_z"], trajectory_parameters["velocity_angular_x"], trajectory_parameters["velocity_angular_y"], trajectory_parameters["velocity_angular_z"]) ], [ self.create_acceleration( trajectory_parameters["acceleration_linear_x"], trajectory_parameters["acceleration_linear_y"], trajectory_parameters["acceleration_linear_z"], trajectory_parameters["acceleration_angular_x"], trajectory_parameters["acceleration_angular_y"], trajectory_parameters["acceleration_angular_z"]) ], rospy.Time(1)) trajectory.points.append(point) pub.publish(trajectory)
def publish_point_command(self, x, y, z, yaw_deg): quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(yaw_deg)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header transforms = Transform(translation=Point(x, y, z), rotation=Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(2)) traj.points.append(point) self.command_pub.publish(traj)
def random_selection(self): if self.got_dnn and self.got_astar and self.restarting is False: \ # Choose every n times if self.choice_current == self.choice_count: choices = ['astar', 'dnn'] prob = [1 - self.dagger_beta, self.dagger_beta] self.active = np.random.choice(choices, p=prob) self.choice_current = 1 else: self.choice_current += 1 wpt = MultiDOFJointTrajectory() header = Header() header.stamp = rospy.Time() header.frame_id = 'frame' wpt.joint_names.append('base_link') wpt.header = header quat = quaternion.from_euler_angles(0, 0, math.radians(-45)) if self.active == 'astar': transforms = Transform(translation=self.astar, rotation=quat) elif self.active == 'dnn': transforms = Transform(translation=self.dnn, rotation=quat) else: print('No Choice Made!') transforms = Transform() print('Selected {}'.format(self.active)) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(2)) wpt.points.append(point) self.waypoint_pub.publish(wpt)
def trajectory_in_camera_fov(self, data): """generates trajectory in camera workspace""" #self.end_point = self.end_point + np.array([-self.target_velocity*0.05,0, 0]) 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 = 'vicon' self.target_odom.publish(odom_msg) start = time.time() 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 points_in_cone = self.generate_regular_pyramid_grid() occupied_points = ros_numpy.numpify(data) #for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s, %s, %s\n' %(self.traj_gen_counter, self.RHP_time, len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) #if self.traj_gen_counter == 0: ## for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s\n' %(len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) if len(occupied_points) != 0: points_in_cone = self.generate_final_grid(points_in_cone, occupied_points) """ points_in_cone = [x[np.newaxis] for x in points_in_cone] pp1 = [np.dot(self.Rcdoc_cdl[0:3, 0:3].T, x.T) for x in points_in_cone] # now translate from visensor frame to cg frame pp2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in pp1] # now rotate the cloud in world frame points_in_cone = [self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in pp2] # remove z points if they are going inside the ground, assuming ground is z = 0 points_in_cone = [x for x in points_in_cone if not x[2] <= 0] points_in_cone = np.concatenate((start_point, points_in_cone), 0) """ #points_in_cone = np.concatenate((start_point, points_in_cone), 0) p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone] #p1 = [self.Rvibase_cl[0:3, 3:4] + 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(self.Rcg_vibase[0:3, 0:3].T, 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] points_in_cone = np.concatenate((start_point, points_in_cone), 0) # publish generated pointcloud header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' p = pc2.create_cloud_xyz32(header, points_in_cone) self.pcl_pub.publish(p) 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') path = np.array([points_in_cone[j] for j in shortest_path]) except: print 'No path between start and end' #f1 = open('path.txt', 'a') #f1.write('%s, %s\n' %(path, points_in_cone[end_point_index])) #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 no_of_segments = 4 no_of_segments_to_track = 1 path = path[:no_of_segments+1] # n segments means n+1 points in the path path = zip(*path) # now construct the minimum snap trajectory on the minimum path waypoint_specified = True waypoint_bc = False planning_counter = 2 T, p1, p2, p3 = Minimum_snap_trajetory(planning_counter, self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory() #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3) N = 8 p1 = [p1[i:i + N] for i in range(0, len(p1), N)] [i.reverse() for i in p1] p2 = [p2[i:i + N] for i in range(0, len(p2), N)] [i.reverse() for i in p2] p3 = [p3[i:i + N] for i in range(0, len(p3), N)] [i.reverse() for i in p3] t = []; xx = []; yy = []; zz = [] vx = []; vy = []; vz = []; accx = []; accy = []; accz = [] jx = []; jy = []; jz = [] #print T 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], 21)) 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)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' traj.header = header traj.joint_names = 'firefly/base_link' # testing for now trajectory_start_time = data.header.stamp.to_sec()#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) # 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) 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]]) self.uav_traj_pub.publish(traj) time_taken = time.time()-start self.traj_gen_counter += 1 print 'total time taken to execute the callbacak is:', time_taken
import time firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size=10) desired_yaw_to_go_degree=-10 desired_x_to_go=2 desired_y_to_go=2.5 desired_z_to_go=2.5 quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(desired_yaw_to_go_degree)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header=header transforms =Transform(translation=Point(desired_x_to_go, desired_y_to_go, desired_z_to_go), rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3])) velocities =Twist() accelerations=Twist() point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Time(2)) traj.points.append(point) time.sleep(1) firefly_command_publisher.publish(traj)
def trajectory_callback(self, data): #initialize and prepare screen #if self.counter == 0: a = time.time() occupied_points = ros_numpy.numpify(data) points_in_cone = self.generate_regular_pyramid_grid() points_in_cone = self.generate_final_grid(points_in_cone, occupied_points) # now convert the points generated in the local frame of the visensor (xyz of the camera where z points outside the camera) # to the points in quadrotor cg xyz frame(x forward z upwards) located at the visensor print '1', time.time() - a p1 = [ self.Rvibase_cl[0:3, 3:4] + np.dot(self.Rcdoc_cdl[0:3, 0:3], x[np.newaxis].T) for x in points_in_cone ] p2 = [np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1] points_in_cone = [ self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in p2 ] points_in_cone = [x for x in points_in_cone if not x[2] <= 0] #points_in_cone = [np.dot(self.Rcg_vibase[0:3, 0:3].T, self.Rvibase_cl[0:3, 3:4]+np.dot(self.Rcdoc_cdl[0:3,0:3],x[np.newaxis].T)).ravel() for x in points_in_cone] print '2', time.time() - a kdt_points_in_cone = cKDTree(points_in_cone) closest_to_end = kdt_points_in_cone.query(self.end_point, 1) print closest_to_end #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] print 'the goal point is', points_in_cone[end_point_index] #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) 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]) #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 no_of_segments = 5 path = path[:no_of_segments + 1] # 5 segments means 6 points in the path path = zip(*path) #no_of_segments = len(path)-1 # now construct the minimum snap trajectory on the minimum path waypoint_specified = True waypoint_bc = False print path T, p1, p2, p3 = Minimum_snap_trajetory( path, waypoint_specified, waypoint_bc).construct_polynomial_trajectory() print '8', time.time() - a #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3) msg = Float32() msg.data = 0.0 self.pub1.publish(msg) N = 8 p1 = [p1[i:i + N] for i in range(0, len(p1), N)] [i.reverse() for i in p1] p2 = [p2[i:i + N] for i in range(0, len(p2), N)] [i.reverse() for i in p2] p3 = [p3[i:i + N] for i in range(0, len(p3), N)] [i.reverse() for i in p3] t = [] xx = [] yy = [] zz = [] vx = [] vy = [] vz = [] accx = [] accy = [] accz = [] print '9', time.time() - a traj_freq = 100 # frequency of the trajectory in Hz for i in range(no_of_segments): t.append( np.linspace(T[i], T[i + 1], int((T[i + 1] - T[i]) * traj_freq + 1))) xx.append(np.poly1d(p1[i])) 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[i])) 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[i])) 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)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'frame' traj.header = header traj.joint_names = 'base_link' time_diff = no_of_segments * len(t[0]) * 30 print 'inverse of time_diff is', 1.0 / time_diff #epoch = rospy.Time() #print epoch, no_of_segments #tt = rospy.Time(1, 30) #print tt msg1 = PolynomialTrajectory() for i in range(no_of_segments): for j in t[i]: 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) 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(0, 0, 0)) point = MultiDOFJointTrajectoryPoint( [transforms], [velocities], [accelerations], rospy.Time(1.0 / time_diff)) traj.points.append(point) #rospy.sleep(1.0/time_diff) # publish the trajcetory to custom ros message that is used for all other algorithm msg1.pdes.x = xdes msg1.pdes.y = ydes msg1.pdes.z = zdes msg1.vdes.x = vxdes msg1.vdes.y = vydes msg1.vdes.z = vzdes msg1.ades.x = axdes msg1.ades.y = aydes msg1.ades.z = azdes msg1.ddes.x = 1 msg1.ddes.y = 0 msg1.ddes.z = 0 msg1.controller = 0 self.custom_uav_traj_pub.publish(msg1) f0 = open('trajectory.txt', 'a') f0.write("%s, %s, %s\n" % (xdes, ydes, zdes)) time.sleep(1.0 / 120) f1 = open('trajectory1.txt', 'a') f1.write("%s\n" % (traj)) self.uav_traj_pub.publish(traj) print '11', time.time() - a
def odom_callback(odom_msg): global waypoint_count global error global x_des, y_des, z_des, x_truth, y_truth, z_truth global wpt global waypoints, completed_waypoints, added_waypoints global graph global map_seen, map_time global astar_publisher update = False x_truth = float(odom_msg.pose.pose.position.x) y_truth = float(odom_msg.pose.pose.position.y) z_truth = float(odom_msg.pose.pose.position.z) position = (x_truth, y_truth) z_des = 1.0 # If a waypoint exists, check if complete if len(waypoints) > 0: x_des = waypoints[0][0] y_des = waypoints[0][1] z_des = waypoints[0][2] theta_des = waypoints[0][3] error = math.sqrt((x_truth - x_des)**2 + (y_truth - y_des)**2 + (z_truth - z_des)**2) if error < 0.1: print('Waypoint complete: %f, %f, %f', (x_des, y_des, z_des)) del waypoints[0] else: if abs(x_truth - 5) < abs(x_truth - 0): waypoints.insert(0, (0, 0, 1.5, -45)) # added_waypoints = [] map_seen = False else: waypoints.insert(0, (5, 0, 1.5, -45)) added_waypoints = [] # If another waypoint still exists, check if new path is required and publish waypoint message to controller if len(waypoints) > 0: # added_waypoints = [] # DELETE IN FUTURE if added_waypoints == [] and map_seen is True or update is True: end = (5.5, 0) path = find_path(graph, start=position, goal=end, debug=False) end_time = rospy.Time.now().to_sec() time_taken = end_time - map_time print('time taken: {}'.format(time_taken)) # added_waypoints = [] # map_seen = False # DELETE IN FUTURE for i in range(0, len(path)): added_waypoints.insert(0, (path[i][0], path[i][1], z_des, -45)) # added_waypoints.insert(0,waypoints) # waypoints.append(added_waypoints) waypoints = added_waypoints wpt = MultiDOFJointTrajectory() header = Header() header.stamp = rospy.Time() header.frame_id = 'frame' wpt.header = header for k in range(0, len(waypoints)): x_des = waypoints[k][0] y_des = waypoints[k][1] z_des = waypoints[k][2] theta_des = waypoints[k][3] # Create controller message for waypoint quat = quaternion.from_euler_angles(0, 0, math.radians(theta_des)) transforms = Transform(translation=Point(x_des, y_des, z_des), rotation=quat) velocities = Twist() accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(k)) wpt.points.append(point) wpt.joint_names.append('base_link') # wp_publisher.publish(wpt) # Goal Point final = Point() final.x = waypoints[-1][0] final.y = waypoints[-1][1] final.z = waypoints[-1][2] # Next Point next_point = Point() next_point.x = waypoints[0][0] next_point.y = waypoints[0][1] next_point.z = waypoints[0][2] astar_publisher.publish(next_point)