diff --git a/launch/example.launch~ b/launch/example.launch~ deleted file mode 100644 index a7b9fe0..0000000 --- a/launch/example.launch~ +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - publishing: - - /state - - /filtered_state - - /usb_cam/image_raw/compressed - - /test_string - types: - - geometry_msgs/Point - - geometry_msgs/Point - - sensor_msgs/CompressedImage - - std_msgs/String - trusted: - - ".*" - - foo bar - - foo - - "ba[a-z]" - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tf_test.launch b/launch/tf_test.launch index 9b1853d..bdabde5 100644 --- a/launch/tf_test.launch +++ b/launch/tf_test.launch @@ -22,12 +22,14 @@ - tf2_msgs/TFMessage trusted: - ".*" + global_frames: + - world + name="tf_test" args="1 1 2 1 0 0 1 world map 100" /> + args="pub -r 100 /tf tf2_msgs/TFMessage '{transforms: [{header : {stamp: {secs: 1457594405, nsecs: 347724387}, frame_id: world, seq: 0}, transform: {translation: {y: 0.0, x: 1.0, z: 2.0}, rotation: {y: 1.0, x: 0.0, z: 0.0, w: 1.0}}, child_frame_id: map}]}'" />--> diff --git a/src/client_node.py b/src/client_node.py index 1c526a0..f2c5c3a 100755 --- a/src/client_node.py +++ b/src/client_node.py @@ -16,7 +16,8 @@ # One instance per client node. class CanopyClientNode(object): - def __init__(self, host, port, name, broadcasting, private_key, description): + def __init__(self, host, port, name, broadcasting, private_key, + description, global_frames): self.host = host self.port = port self.name = name.replace(" ", "").replace("/", "") @@ -27,6 +28,7 @@ def __init__(self, host, port, name, broadcasting, private_key, description): self.broadcasting = broadcasting self.private_key = private_key self.description = description + self.global_frames = global_frames self.pub_man = pm.PublisherManager() self.timer = threading.Timer(0.1, self.descriptionSend) @@ -93,22 +95,44 @@ def callback(msg): if msg_type == "tf2_msgs/TFMessage": exit = False for t in msg.transforms: - if t.header.frame_id.count("/") > 1: + t = self.modify_stamped_message(t) + if t == False: exit = True - break; - if t.header.frame_id[0] != "/": - t.header.frame_id = "/" + t.header.frame_id - t.child_frame_id = "/" + t.child_frame_id - t.header.frame_id = "/{}{}".format(self.name, - t.header.frame_id) - t.child_frame_id = "/{}{}".format(self.name, - t.child_frame_id) + break if exit: return + else: + msg = self.modify_stamped_message(msg) + if msg == False: + return data["Msg"] = mc.convert_ros_message_to_dictionary(msg) self.conn[topic].send_message(data) return callback + def modify_stamped_message(self, message): + if hasattr(message, 'child_frame_id'): + if (message.child_frame_id.find("/") > 0 or + message.child_frame_id.count("/") > 1): + return False + if message.child_frame_id not in self.global_frames: + if message.child_frame_id[0] != "/": + message.child_frame_id = "/" + message.child_frame_id + message.child_frame_id = "{}{}".format(self.name, + message.child_frame_id) + if hasattr(message, 'header'): + if ((not hasattr(message, 'child_frame_id')) and + message.header.frame_id.find("/") > 0 and + message.header.frame_id.count("/") > 1): + return False + if message.header.frame_id not in self.global_frames: + if (message.header.frame_id.find("/") <= 0 and + message.header.frame_id.count("/") <= 1): + if message.header.frame_id[0] != "/": + message.header.frame_id = "/" + message.header.frame_id + message.header.frame_id = "{}{}".format(self.name, + message.header.frame_id) + return message + # Periodic function to send the client description. def descriptionSend(self): data = dict() @@ -131,11 +155,12 @@ def descriptionSend(self): topics = rospy.get_param("~publishing", []) types = rospy.get_param("~types", []) trusted = rospy.get_param("~trusted", []) + global_frames = rospy.get_param("~global_frames", []) host = rospy.get_param("~host") port = rospy.get_param("~port") private_key = rospy.get_param("~private_key") description = rospy.get_param("~description") broadcasting = zip(topics, types, trusted) rcn = CanopyClientNode(host, port, name, broadcasting, private_key, - description) + description, global_frames) rcn.run()