Noetic devel

calibration_estimation/package.xml

@@ -1,4 +1,4 @@
-<package>
+<package format="3">
   <name>calibration_estimation</name>
   <version>0.10.14</version>
   <description>
@@ -12,16 +12,21 @@
   <url>http://ros.org/wiki/calibration_estimation</url>
 
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+  <build_depend>calibration_msgs</build_depend>
 
-  <run_depend>calibration_msgs</run_depend>
-  <run_depend>python-matplotlib</run_depend>
-  <run_depend>python_orocos_kdl</run_depend>
-  <run_depend>python-scipy</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>rostest</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>urdfdom_py</run_depend>
-  <run_depend>visualization_msgs</run_depend>
+  <exec_depend>calibration_msgs</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-matplotlib</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-matplotlib</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python_orocos_kdl</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-pykdl</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-scipy</exec_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rostest</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>urdfdom_py</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
 
   <export>
     <architecture_independent/>

calibration_estimation/src/calibration_estimation/error_visualization.py

@@ -77,7 +77,7 @@ def usage():
         rospy.logerr("Bagfile does not exist. Exiting...")
         sys.exit(1)
 
-    loop_list = yaml.load(open(loop_list_filename))
+    loop_list = yaml.safe_load(open(loop_list_filename))
     robot_description = get_robot_description(bag_filename)
 
     config_param_name = "calibration_config"
@@ -100,10 +100,10 @@ def usage():
     cur_step = step_list[-1]
 
     # Load the resulting system definition
-    system_def_dict = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml"))
+    system_def_dict = yaml.safe_load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml"))
     system_def = UrdfParams(robot_description, system_def_dict)
-    cb_poses = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml"))
-    free_dict = yaml.load(cur_step["free_params"])
+    cb_poses = yaml.safe_load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml"))
+    free_dict = yaml.safe_load(cur_step["free_params"])
 
     sample_skip_list = rospy.get_param('calibration_skip_list', [])
     scatter_list = []

calibration_estimation/src/calibration_estimation/full_chain.py

@@ -88,9 +88,9 @@ def build_sparsity_dict(self):
         sparsity['transforms'] = {}
         sparsity['chains'] = {}
         if self.chain_id is not None:
-            for cur_transform in ( self.calc_block._before_chain_Ts + \
-                                   self.calc_block._chain._transforms.values() + \
-                                   self.calc_block._after_chain_Ts ):
+            for cur_transform in ( list(self.calc_block._before_chain_Ts) + \
+                                   list(self.calc_block._chain._transforms.values()) + \
+                                   list(self.calc_block._after_chain_Ts) ):
                 sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1]
             sparsity['chains'][self.chain_id] = {}
             link_num = self.calc_block._link_num

calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py

@@ -261,7 +261,7 @@ def update_urdf(urdf, calibrated_params):
     if sensors_name not in config.keys():
         rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
         sys.exit(1)
-    #sensors_dump = [yaml.load(x) for x in config[sensors_name].values()]
+    #sensors_dump = [yaml.safe_load(x) for x in config[sensors_name].values()]
     all_sensors_dict = build_sensor_defs(config[sensors_name])
     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
 
@@ -275,7 +275,7 @@ def update_urdf(urdf, calibrated_params):
     if 'initial_poses' in config.keys():
         if os.path.exists(config['initial_poses']):
             with open(config['initial_poses']) as f:
-                previous_pose_guesses = numpy.array(yaml.load(f))
+                previous_pose_guesses = numpy.array(yaml.safe_load(f))
         else:
             rospy.logwarn("cannot find %s" % (config['initial_poses']))
             previous_pose_guesses = numpy.zeros([msg_count,6])
@@ -348,7 +348,7 @@ def update_urdf(urdf, calibrated_params):
             output_dict = robot_params.params_to_config(robot_params.deflate())
             output_poses = previous_pose_guesses
         else:
-            free_dict = yaml.load(cur_step["free_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"

calibration_estimation/src/calibration_estimation/sensors/chain_sensor.py

@@ -135,7 +135,8 @@ def compute_cov(self, target_pts):
         cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
         cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
 
-        if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ):
+        #if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ):
+        if ( 'translation' in self._full_chain.calc_block._chain._cov_dict ):
             translation_var = self._full_chain.calc_block._chain._cov_dict['translation'];
             translation_cov = numpy.diag(translation_var*(self.get_residual_length()/3))
             cov = cov + translation_cov

calibration_estimation/src/calibration_estimation/sensors/tilting_laser_sensor.py

@@ -47,6 +47,7 @@
 import roslib; roslib.load_manifest('calibration_estimation')
 import rospy
 import numpy
+import math
 
 class TiltingLaserBundler:
     def __init__(self, valid_configs):
@@ -97,7 +98,7 @@ def compute_marginal_gamma_sqrt(self, target_pts):
         # ----- Populate Here -----
         cov = self.compute_cov(target_pts)
         gamma = matrix(zeros(cov.shape))
-        num_pts = self.get_residual_length()/3
+        num_pts = math.floor(self.get_residual_length()/3)
 
         for k in range(num_pts):
             first = 3*k

calibration_estimation/src/calibration_estimation/urdf_params.py

@@ -173,13 +173,13 @@ def configure(self, urdf, config_dict):
                     # we can handle limited rotations here
                     rot = urdf.joint_map[joint_name].origin.rotation
                     if rot != None and (sum([abs(x) for x in rot]) - rot[abs(this_config["axis"][-1])-4]) > 0.001:   
-                        print 'Joint origin is rotated, calibration will fail: ', joint_name
+                        print ('Joint origin is rotated, calibration will fail: ', joint_name)
                 elif urdf.joint_map[joint_name].joint_type == 'prismatic':
                     this_config["active_joints"].append(joint_name)
                     axis = urdf.joint_map[joint_name].axis
                     this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([1,2,3], axis)] ) )
                 elif urdf.joint_map[joint_name].joint_type != 'fixed':
-                    print 'Unknown joint type:', urdf.joint_map[joint_name].joint_type
+                    print ('Unknown joint type:', urdf.joint_map[joint_name].joint_type)
             # put a checkerboard in it's hand
             self.urdf.add_link(Link(chain_name+"_cb_link"))
             self.urdf.add_joint(Joint(chain_name+"_cb",this_config['tip'],chain_name+"_cb_link","fixed",origin=Pose([0.0,0.0,0.0],[0.0,0.0,0.0])))

calibration_estimation/test/camera_chain_sensor_unittest.py

@@ -69,7 +69,7 @@ def loadConfigList():
     link_num:     1
     after_chain:  [transformB]
 '''
-    config_dict = yaml.load(config_yaml)
+    config_dict = yaml.safe_load(config_yaml)
 
     return config_dict
 
@@ -111,7 +111,7 @@ def test_basic_no_match(self):
 
 class TestCameraChainSensor(unittest.TestCase):
     def load(self):
-        config = yaml.load('''
+        config = yaml.safe_load('''
             camera_id: camA
             chain:
               before_chain: [transformA]
@@ -121,7 +121,7 @@ def load(self):
             ''')
 
         robot_params = RobotParams()
-        robot_params.configure( yaml.load('''
+        robot_params.configure( yaml.safe_load('''
             chains:
               chainA:
                 transforms:
@@ -179,7 +179,7 @@ def test_cov(self):
                          [1, 1]])
 
         cov = sensor.compute_cov(target)
-        print "\ncov:\n", cov
+        print ("\ncov:\n", cov)
 
         self.assertAlmostEqual(cov[0,0], 1.0, 6)
         self.assertAlmostEqual(cov[1,0], 0.0, 6)

calibration_estimation/test/camera_unittest.py

@@ -70,8 +70,8 @@ def test_project_easy_1(self):
 
         result = cam.project(P_list, pts)
 
-        print ""
-        print result
+        print ("")
+        print (result)
 
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
 
@@ -91,8 +91,8 @@ def test_project_easy_2(self):
 
         result = cam.project(P_list, pts)
 
-        print ""
-        print result
+        print ("")
+        print (result)
 
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
 
@@ -114,8 +114,8 @@ def test_project_easy_3(self):
 
         result = cam.project(P_list, pts)
 
-        print ""
-        print result
+        print ("")
+        print (result)
 
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
 

calibration_estimation/test/chain_sensor_unittest.py

