def test_param_api(self): # test get_param_names param_names = rospy.get_param_names() for n in ['/param1', 'param1', '~param1', 'param_int', 'param_float']: self.assert_(rospy.resolve_name(n) in param_names) # test has_param self.assert_(rospy.has_param('/run_id')) self.assert_(rospy.has_param('/param1')) self.assert_(rospy.has_param('param1')) self.assert_(rospy.has_param('~param1')) # test search_param self.assertEquals(None, rospy.search_param('not_param1')) self.assertEquals(rospy.resolve_name('~param1'), rospy.search_param('param1')) self.assertEquals(rospy.resolve_name('parent_param'), rospy.search_param('parent_param')) self.assertEquals("/global_param", rospy.search_param('global_param')) # test get_param on params set in rostest file self.assertEquals('global_param1', rospy.get_param('/param1')) self.assertEquals('parent_param1', rospy.get_param('param1')) self.assertEquals('private_param1', rospy.get_param('~param1')) # - type tests self.assertEquals(1, rospy.get_param('param_int')) self.assertAlmostEquals(2., rospy.get_param('param_float')) self.assertEquals(True, rospy.get_param('param_bool')) self.assertEquals("hello world", rospy.get_param('param_str')) # test default behavior get_param try: rospy.get_param('not_param1') self.fail("should have raised KeyError") except KeyError: pass self.assertEquals('parent_param1', rospy.get_param('param1', 'foo')) self.assertEquals('private_param1', rospy.get_param('~param1', 'foo')) self.assertEquals('myval', rospy.get_param('not_param1', 'myval')) self.assertEquals('myval', rospy.get_param('~not_param1', 'myval')) self.assertEquals(None, rospy.get_param('not_param1', None)) self.assertEquals(None, rospy.get_param('~not_param1', None)) # test set/get roundtrips vals = [ '1', 1, 1., [1, 2, 3, 4], {'a': 1, 'b': 2}] for v in vals: self.failIf(rospy.has_param("cp_param")) try: rospy.get_param('cp_param1') self.fail("should have thrown KeyError") except KeyError: pass self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) rospy.set_param("cp_param", v) self.assert_(rospy.has_param("cp_param")) self.assertEquals(v, rospy.get_param("cp_param")) self.assertEquals(rospy.resolve_name('cp_param'), rospy.search_param('cp_param')) # erase the param and recheck state rospy.delete_param('cp_param') self.failIf(rospy.has_param("cp_param")) self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) self.assertEquals(None, rospy.search_param('cp_param'))
def wait_for_subscriber(node_name, topic, timeout=5.0): '''Blocks until $node_name subscribes to $topic Useful mostly in integration tests -- I would counsel against use elsewhere ''' end_time = time.time() + timeout # Wait for time-out or ros-shutdown while (time.time() < end_time) and (not rospy.is_shutdown()): subscribed = rostest.is_subscriber( rospy.resolve_name(topic), rospy.resolve_name(node_name) ) # Success scenario: node subscribes if subscribed: break time.sleep(0.1) # Could do this with a while/else # But chose to explicitly check success = rostest.is_subscriber( rospy.resolve_name(topic), rospy.resolve_name(node_name) ) return success
def main(): rospy.init_node("long_term_relations") global storedRelations, lastDataReceivedAt storedRelations = dict() lastDataReceivedAt = rospy.Time.now() growthAmount = rospy.get_param("~growth", 0.5 / 3.5) # growth of relation strength (between 0.0 and 1.0) per second slowDecayAmount = rospy.get_param("~slow_decay", 0.5 / 40.0) # slow decay of relation strength (between 0.0 and 1.0) per second fastDecayAmount = rospy.get_param("~fast_decay", 0.5 / 5.0) # fast decay, when maximum distance is exceeded maxDistance = rospy.get_param("~max_distance", 4.0) # distance above which relation strength will be forced to 0.0 trackedPersonsTopic = rospy.resolve_name("/spencer/perception/tracked_persons") inputRelationsTopic = rospy.resolve_name("/spencer/perception/social_relations") outputRelationsTopic = rospy.resolve_name("/spencer/perception/long_term_social_relations") trackSubscriber = message_filters.Subscriber(trackedPersonsTopic, TrackedPersons) inputRelationsSubscriber = message_filters.Subscriber(inputRelationsTopic, SocialRelations) timeSynchronizer = message_filters.TimeSynchronizer([trackSubscriber, inputRelationsSubscriber], 5) timeSynchronizer.registerCallback(newDataAvailable) rospy.loginfo("Subscribing to " + inputRelationsTopic + " and " + trackedPersonsTopic) global pub pub = rospy.Publisher(outputRelationsTopic, SocialRelations, queue_size=3) rospy.loginfo("Publishing long-term relations on " + outputRelationsTopic) rospy.spin()
def __init__(self): laserscanTopic = rospy.resolve_name("laser") segmentationTopic = rospy.resolve_name("laser_segmentation") cloudTopic = segmentationTopic + "_cloud" markersTopic = segmentationTopic + "_markers" ### Configurable parameters ########## self.unlabelledColor = rospy.get_param("~unlabelled_color", [0.7, 0.7, 0.7]) self.fontScale = rospy.get_param("~font_scale", 1.0) self.minDistanceToSensor = rospy.get_param("~min_distance_to_sensor", 0.0) # to filter out wrong echoes close to the sensor ### End of configurable parameters ### self._lastMarkerCount = 0 self.cloudPublisher = rospy.Publisher(cloudTopic, PointCloud2) self.markerArrayPublisher = rospy.Publisher(markersTopic, MarkerArray) laserSubscriber = message_filters.Subscriber(laserscanTopic, LaserScan) segmentationSubscriber = message_filters.Subscriber(segmentationTopic, LaserscanSegmentation) self.timeSynchronizer = message_filters.TimeSynchronizer([laserSubscriber, segmentationSubscriber], 20) self.timeSynchronizer.registerCallback(self.newSegmentationReceived) rospy.loginfo("Visualizing laser scan segmentation (for laser scans published at %s) at %s on topics %s and %s" % (laserscanTopic, segmentationTopic, cloudTopic, markersTopic) ) rospy.spin()
def testRegistrations(self): # setUp() ensures the node has registered with the master topics = self.apiSuccess(self.master.getPublishedTopics(self.caller_id, '')) topic_names = [t for t, type in topics] required_topic_pubs = [rospy.resolve_name(t) for t in _required_publications] required_topic_subs = [rospy.resolve_name(t) for t in _required_subscriptions] self._checkTopics(required_topic_pubs, topics) # now check types topicTypeMap = self._createTopicTypeMap() for topic, type in topics: if topic in topicTypeMap: self.assertEquals(type, topicTypeMap[topic], "topic [%s]: type [%s] does not match expected [%s]"%(type, topic, topicTypeMap[topic])) # now check actual URIs node_name = self.test_node systemState = self.apiSuccess(self.master.getSystemState(self.caller_id)) pubs, subs, srvs = systemState for topic, list in pubs: if topic in required_topic_pubs: self.assert_(node_name in list, "%s not in %s"%(self.node_api, list)) for topic, list in subs: if topic in required_topic_subs: self.assert_(node_name in list, "%s not in %s"%(self.node_api, list)) for service, list in srvs: #TODO: no service tests yet pass
def test_subscriber_first(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.Publisher(PUBTOPIC, MSG) rospy.Subscriber(LPTOPIC, MSG, self._test_subscriber_first_callback) # publish about 10 messages for fun import random val = random.randint(0, 109812312) msg = "hi [%s]"%val for i in xrange(0, 10): pub.publish(MSG(msg)) 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") self.assertEquals(msg, self.callback_data.data, "callback data from listenerpublisher does not match")
def test_set_param(self): try: rospy.set_param(None, 'foo'); self.fail("set_param(None) succeeded") except: pass try: rospy.set_param('', 'foo'); self.fail("set_param('') succeeded") except: pass rostest_tests = { 'spstring': "string", 'spint0': 0, 'spint10': 10, 'spfloat0': 0.0, 'spfloat10': 10.0, "spnamespace/string": "namespaced string", } initial_param_names = rospy.get_param_names() param_names = [rospy.resolve_name(k) for k in rostest_tests.keys()] for k, v in rostest_tests.iteritems(): self.failIf(rospy.has_param(k)) self.failIf(rospy.has_param(rospy.resolve_name(k))) rospy.set_param(k, v) self.assert_(rospy.has_param(k)) self.assert_(rospy.has_param(rospy.resolve_name(k))) self.assertEquals(v, rospy.get_param(k)) self.assertEquals(v, rospy.get_param(rospy.resolve_name(k))) correct_state = set(initial_param_names + param_names) self.failIf(correct_state ^ set(rospy.get_param_names()))
def main(): rospy.init_node("tracked_groups") trackGroups.largestGroupId = -1 trackGroups.groupIdAssignmentMemory = deque(maxlen=300) # to remember which sets of track IDs were assigned which group IDs trackGroups.groupIdRemapping = deque(maxlen=50) # to remap published(!) group IDs so that they are consecutive, even if internally they are not trackGroups.largestPublishedGroupId = -1 trackedPersonsTopic = rospy.resolve_name("/spencer/perception/tracked_persons") socialRelationsTopic = rospy.resolve_name("/spencer/perception/social_relations") trackedGroupsTopic = rospy.resolve_name("/spencer/perception/tracked_groups") trackSubscriber = message_filters.Subscriber(trackedPersonsTopic, TrackedPersons) socialRelationsSubscriber = message_filters.Subscriber(socialRelationsTopic, SocialRelations) timeSynchronizer = message_filters.TimeSynchronizer([trackSubscriber, socialRelationsSubscriber], 100) timeSynchronizer.registerCallback(newDataAvailable) rospy.loginfo("Subscribing to " + socialRelationsTopic + " and " + trackedPersonsTopic) global pub pub = rospy.Publisher(trackedGroupsTopic, TrackedGroups, queue_size=3) rospy.loginfo("Publishing tracked groups on " + trackedGroupsTopic) rospy.spin()
def __init__(self): self.lockobj = thread.allocate_lock() # variables self.reference_image_msg = None self.templates = {} # frame_id : ref_image,ser_frame,ser_rect self.clock = [rospy.Time.now()] # publisher self.reference_pub = rospy.Publisher( "current_template", Image, queue_size=1) self.result_pub = rospy.Publisher( "result", TransformStamped, queue_size=1) self.debug_pub = rospy.Publisher("debug_image", Image, queue_size=1) # subscriber self.reference_image_sub = rospy.Subscriber(rospy.resolve_name("reference"), Image,self.ref_image_callback,queue_size=1) self.search_image_sub = rospy.Subscriber(rospy.resolve_name("search"), Image,self.ser_image_callback,queue_size=1) self.reference_point_sub = rospy.Subscriber( rospy.resolve_name("set_reference_point"), PointStamped,self.set_reference_point_callback,queue_size=1) self.search_sub = rospy.Subscriber(rospy.resolve_name("set_search_rect"), Rect,self.set_search_callback,queue_size=1) self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
def __init__(self): laserscanTopic = rospy.resolve_name("laser") segmentationTopic = rospy.resolve_name("laser_annotations") trackedPersonsTopic = rospy.resolve_name("tracked_persons") detectedPersonsTopic = rospy.resolve_name("detected_persons") self.trackedPersonsPublisher = rospy.Publisher(trackedPersonsTopic, TrackedPersons) self.detectedPersonsPublisher = rospy.Publisher(detectedPersonsTopic, DetectedPersons) self._detectionIdCounter = 0 self._lastDataStamp = None self._firstTrackEncounterLookup = dict() self._previousCentroidLookup = dict() self.laserSubscriber = message_filters.Subscriber(laserscanTopic, LaserScan) self.segmentationSubscriber = message_filters.Subscriber(segmentationTopic, LaserscanSegmentation) self.timeSynchronizer = message_filters.TimeSynchronizer( [self.laserSubscriber, self.segmentationSubscriber], 50 ) self.timeSynchronizer.registerCallback(self.newSegmentationReceived) rospy.loginfo( "Publishing detected and tracked persons from laser scan segmentation (%s and %s) at %s and %s" % (laserscanTopic, segmentationTopic, detectedPersonsTopic, trackedPersonsTopic) ) rospy.spin()
def set_transform(self, req): parent_frame = rospy.resolve_name(req.transform.header.frame_id) child_frame = rospy.resolve_name(req.transform.child_frame_id) self._transforms[(parent_frame, child_frame)] = req.transform # Publish the transform at least once before returning. req.transform.header.stamp = rospy.Time.now() self._broadcaster.sendTransform(req.transform) return SetStaticTransformResponse(True)
def main(): rospy.init_node('localizer',log_level=rospy.DEBUG ) loc = Localizer() sub0 = message_filters.Subscriber(rospy.resolve_name("image0"), Image) sub1 = message_filters.Subscriber(rospy.resolve_name("image1"), Image) ts = ApproximateSynchronizer(0.5, [sub0, sub1], 10) ts.registerCallback(loc.callback) rospy.spin()
def test_get_has_param(self): # test error conditions try: rospy.get_param(None) self.fail("get_param(None) succeeded") except: pass try: rospy.get_param("") self.fail("get_param('') succeeded") except: pass # should be keyerror try: rospy.get_param("non-existent") self.fail("get_param('non-existent') succeeded") except: pass try: rospy.has_param(None, "foo") self.fail("has_param(None) succeeded") except: pass try: rospy.has_param("") self.fail("has_param('') succeeded") except: pass self.failIf(rospy.has_param("non-existent"), "has_param('non-existent') succeeded") # validate get_param against values on the param server rostest_tests = { "string": "string", "int0": 0, "int10": 10, "float0": 0.0, "float10": 10.0, "namespace/string": "namespaced string", } param_names = [rospy.resolve_name(k) for k in rostest_tests.keys()] # test get parameter names diff = set(param_names) ^ set(test_param_filter(rospy.get_param_names())) self.failIf(diff, diff) # test for existing and value for k, v in rostest_tests.iteritems(): self.assert_(rospy.has_param(k)) self.assert_(rospy.has_param(rospy.resolve_name(k))) if not type(v) == float: self.assertEqual(v, rospy.get_param(k)) self.assertEqual(v, rospy.get_param(rospy.resolve_name(k))) else: self.assertAlmostEqual(v, rospy.get_param(k), 1) self.assertAlmostEqual(v, rospy.get_param(rospy.resolve_name(k)), 1)
def param_talker(): rospy.init_node('param_talker') # Fetch values from the Parameter Server. In this example, we fetch # parameters from three different namespaces: # # 1) global (/global_example) # 2) parent (/foo/utterance) # 3) private (/foo/param_talker/topic_name) ''' # fetch a /global parameter #global_example = rospy.get_param("/global_example") #rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example) # fetch the utterance parameter from our parent namespace #utterance = rospy.get_param('utterance') rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance) # fetch topic_name from the ~private namespace topic_name = rospy.get_param('~topic_name') rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name) # fetch a parameter, using 'default_value' if it doesn't exist default_param = rospy.get_param('default_param', 'default_value') rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param) # fetch a group (dictionary) of parameters gains = rospy.get_param('gains') p, i, d = gains['P'], gains['I'], gains['D'] rospy.loginfo("gains are %s, %s, %s", p, i, d) ''' # set some parameters rospy.loginfo('setting parameters...') rospy.set_param('list_of_floats', [1., 2., 3., 4.]) rospy.set_param('bool_True', True) rospy.set_param('~private_bar', 1+2) rospy.set_param('to_delete', 'baz') rospy.loginfo('...parameters have been set') # delete a parameter if rospy.has_param('to_delete'): rospy.delete_param('to_delete') rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete')) else: rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete')) # search for a parameter #param_name = rospy.search_param('global_example') #rospy.loginfo('found global_example parameter under key: %s'%param_name) # publish the value of utterance repeatedly #pub = rospy.Publisher(topic_name, String, queue_size=10) while not rospy.is_shutdown(): #pub.publish(utterance) rospy.loginfo(1)#utterance) rospy.sleep(1)
def main(configfile): rospy.init_node('localizer_from_detections', log_level=rospy.DEBUG ) conf = config.Config(open(configfile)) loc = Localizer(conf) sub0 = message_filters.Subscriber( rospy.resolve_name("turtlebot03/marker_detections"), PolygonStamped) sub1 = message_filters.Subscriber( rospy.resolve_name("turtlebot04/marker_detections"), PolygonStamped) ts = ApproximateSynchronizer(0.75, [sub0, sub1], 10) ts.registerCallback(loc.callback) rospy.spin()
def wait_for_pubsub(self): # 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)
def main(configfile, resultsout): rospy.init_node('tilesexperiment') conf = config.Config(open(configfile)) tfsaver = VisualizeAndSave(conf, resultsout) tbot03 = message_filters.Subscriber( rospy.resolve_name("turtlebot03/camera/rgb/image_raw"), Image) tbot04 = message_filters.Subscriber( rospy.resolve_name("turtlebot04/camera/rgb/image_raw"), Image) ts = ApproximateSynchronizer(0.5, [tbot03, tbot04], 10) ts.registerCallback(tfsaver.roscallback) #rospy.Service('tilesexperiment_saver', # simple, # tfsaver.on_mouse_click) rospy.spin()
def __init__(self): trackedPersonsTopic = rospy.resolve_name("tracked_persons") detectedPersonsTopic = rospy.resolve_name("detected_persons") self.trackedPersonsPublisher = rospy.Publisher(trackedPersonsTopic, TrackedPersons, queue_size=100000) self.detectedPersonsPublisher = rospy.Publisher(detectedPersonsTopic, DetectedPersons, queue_size=100000) self._detectionIdCounter = 0 self._lastDataStamp = None self._firstTrackEncounterLookup = dict() self._previousCentroidLookup = dict() rospy.loginfo("Publishing detected and tracked persons from laser scan segmentation at %s and %s" % (detectedPersonsTopic, trackedPersonsTopic) )
def run(self): self.backproject_mode = False self.hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0,180)], 1 ) rospy.init_node('blob_tracker') image_topic = rospy.resolve_name("image") self.bbpub = rospy.Publisher("~blob",RegionOfInterest) self.bppub = rospy.Publisher("~backproject",Image) self.disp_hist = rospy.get_param("~display_histogram",True) rospy.Subscriber(image_topic, Image, self.detect_and_draw) rate = rospy.Rate(5) while not rospy.is_shutdown(): # rospy.spin_once() if not self.pause: if not self.backproject_mode: cv.ShowImage( "CamShiftDemo", self.frame ) else: cv.ShowImage( "CamShiftDemo", self.backproject) if self.disp_hist: cv.ShowImage( "Histogram", self.hue_histogram_as_image(self.hist)) c = cv.WaitKey(7) & 0x0FF if c == 27: rospy.signal_shutdown("OpenCV said so") elif c == ord("p"): self.pause = not self.pause elif c == ord("b"): self.backproject_mode = not self.backproject_mode
def testGetSubscriptions(self): # test with bad arity self.apiError(self.node.getSubscriptions()) self.apiError(self.node.getSubscriptions(self.caller_id, 'something extra')) # test success self._checkTopics([rospy.resolve_name(t) for t in _required_subscriptions], self.apiSuccess(self.node.getSubscriptions(self.caller_id)))
def _createPublishers(self): rospy.loginfo("Creating publishers...") topics = [] laserIndex = 0 for laserID in self._laserIDs: suffix = ("_%s" % laserIndex if len(self._laserIDs) > 1 else "") topic = "laser%s" % suffix topics.append(topic) self._laserscanPublishers[laserID] = rospy.Publisher(topic, LaserScan) topic = "laser%s_annotations" % suffix topics.append(topic) self._laserscanAnnotationsPublishers[laserID] = rospy.Publisher(topic, LaserscanSegmentation) laserIndex += 1 cameraIndex = 0 for cameraID in self._cameraIDs: suffix = ("_%s" % cameraIndex if len(self._cameraIDs) > 1 else "") topic = "camera%s/image" % suffix topics.append(topic) self._imagePublishers[cameraID] = rospy.Publisher(topic, Image) cameraIndex += 1 for topic in topics: rospy.loginfo(" Topic: " + rospy.resolve_name(topic))
def test_delete_param(self): try: rospy.delete_param(None); self.fail("delete_param(None) succeeded") except: pass try: rospy.delete_param(''); self.fail("delete_param('') succeeded") except: pass rostest_tests = { 'dpstring': "string", 'dpint0': 0, 'dpint10': 10, 'dpfloat0': 0.0, 'dpfloat10': 10.0, "dpnamespace/string": "namespaced string", } initial_params = rospy.get_param_names() # implicitly depends on set param for k, v in rostest_tests.iteritems(): rospy.set_param(k, v) # test delete for k, v in rostest_tests.iteritems(): self.assert_(rospy.has_param(k)) rospy.delete_param(k) self.failIf(rospy.has_param(k)) self.failIf(rospy.has_param(rospy.resolve_name(k))) try: rospy.get_param(k) self.fail("get_param should fail on deleted key") except KeyError: pass # make sure no new state self.failIf(set(initial_params) ^ set(rospy.get_param_names()))
def __init__(self, topic_name, timeout): self.topic_name = topic_name self.deadline = rospy.Time.now() + rospy.Duration(timeout) msg_class, _, _ = rostopic.get_topic_class( rospy.resolve_name(topic_name), blocking=True) self.msg = None self.sub = rospy.Subscriber(topic_name, msg_class, self._callback)
def __init__(self, kp, ki, kd, namespace): # constructor # namespace is location for tuning parameters # store own namespace self.namespace = rospy.resolve_name(namespace) rospy.loginfo('Setting up new PID controller in %s', self.namespace) # store default gains self.init_gains(kp, ki, kd) # stores for last input values for differentiating self.last_t = 0.0 self.last_y = 0.0 # flag for whether or not stores have been initialized self.has_run = False # integrator self.integ = 0.0 self.last_e = 0.0 # option to disable integral action self.freeze_integrator_flag = False # create gain update subscribers self.sub_kp = rospy.Subscriber(self.namespace + "/tune_gains/kp", Float32, self.tune_kp_callback) self.sub_ki = rospy.Subscriber(self.namespace + "/tune_gains/ki", Float32, self.tune_ki_callback) self.sub_kd = rospy.Subscriber(self.namespace + "/tune_gains/kd", Float32, self.tune_kd_callback) # closing message rospy.loginfo('PID ready in %s', self.namespace)
def main(): rospy.init_node('marker_detector', log_level=rospy.DEBUG) detector = Detector() resolved_topic = rospy.resolve_name('camera/rgb/image_raw') rospy.logdebug("Listening to %s" % resolved_topic) rospy.Subscriber(resolved_topic, Image, detector.callback) rospy.spin()
def test_unpublish(self): node_proxy = ServerProxy(rospy.get_node_uri()) _, _, pubs = node_proxy.getPublications('/foo') pubs = [p for p in pubs if p[0] != '/rosout'] self.assert_(not pubs, pubs) print("Publishing ", PUBTOPIC) pub = rospy.Publisher(PUBTOPIC, String) topic = rospy.resolve_name(PUBTOPIC) _, _, pubs = node_proxy.getPublications('/foo') pubs = [p for p in pubs if p[0] != '/rosout'] self.assertEquals([[topic, String._type]], pubs, "Pubs were %s"%pubs) # publish about 10 messages for fun for i in range(0, 10): pub.publish(String("hi [%s]"%i)) time.sleep(0.1) # begin actual test by unsubscribing pub.unregister() # make sure no new messages are received in the next 2 seconds timeout_t = time.time() + 2.0 while timeout_t < time.time(): time.sleep(1.0) self.assert_(_last_callback is None) # verify that close cleaned up master and node state _, _, pubs = node_proxy.getPublications('/foo') pubs = [p for p in pubs if p[0] != '/rosout'] self.assert_(not pubs, "Node still has pubs: %s"%pubs) n = rospy.get_caller_id() self.assert_(not rostest.is_publisher(topic, n), "publication is still active on master")
def test_unservice(self): import rosgraph master = rosgraph.Master('/test_dereg') state = master.getSystemState() _, _, srv = state # filter out rosout services #[['/rosout/set_logger_level', ['/rosout']], ['/rosout/get_loggers', ['/rosout']]] srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')] self.failIf(srv, srv) print("Creating service ", SERVICE) service = rospy.Service(SERVICE, EmptySrv, callback) # we currently cannot interrogate a node's services, have to rely on master # verify that master has service state = master.getSystemState() _, _, srv = state srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')] self.assertEquals(srv, [[rospy.resolve_name(SERVICE), [rospy.get_caller_id()]]]) # begin actual test by unsubscribing service.shutdown() time.sleep(1.0) # give API 1 second to sync with master state = master.getSystemState() _, _, srv = state srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')] self.failIf(srv, srv)
def __init__(self): # Get topic class topic_class = None rospy.loginfo("Waiting for topic to become available...") while not rospy.is_shutdown() and topic_class == None: topic_class, topic, _ = rostopic.get_topic_class(rospy.resolve_name('in')) rospy.sleep(0.5) if hasattr(topic_class, 'header'): cb = self.header_cb elif topic_class == tf2_msgs.msg.TFMessage: cb = self.tf_cb self.frames = set(rospy.get_param('~frames')) self.frame_prefix = rospy.get_param('~frame_prefix') else: print("Message type {} doesn't have a header field.".format(topic_class)) sys.exit(1) self.start_time = None self.actual_start_time = None self.time_offset = None self.sub = rospy.Subscriber('in', topic_class, cb) self.pub = rospy.Publisher('out', topic_class, queue_size=1)
def __init__(self, session="controller_session"): sessionpath = os.path.split(session)[0] rospy.init_node("robot_openrave_client", anonymous=True) self.controller_session_srv = rospy.ServiceProxy(session, controller_session) req = controller_sessionRequest(0, controller_sessionRequest.Access_ForceControl) res = self.controller_session_srv(req) self.sessionid = res.sessionid self.connection_header = {rospy.resolve_name(session): str(self.sessionid)} self.query_srv = rospy.ServiceProxy( sessionpath + "/Query", Query, persistent=True, headers=self.connection_header ) self.wait_srv = rospy.ServiceProxy(sessionpath + "/Wait", Wait, persistent=True, headers=self.connection_header) self.cancel_srv = rospy.ServiceProxy( sessionpath + "/Cancel", Cancel, persistent=True, headers=self.connection_header ) self.brake_srv = rospy.ServiceProxy( sessionpath + "/Brake", Brake, persistent=True, headers=self.connection_header ) self.StartTrajectory_srv = rospy.ServiceProxy( sessionpath + "/StartTrajectory", StartTrajectory, persistent=True, headers=self.connection_header ) self.StartVelocity_srv = rospy.ServiceProxy( sessionpath + "/StartVelocity", StartVelocity, persistent=True, headers=self.connection_header ) self.StartTorque_srv = rospy.ServiceProxy( sessionpath + "/StartTorque", StartTorque, persistent=True, headers=self.connection_header ) self.StartCustomString_srv = rospy.ServiceProxy( sessionpath + "/StartCustomString", StartCustomString, persistent=True, headers=self.connection_header )
def __init__(self): cv2.namedWindow("Optic flow window", 1) self.bridge = CvBridge() # get the image topic to subscribe to rospy.loginfo("Subscribing to " + rospy.resolve_name('image')) self.image_sub = rospy.Subscriber('image',Image,self.callback) self.data_pub = rospy.Publisher('opticflow',OptflowLK,queue_size=1) # running frame counter self.frame_ctr = 0 # number of frames between fresh feature track self.corner_interval = rospy.get_param('~corner_interval',10) rospy.loginfo("Refreshing corners every %i frames"%self.corner_interval) # params for ShiTomasi corner detection self.feature_params = dict( maxCorners = 100, qualityLevel = 0.3, minDistance = 7, blockSize = 7 ) # Parameters for lucas kanade optical flow self.lk_params = dict( winSize = (15,15), maxLevel = 2, criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) # Create some random colors self.color = np.random.randint(0,255,(100,3))
global delayed_topics rospy.loginfo("RESET RECEIVED") for dt in delayed_topics: dt.reset() return std_srvs.srv.EmptyResponse() def stop_callback(req): global delayed_topics rospy.loginfo("STOP RECEIVED") for dt in delayed_topics: dt.stop() return std_srvs.srv.EmptyResponse() def start_callback(req): global delayed_topics rospy.loginfo("START RECEIVED") for dt in delayed_topics: dt.run() return std_srvs.srv.EmptyResponse() reset_service = s = rospy.Service( rospy.resolve_name("~") + 'reset', std_srvs.srv.Empty, reset_callback) stop_service = s = rospy.Service( rospy.resolve_name("~") + 'stop', std_srvs.srv.Empty, stop_callback) start_service = s = rospy.Service( rospy.resolve_name("~") + 'start', std_srvs.srv.Empty, start_callback) start_callback(None) rospy.spin()
def main(psmName): rospy.init_node('dvrk_registration', anonymous=True) robot = psm(psmName) toolOffset = .012 # distance from pinching axle to center of orange nub frameRate = 15 slop = 1.0 / frameRate cams = StereoCameras("left/image_rect", "right/image_rect", "left/camera_info", "right/camera_info", slop=slop) tfSync = CameraSync( '/stereo/left/camera_info', topics=['/dvrk/' + psmName + '/position_cartesian_current'], frames=[ psmName + '_psm_base_link', psmName + '_tool_wrist_link', psmName + '_tool_wrist_caudier_link_shaft' ]) camModel = StereoCameraModel() topicLeft = rospy.resolve_name("left/camera_info") msgL = rospy.wait_for_message(topicLeft, CameraInfo, 10) topicRight = rospy.resolve_name("right/camera_info") msgR = rospy.wait_for_message(topicRight, CameraInfo, 10) camModel.fromCameraInfo(msgL, msgR) # Set up GUI scriptDirectory = os.path.dirname(os.path.abspath(__file__)) filePath = os.path.join(scriptDirectory, '..', '..', 'defaults', 'registration_params.yaml') print(filePath) with open(filePath, 'r') as f: data = yaml.load(f) if 'H' not in data: rospy.logwarn('dVRK Registration: defaults/registration_params.yaml \ empty or malformed. Using defaults for orange tip') data = { 'H': 23, 'minS': 173, 'minV': 68, 'maxV': 255, 'transform': np.eye(4).tolist() } cv2.namedWindow(_WINDOW_NAME) cv2.createTrackbar('H', _WINDOW_NAME, data['H'], 180, nothingCB) cv2.createTrackbar('min S', _WINDOW_NAME, data['minS'], 255, nothingCB) cv2.createTrackbar('min V', _WINDOW_NAME, data['minV'], 255, nothingCB) cv2.createTrackbar('max V', _WINDOW_NAME, data['maxV'], 255, nothingCB) cv2.createTrackbar('masked', _WINDOW_NAME, 0, 1, nothingCB) transformOld = np.array(data['transform']) # Wait for registration to start displayRegistration(cams, camModel, toolOffset, transformOld, tfSync) # Main registration (pointsA, pointsB) = getRegistrationPoints(robot, cams, camModel, toolOffset, tfSync) tf = calculateRegistration(pointsA, pointsB) # Save all parameters to YAML file data['transform'] = tf.tolist() data['H'] = cv2.getTrackbarPos('H', _WINDOW_NAME) data['minS'] = cv2.getTrackbarPos('min S', _WINDOW_NAME) data['minV'] = cv2.getTrackbarPos('min V', _WINDOW_NAME) data['maxV'] = cv2.getTrackbarPos('max V', _WINDOW_NAME) with open(filePath, 'w') as f: yaml.dump(data, f) # Publish transform as message to camera_transform_pub msg = posemath.toMsg(posemath.fromMatrix(tf)) pub = rospy.Publisher('set_camera_transform', Pose, latch=True, queue_size=10) pub.publish(msg) # Show Registration displayRegistration(cams, camModel, toolOffset, tf, tfSync, started=True) print('Done') cv2.destroyAllWindows()
def execute_cb(self, goal): if not self.scan: rospy.logwarn('No messages received yet on topic %s, aborting goal!' % rospy.resolve_name('base_scan')) self.action_server.set_aborted() return # Get base laser to base footprint frame offset while not rospy.is_shutdown(): try: self.tf_listener.waitForTransform(self.footprint_frame, self.laser_frame, rospy.Time(), rospy.Duration(3.0)) self.LASER_BASE_OFFSET = self.tf_listener.lookupTransform(self.footprint_frame, self.laser_frame, rospy.Time())[0][0] print self.LASER_BASE_OFFSET break except (tf.Exception) as e: rospy.logwarn("MoveBaseBlind tf exception! Message!") rospy.sleep(0.1) continue # helper variables target_pose = goal.target_pose # publish info to the console rospy.loginfo('%s: Executing, moving to position: (%f, %f)' % (self.action_name, target_pose.pose.position.x, target_pose.pose.position.y)) rate = rospy.Rate(hz = MoveBaseStraightAction.FREQ) while not rospy.is_shutdown(): rate.sleep() # get transform relative to /base_footprint (--> target_pose_transformed) while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(): try: # set the time stamp to now so we actually check for the # same transform in waitForTransform and transformPose. target_pose.header.stamp = rospy.Time.now() self.tf_listener.waitForTransform('/base_footprint', target_pose.header.frame_id, rospy.Time.now(), rospy.Duration(4.0)) target_pose_transformed = self.tf_listener.transformPose('/base_footprint', target_pose) break except (tf.Exception) as e: rospy.logwarn("MoveBaseBlind tf exception! Message: %s" % e.message) rospy.sleep(0.1) continue # check that preempt has not been requested by the client if self.action_server.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self.action_name) self.action_server.set_preempted() break self.x_diff = target_pose_transformed.pose.position.x self.y_diff = target_pose_transformed.pose.position.y self.dist = np.sqrt(self.x_diff ** 2 + self.y_diff ** 2) euler = tf.transformations.euler_from_quaternion([target_pose_transformed.pose.orientation.x, target_pose_transformed.pose.orientation.y, target_pose_transformed.pose.orientation.z, target_pose_transformed.pose.orientation.w]) self.target_angle = np.arctan2(target_pose_transformed.pose.position.y, target_pose_transformed.pose.position.x) # target_angle is 0 in the center of the laser scan # Can we see enough? if ((self.target_angle - self.REQUIRED_APERTURE/2) < self.scan.angle_min or (self.target_angle + self.REQUIRED_APERTURE/2) > self.scan.angle_max): # Driving blind (maybe activate warning signals here) pass cmd = Twist() # Goal not yet reached? # Translate holonomically if (self.HOLONOMIC and self.dist > self.Goal_THRESHOLD and not self.blocked()): self.translate_towards_goal_holonomically(cmd) # Translate non-holonomically elif (self.dist > self.GOAL_THRESHOLD and not self.blocked()): self.translate_towards_goal(cmd) # If goal distance falls below xy-tolerance, rotate: elif (abs(euler[2]) > self.YAW_GOAL_TOLERANCE): self.rotate_in_place(cmd, euler[2]) # Arrived else: rospy.loginfo('%s: Succeeded' % self.action_name) self.action_server.set_succeeded() break