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()
