Esempio n. 1
0
def main(args):
  vod = VO(int(args[1]))

  rospy.ready('vo')
  rospy.spin()
  vod.vo.summarize_timers()
  vod.dump()
Esempio n. 2
0
def listener():
    app = wx.PySimpleApp()
    rospy.ready(NAME, anonymous=True)
    
    frame = MainWindow(None, -1, "Runtime Monitor")
    frame.Show()
    
    app.MainLoop()
Esempio n. 3
0
def listener():
    app = wx.PySimpleApp()
    rospy.ready(NAME, anonymous=True)
    
    frame = MainWindow(None, -1, "PR2 Power Board")
    frame.Show()
    
    app.MainLoop()
Esempio n. 4
0
def main(args):

  vod = lineperftest(args[1])

  rospy.ready('lineperf_%s' % args[1])
  rospy.spin()
  if args[1] == 'send':
    vod.report()
Esempio n. 5
0
 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)
Esempio n. 6
0
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()
Esempio n. 7
0
    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)
Esempio n. 8
0
 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)
Esempio n. 9
0
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()
Esempio n. 10
0
  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
Esempio n. 11
0
  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
Esempio n. 12
0
 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)
Esempio n. 13
0
 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)
Esempio n. 14
0
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)
Esempio n. 15
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()
Esempio n. 16
0
  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
Esempio n. 17
0
    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()
Esempio n. 18
0
    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)
Esempio n. 19
0
    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)
Esempio n. 20
0
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"
Esempio n. 21
0
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()
Esempio n. 22
0
            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)
Esempio n. 23
0
def listener_with_user_data():
    rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()
Esempio n. 24
0
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'
Esempio n. 25
0
 def run(self):
   rospy.ready(NAME, anonymous=True)
   rospy.spin()
Esempio n. 26
0
## @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'


Esempio n. 27
0
def listener():
    pub = rospy.TopicPub("/diagnostics", DiagnosticMessage)
    rospy.ready(NAME, anonymous=True)
    while not rospy.is_shutdown():
        loop(pub)
        time.sleep(1)
Esempio n. 28
0
def listener_with_user_data():
    rospy.TopicSub("/mechanism_state", MechanismState, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()
Esempio n. 29
0
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)
Esempio n. 30
0
def listener_with_user_data():
    print "Starting Listener"
    rospy.TopicSub("/phase_space_snapshot", PhaseSpaceSnapshot, callback)
    rospy.ready(NAME, anonymous=True)
    print "Spinning"
    rospy.spin()
Esempio n. 31
0
def listener_publisher():
    rospy.ready(NAME)
    rospy.TopicSub(IN, MSG, callback)
    rospy.spin()
Esempio n. 32
0
def listener_with_user_data():
    rospy.TopicSub("/pressure", PressureState, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()
Esempio n. 33
0
    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)
Esempio n. 34
0
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)
Esempio n. 35
0
def listener_with_user_data():
    rospy.TopicSub("/mechanism_state", MechanismState, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()
Esempio n. 36
0
        # 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)
Esempio n. 37
0
def listener_with_user_data():
    rospy.TopicSub("/goal", Planner2DGoal, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()
Esempio n. 38
0
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()