def work(self): # initialize node rospy.init_node("asctec_follower_planner_node") # instantiate the publisher topic = rospy.get_param("follower_reference_trajectory_topic", default="command/trajectory") pub = rospy.Publisher(topic, tm.MultiDOFJointTrajectory, queue_size=10) # to get the initial position of the quad self._quad_initial_pos = None self._got_quad_initial_pos_flag = False topic = rospy.get_param("follower_pos_vel_topic", default="msf_core/odometry") self.follower_pose_subscriber = rospy.Subscriber(topic, nm.Odometry, self._get_quad_initial_pos) # subscriber to the position of the leader leader_name = rospy.get_param("leader_name", default="") topic = leader_name + "/msf_core/odometry" self._got_leader_state_flag = False self._leader_state_subscriber = rospy.Subscriber(topic, nm.Odometry, self._get_leader_state) # setting the frequency of execution rate = rospy.Rate(1e1) # get initial quad position while not self._got_leader_state_flag and not rospy.is_shutdown(): print("The follower planner is waiting for the leader position.") rate.sleep() # desired offset with respect to the leader # self._offset = numpy.array([1.0, 0.0, 0.0, 0.0]) # any self._offset = self._initial_pos - self._lead_pos self._offset[2] = 1.0 # do work while not rospy.is_shutdown(): p = self._lead_pos + self._offset v = self._lead_vel a = numpy.zeros(4) j = numpy.zeros(4) sn = numpy.zeros(4) cr = numpy.zeros(4) # print(p) msg = umc.reference_point_to_multidofjointtrajectory_msg(p, v, a, j, sn, cr) pub.publish(msg) rate.sleep() rospy.spin()
def work(self): # initialize node rospy.init_node('rotors_follower_planner_node') # instantiate the publisher topic = rospy.get_param('follower_reference_trajectory_topic', default='command/trajectory') pub = rospy.Publisher(topic, tm.MultiDOFJointTrajectory, queue_size=10) # to get the initial position of the quad self._quad_initial_pos = None self._got_quad_initial_pos_flag = False topic = rospy.get_param('follower_pose_topic', default='ground_truth/odometry') self._sub = rospy.Subscriber(topic, nm.Odometry, self._get_quad_initial_pos) # subscriber to the position of the leader topic = rospy.get_param('leader_state_topic', default='/hummingbird_leader/ground_truth/odometry') self._got_leader_pos_vel_flag = False rospy.Subscriber(topic, nm.Odometry, self._get_leader_pos_vel) # setting the frequency of execution rate = rospy.Rate(1e1) # get initial quad position while not self._got_leader_pos_vel_flag and not rospy.is_shutdown(): rate.sleep() # desired offset with respect to the leader self._offset = numpy.array([1.0, 0.0, 0.0, 0.0]) #self._trajectory = ct.TrajectoryCircle(self._quad_initial_pos, numpy.eye(3), delay, delay+duration, radius, ang_vel) # do work while not rospy.is_shutdown(): p = self._lead_pos + self._offset v = self._lead_vel a = numpy.zeros(4) j = numpy.zeros(4) sn = numpy.zeros(4) cr = numpy.zeros(4) #print(p) msg = umc.reference_point_to_multidofjointtrajectory_msg(p, v, a, j, sn, cr) pub.publish(msg) rate.sleep() rospy.spin()
def work(self): # initialize node rospy.init_node('asctec_point_sequence_planner_node') # local time #initial_time = rospy.get_time() #self.time = rospy.get_time() - initial_time # instantiate the publisher topic = rospy.get_param('ref_traj_topic', default='command/trajectory') pub = rospy.Publisher(topic, tm.MultiDOFJointTrajectory, queue_size=10) # to get the pose of the quad self._pos = None self._vel = None self._got_initial_pos_vel_flag = False topic = rospy.get_param('quad_pos_topic', default='msf_core/odometry') rospy.Subscriber(topic, nm.Odometry, self._get_quad_pos_vel) # setting the frequency of execution rate = rospy.Rate(1e1) # get initial time #aux = rospy.get_time() #while rospy.get_time() == aux: # pass #self.initial_time = rospy.get_time() #self.time = rospy.get_time() - self.initial_time #print(self.time) #print(type(self.initial_time)) # get initial quad position while not self._got_initial_pos_vel_flag and not rospy.is_shutdown(): print("The planner is waiting for the initial quad position.") rate.sleep() # generate initial waypoint self._generate_initial_waypoint() # do work while not rospy.is_shutdown(): # get current time #self.time = rospy.get_time() - self.initial_time #print(self.time) # see if the current waypoint is reached print self._pos print self._waypoint print numpy.linalg.norm(self._pos-self._waypoint) if numpy.linalg.norm(self._pos-self._waypoint) < 0.1: self._generate_waypoint() waypoint_counter += 1 print("\nNew waypoint: " + str(waypoint_counter) +"\n") #p, v, a, j, s, c = self.trajectory.get_point(self.time) #a = self._planner.get_acceleration(self._pos, self._vel) v = self._planner.get_velocity(self._pos) #v = numpy.array(self._vel) #aux = self._planner.get_velocity(self._pos) #v[3] = aux[3] p = numpy.array(self._pos) a = numpy.zeros(4) j = numpy.zeros(4) sn = numpy.zeros(4) cr = numpy.zeros(4) msg = umc.reference_point_to_multidofjointtrajectory_msg(p, v, a, j, sn, cr) pub.publish(msg) rate.sleep() rospy.spin()
def work(self): # initialize node rospy.init_node('rotors_planner_node') # get initial time #aux = rospy.get_time() #while rospy.get_time() == aux: # pass self._initial_time = rospy.get_time() #print(self.time) #print(type(self.initial_time)) # instantiate the publisher topic = rospy.get_param('reference_trajectory_topic', default='command/trajectory') pub = rospy.Publisher(topic, tm.MultiDOFJointTrajectory, queue_size=10) # to get the initial position of the quad self._quad_initial_pos = None self._got_quad_initial_pos_flag = False topic = rospy.get_param('quad_pos_topic', default='ground_truth/odometry') self._sub = rospy.Subscriber(topic, nm.Odometry, self._get_quad_initial_pos) # setting the frequency of execution rate = rospy.Rate(1e1) # get initial quad position while not self._got_quad_initial_pos_flag and not rospy.is_shutdown(): print("The planner is waiting for the quad initial position.") rate.sleep() # trajectory to be published #delay = rospy.get_param('delay', default=0.0) #self.trajectory = qt.TrajectoryQuintic(self.quad_initial_pos, numpy.eye(3), delay, delay+duration, displacement) radius = 1.5 ang_vel = 0.2 start_point = numpy.array(self._quad_initial_pos) start_point[2] = 1.0 duration = 10000.0 #displacement = rospy.get_param('displacement', default=[1.0, 0.0, 1.0, 0.0]) #duration = rospy.get_param('duration', default=3.0*numpy.linalg.norm(displacement)) delay = rospy.get_param('delay', default=5.0) time = rospy.get_time() - self._initial_time self._trajectory = ct.TrajectoryCircle(start_point, numpy.eye(3), time+delay, time+delay+duration, radius, ang_vel) #self._trajectory = qt.TrajectoryQuintic(self._quad_initial_pos, numpy.eye(3), time+delay, time+delay+duration, self._quad_initial_pos+displacement) #rospy.loginfo("Here!" + str(self._quad_initial_pos)) #rospy.loginfo(self._quad_initial_pos+displacement) # do work while not rospy.is_shutdown(): self._time = rospy.get_time() - self._initial_time #print(self.time) #print(self.initial_time) p, v, a, j, s, c = self._trajectory.get_point(self._time) print(p) msg = umc.reference_point_to_multidofjointtrajectory_msg(p, v, a, j, s, c) pub.publish(msg) rate.sleep() rospy.spin()
def work(self): # initialize node rospy.init_node('asctec_multiple_collision_avoidance_point_sequence_planner_node') # planner to compute the collision avoidance contributions gain = 1.0 ths = 0.5 self._collision_avoidance_planner = pcam.PlannerCollisionAvoidanceMultiple(gain, ths) # planner to roam around # it will be set up in the callback self._roaming_planner = None # instantiate the publisher topic = rospy.get_param('ref_traj_topic', default='command/trajectory') pub = rospy.Publisher(topic, tm.MultiDOFJointTrajectory, queue_size=10) # to get the initial position of the quad self._pos = None self._vel = None self._got_initial_pos_vel_flag = False topic = rospy.get_param('quad_pos_vel_topic', default='msf_core/odometry') self._pos_subscriber = rospy.Subscriber(topic, nm.Odometry, self._get_quad_pos_vel) # subscribers to the positions of the other quads self._other_quads = {} other_quads_names_string = rospy.get_param('other_quads_names_string', default="") other_quads_names_list = other_quads_names_string.split() for quad_name in other_quads_names_list: self._other_quads[quad_name] = PartnerQuad() rospy.Subscriber("/" + quad_name + "/msf_core/odometry", nm.Odometry, self._get_other_pos, quad_name) # setting the frequency of execution freq = 30.0 rate = rospy.Rate(freq) # get initial time #aux = rospy.get_time() #while rospy.get_time() == aux: # pass #self.initial_time = rospy.get_time() #self.time = rospy.get_time() - self.initial_time #print(self.time) #print(type(self.initial_time)) # get initial quad position while not self._got_initial_pos_vel_flag and not rospy.is_shutdown(): print("The planner is waiting for the quad initial position.") rate.sleep() # get initial position of the other while not all([partner.got_initial_pos_flag for partner in self._other_quads.values()]) and not rospy.is_shutdown(): print("The planner is waiting for the partners initial positions.") print self._other_quads.keys() rate.sleep() # generate initial waypoint self._generate_initial_waypoint() waypoint_counter = 0 # do work while not rospy.is_shutdown(): # compute collision avoidance contribution others_poss = [partner.pos for partner in self._other_quads.values()] #ca_acc = self._collision_avoidance_planner.get_acceleration(self._pos, self._vel, others_poss) ca_vel = self._collision_avoidance_planner.get_velocity(self._pos, others_poss) # get current time #self.time = rospy.get_time() - self.initial_time #print(self.time) # see if the current waypoint is reached #print self._pos #print self._waypoint #print numpy.linalg.norm(self._pos-self._waypoint) if numpy.linalg.norm(self._pos-self._waypoint) < 0.15: #print("\nNEW WAYPOINT!!!: " + str(waypoint_counter) + "\n") #print("\nTIME: " + str(rospy.get_time()) + "\n") waypoint_counter += 1 self._generate_waypoint() #print("TO GOAL: " + str(numpy.linalg.norm(self._pos-self._waypoint))) print("GOAL: " + str(self._waypoint)) print("CURRENT: " + str(self._pos)) print("TIME: " + str(rospy.get_time())) print("WAYPOINTS REACHED: " + str(waypoint_counter)) #r_acc = self._roaming_planner.get_acceleration(self._pos, self._vel) r_vel = self._roaming_planner.get_velocity(self._pos) #acc = ca_acc + r_acc vel = ca_vel + r_vel #aux = self._roaming_planner.get_velocity(self._pos) #vel = numpy.array(self._vel) #vel[3] = aux[3] pos = numpy.array(self._pos) acc = numpy.zeros(4) j = numpy.zeros(4) sn = numpy.zeros(4) cr = numpy.zeros(4) msg = umc.reference_point_to_multidofjointtrajectory_msg(pos, vel, acc, j, sn, cr) pub.publish(msg) rate.sleep() rospy.spin()