class CorobotMonitor():
"""ROS Node for data monitoring and interaction"""
def __init__(self):
self.win = CorobotMonitorUI(rospy.get_param("/monitor/fullscreen"))
self.init_ros_node()
self.win.mainloop()
def pose_callback(self, pose_message):
self.win.setPose(pose_message.x, pose_message.y, pose_message.theta, pose_message.cov)
#self.win.after(100,rospy.spin_once())
def laserpose_callback(self,pose_message):
if pose_message.x < -999 and pose_message.y < -999:
self.win.lasercolor = 'red'
else:
self.win.lasercolor = 'green'
def rawnav_callback(self, rawnav_message):
self.win.setRawnavMsg(rawnav_message.name)
def obstacle_callback(self, obs_message):
self.win.setObsMsg(obs_message.name)
def absGoal_callback(self, absGoal_message):
self.win.setAbsGoalMsg(absGoal_message.name)
def netForce_callback(self, netForce_message):
self.win.setNetForceMsg(netForce_message.name)
def velcmd_callback(self, velcmd_message):
self.win.setVelCmdMsg(velcmd_message.name)
def qrCount_callback(self, qrCount_msg):
self.win.setQrCountMsg(qrCount_msg.name)
def recovery_callback(self, recovery_msg):
self.win.setRecoveryMsg(recovery_msg.name)
def laserDisp_callback(self, laserDat):
self.win.setLaserMap(laserDat.angle_min, laserDat.range_min, laserDat.range_max, laserDat.angle_increment, laserDat.ranges)
def diagnostics_callback(self, dArray):
#print len(dArray.status)
#print len(dArray.status[2].values)
pub = rospy.Publisher("batman", String);
'''for i in range(len(dArray.status[2].values)):
print dArray.status[2].values[i].key'''
if(len(dArray.status) >= 3):
batteryLevel = float(dArray.status[2].values[3].value) / float(dArray.status[2].values[4].value)*100
batteryLevel = math.ceil(batteryLevel * 100) / 100
else:
batteryLevel = "low"
#print batteryLevel
pub.publish(String(str(batteryLevel)))
self.win.setBatteryMsg(str(batteryLevel))
def laptop_battery(self):
while True:
p = subprocess.Popen(["acpi", "-V"], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
out, err = p.communicate()
print out.split('\n')[0].split(', ')[1]
def init_ros_node(self):
"""Initialize all ROS node/sub/pub/srv stuff."""
rospy.init_node("corobot_monitor")
rospy.Subscriber("pose", Pose, self.pose_callback)
rospy.Subscriber("laser_pose", Pose, self.laserpose_callback)
rospy.Subscriber("ch_rawnav", Goal, self.rawnav_callback)
rospy.Subscriber("ch_velcmd", Goal, self.velcmd_callback)
rospy.Subscriber("ch_obstacle", Goal, self.obstacle_callback)
rospy.Subscriber("ch_absgoal", Goal, self.absGoal_callback)
rospy.Subscriber("ch_netforce", Goal, self.netForce_callback)
rospy.Subscriber("ch_qrcodecount", Goal, self.qrCount_callback)
rospy.Subscriber("ch_recovery", Goal, self.recovery_callback)
rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_callback)
rospy.Subscriber("scan", LaserScan, self.laserDisp_callback, queue_size = 1)
t = batteryThread(0.5, self.win)
t.daemon = True
t.start()
class CorobotMonitor():
"""ROS Node for data monitoring and interaction"""
def __init__(self):
self.win = CorobotMonitorUI()
self.init_ros_node()
self.win.mainloop()
def pose_callback(self, pose_message):
self.win.setPose(pose_message.x, pose_message.y, pose_message.theta)
#self.win.after(100,rospy.spin_once())
def rawnav_callback(self, rawnav_message):
self.win.setRawnavMsg(rawnav_message.name)
def obstacle_callback(self, obs_message):
self.win.setObsMsg(obs_message.name)
def absGoal_callback(self, absGoal_message):
self.win.setAbsGoalMsg(absGoal_message.name)
def netForce_callback(self, netForce_message):
self.win.setNetForceMsg(netForce_message.name)
def velcmd_callback(self, velcmd_message):
self.win.setVelCmdMsg(velcmd_message.name)
def qrCount_callback(self, qrCount_msg):
self.win.setQrCountMsg(qrCount_msg.name)
def recovery_callback(self, recovery_msg):
self.win.setRecoveryMsg(recovery_msg.name)
def diagnostics_callback(self, dArray):
#print len(dArray.status)
#print len(dArray.status[2].values)
pub = rospy.Publisher("batman", String)
'''for i in range(len(dArray.status[2].values)):
print dArray.status[2].values[i].key'''
if (len(dArray.status) >= 3):
batteryLevel = float(dArray.status[2].values[3].value) / float(
dArray.status[2].values[4].value) * 100
batteryLevel = math.ceil(batteryLevel * 100) / 100
else:
batteryLevel = "low"
#print batteryLevel
pub.publish(String(str(batteryLevel)))
self.win.setBatteryMsg(str(batteryLevel))
def laptop_battery(self):
while True:
p = subprocess.Popen(["acpi", "-V"],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
out, err = p.communicate()
print out.split('\n')[0].split(', ')[1]
def init_ros_node(self):
"""Initialize all ROS node/sub/pub/srv stuff."""
rospy.init_node("corobot_monitor")
rospy.Subscriber("pose", Pose, self.pose_callback)
rospy.Subscriber("ch_rawnav", Goal, self.rawnav_callback)
rospy.Subscriber("ch_velcmd", Goal, self.velcmd_callback)
rospy.Subscriber("ch_obstacle", Goal, self.obstacle_callback)
rospy.Subscriber("ch_absgoal", Goal, self.absGoal_callback)
rospy.Subscriber("ch_netforce", Goal, self.netForce_callback)
rospy.Subscriber("ch_qrcodecount", Goal, self.qrCount_callback)
rospy.Subscriber("ch_recovery", Goal, self.recovery_callback)
rospy.Subscriber("diagnostics", DiagnosticArray,
self.diagnostics_callback)
t = batteryThread(0.5, self.win)
t.daemon = True
t.start()
class CorobotMonitor():
"""ROS Node for data monitoring and interaction"""
def __init__(self):
self.win = CorobotMonitorUI(rospy.get_param("/monitor/fullscreen"))
self.init_ros_node()
self.win.mainloop()
def pose_callback(self, pose_message):
self.win.setPose(pose_message.x, pose_message.y, pose_message.theta, pose_message.cov)
#self.win.after(100,rospy.spin_once())
def laserpose_callback(self,pose_message):
if pose_message.x < -999 and pose_message.y < -999:
self.win.lasercolor = 'red'
else:
self.win.lasercolor = 'green'
def rawnav_callback(self, rawnav_message):
self.win.setRawnavMsg(rawnav_message.name)
def perpForce_callback(self, perpForce_message):
self.win.setPerpForce(perpForce_message.name)
def repForce_callback(self, repForce_message):
self.win.setRepForce(repForce_message.name)
def obstacle_callback(self, obs_message):
self.win.setObsMsg(obs_message.name)
def absGoal_callback(self, absGoal_message):
self.win.setAbsGoalMsg(absGoal_message.name)
def netForce_callback(self, netForce_message):
self.win.setNetForceMsg(netForce_message.name)
def velcmd_callback(self, velcmd_message):
self.win.setVelCmdMsg(velcmd_message.name)
def qrCount_callback(self, qrCount_msg):
self.win.setQrCountMsg(qrCount_msg.name)
def recovery_callback(self, recovery_msg):
self.win.setRecoveryMsg(recovery_msg.name)
def laserDisp_callback(self, laserDat):
self.win.setLaserData(laserDat)
self.win.after(0,self.win.drawLaserMap)
def wallDisp_callback(self, wallDat):
self.win.setWalls(wallDat)
self.win.after(0,self.win.drawWalls)
def diagnostics_callback(self, dArray):
#print len(dArray.status)
#print len(dArray.status[2].values)
pub = rospy.Publisher("batman", String, queue_size=1);
'''for i in range(len(dArray.status[2].values)):
print dArray.status[2].values[i].key'''
if(len(dArray.status) >= 3):
batteryLevel = float(dArray.status[2].values[3].value) / float(dArray.status[2].values[4].value)*100
batteryLevel = math.ceil(batteryLevel * 100) / 100
else:
batteryLevel = "low"
#print batteryLevel
pub.publish(String(str(batteryLevel)))
self.win.setBatteryMsg(str(batteryLevel))
def wallf_callback(self, wallf_msg):
self.win.setPathColor(wallf_msg.name)
def laptop_battery(self):
while True:
p = subprocess.Popen(["acpi", "-V"], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
out, err = p.communicate()
print out.split('\n')[0].split(', ')[1]
# Particle Filter Debugger
def particles_callback(self, particles_message):
self.win.setParticles(particles_message)
self.win.after(0,self.win.drawParticles)
def particle_filter_pose_callback(self, particleFilterPose_msg):
self.win.setParticleFilterPose(particleFilterPose_msg.x, particleFilterPose_msg.y)
self.win.after(0,self.win.drawParticleFilterPose)
def init_ros_node(self):
"""Initialize all ROS node/sub/pub/srv stuff."""
rospy.init_node("corobot_monitor")
rospy.Subscriber("pose", Pose, self.pose_callback)
rospy.Subscriber("laser_pose", Pose, self.laserpose_callback)
rospy.Subscriber("particleList", PoseArray, self.particles_callback)
rospy.Subscriber("particleFilterPose", Pose, self.particle_filter_pose_callback)
rospy.Subscriber("ch_rawnav", Goal, self.rawnav_callback)
rospy.Subscriber("ch_velcmd", Goal, self.velcmd_callback)
rospy.Subscriber("ch_obstacle", Goal, self.obstacle_callback)
rospy.Subscriber("ch_absgoal", Goal, self.absGoal_callback)
rospy.Subscriber("ch_netforce", Goal, self.netForce_callback)
#rospy.Subscriber("ch_repforce", Goal, self.repForce_callback)
#rospy.Subscriber("ch_perpforce", Goal, self.perpForce_callback)
#rospy.Subscriber("ch_repforce", Goal, self.repForce_callback)
rospy.Subscriber("ch_qrcodecount", Goal, self.qrCount_callback)
rospy.Subscriber("ch_recovery", Goal, self.recovery_callback)
rospy.Subscriber("ch_wallf", Goal, self.wallf_callback)
rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_callback)
rospy.Subscriber("scan", LaserScan, self.laserDisp_callback, queue_size = 1)
rospy.Subscriber("wall", Wall, self.wallDisp_callback, queue_size = 1)
t = batteryThread(0.5, self.win)
t.daemon = True
t.start()