[ROS-O] API updates to fix build with current systems

calibration_estimation/setup.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from distutils.core import setup
+from setuptools import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup(

calibration_estimation/src/calibration_estimation/cal_bag_helpers.py

@@ -44,7 +44,7 @@ def get_cam_info(bag_filename, cam_name):
         if topic == "robot_measurement":
             for cam_measurement in msg.M_cam:
                 if cam_measurement.camera_id == cam_name:
-                    print "Found a sample with camera [%s] in it" % cam_measurement.camera_id
+                    print("Found a sample with camera [%s] in it" % cam_measurement.camera_id)
                     cam_info = cam_measurement.cam_info
                     break
             if cam_info != None:
@@ -74,7 +74,7 @@ def get_robot_measurement_count(bag_filename, sample_skip_list=[]):
     for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
         if topic == 'robot_measurement':
             if index in sample_skip_list:
-                print "Skipping sample:", index
+                print("Skipping sample:", index)
             else:
                 msg_count+=1
             index += 1
@@ -90,7 +90,7 @@ def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[]):
     for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
         if topic == "robot_measurement":
             if index in sample_skip_list:
-                print "Skipping sample:", index
+                print("Skipping sample:", index)
             else:
                 # Hack to rename laser id
                 for cur_laser in msg.M_laser:

calibration_estimation/src/calibration_estimation/error_visualization.py

@@ -55,15 +55,15 @@
 
 def usage():
     rospy.logerr("Not enough arguments")
-    print "Usage:"
-    print " ./error_visualization.py [bagfile] [output_dir] [loop_list.yaml]"
+    print("Usage:")
+    print(" ./error_visualization.py [bagfile] [output_dir] [loop_list.yaml]")
     sys.exit(0)
 
 if __name__ == '__main__':
     rospy.init_node("error_visualization")
     marker_pub = rospy.Publisher("calibration_error", Marker)
 
-    print "Starting The Error Visualization App\n"
+    print("Starting The Error Visualization App\n")
 
     if (len(rospy.myargv()) < 3):
         usage()
@@ -72,7 +72,7 @@ def usage():
         output_dir = rospy.myargv()[2]
         loop_list_filename = rospy.myargv()[3]
 
-    print "Using Bagfile: %s\n" % bag_filename
+    print("Using Bagfile: %s\n" % bag_filename)
     if not os.path.isfile(bag_filename):
         rospy.logerr("Bagfile does not exist. Exiting...")
         sys.exit(1)
@@ -116,7 +116,7 @@ def usage():
         multisensors = get_multisensors(bag_filename, sensor_defs, sample_skip_list)
 
         if (len([ms for ms in multisensors if len(ms.sensors) == 2]) == 0):
-            print "********** No Data for [%s] + [%s] pair **********" % (cur_loop['cam'], cur_loop['3d'])
+            print("********** No Data for [%s] + [%s] pair **********" % (cur_loop['cam'], cur_loop['3d']))
             continue
         label_list.append(cur_loop)
 
@@ -125,8 +125,8 @@ def usage():
         sample_ind = [k for k,ms in zip(range(len(multisensors)), multisensors) if len(ms.sensors) == 2]
         sample_ind = [i for i in sample_ind if i not in sample_skip_list]
 
-        print "Sample Indices:"
-        print ", ".join(["%u" % i for i in sample_ind])
+        print("Sample Indices:")
+        print(", ".join(["%u" % i for i in sample_ind]))
 
         for ms in multisensors:
             ms.update_config(system_def)
@@ -138,19 +138,19 @@ def usage():
         # Display error breakdown
         errors_dict = opt_runner.compute_errors_breakdown(error_calc, multisensors_pruned, numpy.array(cb_poses_pruned))
 
-        print ""
-        print "Errors Breakdown:"
+        print("")
+        print("Errors Breakdown:")
         for sensor_id, error_list in errors_dict.items():
-            print "  %s:" % sensor_id
+            print("  %s:" % sensor_id)
             i = 0
             for error in error_list:
                 if i in sample_ind:        
                     rms_error = numpy.sqrt( numpy.mean(error**2) )
-                    print "    Sample %d: %.6f" % (i, rms_error)
+                    print("    Sample %d: %.6f" % (i, rms_error))
                 i += 1
             error_cat = numpy.concatenate(error_list)
             rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
