added gps_to_std_gt and services_timer from fuerte

Author: miquelmassot

bag_tools/scripts/bag_tools/gps_to_std_gt.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('stereo_slam')
+import pylab
+import numpy as np
+from matplotlib import pyplot
+from mpl_toolkits.mplot3d import Axes3D
+
+class Error(Exception):
+  """ Base class for exceptions in this module. """
+  pass
+
+def get_xyz(gps_point):
+  lat = gps_point[3]*np.pi/180 #converting to radians.
+  lon = gps_point[4]*np.pi/180 #converting to radians.
+  alt = gps_point[5]*np.pi/180 #converting to radians.  
+  a = 6378137.0 # earth semimajor axis in meters.   
+  f = 1/298.257223563 # reciprocal flattening. 
+  e2 = 2*f - np.power(f,2) # eccentricity squared. 
+
+  chi = np.sqrt(1-e2 * np.power(np.sin(lat),2)); 
+  x = (a/chi +alt) * np.cos(lat) * np.cos(lon);   
+  y = (a/chi +alt) * np.cos(lat) * np.sin(lon);   
+  z = (a*(1-e2)/chi + alt) * np.sin(lat); 
+  return x, y, z
+
+if __name__ == "__main__":
+  import argparse
+  parser = argparse.ArgumentParser(
+          description='Convert gps/fix topic to standard ground truth file',
+          formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument('gps_topic_file', 
+          help='file with the gps/fix topic. Saved with "rostopic echo -p /gps/fix > data/anselm_turmeda/gt_gps.txt"')
+  parser.add_argument('output_file', 
+          help='output file where the standard ground truth values will be saved.')
+  args = parser.parse_args()
+
+  # Read the 
+  gps_topic = pylab.loadtxt(args.gps_topic_file, delimiter=',', skiprows=1, usecols=(0,1,2,6,7,8))
+
+  # Write the x, y, z data to the output file
+  with open(args.output_file, 'w') as outfile:
+    outfile.write("%time,field.header.seq,field.header.stamp,field.header.frame_id,field.child_frame_id,x,y,z,qx,qy,qz,qw\n")
+    x0, y0, z0 = get_xyz(gps_topic[0])
+    gt = []
+    gt.append([0.0, 0.0, 0.0])
+    for i in range(len(gps_topic)-1):
+      x, y, z = get_xyz(gps_topic[i+1])
+      x = x-x0
+      y = y-y0
+      z = z-z0
+      gt.append([x, y, z])
+      outfile.write("%.9F," % gps_topic[i+1, 0])
+      outfile.write(str(gps_topic[i+1, 1]) + "," + str(gps_topic[i+1, 2]) + ",/gps,,")
+      outfile.write("%.9F," % x)
+      outfile.write("%.9F," % y)
+      outfile.write("%.9F," % z)
+      outfile.write("0.0,0.0,0.0,0.0")
+      outfile.write("\n")
+  gt = np.array(gt)
+
+  # Init figure
+  fig = pylab.figure(1)
+  ax = Axes3D(fig)
+  ax.grid(True)
+  ax.set_title("GT Viewer")
+  ax.set_xlabel("X")
+  ax.set_ylabel("Y")
+  ax.set_zlabel("Z")
+
+  ax.plot(gt[:,0], gt[:,1], gt[:,2], 'g', label='Ground Truth')
+  ax.legend()
+
+  pyplot.draw()
+  pylab.show()
+    

launch_tools/scripts/launch_tools/services_timer.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+
+"""
+Copyright (c) 2013,
+Systems, Robotics and Vision Group
+University of the Balearican Islands
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of Systems, Robotics and Vision Group, University of 
+      the Balearican Islands nor the names of its contributors may be used to 
+      endorse or promote products derived from this software without specific 
+      prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+
+
+import roslib; roslib.load_manifest('launch_tools')
+import sys
+import rospy
+import rosservice
+import threading
+
+
+## Class for calling a service using a timer.
+class TimedService(threading.Thread):
+
+	## The constructor
+	# @param self The object pointer.
+	# @param name The service name this class is going to call
+	# @param freq The desired timer period
+	def __init__(self, name, period):
+		threading.Thread.__init__(self)
+		self._service_name = name
+		self._service_period = period
+
+	## Run function required by threading library
+	def run(self):
+		rospy.wait_for_service(self._service_name)
+		rospy.Timer(rospy.Duration(self._service_period), self.callback)
+		rospy.loginfo('Initialized timer for service: \n\t* Name: %s\n\t* Period: %f ', self._service_name, self._service_period)
+
+	## Timer callback
+	# @param event The event that has generated this callback
+	def callback(self,event):
+		rospy.wait_for_service(self._service_name)
+		service_class = rosservice.get_service_class_by_name(self._service_name)
+		try:
+			service = rospy.ServiceProxy(self._service_name,service_class)
+			service()
+			rospy.loginfo('Service %s called.', self._service_name)
+		except rospy.ServiceException, e:
+			rospy.logwarn('Service %s call failed: %s',self._service_name,e)
+
+	## @var _service_name 
+	# The service name going to be called
+	_service_name = "service"
+
+	## @var _service_period
+	# The timer period to call the service
+	_service_period = 1.0
+
+## Print usage for people that does not deserve to use this awesome python node.
+def usage():
+    return "%s service period [service period ...]"%sys.argv[0]
+
+## main function
+if __name__ == "__main__":
+	rospy.init_node('services_timer')
+	if len(sys.argv) >= 3:
+		names = sys.argv[1:len(sys.argv):2]
+		periods = sys.argv[2:len(sys.argv):2]
+		print 'names: ', names
+		print 'periods: ', periods
+		ts_list = []
+		for name,period in zip(names,periods):
+			ts_list.append(TimedService(str(name), float(period)))
+		for ts in ts_list:
+			ts.start()
+	else:
+		print usage()
+		sys.exit(1)
+	rospy.spin()