[WIP] [hrpsys_ros_bridge_tutorials] fix for Noetic

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -28,6 +28,14 @@ catkin_package(
     LIBRARIES # TODO
 )
 
+# patch for Noetic
+if($ENV{ROS_DISTRO} STREQUAL "noetic")
+  execute_process(
+    COMMAND git apply noetic.patch
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/models
+  )
+endif()
+
 ################################
 ## compile_openhrp_model
 ##  Generate OpenHRP3 .xml and .conf file.
@@ -530,8 +538,10 @@ generate_default_launch_eusinterface_files("$(find openhrp3)/share/OpenHRP-3.1/s
 find_package(xacro)
 if(EXISTS ${xacro_SOURCE_PREFIX}/xacro.py)
   set(xacro_exe ${xacro_SOURCE_PREFIX}/xacro.py)
-else()
+elseif(EXISTS ${xacro_PREFIX}/share/xacro/xacro.py)
   set(xacro_exe ${xacro_PREFIX}/share/xacro/xacro.py)
+else()
+  set(xacro_exe ${xacro_PREFIX}/bin/xacro) # xacro.py is deprecated in Noetic
 endif()
 
 find_package(euscollada)

hrpsys_ros_bridge_tutorials/models/noetic.patch

@@ -0,0 +1,75 @@
+diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2G.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2G.urdf.xacro
+index f364de3..a6036a6 100644
+--- a/hrpsys_ros_bridge_tutorials/models/HRP2G.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/HRP2G.urdf.xacro
+@@ -1,6 +1,6 @@
+ <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="HRP2G" >
+   <xacro:include filename="HRP2G.urdf" />
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseS7.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseS7/multisenseS7.urdf.xacro" />
+   <joint name="hrp2_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <child  link="head_root" /> <!-- for multisense / multisense_description -->
+diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.urdf.xacro
+index 70fa4e9..64635c7 100644
+--- a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.urdf.xacro
+@@ -1,6 +1,6 @@
+ <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="HRP2JSK" >
+   <xacro:include filename="HRP2JSK.urdf" />
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL/multisenseSL.urdf.xacro" />
+   <joint name="hrp2_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <child  link="head_root" /> <!-- for multisense / multisense_description -->
+diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro
+index 42ea336..70775ec 100644
+--- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro
+@@ -2,7 +2,7 @@
+   <xacro:include filename="HRP2JSKNT_body.urdf" />
+   <xacro:include filename="HRP3HAND_L.urdf" />
+   <xacro:include filename="HRP3HAND_R.urdf" />
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL/multisenseSL.urdf.xacro" />
+   <joint name="hrp2_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <!--child  link="head" /--> <!-- for drcsim / multisense_sl_description -->
+diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2W.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2W.urdf.xacro
+index 45acd03..2c174f9 100644
+--- a/hrpsys_ros_bridge_tutorials/models/HRP2W.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/HRP2W.urdf.xacro
+@@ -1,6 +1,6 @@
+ <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="HRP2W" >
+   <xacro:include filename="HRP2W.urdf" />
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseS7.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseS7/multisenseS7.urdf.xacro" />
+   <joint name="hrp2_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <child  link="head_root" /> <!-- for multisense / multisense_description -->
+diff --git a/hrpsys_ros_bridge_tutorials/models/JAXON.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/JAXON.urdf.xacro
+index 0b78f97..cc450c5 100644
+--- a/hrpsys_ros_bridge_tutorials/models/JAXON.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/JAXON.urdf.xacro
+@@ -51,7 +51,7 @@
+   </gazebo>
+
+   <!-- HEAD -->
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL/multisenseSL.urdf.xacro" />
+   <joint name="head_tip_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <child  link="head_root" />
+diff --git a/hrpsys_ros_bridge_tutorials/models/JAXON_RED.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/JAXON_RED.urdf.xacro
+index 81a2e6b..64e26f3 100644
+--- a/hrpsys_ros_bridge_tutorials/models/JAXON_RED.urdf.xacro
++++ b/hrpsys_ros_bridge_tutorials/models/JAXON_RED.urdf.xacro
+@@ -51,7 +51,7 @@
+   </gazebo>
+
+   <!-- HEAD -->
+-  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL.urdf" />
++  <xacro:include filename="$(find multisense_description)/urdf/multisenseSL/multisenseSL.urdf.xacro" />
+   <joint name="head_tip_to_multisense" type="fixed">
+     <parent link="HEAD_LINK1" />
+     <child  link="head_root" />

hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py

@@ -22,7 +22,7 @@ def fixed_writexml(self, writer, indent="", addindent="", newl=""):
     writer.write(indent+"<" + self.tagName)
 
     attrs = self._get_attributes()
-    a_names = attrs.keys()
+    a_names = list(attrs.keys())
     a_names.sort()
 
     for a_name in a_names:
@@ -118,9 +118,9 @@ def add_object_to_projectfile(objname, url, robot=False, add_collision=True, tra
     argc = len(argvs)
 
     if argc < 4:
-        print '### usage   : <in>.xml <out>.xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0'
-        print '### example : sample_in.xml sample_out.xml  table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0'
-        print '###                             string,string      ,bool,bool,trans<3>   ,rotation<4>'
+        print('### usage   : <in>.xml <out>.xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0')
+        print('### example : sample_in.xml sample_out.xml  table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0')
+        print('###                             string,string      ,bool,bool,trans<3>   ,rotation<4>')
         exit(0)
 
     infile = argvs[1]

hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py

@@ -13,7 +13,7 @@ def publishEndEffectorOne(eef_name, eef_info, stamp):
                      stamp, eef_name, eef_info['parent'])
 
 def publishEndEffectorAll():
-    for limb_name, eef_info in g_eef_info_list.items():
+    for limb_name, eef_info in list(g_eef_info_list.items()):
         publishEndEffectorOne(limb_name+"_end_coords", eef_info, rospy.Time.now())
 
 if __name__ == "__main__":
@@ -33,7 +33,7 @@ def publishEndEffectorAll():
         parent_link_name_next = parent_link_name
         while True:
             parent_link_name = parent_link_name_next
-            if robot_model.child_map.has_key(parent_link_name):
+            if parent_link_name in robot_model.child_map:
                 parent_link_name_next = robot_model.child_map[parent_link_name][0][1]
                 if not (parent_link_name_next.startswith(limb.upper()) or parent_link_name_next.startswith(limb.lower())):
                     break
@@ -43,16 +43,16 @@ def publishEndEffectorAll():
         param_name = '~'+limb+'-end-coords'
         if rospy.has_param(param_name):
             g_eef_info_list[limb] = rospy.get_param(param_name)
-            if not g_eef_info_list[limb].has_key('translate'):
+            if 'translate' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['translate'] = [0, 0, 0, 0]
-            if not g_eef_info_list[limb].has_key('rotate'):
+            if 'rotate' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['rotate'] = [0, 0, 0, 0]
-            if not g_eef_info_list[limb].has_key('parent'):
+            if 'parent' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['parent'] = parent_link_name
     # print for debug
-    print 'eef_infos param list:'
-    for limb, eef_info in g_eef_info_list.items():
-        print '%s: %s' % (limb, eef_info)
+    print('eef_infos param list:')
+    for limb, eef_info in list(g_eef_info_list.items()):
+        print('%s: %s' % (limb, eef_info))
 
     while not rospy.is_shutdown():
         publishEndEffectorAll()

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py

@@ -14,7 +14,7 @@ def getRTCList (self):
         return self.getRTCListUnstable()
     def init (self, robotname="Robot", url=""):
         HrpsysConfigurator.init(self, robotname, url)
-        print "initialize rtc parameters"
+        print("initialize rtc parameters")
         self.setStAbcParameters()
         self.loadForceMomentOffsetFile()
 
@@ -122,7 +122,7 @@ def setStAbcParametershrp2017c(self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -219,7 +219,7 @@ def setStAbcParametershrp2016c (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -316,7 +316,7 @@ def setStAbcParametershrp2007c (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -380,7 +380,7 @@ def loadForceMomentOffsetFile (self):
         elif self.ROBOT_NAME == "HRP2JSK":
             self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_thumb_60deg_HRP2JSK")
         else:
-            print "No force moment offset file"
+            print("No force moment offset file")
 
     def __init__(self, robotname=""):
         self.ROBOT_NAME = robotname

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py

@@ -12,7 +12,7 @@ def getRTCList (self):
         return self.getRTCListUnstable()
     def init (self, robotname="SampleRobot", url=""):
         HrpsysConfigurator.init(self, robotname, url)
-        print "initialize rtc parameters"
+        print("initialize rtc parameters")
         self.setStAbcParameters()
 
     def defJointGroups (self):

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py

@@ -144,7 +144,7 @@ def setStAbcParametersSTARO (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])]
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]]
         stp.eefm_zmp_delay_time_const=[0.055, 0.055]
         #stp.eefm_cogvel_cutoff_freq = 3.18
         #stp.eefm_cogvel_cutoff_freq = 6.18 # servooff+walk
