def callback(self, pdf_intention):
"""
ros topic listener
receive message in factor graph
:param pdf_intention: pdf of intention over the space
:type pdf_intention: Belief
"""
if pdf_intention.header.frame_id[2] == self.name:
intention_data = np.nan_to_num(
np.asanyarray(pdf_intention.data, dtype=np.float32))
msg_formatted = Belief()
msg_formatted.header = pdf_intention.header
msg_formatted.data = intention_data.ravel()
self._msg_received[
pdf_intention.header.frame_id[0]] = msg_formatted
def start(self):
rospy.init_node(self._name, log_level=rospy.DEBUG)
rate = rospy.Rate(2)
rospy.Subscriber("/UAV/" + self._name + "/pose",
euclidean_location,
callback=self.callback_pose)
pub_unexplored = rospy.Publisher(self._name + '/unexplored',
numpy_msg(Belief),
queue_size=10.)
while not rospy.is_shutdown():
msg = Belief()
msg.header.frame_id = self._name
msg.header.stamp = rospy.Time.now()
msg.data = self._phi_unexplored_norm.ravel()
pub_unexplored.publish(msg)
rate.sleep()
def start(self):
rospy.init_node(self._name, log_level=rospy.DEBUG)
rate = rospy.Rate(2)
rospy.Subscriber("/UAV/{}/pose".format(self._name),
euclidean_location,
callback=self.callback_sensor_pose_euclid)
pub_avoid_collision = rospy.Publisher("/PHI/{}/avoid_collision".format(
self._name),
numpy_msg(Belief),
queue_size=10.)
while not rospy.is_shutdown():
msg = Belief()
msg.header.frame_id = self._name
msg.header.stamp = rospy.Time.now()
msg.data = self._phi_avoid_collision.ravel()
pub_avoid_collision.publish(msg)
rate.sleep()
def __init__(self, name, d):
self.name = name
self.scale = d
self.pose = np.zeros(2)
self.neighbor_names = []
self._pose_received = {}
self._intention_received = {}
self._intention_sent = {}
self._intention_self = Belief()
def start(self):
rospy.init_node(self._name, log_level=rospy.DEBUG)
rate = rospy.Rate(2)
rospy.Subscriber("/Sensors/" + self._name + "/temperature",
Float32,
callback=self.callback_temp)
rospy.Subscriber("/UAV/{}/pose".format(self._name),
euclidean_location,
callback=self.callback_sensor_pose_euclid)
pub_temp_change = rospy.Publisher("/PHI/{}/temp_change".format(
self._name),
numpy_msg(Belief),
queue_size=10.)
while not rospy.is_shutdown():
msg = Belief()
msg.header.frame_id = self._name
msg.header.stamp = rospy.Time.now()
msg.data = self._phi_temp_change.ravel()
pub_temp_change.publish(msg)
rate.sleep()
def start(self):
rospy.init_node(self._name, log_level=rospy.DEBUG)
rate = rospy.Rate(2)
rospy.Subscriber("/UAV/{}/pose".format(self._name),
Pose,
callback=self.callback_sensor_pose_euclid)
rospy.Subscriber("/Human/{}/interesting".format(self._name),
Pose,
callback=self.callback_human_interesting)
rospy.Subscriber("/Human/{}/annoying".format(self._name),
Pose,
callback=self.callback_human_annoying)
pub_human_annoying = rospy.Publisher("/PHI/{}/human_annoying".format(
self._name),
numpy_msg(Belief),
queue_size=10.)
pub_human_interesting = rospy.Publisher(
"/PHI/{}/human_interesting".format(self._name),
numpy_msg(Belief),
queue_size=10.)
while not rospy.is_shutdown():
msg1 = Belief()
msg1.header.frame_id = self._name
msg1.header.stamp = rospy.Time.now()
msg1.data = self.phi_human_annoying.ravel()
pub_human_annoying.publish(msg1)
msg2 = Belief()
msg2.header.frame_id = self._name
msg2.header.stamp = rospy.Time.now()
msg2.data = self.phi_human_interesting.ravel()
pub_human_interesting.publish(msg2)
rate.sleep()
def take_off(self, start_at):
"""
uav rosnode launcher and intention publisher
"""
rospy.init_node(self._name, log_level=rospy.DEBUG)
rate = rospy.Rate(5)
vendor = rospy.get_param('/{}s_vendor'.format(self._name))
rospy.Subscriber("/{}/{}/ready".format(vendor, self._name),
Bool,
callback=self.callback_is_robot_ready)
# rospy.Subscriber("/" + vendor + "/{}/fly_grad".format(self._name), String, callback=self.callback_fly)
rospy.logdebug("{} UAV autonomous waiting for {} to be ready".format(
self.tag, vendor))
while not self._solo_is_ready:
rospy.sleep(1)
rospy.logdebug("{} {} is ready".format(self.tag, vendor))
# choice = raw_input("Start autonomous node? yes/no:\n>> ").lower()
# while not choice == "yes":
# choice = raw_input("Start autonomous node? yes/no:\n>> ").lower()
# rospy.sleep(10)
rospy.logdebug(
"{} Starting Autonomouse mode......!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
.format(self.tag))
rospy.Subscriber("/{}/{}/pose_euclid".format(vendor, self._name),
Pose,
callback=self.callback_sensor_pose)
# rospy.Subscriber("/solo/{}/distance_from_goal".format(self._name), Float32, callback=self.callback_goal_reached)
for from_uav in self._neighbors_names:
# initialize all other uavs intention uniformly
init_belief = Belief()
init_belief.header.frame_id = "{}>{}".format(from_uav, self.name)
init_belief.header.stamp = rospy.Time.now()
init_belief.data = np.ones(self._scale**self._dim,
dtype=np.float32).ravel()
self._msg_received[from_uav] = init_belief
topic = "/UAV/" + from_uav + "/intention_sent"
rospy.Subscriber(topic, Belief, callback=self.callback)
self._pose = Pose(Point(start_at[0], start_at[1], start_at[2]),
Quaternion(*quaternion_from_euler(0., 0., np.pi)))
q_size = 10
# fix3d
pub_pose = rospy.Publisher(self.name + '/pose',
Pose,
queue_size=q_size)
pub_inbox = rospy.Publisher(self.name + '/intention_received',
numpy_msg(Belief),
queue_size=q_size)
pub_self = rospy.Publisher(self.name + '/intention_self',
numpy_msg(Belief),
queue_size=q_size)
pub_outbox = rospy.Publisher(self.name + '/intention_sent',
numpy_msg(Belief),
queue_size=q_size)
while not rospy.is_shutdown():
self.sum_product_algo2()
# if self._solo_wants_to_fly:
self.fly()
# self._solo_wants_to_fly = False
for to_uav in self._neighbors_names:
msg = Belief()
msg.header.frame_id = "{}>{}".format(self.name, to_uav)
msg.data = self._msg_send[to_uav].ravel()
msg.header.stamp = rospy.Time.now()
pub_outbox.publish(msg)
# ---------------------publishing own belief for visualization----------------------------------------------
pub_pose.publish(self._pose)
msg_viz = Belief()
msg_viz.header.frame_id = self.name
msg_viz.data = self._intention_fusion.ravel()
msg_viz.header.stamp = rospy.Time.now()
pub_self.publish(msg_viz)
for from_uav in self._neighbors_names:
msg_viz = self._msg_received[from_uav]
msg_viz.header.frame_id = from_uav
pub_inbox.publish(msg_viz)
# ----------------------------------------------------------------------------------------------------------
rate.sleep()