-            print "    Total: %.6f" % rms_error
+            print("    Total: %.6f" % rms_error)
 
         # Calculate loop errors
         chain_sensors = [[s for s in ms.sensors if s.sensor_id == cur_loop['3d']][0]  for ms in multisensors_pruned]

calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py

@@ -60,16 +60,16 @@
 # Re-enable Ctrl+C cancelling
 import signal
 def signal_handler(signal, frame):
-    print '\n\nYou pressed Ctrl+C!\n'
+    print('\n\nYou pressed Ctrl+C!\n')
     sys.exit(-1)
 
 signal.signal(signal.SIGINT, signal_handler)
 
 
 def usage():
     rospy.logerr("Not enough arguments")
-    print "Usage:"
-    print " ./proto1.py [bagfile] [output_dir]"
+    print("Usage:")
+    print(" ./proto1.py [bagfile] [output_dir]")
     sys.exit(0)
 
 
@@ -91,17 +91,17 @@ def build_sensor_defs(sensors):
             all_sensors_dict[cur_sensor['sensor_id']] = cur_sensor
             all_sensors_dict[cur_sensor['sensor_id']]['sensor_type'] = cur_sensor_type
 
-    print "The following error sensors have been loaded:\n"
+    print("The following error sensors have been loaded:\n")
     # We want to sort by the types of blocks
     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
 
     for cur_sensor_type in all_sensor_types:
-        print "  %s sensors" % cur_sensor_type
+        print("  %s sensors" % cur_sensor_type)
         cur_sensor_ids = [cur_sensor_id for cur_sensor_id,cur_sensor in all_sensors_dict.items() if cur_sensor['sensor_type'] == cur_sensor_type]
         cur_sensor_ids.sort()
         for cur_sensor_id in cur_sensor_ids:
-            print "   - %s" % cur_sensor_id
-        print ""
+            print("   - %s" % cur_sensor_id)
+        print("")
 
     return all_sensors_dict
 
@@ -114,9 +114,9 @@ def load_calibration_steps(steps_dict):
         # Add the step name to the dict (since we'll lose this info once we listify)
         steps_dict[step_name]["name"] = step_name
         step_list.append(steps_dict[step_name])
-    print "Loaded the calibration steps to execute in the following order:"
+    print("Loaded the calibration steps to execute in the following order:")
     for cur_step in step_list:
-        print " - %s" % cur_step['name']
+        print(" - %s" % cur_step['name'])
     return step_list
 
 # Verifies that the given filename has the correct permissions.
@@ -168,7 +168,7 @@ def update_transmission(urdf, joint, gearing):
         if transmission.joint == joint:
             transmission.mechanicalReduction = transmission.mechanicalReduction * gearing
             return
-    print "No transmission found for:", joint
+    print("No transmission found for:", joint)
 
 def update_urdf(urdf, calibrated_params):
     ''' Given urdf and calibrated robot_params, updates the URDF. '''
@@ -195,7 +195,7 @@ def update_urdf(urdf, calibrated_params):
         try:
             updated_link_params = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
             if diff(updated_link_params[0:3],  urdf.joint_map[joint_name].origin.position):