@@ -280,7 +280,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         stp.eefm_cogvel_cutoff_freq = 4.0
         stp.eefm_k1=[-1.48412,-1.48412]
         stp.eefm_k2=[-0.486727,-0.486727]
@@ -385,7 +385,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         stp.eefm_cogvel_cutoff_freq = 4.0
         # for only leg
         stp.eefm_k1=[-1.36334,-1.36334]
@@ -472,7 +472,7 @@ def setStAbcParametersURATALEG (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])]
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]]
         stp.eefm_zmp_delay_time_const=[0.055, 0.055]
         stp.eefm_cogvel_cutoff_freq = 4.25
         stp.eefm_k1=[-1.38077, -1.38077]
@@ -481,7 +481,7 @@ def setStAbcParametersURATALEG (self):
         self.st_svc.setParameter(stp)
 
     def setStAbcParametersYSTLEG (self):
-        print "Not implemented yet"
+        print("Not implemented yet")
 
     def setStAbcParametersCHIDORI (self):
         # abc setting
@@ -545,7 +545,7 @@ def setStAbcParametersCHIDORI (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]]
         # stp.eefm_zmp_delay_time_const=[0.055, 0.055]
         stp.eefm_cogvel_cutoff_freq = 4.0
         stp.eefm_k1=[-1.48412,-1.48412]
@@ -624,7 +624,7 @@ def setStAbcParametersTQLEG0 (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]),
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])]
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]]
         stp.eefm_zmp_delay_time_const=[0.055, 0.055]
         stp.eefm_cogvel_cutoff_freq = 4.25
         stp.eefm_k1=[-1.38077, -1.38077]
@@ -688,7 +688,7 @@ def setStAbcParametersTABLIS(self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         # stp.eefm_zmp_delay_time_const=[0.055, 0.055]
         stp.eefm_cogvel_cutoff_freq = 4.0
         # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori*
@@ -903,4 +903,4 @@ def loadForceMomentOffsetFile (self):
         elif self.ROBOT_NAME == "JAXON_RED":
             self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_JAXON_RED")
         else:
-            print "No force moment offset file"
+            print("No force moment offset file")