Merge branch 'indigo' of github.com:srv/srv_tools into indigo

plnegre · plnegre · commit efa11bb706d9 · 2016-06-20T10:35:59.000+02:00
diff --git a/bag_tools/scripts/add_header_time_offset.py b/bag_tools/scripts/add_header_time_offset.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -60,6 +60,7 @@ def add_offset(inbags, topics, offset):
 
 
 if __name__ == "__main__":
+  rospy.init_node('add_header_time_offset')
   parser = argparse.ArgumentParser(
       description='Changes header timestamps using given offset, can change'
                   '/tf as well.')
diff --git a/bag_tools/scripts/bag_add_time_offset.py b/bag_tools/scripts/bag_add_time_offset.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -66,7 +66,7 @@ def fix_bagfile(inbag, outbag, topics, offset):
       rospy.loginfo( '%s:%s messages.',k,v)
 
 if __name__ == "__main__":
-
+  rospy.init_node('bag_add_time_offset')
   parser = argparse.ArgumentParser(
       description='Shift the publishing time of given topics in input bagfile.')
   parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile')
diff --git a/bag_tools/scripts/batch_process.py b/bag_tools/scripts/batch_process.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -55,6 +55,7 @@ def process(in_dir,out_dir,command):
             subprocess.check_call(cmd)
 
 if __name__ == "__main__":
+  rospy.init_node('batch_process')
   parser = argparse.ArgumentParser(
       description='batch processes all bagfiles in INPUT_DIR, writing output to OUTPUT_DIR by calling given command with -i and -o arguments.')
   parser.add_argument('-i', metavar='INPUT_DIR', required=True, help='input directory with input bagfiles')
diff --git a/bag_tools/scripts/camera_info_parser.py b/bag_tools/scripts/camera_info_parser.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -42,6 +42,7 @@ def parse_yaml(filename):
   cam_info = sensor_msgs.msg.CameraInfo()
   cam_info.width = calib_data['image_width']
   cam_info.height = calib_data['image_height']
+  cam_info.header.frame_id = calib_data['camera_name']
   cam_info.K = calib_data['camera_matrix']['data']
   cam_info.D = calib_data['distortion_coefficients']['data']
   cam_info.R = calib_data['rectification_matrix']['data']
@@ -50,6 +51,7 @@ def parse_yaml(filename):
   return cam_info
 
 if __name__ == "__main__":
+  rospy.init_node('camera_info_parser')
   import argparse
   parser = argparse.ArgumentParser(description='Parses camera info yaml files and returns them as sensor_msgs.msg.CameraInfo.')
   parser.add_argument('filename', help='input yaml file')
diff --git a/bag_tools/scripts/change_camera_info.py b/bag_tools/scripts/change_camera_info.py
@@ -84,6 +84,7 @@ def replacement(replace_string):
   return pair[0], cam_info
 
 if __name__ == "__main__":
+  rospy.init_node('change_camera_info')
   parser = argparse.ArgumentParser(description='Change camera info messages in a bagfile.')
   parser.add_argument('inbag', help='input bagfile')
   parser.add_argument('outbag', help='output bagfile')
diff --git a/bag_tools/scripts/change_frame_id.py b/bag_tools/scripts/change_frame_id.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -55,7 +55,7 @@ def change_frame_id(inbag,outbag,frame_id,topics):
   outbag.close();
 
 if __name__ == "__main__":
-
+  rospy.init_node('change_frame_id')
   parser = argparse.ArgumentParser(
       description='reate a new bagfile from an existing one replacing the frame id of requested topics.')
   parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile')
diff --git a/bag_tools/scripts/change_topics.py b/bag_tools/scripts/change_topics.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -63,6 +63,7 @@ def replacement(replace_string):
   return pair[0], pair[1]
 
 if __name__ == "__main__":
+  rospy.init_node('change_topics')
   parser = argparse.ArgumentParser(
       description='changes topic names inside a bagfile.')
   parser.add_argument('inbag', help='input bagfile')
