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