Merge pull request #906 from tue-robotics/fix/odometer

Fix odometer

test_tools/CMakeLists.txt

@@ -1,7 +1,12 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(test_tools)
 
-find_package(catkin REQUIRED COMPONENTS
-)
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
 
 catkin_package()
+
+# if (CATKIN_ENABLE_TESTING)
+#   catkin_add_nosetests(test)
+# endif()

test_tools/scripts/odometer.py

@@ -0,0 +1,19 @@
+#! /usr/bin/env python
+
+import rospy
+from test_tools import odometer
+
+rospy.init_node("odometer")
+
+r = float(rospy.get_param("~rate", 1/60.0))
+length = int(rospy.get_param("~buffer_length", 1))
+path = rospy.get_param("~path", odometer.DEFAULT_PATH)
+filename = rospy.get_param("~filename", odometer.DEFAULT_FILENAME)
+
+meter = odometer.Odometer(path, filename)
+rate = rospy.Rate(max(r, 1e-3))
+
+while not rospy.is_shutdown():
+    meter.sample()
+    meter.activate_write(length)
+    rate.sleep()

test_tools/setup.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    # #  don't do this unless you want a globally visible script
+    # scripts=['bin/myscript'],
+    packages=['test_tools'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

test_tools/src/test_tools/odometer.py

@@ -95,9 +95,15 @@ def __init__(self, path=DEFAULT_PATH, filename=DEFAULT_FILENAME):
                         break
 
                 if found_header:
-                    # Going over all lines.(this is the only option to get the last line) Doing nothing with other lines
-                    for last_row in reader:
-                        pass
+                    # Iterate over all lines and get the last valid line. So it continues after an invalid line.
+                    last_row = None
+                    while True:
+                        try:
+                            last_row = next(reader)
+                        except csv.Error:
+                            pass
+                        except StopIteration:
+                            break
                     if last_row:
                         last_row = dict(zip(header, last_row))
                         try:
@@ -142,23 +148,24 @@ def write(self):
         """
         # Create today's file if not already there
         if os.path.exists(self.newfilepath):
-            new_file = open(self.newfilepath, "a", 1)  # 1=line-buffered
             rospy.logdebug("Today's file already exists")
         else:
-            new_file = open(self.newfilepath, "w+", 1)  # 1=line-buffered
             rospy.logdebug("First time writing in today's file")
 
-        writer = csv.DictWriter(new_file, fieldnames=['timestamp', 'distance', 'rotation', 'time'])
-        if not self.file_has_header:
-            rospy.logdebug("Printing header of csv file")
-            writer.writeheader()
-            self.file_has_header = True
-        if self.data:
-            rospy.logdebug("Writing data to csv file")
-            writer.writerows(self.data)
-            self.data = []
-
-        new_file.close()
+        with open(self.newfilepath, "a", 1) as new_file:  # 1=line-buffered
+            try:
+                writer = csv.DictWriter(new_file, fieldnames=['timestamp', 'distance', 'rotation', 'time'])
+                if not self.file_has_header:
+                    rospy.logdebug("Printing header of csv file")
+                    writer.writeheader()
+                    self.file_has_header = True
+                if self.data:
+                    rospy.logdebug("Writing data to csv file")
+                    writer.writerows(self.data)
+                    self.data = []
+
+            except Exception as e:
+                rospy.logerr(e)
 
     def callback(self, msg):
         """
@@ -213,20 +220,3 @@ def shutdown(self):
         """
         self.sample()
         self.write()
-
-
-if __name__ == '__main__':
-    rospy.init_node("odometer")
-
-    r = float(rospy.get_param("~rate", 1/60.0))
-    length = int(rospy.get_param("~buffer_length", 1))
-    path = rospy.get_param("~path", DEFAULT_PATH)
-    filename = rospy.get_param("~filename", DEFAULT_FILENAME)
-
-    meter = Odometer(path, filename)
-    rate = rospy.Rate(max(r, 1e-3))
-
-    while not rospy.is_shutdown():
-        meter.sample()
-        meter.activate_write(length)
-        rate.sleep()