Skip to content

Commit

Permalink
move local time to publisher
Browse files Browse the repository at this point in the history
  • Loading branch information
bshimanuki committed Nov 8, 2017
1 parent 33df59e commit bde1d97
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 9 deletions.
11 changes: 3 additions & 8 deletions src/client_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ def __init__(self, host, port, name, broadcasting, private_key,
self.description = description
self.global_frames = global_frames
self.leaflets = leaflets
self.pub_man = pm.PublisherManager()
self.pub_man = pm.PublisherManager(use_local_time)
self.timer = threading.Timer(0.1, self.descriptionSend)

def post_leaflet_urls(self):
Expand Down Expand Up @@ -113,7 +113,7 @@ def callback(msg):
self.conn[topic].send_message(data)
return callback

def modify_child_frame(self, message):
def modify_child_frame_id(self, message):
if (message.child_frame_id.find("/") > 0 or
message.child_frame_id.count("/") > 1):
return message
Expand All @@ -137,17 +137,12 @@ def modify_header_frame_id(self, message):
message.header.frame_id = "{}{}".format(
self.name, message.header.frame_id)

def modify_header_stamp(self, message):
message.header.stamp = rospy.get_rostime()

def modify_stamped_message(self, message):
if hasattr(message, 'child_frame_id'):
self.modify_child_frame(message)
self.modify_child_frame_id(message)

if hasattr(message, 'header'):
self.modify_header_frame_id(message)
if self.use_local_time:
self.modify_header_stamp(message)
return message

# Periodic function to send the client description.
Expand Down
5 changes: 4 additions & 1 deletion src/publishermanager.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,8 @@
# Manages ROS publishers for received messages.
class PublisherManager(object):

def __init__(self):
def __init__(self, use_local_time):
self.use_local_time = use_local_time
self.pubs = dict()

# Converts message dictionary to ROS message for publishing.
Expand All @@ -16,6 +17,8 @@ def create_msg(self, msg_dict, msg_type):
msg_cls = getattr(mod.msg, msg_name)
msg = message_converter.convert_dictionary_to_ros_message(
msg_type, msg_dict)
if self.use_local_time and hasattr(msg, "header"):
msg.header.stamp = rospy.get_rostime()
return msg, msg_cls

# Publishes newly received messages.
Expand Down

0 comments on commit bde1d97

Please sign in to comment.