From 9409d3da846f9dba26508ce3ce604b8204d0ec4a Mon Sep 17 00:00:00 2001
From: Jiaqi <jiaqi.li@vanderbilt.edu>
Date: Tue, 12 Mar 2019 19:21:38 -0500
Subject: [PATCH] WIP #15: Added locate function, removed small bugs.

---
 .../scripts/update_configs/test_config.csv      |  6 +++---
 robot_misc_tools/scripts/update_publisher.py    | 17 ++++++++++-------
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/robot_misc_tools/scripts/update_configs/test_config.csv b/robot_misc_tools/scripts/update_configs/test_config.csv
index 71e18c9..38bcd9a 100644
--- a/robot_misc_tools/scripts/update_configs/test_config.csv
+++ b/robot_misc_tools/scripts/update_configs/test_config.csv
@@ -1,3 +1,3 @@
-cur_pose, Pose
-cur_path, Path
-drive_cmd, Twist
\ No newline at end of file
+cur_pose,geometry_msgs,Pose
+cur_path,nav_msgs,Path
+drive_cmd,geometry_msgs,Twist
\ No newline at end of file
diff --git a/robot_misc_tools/scripts/update_publisher.py b/robot_misc_tools/scripts/update_publisher.py
index 40c6110..f98353b 100644
--- a/robot_misc_tools/scripts/update_publisher.py
+++ b/robot_misc_tools/scripts/update_publisher.py
@@ -10,13 +10,15 @@
 from geometry_msgs.msg import *
 from nav_msgs.msg import *
 from sensor_msgs.msg import *
+from pydoc import locate
 
 class UpdatePublisher:
 
+#geometry_msgs.msg._Pose.Pose
     def __init__ (self, topic_name, message_type):
 
         #Initialize variable to be updated
-        self.latest_var = 0
+        self.latest_var = message_type()
 
         #Initialize Publisher
         self.publisher = rospy.Publisher('updates/' + topic_name, message_type, queue_size=0)
@@ -45,9 +47,11 @@ def load_config_file(file_name):
 
         # Loop and append the updaters list with UpdatePublishers
         for item in updaters_list:
-            comma = item.find(",")
-            topic_name = item[:comma]
-            message_type = item[comma+2:]
+            topic_name, message_package, message_type = item.split(",")
+
+            #Modify the string to locate
+            full_string = message_package + ".msg._" + message_type + "." + message_type
+            message_type = locate(full_string)
             updaters.append(UpdatePublisher(topic_name, message_type))
 
         return updaters
@@ -60,8 +64,8 @@ def load_config_file(file_name):
     #Read a file containing a bunch of [topic name, message type] pairs, create an UpdatePublisher for each
     updaters = UpdatePublisher.load_config_file("update_configs/test_config.csv")
 
-    #read values from server
-    update_rate = rospy.get_param("update_rate")
+    #read values from serverros
+    update_rate = rospy.get_param("update_rate", default=1)
 
     #Log initialization message
     rospy.loginfo("Update node initialized...")
@@ -71,7 +75,6 @@ def load_config_file(file_name):
 
         # Send an update from each UpdatePublisher in updaters list
         for item in updaters:
-            item.update_message()
             item.publish_message()
 
         rate.sleep()