k-okada's local patch

hrpsys_ros_bridge/cmake/compile_robot_model.cmake

@@ -92,7 +92,9 @@ macro(compile_openhrp_model wrlfile)
   # use controller_config_converter
   get_controller_config_converter(_controller_config_converter)
   # get path to collada_to_urdf
-  get_collada_to_urdf(_collada_to_urdf_exe)
+  if(NOT $ENV{ROS_DISTRO} STREQUAL "noetic")
+    get_collada_to_urdf(_collada_to_urdf_exe) # not working on Noetic : https://github.com/ros/collada_urdf/issues/43
+  endif()
 
   # ready to go
   message(STATUS " wrl file ${_wrlfile}")
@@ -123,9 +125,15 @@ macro(compile_openhrp_model wrlfile)
   # output urdf files (collada -> urdf)
   set(_mesh_dir "${_workdir}/${_name}_meshes")
   set(_mesh_prefix "package://${PROJECT_NAME}/models/${_name}_meshes")
-  add_custom_command(OUTPUT ${_urdffile} ${_mesh_dir}
-    COMMAND ${_collada_to_urdf_exe} ${_daefile} --output_file ${_urdffile} -G -A --mesh_output_dir ${_mesh_dir} --mesh_prefix ${_mesh_prefix}
-    DEPENDS ${_sname}_${PROJECT_NAME}_compile_dae)
+  if($ENV{ROS_DISTRO} STREQUAL "noetic")
+    add_custom_command(OUTPUT ${_urdffile} ${_mesh_dir}
+      COMMAND rosrun collada_urdf collada_to_urdf ${_daefile} --output_file ${_urdffile} -G -A --mesh_output_dir ${_mesh_dir} --mesh_prefix ${_mesh_prefix}
+      DEPENDS ${_sname}_${PROJECT_NAME}_compile_dae)
+  else()
+    add_custom_command(OUTPUT ${_urdffile} ${_mesh_dir}
+      COMMAND ${_collada_to_urdf_exe} ${_daefile} --output_file ${_urdffile} -G -A --mesh_output_dir ${_mesh_dir} --mesh_prefix ${_mesh_prefix}
+      DEPENDS ${_sname}_${PROJECT_NAME}_compile_dae)
+  endif()
   add_custom_target(${_sname}_${PROJECT_NAME}_compile_urdf DEPENDS ${_urdffile} ${_mesh_dir})
   list(APPEND ${_sname}_${PROJECT_NAME}_compile_all_target ${_sname}_${PROJECT_NAME}_compile_urdf)
 

hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

@@ -73,7 +73,7 @@
 
   <group if="$(arg INSTALL_ROBOT_DESCRIPTION)" >
     <param name="robot_description" textfile="$(arg COLLADA_FILE)" />
-    <node pkg="robot_state_publisher" type="state_publisher" name="hrpsys_state_publisher" />
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="hrpsys_state_publisher" />
   </group>
 
   <!-- diagnostics -->

hrpsys_ros_bridge/package.xml

@@ -1,4 +1,4 @@
-<package>
+<package format="3">
   <name>hrpsys_ros_bridge</name>
   <version>1.4.3</version>
   <description>hrpsys_ros_bridge package provides basic functionalities to bind
@@ -45,7 +45,8 @@
   <build_depend>pr2_controllers_msgs</build_depend>
   <build_depend>pr2_msgs</build_depend>
   <build_depend>procps</build_depend>
-  <build_depend>python-rosdep</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 2">python-rosdep</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 3">python3-rosdep</build_depend>
   <build_depend>robot_pose_ekf</build_depend>
   <build_depend>robot_state_publisher</build_depend>
   <build_depend>rosbuild</build_depend>
@@ -63,36 +64,40 @@
   <build_depend>tf</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>python-yaml</build_depend> <!-- even on noetic, rtc-template uses python2 -->
+  <build_depend condition="$ROS_PYTHON_VERSION == 3">python3-yaml</build_depend>
 
