Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 from __future__ import with_statement
00039 PKG = 'pr2_hardware_test_monitor'
00040 import roslib; roslib.load_manifest(PKG)
00041
00042
00043 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00044 from std_srvs.srv import *
00045 from std_msgs.msg import Bool
00046 from pr2_mechanism_msgs.msg import MechanismStatistics, JointStatistics, ActuatorStatistics
00047 from ectools.msg import ecstats
00048
00049 import rospy
00050 import threading
00051
00052 from time import sleep
00053
00054 import math
00055
00056 class PR2HardwareSimulator:
00057 """
00058 Publishes representative output from a PR2 during operation.
00059 """
00060 def __init__(self):
00061 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00062 self.diag_agg_pub = rospy.Publisher('/diagnostics_agg', DiagnosticArray)
00063 self.motors_pub = rospy.Publisher('pr2_etherCAT/motors_halted', Bool)
00064 self.cal_pub = rospy.Publisher('calibrated', Bool, latch=True)
00065 self.ecstat_pub = rospy.Publisher('ecstats', ecstats)
00066 self.mech_pub = rospy.Publisher('mechanism_statistics', MechanismStatistics)
00067 self.trans_pub = rospy.Publisher('pr2_transmission_check/transmission_status', Bool, latch = True)
00068
00069 self._reset_srv = rospy.Service('pr2_etherCAT/reset_motors', Empty, self.on_reset)
00070 self._halt_srv = rospy.Service('pr2_etherCAT/halt_motors', Empty, self.on_halt)
00071
00072 self._mutex = threading.Lock()
00073 self._ok = True
00074
00075 self._has_pub_cal = False
00076
00077 self._start_time = rospy.get_time()
00078
00079 self._last_diag_pub = rospy.get_time()
00080 self._last_diag_agg_pub = rospy.get_time()
00081 self._last_motor_pub = rospy.get_time()
00082 self._last_ecstats_pub = rospy.get_time()
00083
00084 self._total_sent = 0
00085
00086 def on_halt(self, srv):
00087 with self._mutex:
00088 self._ok = True
00089 self.motors_pub.publish(Bool(True))
00090 return EmptyResponse()
00091
00092 def on_reset(self, srv):
00093 with self._mutex:
00094 self._ok = False
00095 self.motors_pub.publish(Bool(False))
00096 return EmptyResponse()
00097
00098 def _publish_mech_stats(self):
00099 ok = False
00100 with self._mutex:
00101 ok = self._ok
00102
00103
00104 trig_arg = rospy.get_time() - self._start_time
00105
00106 sine = math.sin(trig_arg)
00107 cosine = math.cos(trig_arg)
00108
00109 jnt_st = JointStatistics()
00110 jnt_st.name = 'fake_joint'
00111 jnt_st.position = float(2 * sine)
00112 jnt_st.velocity = float(2 * cosine)
00113 jnt_st.measured_effort = float(-2 * sine)
00114 jnt_st.commanded_effort = float(-2 * sine)
00115 jnt_st.is_calibrated = 1
00116
00117 cont_st = JointStatistics()
00118 cont_st.name = 'cont_joint'
00119 cont_st.position = 5 * float(0.5 * sine)
00120 cont_st.velocity = 2.5 * float(0.5 * cosine)
00121 cont_st.is_calibrated = 1
00122
00123 cont_act_st = ActuatorStatistics()
00124 cont_act_st.name = 'cont_motor'
00125 cont_act_st.calibration_reading = False
00126 wrapped_position = (cont_st.position % 6.28)
00127 if wrapped_position > 3.14:
00128 cont_act_st.calibration_reading = True
00129
00130 act_st = ActuatorStatistics()
00131 act_st.name = 'fake_motor'
00132 act_st.calibration_reading = True
00133 if sine > 0.0:
00134 act_st.calibration_reading = False
00135
00136 mech_st = MechanismStatistics()
00137 mech_st.actuator_statistics = [ act_st, cont_act_st ]
00138 mech_st.joint_statistics = [ jnt_st, cont_st ]
00139 mech_st.header.stamp = rospy.get_rostime()
00140
00141 self.mech_pub.publish(mech_st)
00142
00143 self.trans_pub.publish(ok)
00144
00145 def _publish_ecstats(self):
00146 self._total_sent += 20
00147
00148 ecstat = ecstats()
00149 ecstat.has_link = True
00150 ecstat.max_device_count = 0
00151 ecstat.total_sent_packets = self._total_sent
00152 ecstat.interval_sent_packets = 100
00153 ecstat.total_dropped_packets = 0
00154 ecstat.interval_dropped_packets = 0
00155 ecstat.total_bandwidth_mbps = 100
00156 ecstat.interval_bandwidth_mbps = 100
00157
00158 self.ecstat_pub.publish(ecstat)
00159
00160 self._last_ecstats_pub = rospy.get_time()
00161
00162 def _publish_diag(self):
00163 ok = False
00164 with self._mutex:
00165 ok = self._ok
00166
00167 msg = DiagnosticArray()
00168 stat = DiagnosticStatus()
00169 msg.status.append(stat)
00170
00171
00172 stat.level = 0
00173 if not ok:
00174 stat.level = 2
00175 stat.name = 'EtherCAT Master'
00176 stat.message = 'OK'
00177 stat.values.append(KeyValue('Dropped Packets', '0'))
00178 stat.values.append(KeyValue('RX Late Packet', '0'))
00179
00180
00181 mcb_stat = DiagnosticStatus()
00182 mcb_stat.name = 'EtherCAT Device (my_motor)'
00183 mcb_stat.level = 0
00184 mcb_stat.values.append(KeyValue('Num encoder_errors', '0'))
00185
00186
00187 stat_cam = DiagnosticStatus()
00188 stat_cam.level = 0
00189 stat_cam.name = 'wge100: Frequency Status'
00190 stat_cam.message = 'OK'
00191
00192
00193 stat_hk = DiagnosticStatus()
00194 stat_hk.level = 0
00195 stat_hk.name = 'tilt_hokuyo_node: Frequency Status'
00196 stat_hk.message = 'OK'
00197
00198 msg.status.append(stat_cam)
00199 msg.status.append(mcb_stat)
00200 msg.status.append(stat_hk)
00201 msg.header.stamp = rospy.get_rostime()
00202
00203 self.diag_pub.publish(msg)
00204
00205 self._last_diag_pub = rospy.get_time()
00206
00207 def _publish_diag_agg(self):
00208 msg = DiagnosticArray()
00209 msg.status = [
00210 DiagnosticStatus(level = 0, name='/Cameras', message='OK'),
00211 DiagnosticStatus(level = 1, name='/Cameras/wge100', message='Uh Oh'),
00212 DiagnosticStatus(level = 0, name='/Lasers', message='OK'),
00213 DiagnosticStatus(level = 2, name='/Other', message='Error')]
00214
00215 msg.header.stamp = rospy.get_rostime()
00216
00217 self.diag_agg_pub.publish(msg)
00218 self._last_diag_agg_pub = rospy.get_time()
00219
00220
00221
00222 def _publish_motor_state(self):
00223 self.motors_pub.publish(Bool(not self._ok))
00224 self._last_motor_pub = rospy.get_time()
00225
00226 def _pub_cal(self):
00227 self.cal_pub.publish(Bool(True))
00228 self._has_pub_cal = True
00229
00230
00231 def publish(self):
00232 if not self._has_pub_cal:
00233 self._pub_cal()
00234
00235 self._publish_mech_stats()
00236 if rospy.get_time() - self._last_diag_pub > 1.0:
00237 self._publish_diag()
00238 if rospy.get_time() - self._last_diag_agg_pub > 1.0:
00239 self._publish_diag_agg()
00240 if rospy.get_time() - self._last_motor_pub > 0.02:
00241 self._publish_motor_state()
00242 if rospy.get_time() - self._last_ecstats_pub > 0.2:
00243 self._publish_ecstats()
00244
00245
00246