diff --git a/bag_tools/scripts/check_delay.py b/bag_tools/scripts/check_delay.py
@@ -68,6 +68,7 @@ def check_delay(inbags):
     rospy.loginfo('%s : mean = %s, min = %s, max = %s, median = %s', topic.ljust(max_len + 2), dmean, dmin, dmax, dmedian)
 
 if __name__ == "__main__":
+  rospy.init_node('check_delay')
   parser = argparse.ArgumentParser(
       description='Checks the delay in a bagfile between publishing (recording) '
                   'time and the time stamp in the header (if exists). Prints '
diff --git a/bag_tools/scripts/cut.py b/bag_tools/scripts/cut.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -62,6 +62,7 @@ def cut(inbags, outbagfile, start, duration):
 
 
 if __name__ == "__main__":
+  rospy.init_node('cut')
   parser = argparse.ArgumentParser(
       description='Cuts out a section from an input bagfile and writes it to an output bagfile')
   parser.add_argument('--inbag', help='input bagfile(s)', nargs='+', required=True)
diff --git a/bag_tools/scripts/extract_topics.py b/bag_tools/scripts/extract_topics.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -52,13 +52,14 @@ def extract_topics(inbag,outbag,topics):
   outbag.close();
 
 if __name__ == "__main__":
+  rospy.init_node('extract_topics')
   parser = argparse.ArgumentParser(
       description='Extracts topics from a bagfile into another bagfile.')
   parser.add_argument('inbag', help='input bagfile')
   parser.add_argument('outbag', help='output bagfile')
   parser.add_argument('topics', nargs='+', help='topics to extract')
   args = parser.parse_args()
- 
+
   try:
     extract_topics(args.inbag,args.outbag,args.topics)
   except Exception, e:
diff --git a/bag_tools/scripts/gps_to_std_gt.py b/bag_tools/scripts/gps_to_std_gt.py
@@ -12,29 +12,30 @@ class Error(Exception):
 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. 
+  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); 
+  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__":
+  rospy.init_node('gps_to_std_gt')
   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', 
+  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', 
+  parser.add_argument('output_file',
           help='output file where the standard ground truth values will be saved.')
   args = parser.parse_args()
 
-  # Read the 
+  # 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
@@ -72,4 +73,3 @@ def get_xyz(gps_point):
 
   pyplot.draw()
   pylab.show()
-    
diff --git a/bag_tools/scripts/image_sequence_publisher.py b/bag_tools/scripts/image_sequence_publisher.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -38,33 +38,56 @@
 import cv_bridge
 import camera_info_parser
 import glob
-import cv
+import cv2
 import numpy as np
+from numpy import genfromtxt
+import math
+import tf
+import os
+
+fake_green = False
 
 def collect_image_files(image_dir,file_pattern):
   images = glob.glob(image_dir + '/' + file_pattern)
   images.sort()
+  #images = images[:1101]
+  #images = images[1102:2202]
+  # images = images[2203:]
   return images
 
-def playback_images(image_dir,file_pattern,camera_info_file,publish_rate):
+def collect_poses(file):
+  poses = genfromtxt(file, delimiter=',')
+  return poses
+
+def playback_images(image_dir,file_pattern,camera_info_file,pose_file,publish_rate):
   if camera_info_file != "":
     cam_info = camera_info_parser.parse_yaml(camera_info_file)
     publish_cam_info = True
   else:
     publish_cam_info = False
+  if pose_file != "":
+    poses = collect_poses(pose_file)
+    publish_poses = True
+  else:
+    publish_poses = False
   image_files = collect_image_files(image_dir,file_pattern)
   rospy.loginfo('Found %i images.',len(image_files))
   bridge = cv_bridge.CvBridge()
   rate = rospy.Rate(publish_rate)
-  image_publisher = rospy.Publisher('camera/image_color', sensor_msgs.msg.Image, queue_size = 1)
+  image_publisher = rospy.Publisher('camera/image_color', sensor_msgs.msg.Image, queue_size = 5)
   if publish_cam_info:
