def __init__(self, decimate): rospy.TopicSub('/videre/images', ImageArray, self.handle_array, queue_size=2, buff_size=7000000) rospy.TopicSub('/videre/cal_params', String, self.handle_params) self.pub_vo = rospy.Publisher("/vo", VOPose) self.vo = None self.decimate = decimate self.modulo = 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()
def main(argv): people_tracker = PeopleTracker() if len(argv) > 0 and argv[0] == "-bag": people_tracker.usebag = True # Use a bag file if people_tracker.usebag: import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass num_frames = 0 start_frame = 4700 end_frame = 4900 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) num_frames += 1 else: pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else: print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass people_tracker.pub = rospy.Publisher('head_controller/track_point', PointStamped) rospy.init_node('videre_face_tracker', anonymous=True) rospy.TopicSub('/head_controller/track_point', PointStamped, people_tracker.point_stamped) rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame) rospy.TopicSub('/videre/cal_params', String, people_tracker.params) rospy.spin()
def __init__(self, mode, library): self.mode = mode self.library = library rospy.TopicSub('/videre/images', ImageArray, self.display_array) rospy.TopicSub('/videre/cal_params', String, self.display_params) rospy.TopicSub('/vo/tmo', Pose44, self.handle_corrections) self.viz_pub = rospy.Publisher("/overlay", Lines) self.vo_key_pub = rospy.Publisher("/vo/key", Frame) self.Tmo = Pose() self.mymarker1 = Marker(10) self.mymarkertrail = [Marker(11 + i) for i in range(10)] self.trail = [] self.vis = Vis()
def test_scan(self): print "LNK\n" rospy.TopicSub("base_scan", LaserScan, self.pointInput) rospy.init_node(NAME, anonymous=True) timeout_t = time.time() + TEST_DURATION 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 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 __init__(self, vo, library): self.vo = vo self.library = library rospy.TopicSub('/vo/key', Frame, self.incoming_frame) self.pub_tmo = rospy.Publisher("/vo/tmo", Pose44) self.frameq = Queue() self.mark0 = Marker(1) self.mark1 = Marker(2)
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 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.init_node(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 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 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 testcameras(self): print " wait TEST_INIT_WAIT sec for objects to settle " time.sleep(TEST_INIT_WAIT) print " subscribe stereo left image from ROS " #rospy.TopicSub("test_camera/image", Image, self.imageInput) # this is a test camera, simply looking at the cups rospy.TopicSub("stereo_l/image", Image, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera) rospy.init_node(NAME, anonymous=True) #self.pollThread.start() timeout_t = time.time() + TEST_DURATION while not rospy.is_shutdown() and not self.success 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 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 trex_fast") self.assert_(self.success)
def subscribe(self, itemPath, style, ChannelType=RosChannel): item = self._getItem(itemPath) topicName = self._getTopic(itemPath) assert topicName in self.topics if not self.topics[topicName].isRegistered: self.topicsSubs[topicName] = RosSubscriber(self.topics[topicName]) messageName, messageModName, messageType = self.topics[topicName].qualifiedMessageType exec('import %s' % messageModName) rospy.TopicSub(self._getTopic(itemPath), eval(messageType), self.topicsSubs[topicName].callback) channel = ChannelType(itemPath, style) item.channels.append(channel) return channel
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 registerTopic(self, topicName, topicTypeName, callback): """Register to the topic using only the names (strings) of the topic, type.""" # TODO: encapsulate with exceptions # Loading the packageName # TODO: how to check if it is already loaded? print 'topic type: %s' % topicTypeName (modName, messageName) = messageNames(topicTypeName) importMessageModule(modName, messageName) exec('import %s' % messageName) print eval(messageName) rospy.TopicSub(topicName, eval(messageName), callback) print 'registering done'
def test_ms_msg(self): self.assertEquals(0, len(self.callback_data), "invalid test fixture") if 0: # wait at most 5 seconds for publisher to be registered timeout_t = time.time() + 5.0 while not rostest.is_subscriber( rospy.resolve_name(PUBTOPIC), rospy.resolve_name(PUBNODE)) and time.time() < timeout_t: time.sleep(0.1) self.assert_( rostest.is_subscriber(rospy.resolve_name(PUBTOPIC), rospy.resolve_name(PUBNODE)), "%s is not up" % PUBNODE) print "Subscribing to ", SUBTOPIC rospy.TopicSub(SUBTOPIC, MSG, self._test_ms_callback) sleep_time = 3.0 print "Waiting %s seconds to collect messages" % sleep_time time.sleep(sleep_time) # 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_low_transition) self.assertEquals(i + 2, f.last_calibration_high_transition) self.assertEquals(0, f.num_encoder_errors)
self.addPoint(state.PosAct) # A worker class for the ros node # To be deprecated soon class WorkerListener(Thread): def __init__(self): Thread.__init__(self) def run(self): rospy.ready(NAME, anonymous=True) rospy.spin() channel = MyChannel() channel2 = MyChannelAct() rospy.TopicSub('ARM_L_PAN_state', RotaryJointState, channel.callback) rospy.TopicSub('ARM_L_PAN_state', RotaryJointState, channel2.callback) app = wx.PySimpleApp(0) worker = WorkerListener() worker.start() frame = wx.Frame(None, -1, "") panel = MyPlot(frame) panel.addChannel(channel) panel.addChannel(channel2) frame.Show() print '>>>>>>>>>>>>>>' app.MainLoop() print '>>>>>>>>>>>>>>' #rospy.spin()
class ForearmTest(BaseTest): def __init__(self, parent, serial, func): self.ready = False self.serial1 = serial[0:7] print self.serial1 self.count = 1 self.finished = False self.testcount = 1 # Load the XRC resource self.res2 = xrc.XmlResource(execution_path('forearm_test.xrc')) # Initialize the BaseTest with these parts BaseTest.__init__(self, parent, serial, func, 'forearm Test') self.roslaunch = None self.topic = None self.data_topic = None self.response = DiagnosticMessage(None, []) self.data_dict = {} def instruct_panel_gen(self, parent): # Load the instruction and test panels from the XRC resource panel = 'instruct_panel_' + self.serial1 print panel return self.res2.LoadPanel(parent, panel) def test_panel_gen(self, parent): test_panel = self.res2.LoadPanel(parent, 'test_panel') test_panel.Bind(wx.EVT_BUTTON, self.EStop, id=xrc.XRCID('ESTOP_button')) return self.res2.LoadPanel(parent, 'test_panel') # This is what runs once the instructions are read def Run(self): thread.start_new_thread(self.RunThread, (None, )) def RunThread(self, arg): #Create a roslauncher config = roslaunch.ROSLaunchConfig() self.count = 1 self.finished = False self.testcount = 1 self.ready = False # Try loading the test XML file try: xmlFile = execution_path( str('xml/' + self.serial1 + '_forearm_test.xml')) loader = roslaunch.XmlLoader() loader.load(xmlFile, config) except roslaunch.XmlParseException, e: wx.CallAfter( self.Cancel, 'Could not load back-end XML to launch necessary nodes. Make sure the GUI is up to date.' ) return # Make sure we get a fresh master config.master.auto = config.master.AUTO_RESTART # Bring up the nodes self.roslaunch = roslaunch.ROSLaunchRunner(config) self.roslaunch.launch() self.topic = rospy.TopicSub("/diagnostics", DiagnosticMessage, self.OnMsg) self.data_topic = rospy.TopicSub("/hysteresis_data", ChannelFloat32, self.OnData)
def listener_with_user_data(): rospy.TopicSub("/mechanism_state", MechanismState, callback) rospy.ready(NAME, anonymous=True) rospy.spin()
def listener_with_user_data(): rospy.TopicSub("/pressure", PressureState, callback) rospy.ready(NAME, anonymous=True) rospy.spin()
def listener_with_user_data(): rospy.TopicSub("/odom", RobotBase2DOdom, odom_callback) rospy.TopicSub("/localizedpose", RobotBase2DOdom, localized_callback) rospy.ready(NAME, anonymous=True) rospy.spin()
def __init__(self): rospy.init_node("TestPlotter", anonymous=True) self.data_topic = rospy.TopicSub("/test_data", TestData, self.OnData)
import pyrob.util #def callback_array(iar): # left = iar.images[0] # right = iar.images[1] # # start = time.time() # leftcv = to_cv(left) # rightcv = to_cv(right) # print '%.4f' % (time.time() - start) # # print iar.header.seq, # hg.cvShowImage('left', leftcv) # hg.cvShowImage('right', rightcv) # hg.cvWaitKey(5) def callback_image(im): t = time.time() cvim = pyrob.util.ros2cv(im) hg.cvShowImage('left', cvim) hg.cvWaitKey(5) print 'total', time.time() - t if __name__ == '__main__': hg.cvNamedWindow('left', 1) rospy.TopicSub('image', Image, callback_image) rospy.ready('test_pycv') rospy.spin()
def listener_publisher(): rospy.ready(NAME) rospy.TopicSub(IN, MSG, callback) rospy.spin()
def test_ms_msg(self): self.assert_(self.callback_data is None, "invalid test fixture") # wait at most 5 seconds for listenerpublisher to be registered timeout_t = time.time() + 5.0 while not rostest.is_subscriber( rospy.resolve_name(PUBTOPIC), rospy.resolve_name(LPNODE)) and time.time() < timeout_t: time.sleep(0.1) self.assert_( rostest.is_subscriber(rospy.resolve_name(PUBTOPIC), rospy.resolve_name(LPNODE)), "%s is not up" % LPNODE) print "Publishing to ", PUBTOPIC pub = rospy.TopicPub(PUBTOPIC, MSG) print "Subscribing to ", SUBTOPIC rospy.TopicSub(SUBTOPIC, MSG, self._test_ms_callback) # publish about 10 messages for fun import random val = random.randint(0, 109812312) msg = "hi [%s]" % val header = None for i in xrange(0, 10): # The test message could be better in terms of the values # it assigns to leaf fields, but the main focus is trying # to dig up edge conditions in the embeds, especially with # respect to arrays and embeds. actuator_states = [ ActuatorState('name0', 0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0), ActuatorState('name1', 1, 1.0, 1.0, 1.0, 1.0, 1, 1, 1, 1, 1, 1.0, 1.0, 1.0, 1.0, 1), ] joint_states = [ JointState('name0', 0.0, 0.0, 0.0, 0.0), JointState('name1', 1.0, 1.0, 1.0, 1.0), ] pub.publish(MSG(header, 2.0, actuator_states, joint_states)) 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_rising_edge) 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_rising_edge) self.assertEquals( 1, self.callback_data.actuator_states[1].num_encoder_errors)
def main(argv): people_tracker = PeopleTracker() if len(argv) > 0 and argv[0] == "-bag": people_tracker.usebag = True # if len(argv)>1 and argv[1]=="-visualize": # people_tracker.visualize = True # Use a bag file if people_tracker.usebag: import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass # if people_tracker.visualize: # people_tracker.pub = rospy.Publisher('/std_msgs/full_cloud',PointCloud) # rospy.init_node('videre_face_tracker',anonymous=True) num_frames = 0 start_frame = 4700 end_frame = 5000 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) if people_tracker.visualize: s = raw_input( "Press any key in this window to proceed to the next frame." ) elif num_frames >= end_frame: break num_frames += 1 else: pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else: print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass #people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped) people_tracker.pub = rospy.Publisher( '/stereo_face_feature_tracker/position_measurement', PositionMeasurement) rospy.init_node('videre_face_tracker', anonymous=True) #rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped) rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame) rospy.TopicSub('/videre/cal_params', String, people_tracker.params) rospy.spin()
def listener_with_user_data(): rospy.TopicSub("/goal", Planner2DGoal, callback) rospy.ready(NAME, anonymous=True) rospy.spin()