@@ -59,7 +59,7 @@
 
 def loadSystem():
     urdf = '''
-<robot>
+<robot name="test">
   <link name="base_link"/>
   <joint name="j0" type="fixed">
     <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -89,7 +89,7 @@ def loadSystem():
   <link name="j3_link"/>
 </robot>
 '''
-    config = yaml.load('''
+    config = yaml.safe_load('''
 sensors:
   chains:
     chainA:
@@ -162,7 +162,7 @@ def test_cov(self):
                             "boardA")
         block.update_config(robot_params)
         cov = block.compute_cov(None)
-        print cov
+        print (cov)
 
         #self.assertAlmostEqual(cov[0,0], 0.0, 6)
         #self.assertAlmostEqual(cov[1,0], 0.0, 6)
@@ -188,8 +188,8 @@ def test_update1(self):
 
         self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
 
-        print "z=\n",z
-        print "target=\n",target
+        print ("z=\n",z)
+        print ("target=\n",target)
 
         self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
         self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
@@ -213,8 +213,8 @@ def test_update2(self):
 
         self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
 
-        print "z=\n",z
-        print "target=\n",target
+        print ("z=\n",z)
+        print ("target=\n",target)
 
         self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
         self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
@@ -238,8 +238,8 @@ def test_update3(self):
 
         self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
 
-        print "z=\n",z
-        print "target=\n",target
+        print ("z=\n",z)
+        print ("target=\n",target)
 
         self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
         self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)

calibration_estimation/test/checkerboard_unittest.py

@@ -70,8 +70,8 @@ def test_deflate(self):
         param_vec = matrix([10,20], float).T
         cb.inflate( param_vec )
         result = cb.deflate()
-        print ""
-        print result
+        print ("")
+        print (result)
         self.assertAlmostEqual(numpy.linalg.norm(result - param_vec), 0.0, 6)
 
     def test_generate_points(self):
@@ -84,8 +84,8 @@ def test_generate_points(self):
                              [ 20, 20,  0,  0,-20,-20],
                              [ 0,  0,   0,  0,  0,  0],
                              [ 1,  1,   1,  1,  1,  1] ], float)
-        print ""
-        print result
+        print ("")
+        print (result)
         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
 
     def test_get_length(self):

calibration_estimation/test/full_chain_unittest.py

@@ -50,7 +50,7 @@
 
 def loadSystem1():
     urdf = '''
-<robot>
+<robot name="test">
   <link name="base_link"/>
   <joint name="j0" type="fixed">
     <origin xyz="10 0 0" rpy="0 0 0"/>
@@ -80,7 +80,7 @@ def loadSystem1():
   <link name="j3_link"/>
 </robot>
 '''
-    config = yaml.load('''
+    config = yaml.safe_load('''
 sensors:
   chains:
     chain1:
@@ -100,7 +100,7 @@ def loadSystem1():
 class TestFullChainCalcBlock(unittest.TestCase):
 
     def test_fk_1(self):
-        print ""
+        print ("")
         params = loadSystem1()
         chain = FullChainRobotParams('chain1', 'j3_link')
         chain.update_config(params)
@@ -111,11 +111,11 @@ def test_fk_1(self):
                                   [ 0, 1, 0, 0],
                                   [ 0, 0, 1,20],
                                   [ 0, 0, 0, 1]], float )
-        print result
+        print (result)
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
 
     def test_fk_2(self):
-        print ""
+        print ("")
         params = loadSystem1()
         chain = FullChainRobotParams('chain1', 'j3_link')
         chain.update_config(params)
@@ -126,11 +126,11 @@ def test_fk_2(self):
                                   [ 1, 0, 0, 0],
                                   [ 0, 0, 1,20],
                                   [ 0, 0, 0, 1]], float )
-        print result
+        print (result)
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
 
     def test_fk_partial(self):
-        print ""
+        print ("")
         params = loadSystem1()
         chain = FullChainRobotParams('chain1', 'j1_link')
         chain.update_config(params)
@@ -141,7 +141,7 @@ def test_fk_partial(self):
                                   [ 0, 1, 0, 0],
                                   [ 0, 0, 1, 0],
                                   [ 0, 0, 0, 1]], float )
-        print result
+        print (result)
         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)