def go_to_start(self, midpoint, pub):
r = 10.0
rate = rospy.Rate(r)
for i in range(0, 15 * int(r)):
out_msg = QuadPositionDerived()
out_msg.x = 0.0
out_msg.y = 0.0
out_msg.z = 0.6
out_msg.yaw = 0
out_msg.x_vel = 0.0
out_msg.y_vel = 0.0
out_msg.z_vel = 0.0
out_msg.yaw_vel = 0
out_msg.x_acc = 0.0
out_msg.y_acc = 0.0
out_msg.z_acc = 0.0
out_msg.yaw_acc = 0
pub.publish(out_msg)
rate.sleep()
target_point = [0.0, 0.0, 0.0]
target_point[0] = midpoint[0] + self.radius
target_point[1] = midpoint[1]
target_point[2] = midpoint[2]
outpos = self.transform_coordinates(target_point, self.theta)
rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6])
rospy.set_param("trajectory_generator/end_point", outpos)
sl_gen = StraightLineGen()
sl_gen.generate()
return outpos
def go_to_start(self, midpoint, pub):
r = 10.0
rate = rospy.Rate(r)
for i in range(0, 15 * int(r)):
out_msg = QuadPositionDerived()
out_msg.x = 0.
out_msg.y = 0.
out_msg.z = 0.6
out_msg.yaw = 0
out_msg.x_vel = 0.
out_msg.y_vel = 0.
out_msg.z_vel = 0.
out_msg.yaw_vel = 0
out_msg.x_acc = 0.
out_msg.y_acc = 0.
out_msg.z_acc = 0.
out_msg.yaw_acc = 0
pub.publish(out_msg)
rate.sleep()
target_point = [0.0, 0.0, 0.0]
target_point[0] = midpoint[0] + self.radius
target_point[1] = midpoint[1]
target_point[2] = midpoint[2]
outpos = self.transform_coordinates(target_point, self.theta)
rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6])
rospy.set_param("trajectory_generator/end_point", outpos)
sl_gen = StraightLineGen()
sl_gen.generate()
return outpos
def go_to_start(self, midpoint):
target_point = [0.0, 0.0, 0.0]
target_point[0] = midpoint[0] + self.radius
target_point[1] = midpoint[1]
target_point[2] = midpoint[2]
outpos = self.transform_coordinates(target_point, self.theta)
rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.2])
rospy.set_param("trajectory_generator/end_point", outpos)
sl_gen = StraightLineGen()
sl_gen.generate()
return outpos
def go_to_start(self, midpoint):
target_point = [0.0, 0.0, 0.0]
target_point[0] = midpoint[0] + self.radius
target_point[1] = midpoint[1]
target_point[2] = midpoint[2]
outpos = self.transform_coordinates(target_point, self.theta)
rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.2])
rospy.set_param("trajectory_generator/end_point", outpos)
sl_gen = StraightLineGen()
sl_gen.generate()
return outpos
def get_tilted_circle(self):
tilted_midpoint = self.inverse_transform(self.midpoint, self.theta)
a_max = 0.6 ** 2.0 / 0.8
# v_max = (self.radius*a_max)**(0.5)
# if self.velo >= v_max:
# self.velo = 0.9*v_max
rospy.init_node("TG", anonymous=True)
pub = rospy.Publisher("trajectory_gen/target", QuadPositionDerived, queue_size=10)
start_point = self.go_to_start(tilted_midpoint, pub)
sec_pub = rospy.Publisher("trajectory_gen/done", Permission, queue_size=10)
rospy.sleep(4.0)
r = 10.0
rate = rospy.Rate(r)
t_f = self.velo / math.sqrt(a_max ** 2.0 - self.velo ** 4.0 / self.radius ** 2.0)
time = self.accelerate(pub, a_max, r, tilted_midpoint) / 2.0 - 1 / r
period = 2 * math.pi * self.radius / self.velo
points_in_period = int(period * r)
counter = 0
while not rospy.is_shutdown() and not self.done:
out_pos = self.transform_coordinates(
self.get_outpos(self.radius, self.velo, tilted_midpoint, time), self.theta
)
out_velo = self.transform_coordinates(self.get_outvelo(self.radius, self.velo, time), self.theta)
out_acc = self.transform_coordinates(self.get_outacc(self.radius, self.velo, time), self.theta)
out_msg = QuadPositionDerived()
out_msg.x = out_pos[0]
out_msg.y = out_pos[1]
out_msg.z = out_pos[2]
out_msg.yaw = 0
out_msg.x_vel = out_velo[0]
out_msg.y_vel = out_velo[1]
out_msg.z_vel = out_velo[2]
out_msg.yaw_vel = 0
out_msg.x_acc = out_acc[0]
out_msg.y_acc = out_acc[1]
out_msg.z_acc = out_acc[2]
out_msg.yaw_acc = 0
pub.publish(out_msg)
sec_pub.publish(Permission(False))
time += 1.0 / r
counter += 1
rate.sleep()
if time >= 3 * period - t_f / 2:
end_pos = self.decallerate(pub, a_max, self.radius, self.velo, tilted_midpoint, time)
rospy.set_param("trajectory_generator/start_point", end_pos)
rospy.set_param("trajectory_generator/end_point", [0.0, 0.0, 0.6])
sl_gen = StraightLineGen()
sl_gen.generate()
rospy.sleep(4.0)
# self.turn(pub, [0.0,0.0,0.6],0)
self.done = True
sec_pub.publish(Permission(True))
psi = params[0]
enterpoint = params[1]
exitpoint = params[2]
r1 = params[3]
r2 = params[4]
initialvelo = np.cross(np.cross(r1,r2),r1)
norm = LA.norm(initialvelo)
if norm!=0:
initialvelo = (initialvelo*0.283/LA.norm(initialvelo)).tolist()
else:
initialvelo = [0.0,0.0,0.283]
print initialvelo
rospy.set_param("trajectory_generator/end_point",enterpoint)
linegen = StraightLineGen()
linegen.generate()
if not Inside_sphere(end[0],end[1],end[2],centerpoint[0],centerpoint[1],centerpoint[2],r):
if enterpoint != exitpoint:
rospy.set_param("trajectory_generator/midpoint",centerpoint)
rospy.set_param("trajectory_generator/start",enterpoint)
rospy.set_param("trajectory_generator/psi",psi)
rospy.set_param("trajectory_generator/initial_velo",initialvelo)
circlegen = CircleGen()
circlegen.get_tilted_circle()
rospy.set_param("trajectory_generator/start_point",exitpoint)
rospy.set_param("trajectory_generator/end_point",end)
linegen = StraightLineGen()
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1/r
self.__set_done(True)
def is_done(self):
return self.done
def __set_done(self,boolean):
self.done = boolean
if __name__ == '__main__':
try:
rospy.sleep(4.)
tn = TrajectoryNode()
sl_gen_1 = StraightLineGen(tn,[0.,0.,0.2],[0.,0.,0.6])
sl_gen_1.loop(0.)
rospy.sleep(2.)
sl_gen_2 = StraightLineGen(tn,[0.,0.,0.6],[0.8,0.,0.6])
sl_gen_2.loop(0.)
rospy.sleep(10.)
acc_gen = AccGen(tn,[0.,0.,0.6],[0.8,0.,0.6],[0.,0.4,0.],14*math.pi)
t = acc_gen.get_t_f()
a_gen = ArcGen(tn,[0.,0.,0.6],[0.8,0.,0.6],[0.,0.4,0.],14*math.pi)
a_gen.loop(t/2.0)
except rospy.ROSInterruptException:
pass
def get_tilted_circle(self):
tilted_midpoint = self.inverse_transform(self.midpoint, self.theta)
a_max = 0.6**2.0 / 0.8
#v_max = (self.radius*a_max)**(0.5)
#if self.velo >= v_max:
# self.velo = 0.9*v_max
rospy.init_node('TG', anonymous=True)
pub = rospy.Publisher('trajectory_gen/target',
QuadPositionDerived,
queue_size=10)
start_point = self.go_to_start(tilted_midpoint, pub)
sec_pub = rospy.Publisher('trajectory_gen/done',
Permission,
queue_size=10)
rospy.sleep(4.)
r = 10.0
rate = rospy.Rate(r)
t_f = self.velo / math.sqrt(a_max**2.0 -
self.velo**4.0 / self.radius**2.0)
time = self.accelerate(pub, a_max, r, tilted_midpoint) / 2.0 - 1 / r
period = 2 * math.pi * self.radius / self.velo
points_in_period = int(period * r)
counter = 0
while not rospy.is_shutdown() and not self.done:
out_pos = self.transform_coordinates(
self.get_outpos(self.radius, self.velo, tilted_midpoint, time),
self.theta)
out_velo = self.transform_coordinates(
self.get_outvelo(self.radius, self.velo, time), self.theta)
out_acc = self.transform_coordinates(
self.get_outacc(self.radius, self.velo, time), self.theta)
out_msg = QuadPositionDerived()
out_msg.x = out_pos[0]
out_msg.y = out_pos[1]
out_msg.z = out_pos[2]
out_msg.yaw = 0
out_msg.x_vel = out_velo[0]
out_msg.y_vel = out_velo[1]
out_msg.z_vel = out_velo[2]
out_msg.yaw_vel = 0
out_msg.x_acc = out_acc[0]
out_msg.y_acc = out_acc[1]
out_msg.z_acc = out_acc[2]
out_msg.yaw_acc = 0
pub.publish(out_msg)
sec_pub.publish(Permission(False))
time += 1.0 / r
counter += 1
rate.sleep()
if time >= 3 * period - t_f / 2:
end_pos = self.decallerate(pub, a_max, self.radius, self.velo,
tilted_midpoint, time)
rospy.set_param("trajectory_generator/start_point", end_pos)
rospy.set_param("trajectory_generator/end_point",
[0.0, 0.0, 0.6])
sl_gen = StraightLineGen()
sl_gen.generate()
rospy.sleep(4.)
#self.turn(pub, [0.0,0.0,0.6],0)
self.done = True
sec_pub.publish(Permission(True))