-  <run_depend>actionlib</run_depend>
-  <run_depend>camera_info_manager</run_depend>
-  <run_depend>collada_urdf</run_depend>
-  <run_depend>control_msgs</run_depend>
-  <run_depend>diagnostic_aggregator</run_depend>
-  <run_depend>diagnostic_msgs</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>ipython</run_depend>
-  <run_depend>hrpsys</run_depend>
-  <run_depend>hrpsys_tools</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>pr2_controllers_msgs</run_depend>
-  <run_depend>pr2_msgs</run_depend>
-  <run_depend>python-psutil</run_depend>
-  <run_depend>robot_pose_ekf</run_depend>
-  <run_depend>robot_state_publisher</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rosnode</run_depend>
-  <run_depend>rostest</run_depend>
-  <run_depend>rqt_gui</run_depend>
-  <run_depend>rqt_gui_py</run_depend>
-  <run_depend>rqt_robot_dashboard</run_depend>
-  <run_depend>rqt_robot_monitor</run_depend>
-  <run_depend>rtmbuild</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>camera_info_manager</exec_depend>
+  <exec_depend>collada_urdf</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>diagnostic_aggregator</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">ipython</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">ipython3</exec_depend>
+  <exec_depend>hrpsys</exec_depend>
+  <exec_depend>hrpsys_tools</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>pr2_controllers_msgs</exec_depend>
+  <exec_depend>pr2_msgs</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-psutil</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-psutil</exec_depend>
+  <exec_depend>robot_pose_ekf</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rosnode</exec_depend>
+  <exec_depend>rostest</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+  <exec_depend>rqt_robot_dashboard</exec_depend>
+  <exec_depend>rqt_robot_monitor</exec_depend>
+  <exec_depend>rtmbuild</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <!-- <test_depend>hrpsys</test_depend> -->
   <!-- <test_depend>hrpsys_tools</test_depend> -->
   <!-- <test_depend>openrtm_tools</test_depend> -->

hrpsys_ros_bridge/scripts/collision_state.py

@@ -196,12 +196,12 @@ def collision_state() :
         while not rospy.is_shutdown():
             try :
                 collision_state()
-            except (omniORB.CORBA.OBJECT_NOT_EXIST, omniORB.CORBA.TRANSIENT, omniORB.CORBA.BAD_PARAM, omniORB.CORBA.COMM_FAILURE), e :
+            except (omniORB.CORBA.OBJECT_NOT_EXIST, omniORB.CORBA.TRANSIENT, omniORB.CORBA.BAD_PARAM, omniORB.CORBA.COMM_FAILURE) as e :
                 rospy.logerr("[collision_state.py] catch exception, restart rtc_init.\nMake sure collision_pair is set in .conf file. See https://github.com/start-jsk/rtmros_common/issues/247\nOriginal exception: {}".format(e))
                 time.sleep(2)
                 rtc_init()
             except Exception as e:
-                print "[collision_state.py] catch exception", e
+                print("[collision_state.py] catch exception", e)
             r.sleep()
 
     except rospy.ROSInterruptException: pass

hrpsys_ros_bridge/scripts/controller_config_converter.py

@@ -4,7 +4,7 @@
 
 if __name__ == '__main__':
     if len(sys.argv) < 1:
-        print 'usage : %s [ input.yaml ] output.yaml'%sys.argv[0]
+        print('usage : %s [ input.yaml ] output.yaml'%sys.argv[0])
 
 
     if len(sys.argv) > 2:
@@ -17,37 +17,37 @@
 
     of = open(outputfile, 'w')
 
-    print >> of, '##'
-    print >> of, '## auto generated file'
-    print >> of, '##'
-    print >> of, '##controller_configuration:'
-    print >> of, '##  - group_name: <joint group name>'
-    print >> of, '##    controller_name: <name of joint trajectory topic name>'
-    print >> of, '##    joint_list: ## list of using joints'
-    print >> of, '##      - <joint_name>'
+    print('##', file=of)
+    print('## auto generated file', file=of)
+    print('##', file=of)
+    print('##controller_configuration:', file=of)
+    print('##  - group_name: <joint group name>', file=of)
+    print('##    controller_name: <name of joint trajectory topic name>', file=of)
+    print('##    joint_list: ## list of using joints', file=of)
+    print('##      - <joint_name>', file=of)
 
     if inputfile == "":
         of.close()
         exit(0)
 
     lst = yaml.load(open(inputfile).read())
 
-    print >> of, 'controller_configuration:'
+    print('controller_configuration:', file=of)
 
-    for limb in lst.keys():
+    for limb in list(lst.keys()):
         if limb == 'sensors':
             continue
         if limb.endswith('-coords') or limb.endswith('-vector'):
             continue
         if limb == 'links' or limb == 'replace_xmls':
             continue
-        jlst = [j.keys()[0] for j in lst[limb] if isinstance(j, dict) and isinstance(j.values()[0], str)]
+        jlst = [list(j.keys())[0] for j in lst[limb] if isinstance(j, dict) and isinstance(list(j.values())[0], str)]
         if len(jlst) > 0:
-            print >> of, '  - group_name: ' + limb
-            print >> of, '    controller_name: /' + limb + '_controller'
-            print >> of, '    joint_list:'
+            print('  - group_name: ' + limb, file=of)
+            print('    controller_name: /' + limb + '_controller', file=of)
+            print('    joint_list:', file=of)
             for j in jlst:
-                print >> of, '      - ' + j
+                print('      - ' + j, file=of)
 
     of.close()
     exit(0)

