From 90f0a37c81a654497d603bd7a9eb4435cf5409f8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 8 May 2021 17:59:52 +0900 Subject: [PATCH 1/5] [imu_monitor] convert to package format 3 --- imu_monitor/package.xml | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/imu_monitor/package.xml b/imu_monitor/package.xml index de1376f..29df9a6 100644 --- a/imu_monitor/package.xml +++ b/imu_monitor/package.xml @@ -1,4 +1,4 @@ - + imu_monitor 1.6.31 This package contains a single node that monitors the drift of the IMU @@ -15,9 +15,10 @@ are aggregated in the PR2 dashboard. catkin - sensor_msgs - pr2_mechanism_controllers - python_orocos_kdl - rospy - diagnostic_msgs + sensor_msgs + pr2_mechanism_controllers + python_orocos_kdl + python3-pykdl + rospy + diagnostic_msgs From 7d74c2ecda2fad0af7f321d77affad88c547d847 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 8 May 2021 18:09:24 +0900 Subject: [PATCH 2/5] run 2to3 -w -fprint . --- pr2_bringup/scripts/calibrate.py | 10 +++++----- pr2_bringup/scripts/calibrate_pr2.py | 8 ++++---- .../scripts/plot_multi_trigger.py | 16 ++++++++-------- .../synchronizer_classes.py | 16 ++++++++-------- pr2_computer_monitor/scripts/cpu_monitor.py | 2 +- pr2_computer_monitor/scripts/hd_monitor.py | 2 +- pr2_computer_monitor/scripts/wifi_monitor.py | 2 +- pr2_ethercat/scripts/test_halt.py | 10 +++++----- 8 files changed, 33 insertions(+), 33 deletions(-) diff --git a/pr2_bringup/scripts/calibrate.py b/pr2_bringup/scripts/calibrate.py index 15772ba..0c7246e 100755 --- a/pr2_bringup/scripts/calibrate.py +++ b/pr2_bringup/scripts/calibrate.py @@ -70,7 +70,7 @@ def calibrate(controllers): success = False else: launched.append(c) - print "Launched: %s" % ', '.join(launched) + print("Launched: %s" % ', '.join(launched)) # Starts the launched controllers switch_controller(launched, [], SwitchControllerRequest.BEST_EFFORT) @@ -85,7 +85,7 @@ def calibrated(msg, name): # Somewhat not thread-safe # Waits until all the controllers have calibrated while waiting_for and not rospy.is_shutdown(): - print "Waiting for: %s" % ', '.join(waiting_for) + print("Waiting for: %s" % ', '.join(waiting_for)) sleep(0.5) finally: for name in launched: @@ -123,7 +123,7 @@ def wait_for_calibrated(self, topic, timeout): self.sub.unregister() return self.is_calibrated - print "Waiting up to 20s for IMU calibration to complete." + print("Waiting up to 20s for IMU calibration to complete.") helper = is_calibrated_helper() if not helper.wait_for_calibrated("torso_lift_imu/is_calibrated", 20): rospy.logerr("IMU took too long to calibrate.") @@ -158,9 +158,9 @@ def main(): pub_calibrated.publish(True) if not imustatus: - print "Mechanism calibration complete, but IMU calibration failed." + print("Mechanism calibration complete, but IMU calibration failed.") else: - print "Calibration complete" + print("Calibration complete") rospy.spin() diff --git a/pr2_bringup/scripts/calibrate_pr2.py b/pr2_bringup/scripts/calibrate_pr2.py index dc06167..455be14 100755 --- a/pr2_bringup/scripts/calibrate_pr2.py +++ b/pr2_bringup/scripts/calibrate_pr2.py @@ -299,7 +299,7 @@ def main(): force_calibration = True if arms not in ['both', 'none', 'left', 'right', 'auto']: - print 'Arms must be "both", "none", "left", "right", or "auto"' + print('Arms must be "both", "none", "left", "right", or "auto"') sys.exit(1) # load controller configuration @@ -370,9 +370,9 @@ def main(): controller_list = list_controllers() def is_running(c) : return c[1]=='running' running_controllers = [c[0] for c in filter(is_running, zip(controller_list.controllers, controller_list.state))] - print "Running controllers : ", running_controllers + print("Running controllers : ", running_controllers) if not switch_controller([], running_controllers, SwitchControllerRequest.STRICT): - print "Failed to stop controllers" + print("Failed to stop controllers") sys.exit(1) # calibrate imu and torso @@ -440,7 +440,7 @@ def is_running(c) : return c[1]=='running' if recalibrate: if not switch_controller(running_controllers, [], SwitchControllerRequest.STRICT): - print "Could not start previously running controllers" + print("Could not start previously running controllers") if not recalibrate: if joints_status: diff --git a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py index 3c96d84..d1c5938 100755 --- a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py +++ b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py @@ -308,7 +308,7 @@ def __init__(self, plot, silent = False): try: self.gnuplot = subprocess.Popen('gnuplot', stdin = subprocess.PIPE) except: - print "Gnuplot must be installed to allow plotting of waveforms." + print("Gnuplot must be installed to allow plotting of waveforms.") sys.exit(1) atexit.register(self.gnuplot.kill) @@ -367,10 +367,10 @@ def plot(self): self.triggers[i].period, period)) return style = 'with linespoints title "%s"' - print >> self.gnuplot.stdin, 'plot "-"', style%self.triggers[0].name, + print('plot "-"', style%self.triggers[0].name, end=' ', file=self.gnuplot.stdin) for i in range(1, n): - print >> self.gnuplot.stdin, ', "-"', style%self.triggers[i].name, - print >> self.gnuplot.stdin + print(', "-"', style%self.triggers[i].name, end=' ', file=self.gnuplot.stdin) + print(file=self.gnuplot.stdin) for i in range(n): t = self.triggers[i] reps = int(period / t.period) @@ -378,7 +378,7 @@ def plot(self): if len(pts) == 0: pts = [(0, 0)] def plot_pt(x, y): - print >> self.gnuplot.stdin, x, (n - i - 1) * 1.1 + y%2 + print(x, (n - i - 1) * 1.1 + y%2, file=self.gnuplot.stdin) plot_pt(0, pts[-1][1]) for k in range(reps): xoffs = t.period * k @@ -386,12 +386,12 @@ def plot_pt(x, y): plot_pt(pts[j][0] + xoffs, pts[j-1][1]) plot_pt(pts[j][0] + xoffs, pts[j][1]) plot_pt(period, pts[-1][1]) - print >> self.gnuplot.stdin, "e" - print >> self.gnuplot.stdin + print("e", file=self.gnuplot.stdin) + print(file=self.gnuplot.stdin) sys.stdout.flush() def empty_plot(self): - print 'plot x, -x' + print('plot x, -x') import unittest class Test(unittest.TestCase): diff --git a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py index 3e225c9..0b7e3c0 100755 --- a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py +++ b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py @@ -155,7 +155,7 @@ def async_update(self, period, zero_offset, transitions): rospy.logerr("Error setting waveform %s: %s"%(self.name, rslt.status_message)) #print "Done updating waveform ", self.name except KeyboardInterrupt: # Handle CTRL+C - print "Aborted trigger update on", self.name + print("Aborted trigger update on", self.name) def update(self): # Run the update using an Asynchronous Updater so that if something @@ -464,7 +464,7 @@ def async_apply_update(self, reconfig_request): #print "**** Reconfigured client", self.name #print "Done updating camera ", self.name except KeyboardInterrupt: # Handle CTRL+C - print "Aborted camera update on", self.name + print("Aborted camera update on", self.name) def apply_update(self): reconfig_request = { @@ -541,20 +541,20 @@ def print_threads(): for t in threads: if not t.isDaemon() and t != threading.currentThread(): try: - print t.name + print(t.name) except: - print "Unknown thread ", t + print("Unknown thread ", t) n = n + 1 return n def kill(self): - print "\nWaiting for all threads to die..." + print("\nWaiting for all threads to die...") killAsynchronousUpdaters() #time.sleep(1) while self.print_threads() > 0: - print "\nStill waiting for all threads to die..." + print("\nStill waiting for all threads to die...") time.sleep(1) - print + print() def reconfigure(self, config, level): # print "Reconfigure", config @@ -627,4 +627,4 @@ def spin(self): finally: rospy.signal_shutdown("Main thread exiting") self.kill() - print "Main thread exiting" + print("Main thread exiting") diff --git a/pr2_computer_monitor/scripts/cpu_monitor.py b/pr2_computer_monitor/scripts/cpu_monitor.py index d245a93..68ca075 100755 --- a/pr2_computer_monitor/scripts/cpu_monitor.py +++ b/pr2_computer_monitor/scripts/cpu_monitor.py @@ -819,7 +819,7 @@ def publish_stats(self): try: rospy.init_node('cpu_monitor_%s' % hostname) except rospy.exceptions.ROSInitException: - print >> sys.stderr, 'CPU monitor is unable to initialize node. Master may not be running.' + print('CPU monitor is unable to initialize node. Master may not be running.', file=sys.stderr) sys.exit(0) cpu_node = CPUMonitor(hostname, options.diag_hostname) diff --git a/pr2_computer_monitor/scripts/hd_monitor.py b/pr2_computer_monitor/scripts/hd_monitor.py index 190806a..1d2a542 100755 --- a/pr2_computer_monitor/scripts/hd_monitor.py +++ b/pr2_computer_monitor/scripts/hd_monitor.py @@ -364,7 +364,7 @@ def publish_stats(self): try: rospy.init_node('hd_monitor_%s' % hostname) except rospy.exceptions.ROSInitException: - print 'HD monitor is unable to initialize node. Master may not be running.' + print('HD monitor is unable to initialize node. Master may not be running.') sys.exit(0) hd_monitor = hd_monitor(hostname, options.diag_hostname, home_dir) diff --git a/pr2_computer_monitor/scripts/wifi_monitor.py b/pr2_computer_monitor/scripts/wifi_monitor.py index ec79882..7c693f3 100755 --- a/pr2_computer_monitor/scripts/wifi_monitor.py +++ b/pr2_computer_monitor/scripts/wifi_monitor.py @@ -132,7 +132,7 @@ def publish_stats(self): try: rospy.init_node('ddwrt_diag') except rospy.exceptions.ROSInitException: - print 'Wifi monitor is unable to initialize node. Master may not be running.' + print('Wifi monitor is unable to initialize node. Master may not be running.') sys.exit(2) wifi_monitor = WifiMonitor() diff --git a/pr2_ethercat/scripts/test_halt.py b/pr2_ethercat/scripts/test_halt.py index c462586..f29ff17 100755 --- a/pr2_ethercat/scripts/test_halt.py +++ b/pr2_ethercat/scripts/test_halt.py @@ -10,7 +10,7 @@ halted = True def callback(msg): global halted - print "halted: %s" % msg.data + print("halted: %s" % msg.data) halted = msg.data @@ -19,14 +19,14 @@ def callback(msg): reset = rospy.ServiceProxy("pr2_ethercat/reset_motors", std_srvs.srv.Empty) halt = rospy.ServiceProxy("pr2_ethercat/halt_motors", std_srvs.srv.Empty) time.sleep(1) -print "Entering main loop: motors %s" % "halted" if halted else "running" +print("Entering main loop: motors %s" % "halted" if halted else "running") while 1: old_halted = halted if old_halted: - print "Resetting motors" + print("Resetting motors") reset() else: - print "Halting motors" + print("Halting motors") halt() start = time.time() while old_halted == halted: @@ -34,7 +34,7 @@ def callback(msg): if time.time() - start > 1: break if old_halted == halted: - print "failed! old=%s new=%s" % (old_halted, halted) + print("failed! old=%s new=%s" % (old_halted, halted)) break From 87066431d49e346f98eb1f0f0e94d84896733c2c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 8 May 2021 18:35:26 +0900 Subject: [PATCH 3/5] run 2to3 -w -fexcept . --- pr2_bringup/scripts/calibrate.py | 2 +- .../scripts/plot_multi_trigger.py | 2 +- .../synchronizer_classes.py | 2 +- pr2_computer_monitor/scripts/cpu_monitor.py | 16 ++++++++-------- pr2_computer_monitor/scripts/hd_monitor.py | 2 +- pr2_computer_monitor/scripts/ntp_monitor.py | 3 ++- pr2_computer_monitor/scripts/nvidia_temp.py | 2 +- pr2_computer_monitor/scripts/wifi_monitor.py | 2 +- 8 files changed, 16 insertions(+), 15 deletions(-) diff --git a/pr2_bringup/scripts/calibrate.py b/pr2_bringup/scripts/calibrate.py index 0c7246e..d9fd9ed 100755 --- a/pr2_bringup/scripts/calibrate.py +++ b/pr2_bringup/scripts/calibrate.py @@ -96,7 +96,7 @@ def calibrated(msg, name): # Somewhat not thread-safe resp_unload = unload_controller(name) if (resp_unload == 0): rospy.logerr("Failed to unload controller %s" % name) - except Exception, ex: + except Exception as ex: rospy.logerr("Failed to stop/unload controller %s" % name) return success diff --git a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py index d1c5938..280b178 100755 --- a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py +++ b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py @@ -194,7 +194,7 @@ def check(self, projector): with_proj_exp = projector_pulse_len + 0.0015 + 2 * slight_shift if alt and e - s > with_proj_exp: raise Exception("Too long exposure for textured image %f instead of %f, %s."%(e-s,with_proj_exp,info)) - except Exception, e: + except Exception as e: #import traceback #traceback.print_exc() self.parent.set_error(repr(e)) diff --git a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py index 0b7e3c0..cba68db 100755 --- a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py +++ b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py @@ -104,7 +104,7 @@ def run(self): if allargs != None: try: self.f(*allargs[0], **allargs[1]) - except Exception, e: + except Exception as e: rospy.logerr("AsynchronousUpdater failed with exception: %s"%str(e)) pass diff --git a/pr2_computer_monitor/scripts/cpu_monitor.py b/pr2_computer_monitor/scripts/cpu_monitor.py index 68ca075..486a4aa 100755 --- a/pr2_computer_monitor/scripts/cpu_monitor.py +++ b/pr2_computer_monitor/scripts/cpu_monitor.py @@ -172,7 +172,7 @@ def check_ipmi(): diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('CPU Hot Alarm') - except Exception, e: + except Exception as e: diag_vals.append(KeyValue(key = 'Exception', value = traceback.format_exc())) diag_level = DiagnosticStatus.ERROR diag_msgs.append('Exception') @@ -270,7 +270,7 @@ def check_clock_speed(enforce_speed): elif lvl == DiagnosticStatus.ERROR and enforce_speed: msgs = [ 'Core throttled' ] - except Exception, e: + except Exception as e: rospy.logerr(traceback.format_exc()) lvl = DiagnosticStatus.ERROR msgs.append('Exception') @@ -315,7 +315,7 @@ def check_uptime(load1_threshold, load5_threshold): vals.append(KeyValue(key = '15 min Load Average', value = load15)) vals.append(KeyValue(key = 'Number of Users', value = num_users)) - except Exception, e: + except Exception as e: rospy.logerr(traceback.format_exc()) level = DiagnosticStatus.ERROR vals.append(KeyValue(key = 'Load Average Status', value = traceback.format_exc())) @@ -360,7 +360,7 @@ def check_memory(): values.append(KeyValue(key = 'Free Memory', value = free_mem)) msg = mem_dict[level] - except Exception, e: + except Exception as e: rospy.logerr(traceback.format_exc()) msg = 'Memory Usage Check Error' values.append(KeyValue(key = msg, value = str(e))) @@ -463,7 +463,7 @@ def check_mpstat(core_count = -1): has_error_core_count = True return DiagnosticStatus.ERROR, 'Incorrect number of CPU cores', vals - except Exception, e: + except Exception as e: mp_level = DiagnosticStatus.ERROR vals.append(KeyValue(key = 'mpstat Exception', value = str(e))) @@ -591,7 +591,7 @@ def _restart_temp_check(self): self._temps_timer.cancel() self.check_temps() - except Exception, e: + except Exception as e: rospy.logerr('Unable to restart temp thread. Error: %s' % traceback.format_exc()) @@ -660,7 +660,7 @@ def check_nfs_stat(self): vals.append(KeyValue( key = '%s Write Blks srv/s' % file_sys, value=w_blk_srv)) - except Exception, e: + except Exception as e: rospy.logerr(traceback.format_exc()) nfs_level = DiagnosticStatus.ERROR msg = 'Exception' @@ -831,7 +831,7 @@ def publish_stats(self): cpu_node.publish_stats() except KeyboardInterrupt: pass - except Exception, e: + except Exception as e: traceback.print_exc() rospy.logerr(traceback.format_exc()) diff --git a/pr2_computer_monitor/scripts/hd_monitor.py b/pr2_computer_monitor/scripts/hd_monitor.py index 1d2a542..8c582fa 100755 --- a/pr2_computer_monitor/scripts/hd_monitor.py +++ b/pr2_computer_monitor/scripts/hd_monitor.py @@ -376,7 +376,7 @@ def publish_stats(self): hd_monitor.publish_stats() except KeyboardInterrupt: pass - except Exception, e: + except Exception as e: traceback.print_exc() hd_monitor.cancel_timers() diff --git a/pr2_computer_monitor/scripts/ntp_monitor.py b/pr2_computer_monitor/scripts/ntp_monitor.py index 4d8fcb3..1b4b94b 100755 --- a/pr2_computer_monitor/scripts/ntp_monitor.py +++ b/pr2_computer_monitor/scripts/ntp_monitor.py @@ -88,7 +88,8 @@ def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname=None, p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o,e) = p.communicate() - except OSError, (errno, msg): + except OSError as e: + (errno, msg) = e.args if errno == 4: break #ctrl-c interrupt else: diff --git a/pr2_computer_monitor/scripts/nvidia_temp.py b/pr2_computer_monitor/scripts/nvidia_temp.py index 6865c56..639d554 100755 --- a/pr2_computer_monitor/scripts/nvidia_temp.py +++ b/pr2_computer_monitor/scripts/nvidia_temp.py @@ -59,7 +59,7 @@ def pub_status(self): card_out = pr2_computer_monitor.get_gpu_status() gpu_stat = pr2_computer_monitor.parse_smi_output(card_out) stat = pr2_computer_monitor.gpu_status_to_diag(gpu_stat) - except Exception, e: + except Exception as e: import traceback rospy.logerr('Unable to process nVidia GPU data') rospy.logerr(traceback.format_exc()) diff --git a/pr2_computer_monitor/scripts/wifi_monitor.py b/pr2_computer_monitor/scripts/wifi_monitor.py index 7c693f3..ce23d40 100755 --- a/pr2_computer_monitor/scripts/wifi_monitor.py +++ b/pr2_computer_monitor/scripts/wifi_monitor.py @@ -144,7 +144,7 @@ def publish_stats(self): wifi_monitor.publish_stats() except KeyboardInterrupt: pass - except Exception, e: + except Exception as e: import traceback traceback.print_exc() From a076baaaff8846e910390910c94542bade509e7d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 8 May 2021 18:36:15 +0900 Subject: [PATCH 4/5] python3 did not take tuple for lambda : https://stackoverflow.com/questions/11328312/python-lambda-does-not-accept-tuple-argument --- pr2_camera_synchronizer/scripts/plot_multi_trigger.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py index 280b178..c5d6561 100755 --- a/pr2_camera_synchronizer/scripts/plot_multi_trigger.py +++ b/pr2_camera_synchronizer/scripts/plot_multi_trigger.py @@ -25,7 +25,7 @@ def sorted_pts(self): #print >> sys.stderr, self.name #print >> sys.stderr, self.pts pts = [ (x % self.period, y) for (x, y) in self.pts ] - firstpt = pts.index(min(pts, key = lambda (x,y):x)) + firstpt = pts.index(min(pts, key = lambda xy:xy[0])) pts = pts[firstpt:] + pts[:firstpt] #print >> sys.stderr, pts #print >> sys.stderr From e679ed634edfa8c65a782f79beb54811ad008181 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 8 May 2021 18:40:57 +0900 Subject: [PATCH 5/5] async is keyword in Python3.7 --- .../src/pr2_camera_synchronizer/synchronizer_classes.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py index cba68db..533dc4e 100755 --- a/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py +++ b/pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py @@ -128,7 +128,7 @@ def __init__(self, name): self.zero_offset = 0 self.clear_waveform() self.name = name - self.async = AsynchronousUpdater(self.async_update, "Controller "+name) + self.async_ = AsynchronousUpdater(self.async_update, "Controller "+name) self.service = None self.transitions = [] @@ -161,7 +161,7 @@ def update(self): # Run the update using an Asynchronous Updater so that if something # locks up, the rest of the node can keep working. #print "Trigger update on", self.name - self.async.update(self.period, self.zero_offset, self.transitions) + self.async_.update(self.period, self.zero_offset, self.transitions) class ProsilicaInhibitTriggerController(MultiTriggerController): def __init__(self, name, param, true_val, false_val): @@ -359,7 +359,7 @@ def __init__(self, node_name, proj, level, **paramnames): self.level = level self.proj = proj self.reconfigure_client = None - self.async = AsynchronousUpdater(self.async_apply_update, "Camera "+node_name) + self.async_ = AsynchronousUpdater(self.async_apply_update, "Camera "+node_name) self.trig_rising = True def param(self, config, name): @@ -480,7 +480,7 @@ def apply_update(self): } #print self.name, reconfig_request - self.async.update(reconfig_request) + self.async_.update(reconfig_request) # Need to set: # Global period if synchronized