def sendLiveFeed(self):
traject = Trajectory()
traject.reference_frame = self.limb
traject.times = self.times
traject.positions = self.positions
traject.velocities = self.velocities
print "sending trajectory"
self.plane_traj_pub.publish(traject)
self.recycleTrajectoryMsgData()
def sendImagePath(self, paths):
oldpos = Vector3(0, 0, 0)
for path in paths:
loginfo("Starting New Line")
(x, y) = path[0]
self.MoveToScreenPosition(x, y, self.zDist)
traject = Trajectory()
traject.reference_frame = self.limb
traject.times = []
traject.positions = []
traject.velocities = []
i = 0
sumtime = 0
for pixels in path:
if i % 5 == 0:
self.canvas.create_rectangle(
pixels[0], pixels[1], pixels[0] + 2, pixels[1] + 2, outline="#fb0", fill="#fb0"
)
new_p = Vector3(pixels[0] * self.scale, pixels[1] * self.scale, self.zDist)
new_v = Vector3(0, 0, 0)
new_t = 0
if i != 0:
dx = new_p.x - oldpos.x
dy = new_p.y - oldpos.y
dz = new_p.z - oldpos.z
dist = (dx ** 2 + dy ** 2 + dz ** 2) ** 0.5
new_v = Vector3((dx) / dist * self.speed, (dy) / dist * self.speed, dz / dist * self.speed)
new_t = traject.times[len(traject.times) - 1] + dist / self.speed
sumtime += new_t
traject.positions.append(new_p)
traject.velocities.append(new_v)
traject.times.append(new_t)
oldpos = new_p
i += 1
self.canvas.update()
# self.plane_traj_pub.publish(traject)
# rospy.sleep(sumtime)
self.plane_traj_srv(traject.times, traject.positions, traject.velocities)