-      cam_info_publisher = rospy.Publisher('camera/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 1)
+    cam_info_publisher = rospy.Publisher('camera/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5)
+  if publish_poses:
+    tf_pose_publisher = tf.TransformBroadcaster()
   rospy.loginfo('Starting playback.')
   for image_file in image_files:
     if rospy.is_shutdown():
       break
     now = rospy.Time.now()
-    image = cv.LoadImage(image_file)
+    image = cv2.imread(image_file)
+    if fake_green:
+      image[:,:,0] = 0
+      image[:,:,2] = 0
     image_msg = bridge.cv2_to_imgmsg(np.asarray(image[:,:]), encoding='bgr8')
     image_msg.header.stamp = now
     image_msg.header.frame_id = "/camera"
@@ -73,6 +96,15 @@ def playback_images(image_dir,file_pattern,camera_info_file,publish_rate):
       cam_info.header.stamp = now
       cam_info.header.frame_id = "/camera"
       cam_info_publisher.publish(cam_info)
+    if publish_poses:
+      img_name = os.path.basename(image_file)
+      idx = int(''.join(x for x in img_name if x.isdigit())) + 1
+      tf_pose_publisher.sendTransform((poses[idx, 1], poses[idx, 2], poses[idx, 3]),
+                     tf.transformations.quaternion_from_euler(math.radians(poses[idx, 4]), math.radians(poses[idx, 5]), math.radians(poses[idx, 6])),
+                     now,
+                     'dvl',
+                     'world')
+      # print img_name + ' =? ' + str(poses[idx, 0]) + " pose (" + str(poses[idx, 1]) + ", " + str(poses[idx, 2]) + ", " + str(poses[idx, 3]) + ") orientation (" + str(poses[idx, 4]) + ", " + str(poses[idx, 5]) + ", " + str(poses[idx, 6]) + ")"
     rate.sleep()
   rospy.loginfo('No more images left. Stopping.')
 
@@ -82,8 +114,10 @@ def playback_images(image_dir,file_pattern,camera_info_file,publish_rate):
     image_dir = rospy.get_param("~image_dir")
     file_pattern = rospy.get_param("~file_pattern")
     camera_info_file = rospy.get_param("~camera_info_file", "")
+    pose_file = rospy.get_param("~pose_file", "")
     frequency = rospy.get_param("~frequency", 10)
-    playback_images(image_dir, file_pattern, camera_info_file, frequency)
+    fake_green = rospy.get_param("~fake_green", False)
+    playback_images(image_dir, file_pattern, camera_info_file, pose_file, frequency)
   except KeyError as e:
     rospy.logerr('Required parameter missing: %s', e)
   except Exception, e:
diff --git a/bag_tools/scripts/make_video.py b/bag_tools/scripts/make_video.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -62,6 +62,7 @@ def create_video(tmp_dir, args):
 
 
 if __name__ == "__main__":
+  rospy.init_node('make_video')
   import argparse
   parser = argparse.ArgumentParser(
       description=
diff --git a/bag_tools/scripts/remove_tf.py b/bag_tools/scripts/remove_tf.py
@@ -12,9 +12,9 @@
     * 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 
+    * 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
@@ -57,7 +57,7 @@ def remove_tf(inbag,outbag,frame_ids):
   outbag.close();
 
 if __name__ == "__main__":
-
+  rospy.init_node('remove_tf')
   parser = argparse.ArgumentParser(
       description='removes all transforms from the /tf topic that contain one of the given frame_ids in the header as parent or child.')
   parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile')
diff --git a/bag_tools/scripts/replace_msg_time_with_hdr.py b/bag_tools/scripts/replace_msg_time_with_hdr.py
diff --git a/bag_tools/scripts/stereo_sequence_publisher.py b/bag_tools/scripts/stereo_sequence_publisher.py
diff --git a/pointcloud_tools/CMakeLists.txt b/pointcloud_tools/CMakeLists.txt
diff --git a/pointcloud_tools/package.xml b/pointcloud_tools/package.xml
diff --git a/pointcloud_tools/src/pointcloud_mapper.cpp b/pointcloud_tools/src/pointcloud_mapper.cpp
