diff --git a/hrpsys_ros_bridge/cmake/compile_robot_model.cmake b/hrpsys_ros_bridge/cmake/compile_robot_model.cmake index f8f29b296..da33dbac2 100644 --- a/hrpsys_ros_bridge/cmake/compile_robot_model.cmake +++ b/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) @@ -140,8 +148,13 @@ macro(compile_openhrp_model wrlfile) if(_controller_config) # output controller config (yaml -> config) if yaml is not found write dummy files + find_package(PythonInterp QUIET) + if(NOT PYTHONINTERP_FOUND) + find_package(Python REQUIRED) + set(PYTHON_EXECUTABLE ${Python_EXECUTABLE}) + endif() # enable to execute with correct python version (cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5) add_custom_command(OUTPUT ${_controller_config} - COMMAND ${_controller_config_converter} ${_yamlfile} ${_controller_config} + COMMAND ${PYTHON_EXECUTABLE} ${_controller_config_converter} ${_yamlfile} ${_controller_config} DEPENDS ${_yamlfile}) add_custom_target(${_sname}_${PROJECT_NAME}_compile_conf DEPENDS ${_controller_config}) list(APPEND ${_sname}_${PROJECT_NAME}_compile_all_target ${_sname}_${PROJECT_NAME}_compile_conf) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 6963b96ac..8b7721db4 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -73,7 +73,7 @@ - + diff --git a/hrpsys_ros_bridge/package.xml b/hrpsys_ros_bridge/package.xml index c841e96a8..83556333e 100644 --- a/hrpsys_ros_bridge/package.xml +++ b/hrpsys_ros_bridge/package.xml @@ -1,4 +1,4 @@ - + hrpsys_ros_bridge 1.4.3 hrpsys_ros_bridge package provides basic functionalities to bind @@ -45,7 +45,8 @@ pr2_controllers_msgs pr2_msgs procps - python-rosdep + python-rosdep + python3-rosdep robot_pose_ekf robot_state_publisher rosbuild @@ -63,36 +64,40 @@ tf visualization_msgs geometry_msgs + python-yaml + python3-yaml - actionlib - camera_info_manager - collada_urdf - control_msgs - diagnostic_aggregator - diagnostic_msgs - dynamic_reconfigure - ipython - hrpsys - hrpsys_tools - image_transport - nav_msgs - pr2_controllers_msgs - pr2_msgs - python-psutil - robot_pose_ekf - robot_state_publisher - roscpp - rosnode - rostest - rqt_gui - rqt_gui_py - rqt_robot_dashboard - rqt_robot_monitor - rtmbuild - sensor_msgs - tf - visualization_msgs - geometry_msgs + actionlib + camera_info_manager + collada_urdf + control_msgs + diagnostic_aggregator + diagnostic_msgs + dynamic_reconfigure + ipython + ipython3 + hrpsys + hrpsys_tools + image_transport + nav_msgs + pr2_controllers_msgs + pr2_msgs + python-psutil + python3-psutil + robot_pose_ekf + robot_state_publisher + roscpp + rosnode + rostest + rqt_gui + rqt_gui_py + rqt_robot_dashboard + rqt_robot_monitor + rtmbuild + sensor_msgs + tf + visualization_msgs + geometry_msgs diff --git a/hrpsys_ros_bridge/scripts/collision_state.py b/hrpsys_ros_bridge/scripts/collision_state.py index c2e0e4ef0..db437aaa9 100755 --- a/hrpsys_ros_bridge/scripts/collision_state.py +++ b/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 diff --git a/hrpsys_ros_bridge/scripts/controller_config_converter.py b/hrpsys_ros_bridge/scripts/controller_config_converter.py index 7dfd56a9c..adc562fdd 100755 --- a/hrpsys_ros_bridge/scripts/controller_config_converter.py +++ b/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,14 +17,14 @@ of = open(outputfile, 'w') - print >> of, '##' - print >> of, '## auto generated file' - print >> of, '##' - print >> of, '##controller_configuration:' - print >> of, '## - group_name: ' - print >> of, '## controller_name: ' - print >> of, '## joint_list: ## list of using joints' - print >> of, '## - ' + print('##', file=of) + print('## auto generated file', file=of) + print('##', file=of) + print('##controller_configuration:', file=of) + print('## - group_name: ', file=of) + print('## controller_name: ', file=of) + print('## joint_list: ## list of using joints', file=of) + print('## - ', file=of) if inputfile == "": of.close() @@ -32,22 +32,22 @@ 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) diff --git a/hrpsys_ros_bridge/scripts/diagnostics.py b/hrpsys_ros_bridge/scripts/diagnostics.py index 3e9348f38..9dee8b642 100755 --- a/hrpsys_ros_bridge/scripts/diagnostics.py +++ b/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 diff --git a/hrpsys_ros_bridge/scripts/emergence_stop.py b/hrpsys_ros_bridge/scripts/emergence_stop.py index a5043648a..066bd5c55 100755 --- a/hrpsys_ros_bridge/scripts/emergence_stop.py +++ b/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__': diff --git a/hrpsys_ros_bridge/scripts/hrpsys-bridge-connect.py b/hrpsys_ros_bridge/scripts/hrpsys-bridge-connect.py index bf1a2b0dc..b08042257 100755 --- a/hrpsys_ros_bridge/scripts/hrpsys-bridge-connect.py +++ b/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') diff --git a/hrpsys_ros_bridge/scripts/hrpsys_profile.py b/hrpsys_ros_bridge/scripts/hrpsys_profile.py index 919839faa..030c38ec4 100755 --- a/hrpsys_ros_bridge/scripts/hrpsys_profile.py +++ b/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() diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py index c701fdc80..66851b847 100755 --- a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py +++ b/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") diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index a37e6923b..d53199a5d 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/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::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; } } diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 4068ca1a3..d471cd853 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/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 sensor_transformations; boost::mutex sensor_transformation_mutex; diff --git a/hrpsys_ros_bridge/src/hrpsys_ros_bridge/hrpsys_dashboard.py b/hrpsys_ros_bridge/src/hrpsys_ros_bridge/hrpsys_dashboard.py index 026238030..02445c15e 100755 --- a/hrpsys_ros_bridge/src/hrpsys_ros_bridge/hrpsys_dashboard.py +++ b/hrpsys_ros_bridge/src/hrpsys_ros_bridge/hrpsys_dashboard.py @@ -70,15 +70,15 @@ def on_download(self): try: hrpsys_save = rospy.ServiceProxy("/DataLoggerServiceROSBridge/save", OpenHRP_DataLoggerService_save ) name = "/tmp/rtclog-" + time.strftime("%Y%m%d%H%M%S") - print "Writing log data to ",name + print("Writing log data to ",name) hrpsys_save(OpenHRP_DataLoggerService_saveRequest(name)) - print "Done writing",name - except rospy.ServiceException, e: + print("Done writing",name) + except rospy.ServiceException as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", "Failed to save rtcd log: service call failed with error: %s"%(e), QMessageBox.Ok, self.parent()) mb.exec_() - except Exception, e: + except Exception as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", str(e), QMessageBox.Ok, self.parent()) mb.exec_() @@ -147,7 +147,7 @@ def on_power_on(self): try: power = rospy.ServiceProxy("/RobotHardwareServiceROSBridge/power", OpenHRP_RobotHardwareService_power ) power(OpenHRP_RobotHardwareService_powerRequest("all",0)) - except Exception, e: + except Exception as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", "Failed to power on: %s"%(e), QMessageBox.Ok, self.parent()) @@ -156,7 +156,7 @@ def on_power_off(self): try: power = rospy.ServiceProxy("/RobotHardwareServiceROSBridge/power", OpenHRP_RobotHardwareService_power ) power(OpenHRP_RobotHardwareService_powerRequest("all",1)) - except Exception, e: + except Exception as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", "Failed to power off: %s"%(e), QMessageBox.Ok, self.parent()) @@ -195,7 +195,7 @@ def on_servo_on(self): actual(OpenHRP_StateHolderService_goActualRequest()) time.sleep(2) servo(OpenHRP_RobotHardwareService_servoRequest("all",0)) - except Exception, e: + except Exception as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", "Failed to servo on: %s"%(e), QMessageBox.Ok, self.parent()) @@ -212,7 +212,7 @@ def on_servo_off(self): servo(OpenHRP_RobotHardwareService_servoRequest("all", 1)) time.sleep(1) power(OpenHRP_RobotHardwareService_powerRequest("all",1)) - except Exception, e: + except Exception as e: mb = QMessageBox(QMessageBox.NoIcon, "Error", "Failed to servo off: %s"%(e), QMessageBox.Ok, self.parent()) @@ -335,7 +335,7 @@ def execHrpsysConfiguratorCommand(command_str): try: exec command_str except: - print >>sys.stderr, "Button execution for ", command_str, "failed, perhaps because of CORBA connection. Retry it once after resetting HrpsysConfigurator." + print("Button execution for ", command_str, "failed, perhaps because of CORBA connection. Retry it once after resetting HrpsysConfigurator.", file=sys.stderr) hcf=setupHrpsysConfigurator() exec command_str diff --git a/hrpsys_ros_bridge/test/sample4legrobot_hrpsys_config.py b/hrpsys_ros_bridge/test/sample4legrobot_hrpsys_config.py index f59daf878..56d51ad73 100755 --- a/hrpsys_ros_bridge/test/sample4legrobot_hrpsys_config.py +++ b/hrpsys_ros_bridge/test/sample4legrobot_hrpsys_config.py @@ -10,7 +10,7 @@ def getRTCList (self): return self.getRTCListUnstable() def init (self, robotname="Sample4LegRobot", url=""): HrpsysConfigurator.init(self, robotname, url) - print "initialize rtc parameters" + print("initialize rtc parameters") self.setStAbcParameters() def defJointGroups (self): @@ -54,7 +54,7 @@ def setStAbcParameters (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp_org.eefm_leg_rear_margin, stp_org.eefm_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp_org.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]] stp_org.eefm_k1=[-1.39899,-1.39899] stp_org.eefm_k2=[-0.386111,-0.386111] stp_org.eefm_k3=[-0.175068,-0.175068] diff --git a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py index b10bbbcd2..df1dc213c 100755 --- a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py +++ b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py @@ -10,7 +10,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): @@ -48,7 +48,7 @@ def setStAbcParameters (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), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]] stp.eefm_leg_inside_margin=tmp_leg_inside_margin stp.eefm_leg_outside_margin=tmp_leg_outside_margin stp.eefm_leg_front_margin=tmp_leg_front_margin diff --git a/hrpsys_ros_bridge/test/test-samplerobot-hcf.py b/hrpsys_ros_bridge/test/test-samplerobot-hcf.py index 49183e896..ef5d695b8 100755 --- a/hrpsys_ros_bridge/test/test-samplerobot-hcf.py +++ b/hrpsys_ros_bridge/test/test-samplerobot-hcf.py @@ -48,7 +48,7 @@ def test_hcf_init(self): hcf.init("SampleRobot(Robot)0", model_url) assert(True) except AttributeError as e: - print >> sys.stderr, "[test-samplerobot-hcf.py] catch exception", e + print("[test-samplerobot-hcf.py] catch exception", e, file=sys.stderr) assert(False) #unittest.main() diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py index bf36b46be..31f576759 100755 --- a/hrpsys_ros_bridge/test/test-samplerobot.py +++ b/hrpsys_ros_bridge/test/test-samplerobot.py @@ -191,7 +191,7 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co ## wait for joint_states updates rospy.sleep(1) - current_positions = dict(zip(self.joint_states.name, self.joint_states.position)) + current_positions = dict(list(zip(self.joint_states.name, self.joint_states.position))) # goal goal.trajectory.header.stamp = rospy.get_rostime() @@ -221,7 +221,7 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co ## wait for joint_states updates rospy.sleep(1) - current_positions = dict(zip(self.joint_states.name, self.joint_states.position)) + current_positions = dict(list(zip(self.joint_states.name, self.joint_states.position))) goal.trajectory.points = [] ## add current position @@ -242,7 +242,7 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co ## wait for update joint_states rospy.sleep(1) - current_positions = dict(zip(self.joint_states.name, self.joint_states.position)) + current_positions = dict(list(zip(self.joint_states.name, self.joint_states.position))) goal_angles = [ 180.0 / math.pi * current_positions[x] for x in goal.trajectory.joint_names] rospy.logwarn(goal_angles) rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)))) diff --git a/hrpsys_tools/scripts/hrpsys_tools_config.py b/hrpsys_tools/scripts/hrpsys_tools_config.py index dc7ea18a7..56936af67 100755 --- a/hrpsys_tools/scripts/hrpsys_tools_config.py +++ b/hrpsys_tools/scripts/hrpsys_tools_config.py @@ -38,9 +38,9 @@ hcf.waitForRTCManagerAndRoboHardware(robotname=sys.argv[1]) sys.argv = [sys.argv[0]] + sys.argv[2:] hcf.findComps() - print >> sys.stderr, "[hrpsys.py] #\n[hrpsys.py] # use `hcf` as robot interface, for example hcf.getJointAngles()\n[hrpsys.py] #" + print("[hrpsys.py] #\n[hrpsys.py] # use `hcf` as robot interface, for example hcf.getJointAngles()\n[hrpsys.py] #", file=sys.stderr) while args.c != None: - print >> sys.stderr, ">>", args.c[0] + print(">>", args.c[0], file=sys.stderr) exec(args.c[0]) args.c.pop(0) if not (args.i and '__IPYTHON__' in vars(__builtins__)): diff --git a/hrpsys_tools/test/test-pa10.py b/hrpsys_tools/test/test-pa10.py index f5502fde7..11afc9626 100755 --- a/hrpsys_tools/test/test-pa10.py +++ b/hrpsys_tools/test/test-pa10.py @@ -12,7 +12,7 @@ def test(self): execfile(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../scripts", "hrpsys_tools_config.py"), globals(), local_dict) hcf = local_dict['hcf'] hcf.setJointAngles([10,10,10,10,10,10,10,10,10], 1) - print hcf.getJointAngles() + print(hcf.getJointAngles()) self.assertTrue("OK") if __name__ == '__main__': diff --git a/openrtm_tools/scripts/rtmlaunch b/openrtm_tools/scripts/rtmlaunch index f5f010649..c2417d1e5 100755 --- a/openrtm_tools/scripts/rtmlaunch +++ b/openrtm_tools/scripts/rtmlaunch @@ -34,9 +34,9 @@ arg_remap = names.load_mappings(sys.argv) if 'corbaport' in arg_remap: port_number = int(arg_remap['corbaport']) - print "\033[34m[rtmlaunch] user defined corbaport:=", port_number, "will be used\033[0m" + print(("\033[34m[rtmlaunch] user defined corbaport:=", port_number, "will be used\033[0m")) else: - print "\033[34m[rtmlaunch] default corbaport:=", port_number, "will be used\033[0m" + print(("\033[34m[rtmlaunch] default corbaport:=", port_number, "will be used\033[0m")) p = rtmstart.start_cosname(cosnames, port_number) @@ -45,6 +45,6 @@ try: finally: # if omniName is invoked from this script, stop this if p : - print "\033[34m[rtmlaunch] terminate", cosnames, "at port", port_number, "\033[0m" + print(("\033[34m[rtmlaunch] terminate", cosnames, "at port", port_number, "\033[0m")) p.terminate() diff --git a/openrtm_tools/scripts/rtmlaunch.py b/openrtm_tools/scripts/rtmlaunch.py index b1192dd32..fc52e4c06 100755 --- a/openrtm_tools/scripts/rtmlaunch.py +++ b/openrtm_tools/scripts/rtmlaunch.py @@ -1,11 +1,12 @@ #!/usr/bin/env python +from __future__ import print_function from openrtm_tools import rtmlaunchlib import signal, sys def signal_handler(signum, frame): - sigdict = dict((k, v) for v, k in signal.__dict__.iteritems() if v.startswith('SIG')) - print >>sys.stderr, "\033[33m[rtmlaunch] Catch signal %r, exitting...\033[0m"%(sigdict[signum]) + sigdict = dict((k, v) for v, k in signal.__dict__.items() if v.startswith('SIG')) + print("\033[33m[rtmlaunch] Catch signal %r, exitting...\033[0m"%(sigdict[signum]), file=sys.stderr) sys.exit(0) if __name__ == '__main__': diff --git a/openrtm_tools/scripts/rtmtest b/openrtm_tools/scripts/rtmtest index d810dad27..1fb106de5 100755 --- a/openrtm_tools/scripts/rtmtest +++ b/openrtm_tools/scripts/rtmtest @@ -13,6 +13,6 @@ try: finally: # if omniName is invoked from this script, stop this if p : - print "\033[34m[rtmtest] terminate", cosnames, "at port", port_number, "\033[0m" + print(("\033[34m[rtmtest] terminate", cosnames, "at port", port_number, "\033[0m")) p.terminate() diff --git a/openrtm_tools/src/openrtm_tools/rtmlaunchlib.py b/openrtm_tools/src/openrtm_tools/rtmlaunchlib.py index 705b7393c..83bcf18ba 100644 --- a/openrtm_tools/src/openrtm_tools/rtmlaunchlib.py +++ b/openrtm_tools/src/openrtm_tools/rtmlaunchlib.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function import roslib roslib.load_manifest('openrtm_tools') @@ -32,7 +33,7 @@ def wait_component(cmd_path, tree): node = alive_component(path, tree) if not node: while count < 30: - print >>sys.stderr, "\033[33m[rtmlaunch] Wait for ",cmd_path," ",count,"/30\033[0m" + print("\033[33m[rtmlaunch] Wait for ",cmd_path," ",count,"/30\033[0m", file=sys.stderr) node = alive_component(path, tree) if node: return node @@ -92,7 +93,7 @@ def rtconnect(nameserver, tags, tree): sub_type = tag.attributes.get("subscription_type").value sub_type = replace_arg_tag_by_env(sub_type); if not sub_type in ['flush','new','periodic']: - print >>sys.stderr, sub_type+' is not a subscription type' + print(sub_type+' is not a subscription type', file=sys.stderr) continue else: sub_type = 'flush' # this is default value @@ -104,8 +105,8 @@ def rtconnect(nameserver, tags, tree): wait_component(dest_full_path, tree) if check_connect(source_full_path,dest_full_path, tree): continue - except Exception, e: - print >>sys.stderr, '\033[31m[rtmlaunch] [ERROR] Could not Connect (', source_full_path, ',', dest_full_path, '): ', e,'\033[0m' + except Exception as e: + print('\033[31m[rtmlaunch] [ERROR] Could not Connect (', source_full_path, ',', dest_full_path, '): ', e,'\033[0m', file=sys.stderr) return 1 #print source_path, source_full_path, dest_path, dest_full_path; try: @@ -126,26 +127,26 @@ def rtconnect(nameserver, tags, tree): if tag.attributes.get("buffer_length") != None: props['dataport.buffer.length'] = replace_arg_tag_by_env(str(tag.attributes.get("buffer_length").value)) options = optparse.Values({'verbose': False, 'id': '', 'name': None, 'properties': props}) - print >>sys.stderr, "[rtmlaunch] Connected from",source_path - print >>sys.stderr, "[rtmlaunch] to",dest_path - print >>sys.stderr, "[rtmlaunch] with",options + print("[rtmlaunch] Connected from",source_path, file=sys.stderr) + print("[rtmlaunch] to",dest_path, file=sys.stderr) + print("[rtmlaunch] with",options, file=sys.stderr) try : rtcon.connect_ports(source_path, source_full_path, dest_path, dest_full_path, options, tree=tree) - except Exception, e_1_1_0: # openrtm 1.1.0 + except Exception as e_1_1_0: # openrtm 1.1.0 try: rtcon.connect_ports([(source_path,source_full_path), (dest_path, dest_full_path)], options, tree=tree) - except Exception, e_1_0_0: # openrtm 1.0.0 - print >>sys.stderr, '\033[31m[rtmlaunch] {0} did not work on both OpenRTM 1.0.0 and 1.1.0'.format(os.path.basename(sys.argv[1])),'\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] OpenRTM 1.0.0 {0}'.format(e_1_0_0),'\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] OpenRTM 1.1.0 {0}'.format(e_1_1_0),'\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] This is very weird situation, Please check your network\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] configuration with `ifconfig` on both robot and client side. \033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] Having multiple network interface sometimes causes problem, \033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] please see FAQ site http://www.openrtm.org/OpenRTM-aist/html/FAQ2FE38388E383A9E38396E383ABE382B7E383A5E383BCE38386E382A3E383B3E382B0.html#f2bc375d\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] Issue related to this https://github.com/start-jsk/rtmros_hironx/issues/33\033[0m' - print >>sys.stderr, '\033[31m[rtmlaunch] ~/.ros/log may contains usefully informations\033[0m' - except Exception, e: - print >>sys.stderr, '\033[31m[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[1]), e),'\033[0m' + except Exception as e_1_0_0: # openrtm 1.0.0 + print('\033[31m[rtmlaunch] {0} did not work on both OpenRTM 1.0.0 and 1.1.0'.format(os.path.basename(sys.argv[1])),'\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] OpenRTM 1.0.0 {0}'.format(e_1_0_0),'\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] OpenRTM 1.1.0 {0}'.format(e_1_1_0),'\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] This is very weird situation, Please check your network\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] configuration with `ifconfig` on both robot and client side. \033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] Having multiple network interface sometimes causes problem, \033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] please see FAQ site http://www.openrtm.org/OpenRTM-aist/html/FAQ2FE38388E383A9E38396E383ABE382B7E383A5E383BCE38386E382A3E383B3E382B0.html#f2bc375d\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] Issue related to this https://github.com/start-jsk/rtmros_hironx/issues/33\033[0m', file=sys.stderr) + print('\033[31m[rtmlaunch] ~/.ros/log may contains usefully informations\033[0m', file=sys.stderr) + except Exception as e: + print('\033[31m[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[1]), e),'\033[0m', file=sys.stderr) return 0 def rtactivate(nameserver, tags, tree): @@ -175,28 +176,28 @@ def activate_action(object, ec_index): if state == 'Active': continue else: - print >>sys.stderr, "[rtmlaunch] Activate <-",state,full_path - except Exception, e: - print >>sys.stderr, '\033[31m[rtmlaunch] Could not Activate (', cmd_path, ') : ', e,'\033[0m' + print("[rtmlaunch] Activate <-",state,full_path, file=sys.stderr) + except Exception as e: + print('\033[31m[rtmlaunch] Could not Activate (', cmd_path, ') : ', e,'\033[0m', file=sys.stderr) return 1 try: options = optparse.Values({"ec_index": 0, 'verbose': False}) try : state_control_base.alter_component_state(activate_action, cmd_path, full_path, options, tree=tree) - except Exception, e: # openrtm 1.1.0 + except Exception as e: # openrtm 1.1.0 state_control_base.alter_component_states(activate_action, [(cmd_path, full_path)], options, tree=tree) - except Exception, e: - print >>sys.stderr, '[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[0]), e) + except Exception as e: + print('[rtmlaunch] {0}: {1}'.format(os.path.basename(sys.argv[0]), e), file=sys.stderr) return 1 return 0 def main(): usage = '''Usage: %prog [launchfile]''' if len(sys.argv) <= 1: - print >>sys.stderr, usage + print(usage, file=sys.stderr) return 1 fullpathname = sys.argv[1] - print >>sys.stderr, "[rtmlaunch] starting... ",fullpathname + print("[rtmlaunch] starting... ",fullpathname, file=sys.stderr) try: parser = parse(fullpathname) nodes = parser.getElementsByTagName("launch")[0].childNodes @@ -216,46 +217,46 @@ def main(): for remove_node in remove_nodes: nodes.remove(remove_node) - except Exception,e: - print e + except Exception as e: + print(e) return 1 if os.getenv("RTCTREE_NAMESERVERS") == None: - print >>sys.stderr, "[rtmlaunch] RTCTREE_NAMESERVERS is not set, use localhost:15005" + print("[rtmlaunch] RTCTREE_NAMESERVERS is not set, use localhost:15005", file=sys.stderr) nameserver = "localhost:15005" os.environ["RTCTREE_NAMESERVERS"] = nameserver else: nameserver = os.getenv("RTCTREE_NAMESERVERS") - print >>sys.stderr, "\033[32m[rtmlaunch] RTCTREE_NAMESERVERS", nameserver, os.getenv("RTCTREE_NAMESERVERS"), "\033[0m" - print >>sys.stderr, "\033[32m[rtmlaunch] SIMULATOR_NAME", os.getenv("SIMULATOR_NAME","Simulator"), "\033[0m" + print("\033[32m[rtmlaunch] RTCTREE_NAMESERVERS", nameserver, os.getenv("RTCTREE_NAMESERVERS"), "\033[0m", file=sys.stderr) + print("\033[32m[rtmlaunch] SIMULATOR_NAME", os.getenv("SIMULATOR_NAME","Simulator"), "\033[0m", file=sys.stderr) try: tree = rtctree.tree.RTCTree() - except Exception, e: - print >>sys.stderr, "\033[31m[rtmlaunch] Could not start rtmlaunch.py, Caught exception (", e, ")\033[0m" + except Exception as e: + print("\033[31m[rtmlaunch] Could not start rtmlaunch.py, Caught exception (", e, ")\033[0m", file=sys.stderr) # check if host is connected try: hostname = nameserver.split(':')[0] ip_address = socket.gethostbyname(hostname) - except Exception, e: - print >>sys.stderr, "\033[31m[rtmlaunch] .. Could not find IP address of ", hostname, ", Caught exception (", e, ")\033[0m" - print >>sys.stderr, "\033[31m[rtmlaunch] .. Please check /etc/hosts or DNS setup\033[0m" + except Exception as e: + print("\033[31m[rtmlaunch] .. Could not find IP address of ", hostname, ", Caught exception (", e, ")\033[0m", file=sys.stderr) + print("\033[31m[rtmlaunch] .. Please check /etc/hosts or DNS setup\033[0m", file=sys.stderr) return 1 # in this case, it is likely you forget to run name serveer - print >>sys.stderr, "\033[31m[rtmlaunch] .. Could not connect to NameServer at ", nameserver, ", Caught exception (", e, ")\033[0m" - print >>sys.stderr, "\033[31m[rtmlaunch] .. Please make sure that you have NameServer running at %s/`\033[0m"%(nameserver) - print >>sys.stderr, "\033[31m[rtmlaunch] .. You can check with `rtls %s/`\033[0m"%(nameserver) + print("\033[31m[rtmlaunch] .. Could not connect to NameServer at ", nameserver, ", Caught exception (", e, ")\033[0m", file=sys.stderr) + print("\033[31m[rtmlaunch] .. Please make sure that you have NameServer running at %s/`\033[0m"%(nameserver), file=sys.stderr) + print("\033[31m[rtmlaunch] .. You can check with `rtls %s/`\033[0m"%(nameserver), file=sys.stderr) return 1 while 1: - print >>sys.stderr, "[rtmlaunch] check connection/activation" + print("[rtmlaunch] check connection/activation", file=sys.stderr) rtconnect(nameserver, parser.getElementsByTagName("rtconnect"), tree) rtactivate(nameserver, parser.getElementsByTagName("rtactivate"), tree) time.sleep(10) tree.add_name_server(nameserver, []) if os.getenv("RTC_CONNECTION_CHECK_ONCE") and os.getenv("RTC_CONNECTION_CHECK_ONCE").lower() == "true": - print >>sys.stderr, "[rtmlaunch] break from rtmlaunch main loop." - print >>sys.stderr, "[rtmlaunch] If you check the rtc connection in the while loop, real-time loop becomes slow." + print("[rtmlaunch] break from rtmlaunch main loop.", file=sys.stderr) + print("[rtmlaunch] If you check the rtc connection in the while loop, real-time loop becomes slow.", file=sys.stderr) break def get_flag_from_argv(arg): for a in sys.argv: diff --git a/openrtm_tools/src/openrtm_tools/rtmstart.py b/openrtm_tools/src/openrtm_tools/rtmstart.py index 25f09586d..0c6eabbab 100644 --- a/openrtm_tools/src/openrtm_tools/rtmstart.py +++ b/openrtm_tools/src/openrtm_tools/rtmstart.py @@ -13,7 +13,7 @@ def start_cosname(cosnames, port_number): try: # find process using port_number if filter(lambda c: c.local_address[1] == port_number, p.get_connections()): - print "\033[31m[rtmlaunch]", p.name, "is already started with port", port_number,"\033[0m" + print("\033[31m[rtmlaunch]", p.name, "is already started with port", port_number,"\033[0m") try: orb = CORBA.ORB_init(sys.argv, CORBA.ORB_ID) @@ -26,22 +26,22 @@ def findObject(name, kind="", rnc=None) : cxt = findObject(socket.gethostname(), "host_cxt") obj = findObject("manager", "mgr", cxt) start_naming = False - except CosNaming.NamingContext.NotFound, ex: + except CosNaming.NamingContext.NotFound as ex: # this is ok since host_cxt, manager is not bind start_naming = False pass except: - print "\033[31m[rtmlaunch] name server is unreachable so kill process\033[0m" - print "\033[31m[rtmlaunch] kill ", cosnames, " of pid", p.pid,"\033[0m" + print("\033[31m[rtmlaunch] name server is unreachable so kill process\033[0m") + print("\033[31m[rtmlaunch] kill ", cosnames, " of pid", p.pid,"\033[0m") p.terminate() except: pass if not start_naming : - print "\033[31m[rtmlaunch] do not start", cosnames, ", exiting...\033[0m" + print("\033[31m[rtmlaunch] do not start", cosnames, ", exiting...\033[0m") exit(0) else: - print "\033[34m[rtmlaunch] Start", cosnames, "at port", port_number, "\033[0m" + print("\033[34m[rtmlaunch] Start", cosnames, "at port", port_number, "\033[0m") logdir = "/tmp" hostname = socket.gethostname() try : diff --git a/openrtm_tools/test/test-rtmlaunch.py b/openrtm_tools/test/test-rtmlaunch.py index 7dfdd1ba7..04c2252af 100755 --- a/openrtm_tools/test/test-rtmlaunch.py +++ b/openrtm_tools/test/test-rtmlaunch.py @@ -25,7 +25,7 @@ def test_provider_activated(self): try: tree = rtctree.tree.RTCTree(servers='localhost:2809') provider = tree.get_node(['/', 'localhost:2809','MyServiceProvider0.rtc']) - print >>sys.stderr, "Provier : ", provider, provider.get_state_string() + print("Provier : ", provider, provider.get_state_string(), file=sys.stderr) if provider.state==rtctree.component.Component.ACTIVE: break except: @@ -40,7 +40,7 @@ def test_consumer_activated(self): try: tree = rtctree.tree.RTCTree(servers='localhost:2809') consumer = tree.get_node(['/', 'localhost:2809','MyServiceConsumer0.rtc']) - print >>sys.stderr, "Consumer : ", consumer, consumer.get_state_string() + print("Consumer : ", consumer, consumer.get_state_string(), file=sys.stderr) if consumer.state==rtctree.component.Component.ACTIVE: break except: @@ -59,7 +59,7 @@ def test_provider_and_consumer_connected(self): provider_port = provider.get_port_by_name("MyService") consumer_port = consumer.get_port_by_name("MyService") connection = provider_port.get_connection_by_dest(consumer_port) - print >>sys.stderr, "Connection : ", connection.properties + print("Connection : ", connection.properties, file=sys.stderr) break except: pass @@ -75,7 +75,7 @@ def test_seqin_activated(self): try: tree = rtctree.tree.RTCTree(servers='localhost:2809') seqin = tree.get_node(['/', 'localhost:2809','SequenceInComponent0.rtc']) - print >>sys.stderr, "SeqIn : ", seqin, seqin.get_state_string() + print("SeqIn : ", seqin, seqin.get_state_string(), file=sys.stderr) if seqin.state==rtctree.component.Component.ACTIVE: break except: @@ -90,7 +90,7 @@ def test_seqout_activated(self): try: tree = rtctree.tree.RTCTree(servers='localhost:2809') seqout = tree.get_node(['/', 'localhost:2809','SequenceOutComponent0.rtc']) - print >>sys.stderr, "SeqOut : ", seqout, seqout.get_state_string() + print("SeqOut : ", seqout, seqout.get_state_string(), file=sys.stderr) if seqout.state==rtctree.component.Component.ACTIVE: break except: @@ -112,8 +112,8 @@ def test_seqin_and_seqout_connected(self): seqout_port2 = seqout.get_port_by_name("FloatSeq") connection1 = seqin_port1.get_connection_by_dest(seqout_port1) connection2 = seqin_port2.get_connection_by_dest(seqout_port2) - print >>sys.stderr, "Connection : ", connection1.properties - print >>sys.stderr, "Connection : ", connection2.properties + print("Connection : ", connection1.properties, file=sys.stderr) + print("Connection : ", connection2.properties, file=sys.stderr) break except: pass diff --git a/openrtm_tools/test/test-rtshell-setup.py b/openrtm_tools/test/test-rtshell-setup.py index 86cf6f52b..ae5cc3051 100755 --- a/openrtm_tools/test/test-rtshell-setup.py +++ b/openrtm_tools/test/test-rtshell-setup.py @@ -15,7 +15,8 @@ def run_rt_command(self, command): self.assertTrue(os.path.exists(script)) try: check_output("bash -c 'source %s; RTCTREE_NAMESERVERS=localhost:2809 %s'"%(script, command), shell=True, stderr=subprocess.STDOUT) - except subprocess.CalledProcessError, (e): + except subprocess.CalledProcessError as xxx_todo_changeme: + (e) = xxx_todo_changeme self.assertTrue(False, 'subprocess.CalledProcessError: cmd:%s returncode:%s output:%s' % (e.cmd, e.returncode, e.output)) self.assertTrue(True) # ok rtls works diff --git a/rosnode_rtc/scripts/rtmros-data-bridge.py b/rosnode_rtc/scripts/rtmros-data-bridge.py index c07c26c3d..93f2ecc7e 100755 --- a/rosnode_rtc/scripts/rtmros-data-bridge.py +++ b/rosnode_rtc/scripts/rtmros-data-bridge.py @@ -89,7 +89,7 @@ def update_ports(self, in_topic, out_topic): port = topic.lstrip('/').replace('/','_') topic_type = idlman.topicinfo.get(topic, None) _data = idlman.get_rtmobj(topic_type) - if topic in self.inports.keys() or not topic_type or not _data: + if topic in list(self.inports.keys()) or not topic_type or not _data: rospy.loginfo('Failed to add InPort "%s"', port) continue rospy.loginfo('Add InPort "%s"[%s]', port, topic_type) @@ -102,7 +102,7 @@ def update_ports(self, in_topic, out_topic): port = topic.lstrip('/').replace('/','_') topic_type = idlman.topicinfo.get(topic, None) _data = idlman.get_rtmobj(topic_type) - if (topic in self.outports.keys()) or not topic_type or not _data: + if (topic in list(self.outports.keys())) or not topic_type or not _data: rospy.loginfo('Failed to add OutPort "%s"', port) continue rospy.loginfo('Add OutPort "%s"[%s]', port, topic_type) @@ -115,7 +115,7 @@ def update_ports(self, in_topic, out_topic): # InPort -> Publisher def onExecute(self, ec_id): - for topic in self.inports.keys(): + for topic in list(self.inports.keys()): _inport, _ = self.inports[topic] if _inport.isNew(): _data = _inport.read() @@ -206,7 +206,7 @@ def update_topicinfo(self,topics): def rtm2ros(self, data, output=None): datatype = str(data).split('(')[0].split('.')[1] - for msgtype in self.msg2obj.keys(): + for msgtype in list(self.msg2obj.keys()): if datatype == msgtype.replace('/','_'): output = self.msg2obj[msgtype][0]() break @@ -260,7 +260,7 @@ def ros2rtm(self, data, output=None): return self.msg2obj[data._type][1](*args) def get_rtmobj(self, msg): - if not msg in self.msg2obj.keys(): + if not msg in list(self.msg2obj.keys()): return None return self.ros2rtm(self.msg2obj[msg][0]()) @@ -280,7 +280,7 @@ def get_message_definition(self, msg): return text def gen_struct_text(self, msg): - if msg in (self.generated + self.basictab.keys()): + if msg in (self.generated + list(self.basictab.keys())): return True defmsg = self.get_message_definition(msg) @@ -305,7 +305,7 @@ def gen_struct_text(self, msg): defmsg = defmsg.replace('/','_') # for embedded types, sequence/array types - for key in self.basictab.keys(): + for key in list(self.basictab.keys()): p = re.compile( '(^|\n)'+key+'((\[[\d]*\])?[ ]+[\w]+)') defmsg = p.sub( r'\1'+self.basictab[key]+r'\2', defmsg) p = re.compile( '(^|\n)([ \w]+)\[([\d]+)\][ ]([\w]+)') diff --git a/rtmbuild/test/test-compile-idl.py b/rtmbuild/test/test-compile-idl.py index 91ac49694..33a78a985 100755 --- a/rtmbuild/test/test-compile-idl.py +++ b/rtmbuild/test/test-compile-idl.py @@ -24,21 +24,21 @@ def initCORBA() : global rootnc, orb - print "configuration ORB with ", nshost, ":", nsport + print("configuration ORB with ", nshost, ":", nsport) os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:{0}:{1}/NameService'.format(nshost,nsport) try: orb = CORBA.ORB_init(sys.argv, CORBA.ORB_ID) nameserver = orb.resolve_initial_references("NameService"); rootnc = nameserver._narrow(CosNaming.NamingContext) - except omniORB.CORBA.ORB.InvalidName, e: + except omniORB.CORBA.ORB.InvalidName as e: sys.exit('[ERROR] Invalide Name (hostname={0}).\n'.format(nshost) + 'Make sure the hostname is correct.\n' + str(e)) - except omniORB.CORBA.TRANSIENT, e: + except omniORB.CORBA.TRANSIENT as e: sys.exit('[ERROR] Connection Failed with the Nameserver (hostname={0} port={1}).\n'.format(nshost, nsport) + 'Make sure the hostname is correct and the Nameserver is running.\n' + str(e)) except Exception as e: - print str(e) + print(str(e)) return None @@ -49,11 +49,11 @@ def findObject(name) : try: nc = rootnc.resolve([CosNaming.NameComponent(name, "rtc")]) except: - print >>sys.stderr, "Waiting for %s ..."%name + print("Waiting for %s ..."%name, file=sys.stderr) time.sleep(1) continue time.sleep(1) - print >>sys.stderr, "Found for %s (%r)"%(name,nc) + print("Found for %s (%r)"%(name,nc), file=sys.stderr) return nc class TestCompileIDL(unittest.TestCase): @@ -62,22 +62,22 @@ def setUpClass(self): global orb # run opnrtm - print >>sys.stderr, "initCORBA" + print("initCORBA", file=sys.stderr) initCORBA() # wait for MyServiceROSBridge - print >>sys.stderr, "wait for MyServiceROSBridge" + print("wait for MyServiceROSBridge", file=sys.stderr) bridge = findObject("MyServiceROSBridge")._narrow(RTC.RTObject) - print >>sys.stderr, "wait for MyServiceProvider" + print("wait for MyServiceProvider", file=sys.stderr) provider = findObject("MyServiceProvider0")._narrow(RTC.RTObject) # connect - print >>sys.stderr, "connect components" + print("connect components", file=sys.stderr) inP = provider.get_ports()[0] outP = bridge.get_ports()[0] con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP], []) inP.connect(con_prof) # activate - print >>sys.stderr, "activate components" + print("activate components", file=sys.stderr) bridge.get_owned_contexts()[0].activate_component(bridge) provider.get_owned_contexts()[0].activate_component(provider) @@ -89,7 +89,7 @@ def setUpClass(self): def testEcho(self): sys.path.append("/tmp/test_compile_idl/devel/lib/python2.7/dist-packages") - print sys.path + print(sys.path) import rtmbuild_test.srv rospy.logwarn("wait for service") rospy.wait_for_service("/MyServiceROSBridge/echo")