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