-                print 'Updating xyz for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.position, '\n new:', updated_link_params[0:3]
+                print('Updating xyz for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.position, '\n new:', updated_link_params[0:3])
                 urdf.joint_map[joint_name].origin.position = updated_link_params[0:3]
                 link_updated = 1
             r1 = RPY_to_angle_axis(urdf.joint_map[joint_name].origin.rotation)
@@ -205,26 +205,26 @@ def update_urdf(urdf, calibrated_params):
                     cal = urdf.joint_map[joint_name].calibration 
                     a = axis[joints.index(joint_name)]
                     a = int(a) - 1
-                    print 'Updating calibration for', joint_name, 'by', updated_link_params[a]
+                    print('Updating calibration for', joint_name, 'by', updated_link_params[a])
                     if cal.rising != None:
                         urdf.joint_map[joint_name].calibration.rising += updated_link_params[a]   
                     if cal.falling != None:
                         urdf.joint_map[joint_name].calibration.falling += updated_link_params[a]
                     link_updated = 1
                 else:
                     rot = angle_axis_to_RPY(updated_link_params[3:6])
-                    print 'Updating rpy for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.rotation, '\n new:', rot
+                    print('Updating rpy for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.rotation, '\n new:', rot)
                     urdf.joint_map[joint_name].origin.rotation = rot
                     link_updated = 1                
         except KeyError:
-            print "Joint removed:", joint_name
-            print ' xyz:', updated_link_params[0:3]
-            print ' rpy:', angle_axis_to_RPY(updated_link_params[3:6])
+            print("Joint removed:", joint_name)
+            print(' xyz:', updated_link_params[0:3])
+            print(' rpy:', angle_axis_to_RPY(updated_link_params[3:6]))
             link_updated = 1
         if not link_updated:
             unchanged_joints.append( joint_name );
 
-    print "The following joints weren't updated: \n", ', '.join(unchanged_joints)
+    print("The following joints weren't updated: \n", ', '.join(unchanged_joints))
     return urdf
 
 if __name__ == '__main__':
@@ -234,7 +234,7 @@ def update_urdf(urdf, calibrated_params):
     uncalibrated_xml = 'robot_uncalibrated_'+xml_time+'.xml'
 
     rospy.init_node("multi_step_cov_estimator", anonymous=True)
-    print "Starting The Multi Step [Covariance] Estimator Node\n"
+    print("Starting The Multi Step [Covariance] Estimator Node\n")
 
     if (len(rospy.myargv()) < 2):
         usage()
@@ -245,7 +245,7 @@ def update_urdf(urdf, calibrated_params):
         bag_filename = rospy.myargv()[1]
         output_dir = rospy.myargv()[2]
 
-    print "Using Bagfile: %s\n" % bag_filename
+    print("Using Bagfile: %s\n" % bag_filename)
     if not os.path.isfile(bag_filename):
         rospy.logerr("Bagfile does not exist. Exiting...")
         sys.exit(1)
@@ -287,7 +287,7 @@ def update_urdf(urdf, calibrated_params):
         if 'default_floating_initial_pose' in config.keys():
             default_pose = config['default_floating_initial_pose']
             if len(default_pose) != 6:
-                print "The 'default_floating_initial_pose' parameter has", len(default_pose), "elements, but it should have 6!"
+                print("The 'default_floating_initial_pose' parameter has", len(default_pose), "elements, but it should have 6!")
                 sys.exit(-1)
             for p in range(msg_count):
                 previous_pose_guesses[p,] = config['default_floating_initial_pose']
@@ -300,23 +300,23 @@ def update_urdf(urdf, calibrated_params):
     valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
     permissions_valid = all(valid_list)
     if not permissions_valid:
-        print "Invalid file permissions. You need to be able to write to the following files:"
-        print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
+        print("Invalid file permissions. You need to be able to write to the following files:")
+        print("\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid]))
         sys.exit(-1)
 
     # Generate robot parameters
     robot_description = get_robot_description(bag_filename)
     robot_params = UrdfParams(robot_description, config)
     if robot_params == "":
-        print bag_filename, " does not have robot_description, exitting.."
+        print(bag_filename, " does not have robot_description, exiting..")
         sys.exit(-1)
 
 
     # Load all the sensors from the bagfile and config file
     for cur_step in step_list:
-        print ""
-        print "*"*30
-        print "Beginning [%s]" % cur_step["name"]
+        print("")
+        print("*"*30)
+        print("Beginning [%s]" % cur_step["name"])
 
         # Need to load only the sensors that we're interested in
         cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
@@ -325,23 +325,23 @@ def update_urdf(urdf, calibrated_params):
         multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
 
         # Display sensor count statistics
-        print "Executing step with the following Sensors:"
+        print("Executing step with the following Sensors:")
         # Iterate over sensor definitions for this step
         for cur_sensor_type, cur_sensor_list in cur_sensors.items():
-            print "  %s Sensors:" % cur_sensor_type
+            print("  %s Sensors:" % cur_sensor_type)
             cur_sensor_ids = [cur_sensor['sensor_id'] for cur_sensor in cur_sensor_list]
             cur_sensor_ids.sort()
             for cur_sensor_id in cur_sensor_ids:
                 counts = [ len([s for s in ms.sensors if s.sensor_id == cur_sensor_id]) for ms in multisensors]
                 count = sum(counts)
-                print "   - %s (%u)" % (cur_sensor_id, count)
-            print ""
+                print("   - %s (%u)" % (cur_sensor_id, count))
+            print("")
 
-        print "Sensor breakdown (By Sample):"
+        print("Sensor breakdown (By Sample):")
         for k,ms in zip(range(len(multisensors)), multisensors):
-            print " % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors]))
+            print(" % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors])))
 
-        print "Pose Guesses:\n", previous_pose_guesses
+        print("Pose Guesses:\n", previous_pose_guesses)
 
         if len(multisensors) == 0:
             rospy.logwarn("No error blocks were generated for this optimization step. Skipping this step.  This will result in a miscalibrated sensor")
@@ -351,9 +351,9 @@ def update_urdf(urdf, calibrated_params):
             free_dict = yaml.safe_load(cur_step["free_params"])
             use_cov = cur_step['use_cov']
             if use_cov:
-                print "Executing step with covariance calculations"
+                print("Executing step with covariance calculations")
             else:
-                print "Executing step without covariance calculations"
+                print("Executing step without covariance calculations")
             output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
 
         # Dump results to file

calibration_estimation/src/calibration_estimation/opt_runner.py

@@ -138,7 +138,7 @@ def calculate_error(self, opt_all_vec):
 
         rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
 
-        print "\t\t\t\t\tRMS error: %.3f    \r" % rms_error,
+        print("\t\t\t\t\tRMS error: %.3f    \r" % rms_error, end='')
         sys.stdout.flush()
 
         return array(r_vec)
@@ -400,12 +400,12 @@ def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov):
 
     errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr)
 
-    print ""
-    print "Errors Breakdown:"
+    print("")
+    print("Errors Breakdown:")
     for sensor_id, error_list in errors_dict.items():
         error_cat = numpy.concatenate(error_list)
         rms_error = numpy.sqrt( numpy.mean(error_cat**2) )
-        print "  %s: %.6f" % (sensor_id, rms_error)
+        print("  %s: %.6f" % (sensor_id, rms_error))
 
     return output_dict, opt_pose_arr, J
 

calibration_estimation/src/calibration_estimation/sensors/multi_sensor.py

@@ -71,23 +71,23 @@ def sensors_from_message(self, msg):
             cur_sensors = cur_bundler.build_blocks(msg)
             sensors.extend(cur_sensors)
         else:
-            print "[%s] section doesn't exist. Skipping" % sensor_type
+            print("[%s] section doesn't exist. Skipping" % sensor_type)
 
         sensor_type = 'chains'
         if sensor_type in self._sensor_configs.keys():
             cur_bundler = chain_sensor.ChainBundler( self._sensor_configs[sensor_type] )
             cur_sensors = cur_bundler.build_blocks(msg)
             sensors.extend(cur_sensors)
         else:
-            print "[%s] section doesn't exist. Skipping" % sensor_type
+            print("[%s] section doesn't exist. Skipping" % sensor_type)
 
         sensor_type = 'rectified_cams'
         if sensor_type in self._sensor_configs.keys():
             cur_bundler = camera_chain_sensor.CameraChainBundler( self._sensor_configs[sensor_type] )
             cur_sensors = cur_bundler.build_blocks(msg)
             sensors.extend(cur_sensors)
         else:
-            print "[%s] section doesn't exist. Skipping" % sensor_type
+            print("[%s] section doesn't exist. Skipping" % sensor_type)
 
         # Store the sensor list internally
         self.sensors = sensors

calibration_launch/setup.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from distutils.core import setup
+from setuptools import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup(

calibration_launch/src/capture_executive/robot_measurement_cache.py

@@ -173,8 +173,7 @@ def request_robot_measurement(self, interval_start, interval_end, min_duration =
                m= "Failed to capture a sample from %s"%(failed_sensors[0])
            return m
 
-        # TODO: eliminate print statement or convert to rospy.log*
-        print "Received everything!"
+        rospy.loginfo("Received everything!")
 
         # Push everything into a RobotMeasurement message
         m_robot = RobotMeasurement()