hrpsys_ros_bridge/scripts/diagnostics.py

@@ -1,10 +1,10 @@
 #!/usr/bin/env python
 try:
     import hrpsys_ros_bridge
-    print "-- this is catikin environment"
+    print("-- this is catikin environment")
 except:
     import roslib; roslib.load_manifest('hrpsys_ros_bridge')
-    print "-- load manifest for rosbuild environment"
+    print("-- load manifest for rosbuild environment")
 import rospy
 import time
 import copy

hrpsys_ros_bridge/scripts/emergence_stop.py

@@ -22,7 +22,7 @@ def joystick_callback(msg) :
             servo(OpenHRP_RobotHardwareService_servoRequest("all",1));
             time.sleep(1)
             power(OpenHRP_RobotHardwareService_powerRequest("all",1))
-        except rospy.ServiceException, e:
+        except rospy.ServiceException as e:
             rospy.logerr("Failed to put the hrpsys in serv off mode: service call failed with error: %s"%(e))
 
 if __name__ == '__main__':

hrpsys_ros_bridge/scripts/hrpsys-bridge-connect.py

@@ -34,6 +34,6 @@ def myinitCORBA():
 time.sleep(10)
 
 for node1, port1, node2, port2 in conn:
-    print [node1,port1,node2,port2]
+    print([node1,port1,node2,port2])
     connectPorts(findRTC(node1,rootnc).port(port1),
                  findRTC(node2,rootnc).port(port2), subscription='new')

hrpsys_ros_bridge/scripts/hrpsys_profile.py

@@ -28,7 +28,7 @@ def rtc_init () :
     while ms == None :
         time.sleep(1);
         ms = rtm.findRTCmanager(rtm.nshost)
-        print "[hrpsys_profile.py] wait for RTCmanager : ",ms
+        print("[hrpsys_profile.py] wait for RTCmanager : ",ms)
 
 def hrpsys_profile() :
     global ms, rh, eps
@@ -92,10 +92,10 @@ def hrpsys_profile() :
         while not rospy.is_shutdown():
             try :
                 hrpsys_profile()
-            except (omniORB.CORBA.TRANSIENT, omniORB.CORBA.BAD_PARAM, omniORB.CORBA.COMM_FAILURE), e :
-                print "[hrpsys_profile.py] catch exception", e
+            except (omniORB.CORBA.TRANSIENT, omniORB.CORBA.BAD_PARAM, omniORB.CORBA.COMM_FAILURE) as e :
+                print("[hrpsys_profile.py] catch exception", e)
                 rtc_init()
-            except Exception, e:
+            except Exception as e:
                 pass
 
             r.sleep()

hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py

@@ -16,50 +16,50 @@
 program_name = '[sensor_ros_bridge_connect.py] '
 
 def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo, sh, es, rfu, subscription_type = "new", push_policy = 'all', push_rate = 50.0):
-    print program_name, "connecSensorRosBridgePort(", url, ",", rh.name(), ")"
+    print(program_name, "connecSensorRosBridgePort(", url, ",", rh.name(), ")")
     for sen in hcf.getSensors(url):
-        print program_name, "sensor(name: ", sen.name, ", type:", sen.type, ")"
+        print(program_name, "sensor(name: ", sen.name, ", type:", sen.type, ")")
         if sen.type in ['Acceleration', 'RateGyro', 'Force']:
-            print program_name, "rh.port(", sen.name, ") = ", rh.port(sen.name)
+            print(program_name, "rh.port(", sen.name, ") = ", rh.port(sen.name))
             if rh.port(sen.name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
-                print program_name, "connect ", sen.name, rh.port(sen.name).get_port_profile().name, bridge.port(sen.name).get_port_profile().name
+                print(program_name, "connect ", sen.name, rh.port(sen.name).get_port_profile().name, bridge.port(sen.name).get_port_profile().name)
                 connectPorts(rh.port(sen.name), bridge.port(sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
                 if sen.type == 'Force' and rmfo != None:
-                    print program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name
+                    print(program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name)
                     connectPorts(rmfo.port("off_" + sen.name), bridge.port("off_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for abs forces
                 if sen.type == 'Force' and bridge.port("ref_" + sen.name): # for reference forces
                     if rfu != None and rfu.port("ref_"+sen.name+"Out"):
-                        print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
+                        print(program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name)
                         connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
                     elif es != None and es.port(sen.name+"Out"):
-                        print program_name, "connect ", sen.name, es.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
+                        print(program_name, "connect ", sen.name, es.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name)
                         connectPorts(es.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
                     elif sh.port(sen.name+"Out"):
-                        print program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
+                        print(program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name)
                         connectPorts(sh.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
         else:
             continue
     if vs != None:
-        for vfp in filter(lambda x : str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, vs.ports.keys()):
-            print program_name, "connect ", vfp, vs.port(vfp).get_port_profile().name, bridge.port(vfp).get_port_profile().name
+        for vfp in filter(lambda x : str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, list(vs.ports.keys())):
+            print(program_name, "connect ", vfp, vs.port(vfp).get_port_profile().name, bridge.port(vfp).get_port_profile().name)
             connectPorts(vs.port(vfp), bridge.port(vfp), subscription_type, rate=push_rate, pushpolicy=push_policy)
-            print program_name, "connect ", vfp, sh.port(vfp+"Out").get_port_profile().name, bridge.port("ref_"+vfp).get_port_profile().name
+            print(program_name, "connect ", vfp, sh.port(vfp+"Out").get_port_profile().name, bridge.port("ref_"+vfp).get_port_profile().name)
             connectPorts(sh.port(vfp+"Out"), bridge.port("ref_" + vfp), subscription_type, rate=push_rate, pushpolicy=push_policy) # for reference forces
 
 def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerhost, subscription_type, push_policy, push_rate):
-    print program_name, "initSensorRosBridgeConnection"
+    print(program_name, "initSensorRosBridgeConnection")
     hcf.waitForModelLoader()
     hcf.waitForRTCManagerAndRoboHardware(simulator_name, managerhost)
     bridge = rtm.findRTC(rosbridge_name)
     while bridge == None :
         time.sleep(1);
         bridge = rtm.findRTC(rosbridge_name)
-        print program_name, " wait for ", rosbridge_name, " : ",bridge
+        print(program_name, " wait for ", rosbridge_name, " : ",bridge)
     sh=rtm.findRTC('sh')
     while sh == None :
         time.sleep(1);
         sh=rtm.findRTC('sh')
-        print program_name, " wait for 'sh' : ",sh
+        print(program_name, " wait for 'sh' : ",sh)
 
     while sh.isInactive(): ## wait for initalizing all components
         time.sleep(1)
@@ -72,10 +72,10 @@ def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerho
     connecSensorRosBridgePort(url, hcf.rh, bridge, vs, rmfo, sh, es, rfu, subscription_type, push_policy, push_rate)
 
 if __name__ == '__main__':
-    print program_name, "start"
+    print(program_name, "start")
     hcf=HrpsysConfigurator(program_name)
     if len(sys.argv) > 3 :
         initSensorRosBridgeConnection(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5], sys.argv[6], sys.argv[7])
     else :
-        print program_name, " requires url, simulator_name, rosbridge_name"
+        print(program_name, " requires url, simulator_name, rosbridge_name")
 

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

@@ -1114,12 +1114,13 @@ void HrpsysSeqStateROSBridge::updateImu(tf::Transform &base, bool is_base_valid,
       if (not_nan) {
         std::map<std::string, SensorInfo>::const_iterator its = sensor_info.begin();
         while ( its != sensor_info.end() ) {
-          if ( (*its).second.type_name == "RateGyro" ) {
+          if ( (*its).second.type_name == "RateGyro" && base_time > last_updated_imu_tf_stamp ) {
             boost::mutex::scoped_lock lock(tf_mutex);
             tf::StampedTransform imu_floor_stamped_transform(inv, base_time, (*its).first, "imu_floor");
             geometry_msgs::TransformStamped imu_floor_tf;
             tf::transformStampedTFToMsg(imu_floor_stamped_transform, imu_floor_tf);
             tf_transforms.push_back(imu_floor_tf);
+            last_updated_imu_tf_stamp = base_time;
             break;
           }
           ++its;
@@ -1159,9 +1160,10 @@ void HrpsysSeqStateROSBridge::updateSensorTransform(const ros::Time &stamp)
       ++its;
     }
   }
-  if (!sensor_tf_buffer.empty()) {
+  if (!sensor_tf_buffer.empty() && stamp > last_updated_sensor_tf_stamp) {
     boost::mutex::scoped_lock lock(tf_mutex);
     tf_transforms.insert(tf_transforms.end(), sensor_tf_buffer.begin(), sensor_tf_buffer.end());
+    last_updated_sensor_tf_stamp = stamp;
   }
 }
 

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h

@@ -100,9 +100,11 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void updateOdometry(const hrp::Vector3 &trans, const hrp::Matrix33 &R, const ros::Time &stamp);
 
   // imu relatives
+  ros::Time last_updated_imu_tf_stamp;
   void updateImu(tf::Transform &base, bool is_base_valid, const ros::Time &stamp);
 
   // sensor relatives
+  ros::Time last_updated_sensor_tf_stamp;
   void updateSensorTransform(const ros::Time &stamp);
   std::map<std::string, geometry_msgs::Transform> sensor_transformations;
   boost::mutex sensor_transformation_mutex;