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)
