def listener(self):
# setup call back for lgging
print "Starting listener..."
rospy.init_node('log_listener', anonymous=True)
print "Looking for ros params..."
print rospy.search_param('logging_location')
if rospy.has_param("~logging_location"):
print "logging to the following location..."
self.logLocation = rospy.get_param('~logging_location')
print self.logLocation
else:
print "Using default logging location"
if rospy.has_param("~onboard"):
print "logging instance is onboard?"
self.onboard = rospy.get_param("~onboard")
print self.onboard
print "Subscribe to logging messages ..."
rospy.Subscriber('/vigir_logging', OCSLogging, self.callback)
rospy.Subscriber('/vigir_logging_query', String, self.state)
print "Publish topic ..."
self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1)
self.query = "desktop_"
print "Now spin ..."
rospy.spin()
print "Shutdown the desktop logger!"
def listener(self):
# setup call back for lgging
print "Starting listener..."
rospy.init_node('log_listener', anonymous=True)
print "Looking for ros params..."
print rospy.search_param('logging_location')
print rospy.search_param('to_log')
if rospy.has_param('~logger_name'):
print("This logger is named")
self.logger_name = rospy.get_param('~logger_name')
print self.logger_name
if rospy.has_param('~to_log'):
print "logging the following topics..."
self.toLog = rospy.get_param('~to_log')
print self.toLog
else:
print "Failed to find topics to log."
if rospy.has_param('~enable_Log_grabbing'):
self.enableBDILogging = rospy.get_param('~enable_Log_grabbing')
else:
print "Not setting up log grabbing"
if rospy.has_param("~logging_location"):
print "logging to the following location..."
self.logLocation = rospy.get_param('~logging_location')
print self.logLocation
else:
print "Using default logging location"
rospy.Subscriber('/vigir_logging', OCSLogging, self.callback)
rospy.Subscriber('/vigir_logging_query', String, self.state)
self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1)
rospy.spin()
def gazebo_world_state_handler(self, data):
# Retrieve the static world & robot descriptions once on first run
if self.gazebo_world == None:
world_param = rospy.search_param('gazebo_world_description')
world_description = rospy.get_param(world_param, '')
robot_param = rospy.search_param('robot_description')
robot_description = str(rospy.get_param(robot_param, ''))
self.gazebo_world = GazeboDescriptionParser(world_description, robot_description)
# Begin parsing icarus world state
icarus_state = '(list'
regex = re.compile('(env|obj)_(.*?)_(.*?)_(.*?)_body')
for i in range(len(data.name)):
pos = data.pose[i].position
ori = data.pose[i].orientation
lin = data.twist[i].linear
ang = data.twist[i].angular
frc = data.wrench[i].force
trq = data.wrench[i].torque
# Only publish recognizable objects from the world & robot desscriptions
if self.gazebo_world != None and self.gazebo_world.has_object(data.name[i].strip()):
match = re.search(regex, data.name[i])
if match:
if match.group(1) == 'env':
icarus_state += ' \'(environment %s' % (data.name[i])
else:
icarus_state += ' \'(object %s' % (data.name[i])
icarus_state += ' type %s' % (match.group(2))
icarus_state += ' color %s' % (match.group(4))
else:
icarus_state += ' \'(object %s' % (data.name[i])
if data.name[i].strip() == 'base_link':
icarus_state += ' type self'
else:
icarus_state += ' type empty'
icarus_state += ' color empty'
icarus_state += ' role empty'
icarus_state += ' status empty'
icarus_state += ' xpos %s ypos %s zpos %s' % (pos.x, pos.y, pos.z)
icarus_state += ' xori %s yori %s zori %s wori %s' % (ori.x, ori.y, ori.z, ori.w)
icarus_state += ' xlin %s ylin %s zlin %s' % (lin.x, lin.y, lin.z)
icarus_state += ' xang %s yang %s zang %s' % (ang.x, ang.y, ang.z)
icarus_state += ' xtrq %s ytrq %s ztrq %s' % (trq.x, trq.y, trq.z)
obj = self.gazebo_world.get_object(data.name[i].strip())
icarus_state += ' xsiz %s ysiz %s zsiz %s srad %s)' % (obj.size.x, obj.size.y, obj.size.z, obj.s_radius)
icarus_state += ')'
self.icarus_world_state = icarus_state
self.icarus_world_state_pub.publish(icarus_state)
def __init__(self):
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.bridge = CvBridge()
out_image = rospy.get_param(rospy.search_param('out'))
image = rospy.get_param(rospy.search_param('image'))
# subscribed Topic
self.image_pub = rospy.Publisher(out_image, Image)
self.subscriber = rospy.Subscriber(image, CompressedImage, self.callback, queue_size = 1000)
def main():
rospy.init_node("robospect_laser_assembler_node")
_name = rospy.get_name().replace('/','')
arg_defaults = {
'topic_state': 'state',
'desired_freq': DEFAULT_FREQ,
'assembly_scans_service_name': '/assemble_scans2',
'point_cloud_publisher_topic_name': '/assembled_cloud',
'time_point_cloud_back': DEFAULT_TIME_PCLOUD_BACK,
}
args = {}
for name in arg_defaults:
try:
if rospy.search_param(name):
args[name] = rospy.get_param('~%s'%(name)) # Adding the name of the node, because the para has the namespace of the node
else:
args[name] = arg_defaults[name]
#print name
except rospy.ROSException, e:
rospy.logerr('%s: %s'%(e, _name))
def load_config_from_param(self):
# first, make sure parameter server is even loaded
while not rospy.search_param("/joints"):
rospy.loginfo("waiting for parameter server to load with joint definitions")
rospy.sleep(1)
rospy.sleep(1)
joints = rospy.get_param('/joints')
for name in joints:
rospy.loginfo( "found: " + name )
s = Servo()
key = '/joints/' + name + '/'
s.bus = int(rospy.get_param(key + 'bus'))
s.servo = int(rospy.get_param(key + 'servo'))
s.flip = int(rospy.get_param(key + 'flip'))
s.servopin = int(rospy.get_param(key + 'servopin'))
s.sensorpin = int(rospy.get_param(key + 'sensorpin'))
s.minpulse = int(rospy.get_param(key + 'minpulse'))
s.maxpulse = int(rospy.get_param(key + 'maxpulse'))
s.minangle = float(rospy.get_param(key + 'minangle'))
s.maxangle = float(rospy.get_param(key + 'maxangle'))
s.minsensor = int(rospy.get_param(key + 'minsensor'))
s.maxsensor = int(rospy.get_param(key + 'maxsensor'))
s.maxspeed = float(rospy.get_param(key + 'maxspeed'))
s.smoothing = int(rospy.get_param(key + 'smoothing'))
self.servos[name] = s
def initialize(self):
self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True);
self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult);
self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback);
self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);
self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);
#read the frequency with which to publish status from the parameter server
#if not specified locally explicitly, use search param to find actionlib_status_frequency
resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
if rospy.has_param(resolved_status_frequency_name):
self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
else:
search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
if search_status_frequency_name is None:
self.status_frequency = 5.0
else:
self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
self.status_list_timeout = rospy.Duration(status_list_timeout);
self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
self.status_timer.start();
def load_config_from_param():
# first, make sure parameter server is even loaded
while not rospy.search_param("/joints"):
rospy.loginfo("waiting for parameter server to load with joint definitions")
rospy.sleep(1)
rospy.sleep(1)
joints = rospy.get_param('/joints')
for name in joints:
rospy.loginfo( "found: " + name )
s = Servo()
key = '/joints/' + name + '/'
s.bus = rospy.get_param(key + 'bus')
s.servoPin = rospy.get_param(key + 'servoPin')
s.minPulse = rospy.get_param(key + 'minPulse')
s.maxPulse = rospy.get_param(key + 'maxPulse')
s.minGoal = rospy.get_param(key + 'minGoal')
s.maxGoal = rospy.get_param(key + 'maxGoal')
s.rest = rospy.get_param(key + 'rest')
s.maxSpeed = rospy.get_param(key + 'maxSpeed')
s.smoothing = rospy.get_param(key + 'smoothing')
s.sensorpin = rospy.get_param(key + 'sensorPin')
s.minSensor = rospy.get_param(key + 'minSensor')
s.maxSensor = rospy.get_param(key + 'maxSensor')
servos[name] = s
return servos
def __init__(self):
NaoNode.__init__(self, 'nao_sensors')
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))
self.dataNamesList = ["DCM/Time",
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]
tf_prefix_param_name = rospy.search_param('tf_prefix')
if tf_prefix_param_name:
self.tf_prefix = rospy.get_param(tf_prefix_param_name)
else:
self.tf_prefix = ""
# To stop odometry tf being broadcast
self.broadcast_odometry = rospy.get_param('~broadcast_odometry', True)
self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
if not(self.base_frameID[0] == '/'):
self.base_frameID = self.tf_prefix + '/' + self.base_frameID
# use sensor values or commanded (open-loop) values for joint angles
self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
# init. messages:
self.torsoOdom = Odometry()
self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
if not(self.torsoOdom.header.frame_id[0] == '/'):
self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
self.torsoIMU = Imu()
self.torsoIMU.header.frame_id = self.base_frameID
self.jointState = JointState()
self.jointState.name = self.motionProxy.getJointNames('Body')
# simluated model misses some joints, we need to fill:
if (len(self.jointState.name) == 22):
self.jointState.name.insert(6,"LWristYaw")
self.jointState.name.insert(7,"LHand")
self.jointState.name.append("RWristYaw")
self.jointState.name.append("RHand")
msg = "Nao joints found: "+ str(self.jointState.name)
rospy.logdebug(msg)
self.torsoOdomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=10)
self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=10)
self.tf_br = tf.TransformBroadcaster()
rospy.loginfo("nao_sensors initialized")
def __get_parameter(pname):
param_full_name = rospy.search_param(pname)
if not param_full_name:
raise ParamNotFoundError("'{}'".format(pname))
p = Param(param_full_name, rospy.get_param(param_full_name))
rospy.loginfo("Param '{}'. Value: '{}'".format(p.name, p.value))
return p
def main():
rospy.init_node("test_contact_marker")
_name = rospy.get_name().replace('/','')
arg_defaults = {
'topic_state': 'state',
'desired_freq': DEFAULT_FREQ,
'marker_frames': ['sw1', 'sw2', 'sw3', 'sw4'],
'marker_names': ['sw1', 'sw2', 'sw3', 'sw4'],
}
args = {}
for name in arg_defaults:
try:
if rospy.search_param(name):
args[name] = rospy.get_param('~%s'%(name)) # Adding the name of the node, because the para has the namespace of the node
else:
args[name] = arg_defaults[name]
#print name
except rospy.ROSException, e:
rospy.logerr('%s: %s'%(e, _name))
def getFixedTypeProperty(self, propertyName, valueType):
"""Main property getter receiving the property name and the value type
and returning the current value from the low level driver."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if propertyData[PPTY_KIND] == "dynParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
resp1 = self.driverMgr.getParameter(paramName, self.translator.getDynServerPath(propertyName))
elif propertyData[PPTY_KIND] == "publishedTopic":
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType)
elif propertyData[PPTY_KIND] == "readOnlyParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
location = rospy.search_param(param_name)
resp1 = rospy.get_param(location)
elif propertyData[PPTY_KIND] == "topic":
##MICHI: 14Feb2012
if valueType == str:
resp1 = str(propertyData[PPTY_REF])
else: ## Special case for reading disparity images
valueType2 = DisparityImage
if propertyData[PPTY_TYPE].find("Disparity") < 0:
valueType2 = Image
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType2)
else:
rospy.logerr("Error: unable to get property %s of kind %s"%(propertyName, propertyData[PPTY_TYPE]))
resp1 = None
return resp1
def main():
rospy.init_node("axis_twist")
arg_defaults = {
'hostname': '192.168.0.90',
'username': '',
'password': '',
'flip': False, # things get weird if flip=true
'speed_control': False
}
args = {}
# go through all arguments
for name in arg_defaults:
full_param_name = rospy.search_param(name)
# make sure argument was found (https://github.com/ros/ros_comm/issues/253)
if full_param_name == None:
args[name] = arg_defaults[name]
else:
args[name] = rospy.get_param(full_param_name, arg_defaults[name])
# create new PTZ object and start dynamic_reconfigure server
my_ptz = AxisPTZ(**args)
srv = Server(PTZConfig, my_ptz.dynamicReconfigCallback)
rospy.spin()
def search_param(name, params_glob):
if params_glob and not any(fnmatch.fnmatch(str(v), glob) for glob in params_glob):
# If the glob list is not empty and there are no glob matches,
# stop the attempt to find the parameter.
return None
# If the glob list is empty (i.e. false) or the parameter matches
# one of the glob strings, continue to find the parameter.
return rospy.search_param(name)
def start_exploration(self):
'''Starts FlexBe behavior according to param "mission_behavior".'''
msg = BehaviorExecutionActionGoal()
#msg.goal.behavior_name = 'Search Victims'
mission_full_param_name = rospy.search_param('mission_behavior')
msg.goal.behavior_name = rospy.get_param(mission_full_param_name)
self._behavior_publisher.publish(msg)
rospy.loginfo("FlexBe exploration behavior "+ rospy.get_param(mission_full_param_name) +" activated.")
def addTFPrefix(frame_id):
prefix = ""
prefix_param = rospy.search_param("tf_prefix")
if prefix_param:
prefix = rospy.get_param(prefix_param)
if prefix[0] != "/":
prefix = "/%s" % prefix
return "%s/%s" % (prefix, frame_id)
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)
while not rospy.is_shutdown():
pub.publish(utterance)
rospy.loginfo(utterance)
rospy.sleep(1)
def get_frame_id():
frame_id = rospy.get_param('~frame_id', 'gps')
"""Add the TF prefix"""
prefix = ""
prefix_param = rospy.search_param('tf_prefix')
if prefix_param:
prefix = rospy.get_param(prefix_param)
return "%s/%s" % (prefix, frame_id)
else:
return frame_id
def main():
rospy.init_node("axis")
arg_defaults = {"hostname": "192.168.0.90", "username": "", "password": ""}
args = {}
for name in arg_defaults:
args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name])
AxisPTZ(**args)
rospy.spin()
def __init__(self):
param_name = rospy.search_param('restamped_topics')
print "Param name"
print param_name
self.topic_names = rospy.get_param(param_name)
self.subscribers = {}
self.publishers = {}
self.setup_topics()
def main():
rospy.init_node("axis_driver")
arg_defaults = {
'hostname': '192.168.0.90', # default IP address
'username': '******', # default login name
'password': '',
'fps': 0,
'width': 640,
'height': 480,
'delay': 0.0,
'compression': 10,
'frame_id': 'axis_camera',
'camera_info_url': ''}
# Look up parameters starting in the driver's private parameter
# space, but also searching outer namespaces. Defining them in a
# higher namespace allows the axis_ptz.py script to share parameters
# with the driver.
args = {}
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
# resolve frame_id with tf_prefix (unless already absolute)
if args['frame_id'][0] != '/': # not absolute?
tf_prefix = rospy.search_param('tf_prefix')
prefix_val = ''
if tf_prefix is not None: # prefix defined?
prefix_val = rospy.get_param(tf_prefix)
if prefix_val[0] != '/': # prefix not absolute?
prefix_val = '/' + prefix_val
args['frame_id'] = prefix_val + '/' + args['frame_id']
rospy.sleep(args['delay'])
rospy.loginfo("Starting AXIS video acquisition")
Axis(**args)
rospy.spin()
def read_args_with_defaults(arg_defaults):
"""Look up parameters starting in the driver's private parameter space, but also searching outer namespaces.
Defining them in a higher namespace allows the axis_ptz.py script to share parameters with the driver."""
args = {}
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
# resolve frame_id with tf_prefix (unless already absolute)
if args['frame_id'][0] != '/': # not absolute?
tf_prefix = rospy.search_param('tf_prefix')
prefix_val = ''
if tf_prefix is not None: # prefix defined?
prefix_val = rospy.get_param(tf_prefix)
if prefix_val[0] != '/': # prefix not absolute?
prefix_val = '/' + prefix_val
args['frame_id'] = prefix_val + '/' + args['frame_id']
return args
def updateArgs(arg_defaults):
'''Look up parameters starting in the driver's private parameter space, but
also searching outer namespaces. '''
args = {}
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
return(args)
def test_search_param(self):
try:
orig_caller_id = rospy.names.get_caller_id()
rospy.names._set_caller_id('/global_node')
self.assertEquals(None, rospy.search_param('search_param'))
rospy.set_param('/search_param', 1)
self.assertEquals('/search_param', rospy.search_param('search_param'))
rospy.names._set_caller_id('/level1/level2/relative_node')
self.assertEquals('/search_param', rospy.search_param('search_param'))
rospy.set_param('/level1/search_param', 2)
self.assertEquals('/level1/search_param', rospy.search_param('search_param'))
rospy.set_param('~search_param', 3)
# make sure that search starts in our private namespace first
self.assertEquals('/level1/level2/relative_node/search_param', rospy.search_param('search_param'))
finally:
rospy.names._set_caller_id(orig_caller_id)
def __init__(self):
rospy.init_node('ichthus_controller_python_core')
wxPython = ichthus_2.MyApp()
wxPython.MainLoop()
self.MAX_COMMAND_COUNT = 0
self.num_command = 0
self.regex_list = []
self.command_regex = ""
if rospy.search_param("MAX_COMMAND_COUNT") is None:
rospy.logerr("check yaml file")
exit()
self.regex_init()
def updateArgs(arg_defaults):
# Look up parameters starting in the node's private parameter space, but also search outer namespaces.
args = {}
for name, val in arg_defaults.items():
full_name = rospy.search_param(name)
rospy.loginfo("name %s, %s" % (name, full_name))
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
rospy.loginfo("We have args %s value %s" % (val, args[name]))
return (args)
def updateArgs(arg_defaults):
'''Look up parameters starting in the driver's private parameter space, but
also searching outer namespaces. Defining them in a higher namespace allows
the axis_ptz.py script to share parameters with the driver.'''
args = {}
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
# resolve frame_id with tf_prefix (unless already absolute)
if args['frame_id'][0] != '/': # not absolute?
tf_prefix = rospy.search_param('tf_prefix')
prefix_val = ''
if tf_prefix is not None: # prefix defined?
prefix_val = rospy.get_param(tf_prefix)
if prefix_val[0] != '/': # prefix not absolute?
prefix_val = '/' + prefix_val
args['frame_id'] = prefix_val + '/' + args['frame_id']
return (args)
def __init__(self):
"""Initialise node."""
rospy.init_node("range_merger")
self._merged_topic = rospy.Publisher("range_merger/scan",
LaserScan,
queue_size=10)
robot_root = rospy.get_param("~robot_root", rospy.get_namespace())
tf_prefix_key = rospy.search_param("tf_prefix")
if tf_prefix_key:
tf_prefix = rospy.get_param(tf_prefix_key, None)
else:
tf_prefix = None
if tf_prefix is not None and not tf_prefix.endswith("/"):
tf_prefix += "/"
self._tf_reference_frame = str(
rospy.get_param("~reference_frame", REFERENCE_FRAME_ID))
if tf_prefix:
self._tf_reference_frame = tf_prefix + self._tf_reference_frame
self._rate = rospy.Rate(rospy.get_param('~rate', 1))
self._sensor_last_values = {key: INF for key in RANGE_SENSORS}
self._latest_message = rospy.Time.now()
self._scan_start = float(rospy.get_param("~scan_start", SCAN_START))
self._scan_end = float(rospy.get_param("~scan_end", SCAN_END))
self._scan_step = float(rospy.get_param("~scan_step", SCAN_STEP))
self._scan_steps = int(
(self._scan_end - self._scan_start) / self._scan_step)
self._scan_max_influence = float(
rospy.get_param("~scan_max_influence", RELEVANCE_DISTANCE))
self._scan_influence_smoothing = float(
rospy.get_param("~scan_influence_smoothing", EFFECT_SMOOTHING))
self._scan_max_range = float(
rospy.get_param("~scan_max_range", MAX_RANGE))
self._scan_min_range = float(
rospy.get_param("~scan_min_range", MIN_RANGE))
self._scan_robot_radius = float(
rospy.get_param("~scan_robot_radius", ROBOT_RADIUS))
for sensor in RANGE_SENSORS:
rospy.Subscriber(robot_root + SENSOR_PREFIX + sensor, Range,
partial(self.range_handler, sensor=sensor))
self._running = True
def __init__(self):
# Publish binary, thresholded image (for debugging)?
self.binary_publish = True
# Pull ROS parameters from launch file:
param = rospy.search_param("bounding_box_topic")
self.bounding_boxes_top = rospy.get_param(param)
param = rospy.search_param("image_topic")
self.img_top = rospy.get_param(param)
param = rospy.search_param("binary_image_topic")
self.binary_img_top = rospy.get_param(param)
param = rospy.search_param("pose_difference_topic")
self.pose_diff_top = rospy.get_param(param)
param = rospy.search_param("camera_frame")
self.cam_frame = rospy.get_param(param)
param = rospy.search_param("odometry_frame")
self.odom_frame = rospy.get_param(param)
param = rospy.search_param("map_frame")
self.map_frame = rospy.get_param(param)
param = rospy.search_param("true_stop_frame")
self.true_stop_frame = rospy.get_param(param)
# From '/home/robot/dd2419_ws/src/course_packages/dd2419_launch/calibration/camera.yaml' or topic: /cf1/camera/camera_info
# [ fx , s , x0 , 0 , fy , y0 , 0 , 0 , 1 ]
self.cam_mat = [231.250001, 0.000000, 320.519378, 0.000000, 231.065552, 240.631482, 0.000000, 0.000000, 1.000000]
# From '/home/robot/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/______.world.json'
self.sign_dim = [0.20, 0.20]
# Initialize subscriber to bounding box
rospy.Subscriber(self.bounding_boxes_top, BoundingBoxes, self.boxes_cb)
# Initialize subscriber to image
rospy.Subscriber(self.img_top, Image, self.img_cb)
# Initialize CvBridge
self.bridge = CvBridge()
# Initialize publisher for binary, thresholded sign (helps for debugging)
if self.binary_publish:
self.image_pub = rospy.Publisher(self.binary_img_top, Image, queue_size=1)
# Initialize tf broadcaster
self.br = tf2_ros.TransformBroadcaster()
# Listen to other tf transforms
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# Initialize publisher for differecne between detected and true sign pose
self.pose_diff_pub = rospy.Publisher(self.binary_img_top, PoseStamped, queue_size=2)
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 init():
"""
Initialization handles use with just python or in a launch file
:return: start_pt Starting point (x,y) where the car needs to start
:rtype: list
:return: use_rviz Flag to enable/disable rviz visualization
:rtype: bool
:return: use_matplotlib Flag to enable/disable pure pursuit animation
:rtype: bool
:return: cx List of x coordinates
:rtype: list
:return: cy List of y coordinates
:rtype: list
:return cyaw List of yaw values
:rtype: list
:return ck List of curvature values
:rtype: list
"""
# grab parameters from launch-file
start_pt_param = rospy.search_param('start_pt')
use_rviz_param = rospy.search_param('use_rviz')
use_matplotlib_param = rospy.search_param('use_matplotlib')
start_pt = rospy.get_param(start_pt_param, init_pt)
if type(start_pt) == type(''):
start_pt = start_pt.split(',')
start_pt = [float(curr) for curr in start_pt]
coords_param = rospy.search_param('coords_param')
coords = rospy.get_param(coords_param)
# print(coords_param)
coords = json.loads(coords)
# print(coords)
# cx, cy = create_smooth_path(coords)
cx, cy, cyaw, ck = create_smooth_path(coords)
use_rviz = rospy.get_param(use_rviz_param, False)
use_matplotlib = rospy.get_param(use_matplotlib_param, visualize_plot)
return start_pt, use_rviz, use_matplotlib, cx, cy, cyaw, ck
def run():
rospy.init_node('p5a')
full_param_name = rospy.search_param('location')
loc = rospy.get_param(full_param_name)
full_param_name = rospy.search_param('turtlename')
name = rospy.get_param(full_param_name)
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(2 + loc, 9.1 - loc, 0, name)
if loc == 0:
rospy.wait_for_service('kill')
killer = rospy.ServiceProxy('kill', turtlesim.srv.Kill)
killer('turtle1')
rospy.Subscriber(name + '/pose', Pose, callback)
pub = rospy.Publisher(name + '/cmd_vel', Twist, queue_size=10)
rate = rospy.Rate(0.5)
message = Twist()
i = 0
while i < 10:
if i % 2 == 0:
message.linear.x = 2
message.linear.y = 0
message.linear.z = 0
message.angular.x = 0
message.angular.y = 0
message.angular.z = 0
else:
message.linear.x = 0
message.linear.y = 0
message.linear.z = 0
message.angular.x = 0
message.angular.y = 0
message.angular.z = math.pi / 2
i = i + 1
pub.publish(message)
rate.sleep()
def get_frame_id():
frame_id = rospy.get_param("~frame_id", "gps")
if frame_id[0] != "/":
"""Add the TF prefix"""
prefix = ""
prefix_param = rospy.search_param("tf_prefix")
if prefix_param:
prefix = rospy.get_param(prefix_param)
if prefix[0] != "/":
prefix = "/%s" % prefix
return "%s/%s" % (prefix, frame_id)
else:
return frame_id
def start(self):
""" Start the necessary messages to operate the Kobuki. """
if self.started:
rospy.logwarn("Warn[MCCKobuki.start]: Already started.")
return
#rospy.sleep(15)
self.subModelUpdate = rospy.Subscriber(self.subModelUpdateTopic,
ModelUpdate,
self.sub_model_update)
subKobukiOdomTopic = rospy.get_param(
rospy.search_param('sub_kobuki_odom'))
self.subKobukiOdom = rospy.Subscriber(subKobukiOdomTopic, Odometry,
self.sub_kobuki_odom)
subKobukiBumpTopic = rospy.get_param(
rospy.search_param('sub_kobuki_bump'))
self.subKobukiBump = rospy.Subscriber(subKobukiBumpTopic, BumperEvent,
self.sub_kobuki_bump)
pubKobukiVelTopic = rospy.get_param(
rospy.search_param('pub_kobuki_vel'))
self.pubKobukiVel = rospy.Publisher(pubKobukiVelTopic,
Twist,
queue_size=32)
pubKobukiResetOdomTopic = rospy.get_param(
rospy.search_param('pub_kobuki_reset_odom'))
self.pubKobukiResetOdom = rospy.Publisher(pubKobukiResetOdomTopic,
Empty,
queue_size=32)
pubPathTopic = rospy.get_param(rospy.search_param('pub_path'))
self.pubPath = rospy.Publisher(pubPathTopic, Path, queue_size=32)
self.started = True
def __init__(self):
full_param_name = rospy.search_param('mpc_dir')
mpc_dir = rospy.get_param(full_param_name)
mpc_dir = mpc_dir['test_performance']['mpc_dir']
mpc_dir = os.path.join(mpc_dir, 'parameters/default.yaml')
self.mpc_params = os.path.abspath(mpc_dir)
print(self.mpc_params)
self.test_mode = 'mpc' # 'pid' or 'mpc'
self.sampling_mode = 'continuous' # 'grid' or 'continuous'
self.handtuned_params = True
self.params = {}
self.idx_array = None
self.n_combinations = 1 # will be overwritten in case of grid sampling
def get_frame_id():
frame_id = rospy.get_param('~frame_id', 'gps')
if frame_id[0] != "/":
"""Add the TF prefix"""
prefix = ""
prefix_param = rospy.search_param('tf_prefix')
if prefix_param:
prefix = rospy.get_param(prefix_param)
if prefix[0] != "/":
prefix = "/%s" % prefix
return "%s/%s" % (prefix, frame_id)
else:
return frame_id
def __init__(self):
# Pull ROS parameters from launch file:
param = rospy.search_param("bounding_box_topic")
self.bounding_boxes_top = rospy.get_param(param)
param = rospy.search_param("image_topic")
self.img_top = rospy.get_param(param)
# param = rospy.search_param("pose_difference_topic")
# self.pose_diff_top = rospy.get_param(param)
param = rospy.search_param("camera_frame")
self.cam_frame = rospy.get_param(param)
param = rospy.search_param("odometry_frame")
self.odom_frame = rospy.get_param(param)
param = rospy.search_param("map_frame")
self.map_frame = rospy.get_param(param)
# From '/home/robot/dd2419_ws/src/course_packages/dd2419_launch/calibration/camera.yaml' or topic: /cf1/camera/camera_info
# [ fx , s , x0 , 0 , fy , y0 , 0 , 0 , 1 ]
self.cam_mat = [231.250001, 0.000000, 320.519378, 0.000000, 231.065552, 240.631482, 0.000000, 0.000000, 1.000000]
# From '/home/robot/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/______.world.json'
self.sign_dim = [0.20, 0.20]
self.classes = ['narrows_from_left', 'no_bicycle', 'residential', 'roundabout']
# From '/home/robot/dd2419_ws/src/darknet_ros/darknet_ros/config/yolo-lite-cf9-4classes.yaml'
# Initialize subscriber to bounding box
rospy.Subscriber(self.bounding_boxes_top, BoundingBoxes, self.boxes_cb)
# Initialize subscriber to image
rospy.Subscriber(self.img_top, Image, self.img_cb)
# Initialize CvBridge
self.bridge = CvBridge()
# Initialize tf broadcaster
self.br = tf2_ros.TransformBroadcaster()
# Listen to other tf transforms
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
def main():
rospy.init_node('point_planner', anonymous=True)
name = rospy.get_namespace().split('/')[1]
rospy.loginfo("waiting for params to become available")
while not rospy.has_param(
'/planners/update_freq') and not rospy.is_shutdown(
): # wait for the param server to load
pass
rospy.loginfo("params found")
param_name = rospy.search_param('planners/update_freq')
update_period = 1 / int(rospy.get_param(param_name))
p = Planner(name, update_period)
rospy.spin()
def updateArgs(arg_defaults):
'''Look up parameters starting in the driver's private parameter space, but
also searching outer namespaces. '''
args = {}
print("processing args")
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
print("We have args " + val + " value " + args[name])
return (args)
def init():
"""
Grabs parameters from launch file, parses them and returns them.
:return: speed_limit The desired target speed for the car.
:rtype: float
:return: path_radius Radius for the circular path
:rtype: float
:return: geofence radius Radius for the geofence.
:rtype: float
:return: gear The gear at which we want the car to drive. 0 or 1.
:rtype: int
:return: target_laps How many laps the car is supposed to drive.
:rtype: int
"""
# grab parameters from launch-file
speed_limit_param = rospy.search_param('speed_limit')
speed_limit = rospy.get_param(speed_limit_param)
speed_limit = float(speed_limit)
path_radius_param = rospy.search_param('path_radius')
path_radius = rospy.get_param(path_radius_param)
path_radius = float(path_radius)
geofence_radius_param = rospy.search_param('geofence_radius')
geofence_radius = rospy.get_param(geofence_radius_param)
geofence_radius = float(geofence_radius)
gear_param = rospy.search_param('gear')
gear = rospy.get_param(gear_param)
gear = int(gear)
look_ahead_param = rospy.search_param('look_ahead')
look_ahead = rospy.get_param(look_ahead_param)
look_ahead = float(look_ahead)
pure_pursuit.Lfc = look_ahead # look-ahead distance
target_laps_param = rospy.search_param('target_laps')
target_laps = rospy.get_param(target_laps_param)
target_laps = float(target_laps)
front_differentials_param = rospy.search_param('front_differentials')
front_differentials = rospy.get_param(front_differentials_param)
front_differentials = int(front_differentials)
rear_differentials_param = rospy.search_param('rear_differentials')
rear_differentials = rospy.get_param(rear_differentials_param)
rear_differentials = int(rear_differentials)
return speed_limit, path_radius, geofence_radius, gear, target_laps
def param_talker():
rospy.init_node('param_talker')
# fetch a /global parameter
global_example = rospy.get_param("/global_example") # getting from launch def
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') #getting gain from launch
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(utterance)
#print utterance
#print p,i,d
rospy.sleep(1)
def init_regex_list(self):
num_command = 0
if rospy.search_param("MAX_COMMAND_COUNT") is None:
rospy.logerr("check yaml file")
exit()
while True:
str1 = "command" + str(num_command)
if rospy.has_param(str1) == False:
break
command_regex = rospy.get_param(str1)
self.regex_list.append(command_regex)
num_command += 1
for i in range(0, len(self.regex_list)):
print(self.regex_list[i])
def initialize(self):
self.status_pub = rospy.Publisher(rospy.remap_name(self.ns) +
"/status",
GoalStatusArray,
latch=True,
queue_size=50)
self.result_pub = rospy.Publisher(rospy.remap_name(self.ns) +
"/result",
self.ActionResult,
queue_size=50)
self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns) +
"/feedback",
self.ActionFeedback,
queue_size=50)
self.goal_sub = rospy.Subscriber(
rospy.remap_name(self.ns) + "/goal", self.ActionGoal,
self.internal_goal_callback)
self.cancel_sub = rospy.Subscriber(
rospy.remap_name(self.ns) + "/cancel", GoalID,
self.internal_cancel_callback)
# read the frequency with which to publish status from the parameter server
# if not specified locally explicitly, use search param to find actionlib_status_frequency
resolved_status_frequency_name = rospy.remap_name(
self.ns) + "/status_frequency"
if rospy.has_param(resolved_status_frequency_name):
self.status_frequency = rospy.get_param(
resolved_status_frequency_name, 5.0)
rospy.logwarn(
"You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency."
)
else:
search_status_frequency_name = rospy.search_param(
"actionlib_status_frequency")
if search_status_frequency_name is None:
self.status_frequency = 5.0
else:
self.status_frequency = rospy.get_param(
search_status_frequency_name, 5.0)
status_list_timeout = rospy.get_param(
rospy.remap_name(self.ns) + "/status_list_timeout", 5.0)
self.status_list_timeout = rospy.Duration(status_list_timeout)
self.status_timer = threading.Thread(
None, ros_timer, None,
(self.publish_status_async, self.status_frequency))
self.status_timer.start()
def miroline():
global tetha, start_x, start_y
param_name = rospy.search_param('virtual_x')
VX = rospy.get_param(param_name)
param_name = rospy.search_param('virtual_y')
VY = rospy.get_param(param_name)
start_x = VX #i take this information for fix the miro position is move out of the straight line
start_y = VY
ax = float(x) #actual position of miro
ay = float(y) #actual position of miro
#rospy.loginfo('ax :{} , ay:{}'.format(ax, ay))
px = ax - float(
(ay * (VX - ax) / (VY - ay))
) #this point corrispont to the size of the side of the triangole that is along x
py = ay - float((ax * (VY - ay) / (VX - ax)))
#rospy.loginfo('ax :{} , ay:{}'.format(px, py))
sq = px**2 + py**2
py = math.sqrt(sq) # py became the other side of the triangle
if px != 0: #& py!=0 :
tetha = math.acos(px / py)
def main():
rospy.init_node('auv_pf', anonymous=True)
rospy.loginfo("Successful initilization of node")
param = rospy.search_param("measurement_period")
T_meas = float(rospy.get_param(param))
pf = auv_pf()
rospy.loginfo("Particle filter class successfully created")
meas_rate = rospy.Rate(1 / T_meas)
while not rospy.is_shutdown():
pf.measurement()
meas_rate.sleep()
def __init__(self, feedback, uuid=None, priority=0, topic=None,
frequency=common.HEARTBEAT_HZ, lock=None):
""" Constructor. """
if lock is None:
lock = threading.RLock()
self.lock = lock
"""
.. _Big_Requester_Lock:
Big Requester Lock.
This recursive Python threading lock serializes access to
requester data. The requester *feedback* function is always
invoked holding it. All :class:`.Requester` methods acquire
it automatically, whenever needed.
In any other thread, acquire it when updating shared request
set objects. Never hold it when sleeping or waiting for I/O.
"""
if uuid is None:
uuid = unique_id.fromRandom()
self.requester_id = uuid
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet([], self.requester_id)
"""
:class:`.RequestSet` containing the current status of every
:class:`.ResourceRequest` made by this requester. All
requester operations are done using this object and its
contents.
"""
self.priority = priority
""" Default for new requests' priorities if none specified. """
self.feedback = feedback # requester feedback
if topic is None:
topic_param = rospy.search_param('topic_name')
if topic_param is None:
topic = common.SCHEDULER_TOPIC
else:
topic = rospy.get_param(topic_param)
self.pub_topic = topic
self.sub_topic = common.feedback_topic(uuid, topic)
rospy.loginfo('ROCON requester feedback topic: ' + self.sub_topic)
self.sub = rospy.Subscriber(self.sub_topic, SchedulerRequests,
self._feedback,
queue_size=1, tcp_nodelay=True)
self.pub = rospy.Publisher(self.pub_topic, SchedulerRequests,
latch=True, queue_size=1)
self.time_delay = rospy.Duration(1.0 / frequency)
self._set_timer()
def main():
rospy.init_node("axis_driver")
arg_defaults = {
'hostname': '192.168.0.90', # default IP address
'username': '******', # default login name
'password': '',
'width': 640,
'height': 480,
'frame_id': 'axis_camera',
'camera_info_url': ''
}
# Look up parameters starting in the driver's private parameter
# space, but also searching outer namespaces. Defining them in a
# higher namespace allows the axis_ptz.py script to share parameters
# with the driver.
args = {}
for name, val in arg_defaults.iteritems():
full_name = rospy.search_param(name)
if full_name is None:
args[name] = val
else:
args[name] = rospy.get_param(full_name, val)
# resolve frame_id with tf_prefix (unless already absolute)
if args['frame_id'][0] != '/': # not absolute?
tf_prefix = rospy.search_param('tf_prefix')
prefix_val = ''
if tf_prefix is not None: # prefix defined?
prefix_val = rospy.get_param(tf_prefix)
if prefix_val[0] != '/': # prefix not absolute?
prefix_val = '/' + prefix_val
args['frame_id'] = prefix_val + '/' + args['frame_id']
Axis(**args)
rospy.spin()
def __init__(self):
kQueueSize = 15
topics = {}
try:
node_name = rospy.get_name()
topics['image_sub'] = rospy.get_param(
rospy.search_param("camera_image_topic"))
topics['mask_sub'] = rospy.get_param(
rospy.search_param("detector_out_topic"))
topics['track_sub'] = rospy.get_param(
rospy.search_param("tracker_out_topic"))
topics['add_entry_pub'] = rospy.get_param(
rospy.search_param("add_entity_topic"))
topics['detect_pub'] = rospy.get_param(
rospy.search_param("detector_input_topic"))
topics['track_pub'] = rospy.get_param(
rospy.search_param("tracker_input_topic"))
topics['out_pub'] = rospy.get_param(
rospy.search_param("image_out_topic"))
except rospy.ROSException:
print("could not get param name")
self.image_sub = rospy.Subscriber(topics['image_sub'],
CompressedImage,
self._callback_image,
queue_size=kQueueSize)
self.mask_sub = rospy.Subscriber(topics['mask_sub'],
MaskFrame,
self._callback_mask,
queue_size=kQueueSize)
self.track_sub = rospy.Subscriber(topics['track_sub'],
EntitiesFrameMsg,
self._callback_tracker,
queue_size=kQueueSize)
self.add_entry_pub = rospy.Publisher(topics['add_entry_pub'],
AddEntityRequestMsg,
queue_size=kQueueSize)
self.detect_pub = rospy.Publisher(topics['detect_pub'],
CompressedImage,
queue_size=kQueueSize)
self.track_pub = rospy.Publisher(topics['track_pub'],
CompressedImage,
queue_size=kQueueSize)
self.out_pub = rospy.Publisher(topics['out_pub'],
CompressedImage,
queue_size=kQueueSize)
self.shouldInitialize = True
self.isInitialized = False
self.key_frame_msg = None
self.counter = 1
def main():
rospy.init_node('jointstate_publisher')
rospy.loginfo("Joint State Publisher Node Started")
Wheels = JointState()
Wheels.name = ["left_wheel_to_motors", "right_wheel_to_motors"]
Wheels.position = [0.0, 0.0]
# Create wheel joint state publisher
wheels = rospy.Publisher('Wheels', JointState, queue_size=10)
# Look for robot Parameters
robotParams = rospy.search_param('/Robot name')
if robotParams:
name = rospy.get_param('/Robot name')
rospy.loginfo("Robot Parameters loaded for : %s", name)
# Retrieve Parameters for physical properties of the robot.
_leftTPR = rospy.get_param('/leftTicksPerRotation')
_rightTPR = rospy.get_param('/rightTicksPerRotation')
else:
rospy.logwarn("Robot Parameters not loaded")
_lstate = 0
_rstate = 0
def lstateCB(lstat):
global _lstate
_lstate = lstat.data
def rstateCB(rstat):
global _rstate
_rstate = rstat.data
lstateSub = rospy.Subscriber('lstate', Float64, lstateCB)
rstateSub = rospy.Subscriber('rstate', Float64, rstateCB)
def moveWheels():
left_wheel_pos = (2 * pi / _leftTPR) * _lstate
right_wheel_pos = (2 * pi / _rightTPR) * _rstate
if left_wheel_pos > pi:
left_wheel_pos = -pi
if left_wheel_pos < -pi:
left_wheel_pos = pi
if right_wheel_pos > pi:
right_wheel_pos = -pi
if right_wheel_pos < -pi:
right_wheel_pos = pi
Wheels.position[0] = left_wheel_pos
Wheels.position[1] = right_wheel_pos
wheels.publish(Wheels)
def test_search_param(self):
try:
orig_caller_id = rospy.names.get_caller_id()
rospy.names._set_caller_id('/global_node')
self.assertEquals(None, rospy.search_param('search_param'))
rospy.set_param('/search_param', 1)
self.assertEquals('/search_param',
rospy.search_param('search_param'))
rospy.names._set_caller_id('/level1/level2/relative_node')
self.assertEquals('/search_param',
rospy.search_param('search_param'))
rospy.set_param('/level1/search_param', 2)
self.assertEquals('/level1/search_param',
rospy.search_param('search_param'))
rospy.set_param('~search_param', 3)
# make sure that search starts in our private namespace first
self.assertEquals('/level1/level2/relative_node/search_param',
rospy.search_param('search_param'))
finally:
rospy.names._set_caller_id(orig_caller_id)
def main():
rospy.init_node("axis")
arg_defaults = {
'hostname': '192.168.0.64',
'username': '******',
'password': '******',
'flip': True,
}
args = {}
for name in arg_defaults:
args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name])
AxisPTZ(**args)
rospy.spin()
def initialize_origin():
global _origin
rospy.init_node('initialize_origin', anonymous=True)
origin_pub = rospy.Publisher('/local_xy_origin',
PoseStamped,
latch=True,
queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=2)
origin_name = rospy.get_param('~local_xy_origin', 'auto')
rospy.loginfo('Local XY origin is "' + origin_name + '"')
origin_frame_id = rospy.get_param(rospy.search_param('local_xy_frame'),
'map')
origin_frame_identity = rospy.get_param('~local_xy_frame_identity',
origin_frame_id + "__identity")
rospy.loginfo('Local XY frame ID is "' + origin_frame_id + '"')
if len(origin_frame_id):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
if origin_name != "auto":
origin_list = rospy.get_param('~local_xy_origins', [])
_origin = make_origin_from_list(origin_frame_id, origin_name,
origin_list)
if _origin is not None:
origin_pub.publish(_origin)
else:
origin_name = "auto"
if origin_name == "auto":
sub = rospy.Subscriber("gps", NavSatFix)
sub.impl.add_callback(gps_callback, (origin_frame_id, origin_pub, sub))
rospy.loginfo('Subscribed to NavSat on ' + sub.resolved_name)
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1),
rospy.Time.now(),
origin_frame_identity,
origin_frame_id)
diagnostic_pub.publish(
make_diagnostic(_origin, (origin_name != "auto")))
rospy.sleep(1.0)
def obstacles(self):
"""Grabs simulated obstacles simulated Lidar is using"""
if self._obstacles is not None:
return self._obstacles
obstacles_param = rospy.search_param('obstacles')
obstacles_from_param = rospy.get_param(obstacles_param, None)
if type(obstacles_from_param) == type([]):
self._obstacles = obstacles_from_param
for obstacle_edges in self._obstacles:
# add last segment to close polygon
obstacle_edges += [obstacle_edges[0]]
else:
self._obstacles = []
return self._obstacles
def main():
rospy.init_node("axis")
arg_defaults = {
'hostname': '192.168.0.65:554',
'username': '******',
'password': '******',
'width': 640,
'height': 480
}
args = {}
for name in arg_defaults:
args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name])
Axis(**args)
rospy.spin()
def init():
"""
Grabs parameters from launch file, parses them and returns them.
:return: speed_limit The desired target speed for the car.
:rtype: float
:return: path_radius Radius for the circular path
:rtype: float
:return: geofence radius Radius for the geofence.
:rtype: float
:return: gear The gear at which we want the car to drive. 0 or 1.
:rtype: int
:return: target_laps How many laps the car is supposed to drive.
:rtype: int
:return: front_differentials Front differentials for the car. Locked or unlocked.
:rtype: int
:return: rear_differentials Rear differentials for the car. Locked or unlocked.
:rtype: int
"""
speed_limit_param = rospy.search_param('speed_limit')
speed_limit = rospy.get_param(speed_limit_param)
print('speed limit')
print(speed_limit)
speed_limit = float(speed_limit)
path_radius_param = rospy.search_param('path_radius')
path_radius = rospy.get_param(path_radius_param)
path_radius = float(path_radius)
geofence_radius = rospy.search_param('geofence_radius')
geofence_radius = rospy.get_param(geofence_radius)
geofence_radius = float(geofence_radius)
gear_param = rospy.search_param('gear')
gear = rospy.get_param(gear_param)
gear = int(gear)
target_laps_param = rospy.search_param('target_laps')
target_laps = rospy.get_param(target_laps_param)
target_laps = float(target_laps)
front_differentials_param = rospy.search_param('front_differentials')
front_differentials = rospy.get_param(front_differentials_param)
front_differentials = int(front_differentials)
rear_differentials_param = rospy.search_param('rear_differentials')
rear_differentials = rospy.get_param(rear_differentials_param)
rear_differentials = int(rear_differentials)
return speed_limit, path_radius, geofence_radius, gear, target_laps, front_differentials, rear_differentials
def __init__(self):
self.listener = tf.TransformListener()
self.br = tf.TransformBroadcaster()
self.dict_pub = rospy.Publisher('/robomuse/marker_dictionary_map',
idarray,
queue_size=10)
self.id_dictionary = np.array([])
self.id_visible_flags = np.array([])
param_name = rospy.search_param('folder_name')
v = rospy.get_param(param_name)
self.nst = v
print self.nst
self.posearray = []
self.goalarray = []
self.goalpub = []
self.flag1 = 0
def main():
rospy.init_node("get_traj_params_test")
args = {}
groups = None
try:
if rospy.search_param('groups'):
groups = rospy.get_param('groups')
else:
rospy.logerr('Param groups not found!')
#print name
except rospy.ROSException, e:
rospy.logerror('%s: %s' % (e, _name))
def main():
rospy.init_node("axis_twist")
arg_defaults = {
'hostname': '192.168.0.90',
'username': '',
'password': '',
'flip': True,
}
args = {}
for name in arg_defaults:
args[name] = rospy.get_param(rospy.search_param(name),
arg_defaults[name])
AxisPTZ(**args)
rospy.spin()
def main():
rospy.init_node("axis")
arg_defaults = {
'hostname': '192.168.0.90',
'username': '',
'password': '',
'delay': 0.0,
'max_command_rate': 0.0,
'flip': True,
}
args = {}
for name in arg_defaults:
args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name])
rospy.sleep(args['delay'])
AxisPTZ(**args)
rospy.spin()
