def main(args):
vod = VO(int(args[1]))
rospy.ready('vo')
rospy.spin()
vod.vo.summarize_timers()
vod.dump()
def listener():
app = wx.PySimpleApp()
rospy.ready(NAME, anonymous=True)
frame = MainWindow(None, -1, "Runtime Monitor")
frame.Show()
app.MainLoop()
def listener():
app = wx.PySimpleApp()
rospy.ready(NAME, anonymous=True)
frame = MainWindow(None, -1, "PR2 Power Board")
frame.Show()
app.MainLoop()
def main(args):
vod = lineperftest(args[1])
rospy.ready('lineperf_%s' % args[1])
rospy.spin()
if args[1] == 'send':
vod.report()
def test_pointcloud(self):
print "LNK\n"
rospy.TopicSub("base_scan", LaserScan, self.pointInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 5.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def run_me_server():
sn = SegmentNode()
hg.cvNamedWindow('image', 1)
rospy.TopicSub('image', RImage, sn.set_image)
rospy.ready(sys.argv[0])
segment_service = rospy.Service('hrl_grasp', srv.hrl_grasp, sn.segment)
segment_service.register()
rospy.spin()
def __init__(self, parent, id=wx.ID_ANY, title='PR2 GUI', pos=wx.DefaultPosition, size=(1280, 1024), style=wx.DEFAULT_FRAME_STYLE):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
rospy.ready('pr2_gui', anonymous=True)
self._config = wx.Config("pr2_gui")
self._aui_manager = wx.aui.AuiManager(self)
self._visualizer_panel = visualizer_panel.DefaultVisualizationPanel(self)
media_path = rostools.packspec.get_pkg_dir( "gazebo_robot_description" )
media_path += "/world/Media/";
media_paths = [media_path]
media_paths.append(media_path)
media_paths.append(media_path + "fonts")
media_paths.append(media_path + "materials")
media_paths.append(media_path + "materials/scripts")
media_paths.append(media_path + "materials/programs")
media_paths.append(media_path + "materials/textures")
media_paths.append(media_path + "models")
media_paths.append(media_path + "models/pr2")
ogre_tools.initializeResources( media_paths )
self._visualizer_panel.createDefaultVisualizers()
self._rosout_panel = wx_rosout.RosoutPanel(self)
self._monitor_panel = MonitorPanel(self)
self._monitor_panel.set_new_errors_callback(self.on_monitor_errors_received)
self._interpreter = wx.py.shell.Shell(self)
self._hardware_panel = hardware_panel.HardwarePanel(self)
self._aui_manager.AddPane(self._visualizer_panel, wx.aui.AuiPaneInfo().CenterPane().Center().Name('3dvis').Caption('3D Visualization'), '3D Visualization')
self._aui_manager.AddPane(self._interpreter, wx.aui.AuiPaneInfo().BottomDockable().Bottom().Name('interpreter').Caption('Python Interpreter').Hide(), 'Python Interpreter')
self._aui_manager.AddPane(self._rosout_panel, wx.aui.AuiPaneInfo().BottomDockable().Bottom().Layer(1).BestSize(wx.Size(700, 200)).Name('rosout').Caption('Rosout'), 'Rosout')
self._aui_manager.AddPane(self._monitor_panel, wx.aui.AuiPaneInfo().BottomDockable().Bottom().Layer(1).BestSize(wx.Size(700,600)).Name('runtime_monitor').Caption('Runtime Monitor'), 'Runtime Monitor')
self._aui_manager.AddPane(self._hardware_panel, wx.aui.AuiPaneInfo().BottomDockable().Bottom().Layer(1).BestSize(wx.Size(300,200)).Name('hardware').Caption('Hardware'), 'Hardware')
self.add_camera_pane("forearm_r", False)
self.add_camera_pane("forearm_l", False)
self.add_camera_pane("axis_r", True)
self.add_camera_pane("axis_l", True)
self.add_camera_pane("stereo_l", False)
self._aui_manager.Update()
self.load_config()
self.create_menu_bar()
self.Bind(wx.aui.EVT_AUI_PANE_CLOSE, self.on_pane_close)
self.Bind(wx.EVT_CLOSE, self.on_close)
def test_cloud(self):
print "LNK\n"
rospy.TopicSub("world_3d_map", PointCloudFloat32, self.pointInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 30.0 #30 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
self.success = self.errorcheck
os.system("killall gazebo")
self.assert_(self.success)
def main():
vod = VODemo()
if SEE:
hg.cvNamedWindow("channel L", 1)
hg.cvNamedWindow("channel R", 1)
rospy.TopicSub("/videre/images", ImageArray, vod.display_array)
rospy.TopicSub("/videre/cal_params", String, vod.display_params)
rospy.TopicSub("image", Image, display_single)
rospy.ready("camview_py")
rospy.spin()
def OnInit(self):
rospy.ready("TestPlotter", anonymous=True)
self.data_topic = rospy.TopicSub("/test_data", TestData, self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Test Plot')
self.plot.set_crosshairs(True)
self.plot.set_selection(True)
self.plot.set_zoom(True)
return True
def OnInit(self):
rospy.ready("Hysteresis", anonymous=True)
self.data_dict = {}
self.data_topic = rospy.TopicSub("/hysteresis_data", ChannelFloat32, self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Hysteresis Plot')
self.plot.set_crosshairs(False)
self.plot.set_selection(False)
self.plot.set_zoom(False)
return True
def testcameras(self):
print " wait 3 sec for objects to settle "
time.sleep(3)
print " subscribe image from ROS "
rospy.TopicSub("test_camera/image", Image, self.imageInput)
rospy.ready(NAME, anonymous=True)
#self.pollThread.start()
timeout_t = time.time() + 10 #10 seconds delay for processing comparison
while not rospy.is_shutdown() and not self.tested and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(self.success)
def testslide(self):
print "LINK\n"
# rospy.TopicSub("Odom", RobotBase2DOdom, self.positionInput)
rospy.TopicSub("base_pose_ground_truth", TransformWithRateStamped, self.positionInput)
rospy.ready(NAME, anonymous=True)
timeout_t = time.time() + 50.0 # 59 seconds
while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
time.sleep(0.1)
time.sleep(2.0)
# os.system("killall gazebo")
self.assert_(self.success)
def viewGraphMain(argv, stdout, env):
# default arguments
server = "http://localhost"
server_port = rospy.DEFAULT_TEST_PORT
#check arguments for a help flag
optlist, args = getopt.getopt(argv[1:], "h?p:s:", ["help","port=","server=","test"])
for o, a in optlist:
if o in ("-h","-?","--help"):
usage(stdout, argv[0])
return
elif o in ("--test"):
server_port = rospy.DEFAULT_TEST_PORT
elif o in ("-p", "--port"):
server_port = a
elif o in ("-s", "--server"):
server = a
serverUri = '%s:%s/'%(server,server_port)
print "Looking for server at %s"%serverUri
os.environ[rospy.ROS_MASTER_URI] = serverUri
os.environ[rospy.ROS_NODE] = "viewGraph"
os.environ[rospy.ROS_PORT] = str(0) # any
master = rospy.getMaster()
rospy.ready()
while 1:
status_code, statusMessage, [nodes,flows] = master.getGraph()
# print 'nodes' + str(nodes)
# print 'flows ' + str(flows)
print '--------------------------------------------'
print 'This is the graph:'
print 'refreshing every 10 seconds'
print '--------------------------------------------'
for anode in nodes:
status_code, statusMessage, [machine, address, port] = master.getNodeAddress(anode)
print 'NODE: ' + str(anode) + ' on ' + str(machine) + ' at ' + str(address) + ' on port: ' + str(port)
for aflow in flows:
aflow_split= str(aflow[1]).split(':')
destination = aflow_split[0]
aflow_split_source = str(aflow[0]).split(':')
source = aflow_split_source[0]
if destination == anode:
print '\tINFLOW: ' + str(aflow_split[1]) + ' from: ' + str(aflow[0])
if source == anode:
print '\tOUTFLOW: ' + str(aflow_split_source[1]) + ' to: ' + str(aflow[1])
print '--------------------------------------------'
time.sleep(10.0)
def main():
pub = rospy.TopicPub('image', Image)
rospy.ready('test_send')
image = cv.cvCreateImage(cv.cvSize(640,480), 8, 1)
cv.cvSet(image, 133)
while not rospy.is_shutdown():
pub.publish(Image(None, 'test', image.width, image.height, 'none', 'mono8', image.imageData))
time.sleep(.033)
rospy.spin()
def OnInit(self):
rospy.ready("SineSweep", anonymous=True)
self.data_dict = {}
self.count =0
self.data_topic = rospy.TopicSub("/sinesweep_data", ChannelFloat32, self.OnData)
# Configure the plot panel
self.plot = wxmpl.PlotApp('Sine Sweep Plot')
self.plot.set_crosshairs(True)
self.plot.set_selection(True)
self.plot.set_zoom(True)
return True
def __init__(self):
#initialize flows
self.in_imageIn = InflowpyImageString("imageIn")
#connect this node to the graph
rospy.ready()
# initialize Tkinter
self.root = Tkinter.Tk()
# setup the prompts at the top
Tkinter.Label(text="Width:", anchor=Tkinter.E).grid(row=0,column=1)
Tkinter.Label(text="Height:", anchor=Tkinter.E).grid(row=0,column=3)
self.ewidth = Tkinter.StringVar()
self.eheight = Tkinter.StringVar()
self.ew = Tkinter.Entry(self.root, textvariable=self.ewidth)
self.eh = Tkinter.Entry(self.root, textvariable=self.eheight)
self.ew.grid(row=0, column= 2)
self.eh.grid(row=0, column= 4)
# A button to force resizing
self.mb = Tkinter.Button(self.root, text="Click to resize", command=self.startup)
self.mb.grid(row=0, column= 5)
# A checkbox to turn on automatic resizing
self.cbval = Tkinter.IntVar()
self.mb2 = Tkinter.Checkbutton(self.root, text="Click to Autosize", variable=self.cbval, onvalue="1", offvalue="0")
self.mb2.grid(row=0, column= 0)
# Set the default window size
self.ewidth.set(1000)
self.eheight.set(1000)
self.width = 1000 # These don't need to be preset, they are filled in startup before use
self.height = 1000 # but I kept them here for clarity
self.mimage = 0; # This variable nees to be present for the canvas item to fill
# space required for the header
self.header_size = 40
#initialize the first canvas
self.mcanvas = Tkinter.Canvas(self.root)
#run the startup/resize script
self.startup()
# start the loop to recieve flows
self.imageViewerLoop()
# start the Tkinter event loop
Tkinter.mainloop()
def test_wpc(self):
rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
self.test.publishInitalPose2d(NAME)
timeout_t = time.time() + 50.0 #59 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
time.sleep(0.1)
time.sleep(2.0)
os.system("killall trexfast")
self.assert_(self.success)
def test_arm(self):
pub = rospy.TopicPub("initialpose", Pose2DFloat32)
rospy.ready(NAME, anonymous=True)
time.sleep(10.0)
start = Pose2DFloat32()
start.x = 0
start.y = 0
start.th = 0
pub.publish(start)
timeout_t = time.time() + 110.0 #90 seconds
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
self.test.readLogFile()
self.success = self.test.getPassed()
time.sleep(0.2)
os.system("killall trex_fast")
os.system("killall gazebo")
self.assert_(self.success)
def main(args):
f = open("pruned.pickle", "r")
cam = pickle.load(f)
db = pickle.load(f)
f.close()
print cam.params
vo = VisualOdometer(cam)
library = set()
for (id, pose, kp, desc) in db:
lf = LibraryFrame(id, pose, kp, desc)
library.add(lf)
corr = Corrector(vo, library)
rospy.ready('corrector')
try:
corr.workloop()
except KeyboardInterrupt:
print "shutting down"
def main(args):
time.sleep(3)
optlist, args = getopt.getopt(args[1:], 'l')
if ('-l', '') in optlist:
mode = 'learn'
else:
mode = 'play'
if mode == 'play':
f = open("pruned.pickle", "r")
cam = pickle.load(f)
db = pickle.load(f)
f.close()
class LibraryFrame:
def __init__(self, id, pose, kp, desc):
self.id = id
self.pose = pose
self.kp = kp
self.descriptors = desc
library = set()
for (id, pose, kp, desc) in db:
lf = LibraryFrame(id, pose, kp, desc)
library.add(lf)
else:
library = None
if SEE:
hg.cvNamedWindow('channel L', 1)
hg.cvNamedWindow('channel R', 1)
vod = VODemo(mode, library)
rospy.ready('camview_py')
rospy.spin()
vod.dump()
time.sleep(0.1)
# listenerpublisher is supposed to repeat our messages back onto /listenerpublisher,
# make sure we got it
self.assert_(self.callback_data is not None, "no callback data from listenerpublisher")
print "Got ", self.callback_data.time
errorstr = "callback msg field [%s] from listenerpublisher does not match"
self.assertEquals(2.0, self.callback_data.time, errorstr % "time")
self.assertEquals(2, len(self.callback_data.joint_states))
self.assertEquals(2, len(self.callback_data.actuator_states))
self.assertEquals("name0", self.callback_data.joint_states[0].name)
self.assertEquals("name0", self.callback_data.actuator_states[0].name)
self.assertEquals("name1", self.callback_data.joint_states[1].name)
self.assertEquals("name1", self.callback_data.actuator_states[1].name)
self.assertEquals(0, self.callback_data.actuator_states[0].encoder_count)
self.assertEquals(0, self.callback_data.actuator_states[0].calibration_reading)
self.assertEquals(0, self.callback_data.actuator_states[0].last_calibration_high_transition)
self.assertEquals(0, self.callback_data.actuator_states[0].num_encoder_errors)
self.assertEquals(1, self.callback_data.actuator_states[1].encoder_count)
self.assertEquals(1, self.callback_data.actuator_states[1].calibration_reading)
self.assertEquals(1, self.callback_data.actuator_states[1].last_calibration_high_transition)
self.assertEquals(1, self.callback_data.actuator_states[1].num_encoder_errors)
if __name__ == "__main__":
rospy.ready(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
def listener_with_user_data():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
PKG = 'laser_interface'
import sys, os, subprocess
try:
rostoolsDir = (subprocess.Popen(['rospack', 'find', 'rostools'],
stdout=subprocess.PIPE).communicate()[0]
or '').strip()
sys.path.append(os.path.join(rostoolsDir, 'src'))
import rostools.launcher
rostools.launcher.updateSysPath(sys.argv[0], PKG, bootstrapVersion="0.6")
except ImportError:
print >> sys.stderr, "\nERROR: Cannot locate rostools"
sys.exit(1)
#from pkg import *
import rospy
from std_msgs.msg import Position
from std_msgs.msg import RobotBase2DOdom
from std_msgs.msg import Pose2DFloat32
import sys
import time
pub = rospy.TopicPub('odom', RobotBase2DOdom)
rospy.ready(sys.argv[0])
while not rospy.isShutdown():
odom = RobotBase2DOdom(None, Pose2DFloat32(1, 2, 3),
Pose2DFloat32(1, 2, 4), 1)
pub.publish(odom)
time.sleep(.5)
print 'published'
def run(self): rospy.ready(NAME, anonymous=True) rospy.spin()
## @author Hai Nguyen/[email protected] PKG = 'laser_interface' import sys, os, subprocess try: rostoolsDir = (subprocess.Popen(['rospack', 'find', 'rostools'], stdout=subprocess.PIPE).communicate()[0] or '').strip() sys.path.append(os.path.join(rostoolsDir,'src')) import rostools.launcher rostools.launcher.updateSysPath(sys.argv[0], PKG, bootstrapVersion="0.6") except ImportError: print >> sys.stderr, "\nERROR: Cannot locate rostools" sys.exit(1) #from pkg import * import rospy from std_msgs.msg import Position from std_msgs.msg import RobotBase2DOdom from std_msgs.msg import Pose2DFloat32 import sys import time pub = rospy.TopicPub('odom', RobotBase2DOdom) rospy.ready(sys.argv[0]) while not rospy.isShutdown(): odom = RobotBase2DOdom(None, Pose2DFloat32(1,2,3), Pose2DFloat32(1,2,4), 1) pub.publish(odom) time.sleep(.5) print 'published'
def listener():
pub = rospy.TopicPub("/diagnostics", DiagnosticMessage)
rospy.ready(NAME, anonymous=True)
while not rospy.is_shutdown():
loop(pub)
time.sleep(1)
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
def monitorBatteriesMain(argv, stdout, env):
# default arguments
server = "http://localhost"
server_port = rospy.DEFAULT_TEST_PORT
#check arguments for a help flag
optlist, args = getopt.getopt(argv[1:], "h?p:s:",
["help", "port=", "server=", "test"])
for o, a in optlist:
if o in ("-h", "-?", "--help"):
usage(stdout, argv[0])
return
elif o in ("--test"):
server_port = rospy.DEFAULT_TEST_PORT
elif o in ("-p", "--port"):
server_port = a
elif o in ("-s", "--server"):
server = a
serverUri = '%s:%s/' % (server, server_port)
print "Looking for server at %s" % serverUri
os.environ[rospy.ROS_MASTER_URI] = serverUri
os.environ[rospy.ROS_NODE] = NAME
os.environ[rospy.ROS_PORT] = str(0) # any
master = rospy.getMaster()
rospy.ready()
setupPorts()
#f= open('testfile.txt','r')
f0 = open('/dev/ttyUSB0', 'r')
f1 = open('/dev/ttyUSB1', 'r')
f2 = open('/dev/ttyUSB2', 'r')
f3 = open('/dev/ttyUSB3', 'r')
# split on % to string and checksum
# check checksum
myPow = robotPower()
start_time = time.time()
last_time = start_time
while True:
current, blah, blah2 = select.select([f0, f1, f2, f3], [], [], 1)
for f in current:
if f == f0:
port = 0
port_string = 'f0'
if f == f1:
port = 1
port_string = 'f1'
if f == f2:
port = 2
port_string = 'f2'
if f == f3:
port = 3
port_string = 'f3'
line = f.readline()
halves = line.split('%')
if len(halves) != 2:
# print 'I did not split on \% correctly'
continue
halves[1] = halves[1].strip()
halves[0] = halves[0].lstrip('$')
# print line
# print halves
# print len(halves)
# print checksum256(halves[0])
# print hex2dec(halves[1])
# print hex(255)
# read first char and switch
message = halves[0]
# case C controller
# split on commas
# first element is controller number
# for each pair
# read index 01-07
# record value
if len(message) < 2:
print "error message too short: \"%s\" from \"%s\"" % (message,
line)
print "This often indicates a misconfigured serial port check port:"
print f
continue
if message[0] == 'C':
controller_number = int(message[1])
#print 'Controller on port %s says:'%(port_string)
splitmessage = message.split(',')
#print 'batteries present'
#print readmask(splitmessage[2])
mask = readmask(splitmessage[2])
for i in range(0, 8):
myPow.controllers[port].batteries[i].present = mask[i]
#print 'batteries charging'
#print readmask(splitmessage[4])
mask = readmask(splitmessage[2])
for i in range(0, 8):
myPow.controllers[port].batteries[i].charging = mask[i]
#print 'batteries supplying power to system'
#print readmask(splitmessage[6])
mask = readmask(splitmessage[6])
for i in range(0, 8):
myPow.controllers[port].batteries[
i].supplying_power = mask[i]
#print 'batteries which have charge power present'
#print readmask(splitmessage[10])
mask = readmask(splitmessage[10])
for i in range(0, 8):
myPow.controllers[port].batteries[i].w_charge_power = mask[
i]
#print 'batteries with "Power no good"'
#print readmask(splitmessage[12])
mask = readmask(splitmessage[12])
for i in range(0, 8):
myPow.controllers[port].batteries[i].power_no_good = mask[
i]
#print 'batteries charge inhibited'
#print readmask(splitmessage[14])
mask = readmask(splitmessage[14])
for i in range(0, 8):
myPow.controllers[port].batteries[
i].charge_inhibited = mask[i]
else:
pass
if message[0] == 'B':
controller_number = int(message[1])
battery_number = int(message[2])
#print 'Battery %d on Controller %d'%(battery_number, controller_number)
splitmessage = message.split(',')
for i in range(0, (len(splitmessage) - 1) / 2):
key = splitmessage[i * 2 + 1]
value = splitmessage[i * 2 + 2]
# print 'key %s , value %s'%(splitmessage[i*2+1],splitmessage[i*2+2])
# print 'key %s , value dec %s'%(splitmessage[i*2+1],hex2dec(splitmessage[i*2+2]))
if key == '09':
#print 'voltage is %f'%(float(hex2dec(value))/1000.0)
myPow.controllers[
port].batteries[battery_number].voltage = float(
hex2dec(value)) / 1000.0
if key == '0A':
intval = float(hex2dec(value))
if intval < 32767:
val = intval
else:
val = (intval - 65636.0)
#print 'current is %f'%(val/1000.0)
myPow.controllers[port].batteries[
battery_number].current = val / 1000.0
# case B battery
# split on commas
# first number is battery number
# foreach pair
# lookup index
# record value cast properly so pages 13 to 35
#case S system data
if message[0] == 'S':
splitmessage = message.split(',')
#print 'System Message'
for i in range(0, (len(splitmessage) - 1) / 2):
key = splitmessage[i * 2 + 1]
value = splitmessage[i * 2 + 2]
# print 'key %s , value %s'%(key,value)
# print 'key %s , value dec %s'%(key,hex2dec(value))
if key == '01':
#print 'time until exhaustion = %s minutes'%hex2dec(value)
myPow.controllers[port].time_remaining = int(
hex2dec(value))
if key == '02':
#reserved
pass
if key == '03':
#print 'text message to the system %s'%value
myPow.controllers[port].new_system_message(value)
if key == '04':
#print 'average charge percent %d'% hex2dec(value)
myPow.controllers[port].average_charge = int(
hex2dec(value))
#print time.time()
increment = 1.0
if time.time() - last_time > increment:
last_time = last_time + increment
print "displaying to screen"
myPow.print_remaining()
print "updating param server"
myPow.updateParamServer(master)
def listener_with_user_data():
print "Starting Listener"
rospy.TopicSub("/phase_space_snapshot", PhaseSpaceSnapshot, callback)
rospy.ready(NAME, anonymous=True)
print "Spinning"
rospy.spin()
def listener_publisher():
rospy.ready(NAME)
rospy.TopicSub(IN, MSG, callback)
rospy.spin()
def listener_with_user_data():
rospy.TopicSub("/pressure", PressureState, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
def __init__(self,
parent,
id=wx.ID_ANY,
title='PR2 GUI',
pos=wx.DefaultPosition,
size=(1280, 1024),
style=wx.DEFAULT_FRAME_STYLE):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
rospy.ready('pr2_gui', anonymous=True)
self._config = wx.Config("pr2_gui")
self._aui_manager = wx.aui.AuiManager(self)
self._visualizer_panel = visualizer_panel.DefaultVisualizationPanel(
self)
media_path = rostools.packspec.get_pkg_dir("gazebo_robot_description")
media_path += "/world/Media/"
media_paths = [media_path]
media_paths.append(media_path)
media_paths.append(media_path + "fonts")
media_paths.append(media_path + "materials")
media_paths.append(media_path + "materials/scripts")
media_paths.append(media_path + "materials/programs")
media_paths.append(media_path + "materials/textures")
media_paths.append(media_path + "models")
media_paths.append(media_path + "models/pr2_new")
ogre_tools.initializeResources(media_paths)
self._visualizer_panel.createDefaultVisualizers()
self._rosout_panel = wx_rosout.RosoutPanel(self)
self._monitor_panel = MonitorPanel(self)
self._monitor_panel.set_new_errors_callback(
self.on_monitor_errors_received)
self._aui_manager.AddPane(
self._visualizer_panel,
wx.aui.AuiPaneInfo().CenterPane().Center().Name('3dvis').Caption(
'3D Visualization'), '3D Visualization')
self._aui_manager.AddPane(
self._rosout_panel,
wx.aui.AuiPaneInfo().BottomDockable().Bottom().Name(
'rosout').Caption('Rosout'), 'Rosout')
self._aui_manager.AddPane(
self._monitor_panel,
wx.aui.AuiPaneInfo().BottomDockable().Bottom().BestSize(
wx.Size(
500,
600)).Name('runtime_monitor').Caption('Runtime Monitor'),
'Runtime Monitor')
self.add_camera_pane("forearm_right", False)
self.add_camera_pane("forearm_left", False)
self.add_camera_pane("axis_right", True)
self.add_camera_pane("axis_left", True)
self.add_camera_pane("stereo_left", False)
self._aui_manager.Update()
self.load_config()
self.create_menu_bar()
self.Bind(wx.aui.EVT_AUI_PANE_CLOSE, self.on_pane_close)
self.Bind(wx.EVT_CLOSE, self.on_close)
def monitorBatteriesMain(argv, stdout, env):
# default arguments
server = "http://localhost"
server_port = rospy.DEFAULT_TEST_PORT
#check arguments for a help flag
optlist, args = getopt.getopt(argv[1:], "h?p:s:", ["help","port=","server=","test"])
for o, a in optlist:
if o in ("-h","-?","--help"):
usage(stdout, argv[0])
return
elif o in ("--test"):
server_port = rospy.DEFAULT_TEST_PORT
elif o in ("-p", "--port"):
server_port = a
elif o in ("-s", "--server"):
server = a
serverUri = '%s:%s/'%(server,server_port)
print "Looking for server at %s"%serverUri
os.environ[rospy.ROS_MASTER_URI] = serverUri
os.environ[rospy.ROS_NODE] = NAME
os.environ[rospy.ROS_PORT] = str(0) # any
master = rospy.getMaster()
rospy.ready()
setupPorts()
#f= open('testfile.txt','r')
f0 = open('/dev/ttyUSB0','r')
f1 = open('/dev/ttyUSB1','r')
f2 = open('/dev/ttyUSB2','r')
f3 = open('/dev/ttyUSB3','r')
# split on % to string and checksum
# check checksum
myPow = robotPower()
start_time = time.time()
last_time = start_time
while True:
current, blah, blah2 = select.select([f0,f1,f2,f3],[],[],1)
for f in current:
if f == f0:
port = 0;
port_string = 'f0'
if f == f1:
port = 1;
port_string = 'f1'
if f == f2:
port = 2;
port_string = 'f2'
if f == f3:
port = 3;
port_string = 'f3'
line = f.readline()
halves = line.split('%')
if len(halves) != 2:
# print 'I did not split on \% correctly'
continue
halves[1]=halves[1].strip()
halves[0]=halves[0].lstrip('$')
# print line
# print halves
# print len(halves)
# print checksum256(halves[0])
# print hex2dec(halves[1])
# print hex(255)
# read first char and switch
message = halves[0]
# case C controller
# split on commas
# first element is controller number
# for each pair
# read index 01-07
# record value
if len(message) < 2:
print "error message too short: \"%s\" from \"%s\""%(message, line)
print "This often indicates a misconfigured serial port check port:"
print f
continue
if message[0] == 'C':
controller_number = int(message[1])
#print 'Controller on port %s says:'%(port_string)
splitmessage = message.split(',')
#print 'batteries present'
#print readmask(splitmessage[2])
mask = readmask(splitmessage[2])
for i in range(0,8):
myPow.controllers[port].batteries[i].present = mask[i]
#print 'batteries charging'
#print readmask(splitmessage[4])
mask = readmask(splitmessage[2])
for i in range(0,8):
myPow.controllers[port].batteries[i].charging = mask[i]
#print 'batteries supplying power to system'
#print readmask(splitmessage[6])
mask = readmask(splitmessage[6])
for i in range(0,8):
myPow.controllers[port].batteries[i].supplying_power = mask[i]
#print 'batteries which have charge power present'
#print readmask(splitmessage[10])
mask = readmask(splitmessage[10])
for i in range(0,8):
myPow.controllers[port].batteries[i].w_charge_power = mask[i]
#print 'batteries with "Power no good"'
#print readmask(splitmessage[12])
mask = readmask(splitmessage[12])
for i in range(0,8):
myPow.controllers[port].batteries[i].power_no_good = mask[i]
#print 'batteries charge inhibited'
#print readmask(splitmessage[14])
mask = readmask(splitmessage[14])
for i in range(0,8):
myPow.controllers[port].batteries[i].charge_inhibited = mask[i]
else:
pass
if message[0] == 'B':
controller_number = int(message[1])
battery_number = int(message[2])
#print 'Battery %d on Controller %d'%(battery_number, controller_number)
splitmessage = message.split(',')
for i in range(0,(len(splitmessage)-1)/2):
key = splitmessage[i*2+1]
value = splitmessage[i*2+2]
# print 'key %s , value %s'%(splitmessage[i*2+1],splitmessage[i*2+2])
# print 'key %s , value dec %s'%(splitmessage[i*2+1],hex2dec(splitmessage[i*2+2]))
if key == '09':
#print 'voltage is %f'%(float(hex2dec(value))/1000.0)
myPow.controllers[port].batteries[battery_number].voltage = float(hex2dec(value))/1000.0
if key == '0A':
intval = float(hex2dec(value))
if intval < 32767:
val = intval
else:
val = (intval - 65636.0)
#print 'current is %f'%(val/1000.0)
myPow.controllers[port].batteries[battery_number].current = val/1000.0
# case B battery
# split on commas
# first number is battery number
# foreach pair
# lookup index
# record value cast properly so pages 13 to 35
#case S system data
if message[0] == 'S':
splitmessage = message.split(',')
#print 'System Message'
for i in range(0,(len(splitmessage)-1)/2):
key = splitmessage[i*2+1]
value = splitmessage[i*2+2]
# print 'key %s , value %s'%(key,value)
# print 'key %s , value dec %s'%(key,hex2dec(value))
if key == '01':
#print 'time until exhaustion = %s minutes'%hex2dec(value)
myPow.controllers[port].time_remaining = int(hex2dec(value))
if key == '02':
#reserved
pass
if key == '03':
#print 'text message to the system %s'%value
myPow.controllers[port].new_system_message(value)
if key == '04':
#print 'average charge percent %d'% hex2dec(value)
myPow.controllers[port].average_charge = int(hex2dec(value))
#print time.time()
increment = 1.0
if time.time() - last_time > increment:
last_time = last_time + increment
print "displaying to screen"
myPow.print_remaining()
print "updating param server"
myPow.updateParamServer(master)
def listener_with_user_data():
rospy.TopicSub("/mechanism_state", MechanismState, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
# make sure we got it
self.assert_(len(self.callback_data),
"no callback data from %s" % PUBNODE)
for d in self.callback_data:
errorstr = "callback msg field [%%s] from %s does not match" % PUBNODE
self.assertAlmostEqual(3.14, d.time, 2, errorstr % "time")
self.assertEquals(250, len(d.joint_states))
self.assertEquals(260, len(d.actuator_states))
for f in d.joint_states:
self.assertEquals("jointstate", f.name)
self.assertAlmostEqual(1.0, f.position, 1)
self.assertAlmostEqual(1.0, f.velocity, 1)
self.assertAlmostEqual(1.0, f.applied_effort, 1)
self.assertAlmostEqual(1.0, f.commanded_effort, 1)
for i in xrange(0, len(d.actuator_states)):
f = d.actuator_states[i]
self.assertEquals("actuatorstate", f.name)
self.assertEquals(1.0, f.motor_voltage)
self.assertEquals(i, f.encoder_count)
self.assertEquals(0, f.calibration_reading)
self.assertEquals(i + 1, f.last_calibration_falling_edge)
self.assertEquals(i + 2, f.last_calibration_rising_edge)
self.assertEquals(0, f.num_encoder_errors)
if __name__ == '__main__':
rospy.ready(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
def listener_with_user_data():
rospy.TopicSub("/goal", Planner2DGoal, callback)
rospy.ready(NAME, anonymous=True)
rospy.spin()
import sys
import time
from std_msgs.msg import Image
from std_msgs.msg import ImageArray
import rospy
import pyrob.util as ut
import opencv.highgui as hg
def display_array(iar):
left = ut.ros2cv(iar.images[0])
right = ut.ros2cv(iar.images[1])
hg.cvShowImage('channel 1', left)
hg.cvShowImage('channel 2', right)
hg.cvWaitKey(5)
def display_single(im):
imcv = ut.ros2cv(im)
hg.cvShowImage('channel 1', imcv)
hg.cvWaitKey(5)
if __name__ == '__main__':
hg.cvNamedWindow('channel 1', 1)
hg.cvNamedWindow('channel 2', 1)
rospy.TopicSub('images', ImageArray, display_array)
rospy.TopicSub('image', Image, display_single)
rospy.ready('camview_py')
rospy.spin()