예제 #1
0
 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
예제 #2
0
 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
예제 #3
0
 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
예제 #4
0
 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
예제 #5
0
 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))
예제 #6
0
	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() 
예제 #7
0
        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
  
예제 #8
0
 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))