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(robot_interface=None, ft_sensor_interface=None):
if not rospy.core.is_initialized():
rospy.init_node("safe_kinematic_controller")
print rospy.get_param_names()
robot_root_link = rospy.get_param("~robot_root_link", None)
robot_tip_link = rospy.get_param("~robot_tip_link", None)
rate_hz = rospy.get_param("~publish_frequency", 250.0)
robot = rox_urdf.robot_from_parameter_server(root_link=robot_root_link,
tip_link=robot_tip_link)
controller = ROSController(robot,
1.0 / rate_hz,
robot_interface=robot_interface,
ft_sensor_interface=ft_sensor_interface)
rate = rospy.Rate(rate_hz)
for _ in xrange(100):
if rospy.is_shutdown():
break
if controller.read_robot_joint_position()[0] == ControllerMode.SUCCESS:
break
rate.sleep()
while not rospy.is_shutdown():
controller.step()
rate.sleep()
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 get_param_names(params_glob):
with param_server_lock:
if params_glob:
# If there is a parameter glob, filter by it.
return filter(
lambda x: any(
fnmatch.fnmatch(str(x), glob) for glob in params_glob),
rospy.get_param_names())
else:
# If there is no parameter glob, don't filter.
return rospy.get_param_names()
def __init__(self):
rospy.init_node('waypoint_updater', log_level=rospy.DEBUG)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb)
# rospy.Subscriber('/obstacle_waypoint', Lane, self.obstacle_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints',
Lane,
queue_size=1)
# TODO: Add other member variables you need below
self.current_pose = None
self.base_waypoints = None
self.closest_waypoint_ahead = None
self.final_waypoints = Lane()
self.deceleration = rospy.get_param('~deceleration', 0.5)
self.target_velocity = rospy.get_param('~target_velocity', 5.0)
params = rospy.get_param_names()
for i in range(len(params)):
rospy.logdebug("%s", params[i])
rospy.spin()
def _start_gazebo(self, extra_models):
"""
Configures and starts the Gazebo simulator and backend services
:param rng_seed: RNG seed to spawn Gazebo with
:param playback_path: A path to playback information
:param extra_models: An additional models path or None
:param world_file: The world file that should be loaded by Gazebo
"""
self._initial_ros_params = rospy.get_param_names()
self._initial_ros_nodes = rosnode.get_node_names()
# Gazebo configuration and launch
self._notify("Starting Gazebo robotic simulator")
ifaddress = netifaces.ifaddresses(config.config.get('network', 'main-interface'))
local_ip = ifaddress[netifaces.AF_INET][0]['addr']
ros_master_uri = os.environ.get("ROS_MASTER_URI").replace('localhost', local_ip)
self.gzserver.gazebo_died_callback = self._handle_gazebo_shutdown
# experiment specific gzserver command line arguments
gzserver_args = '--seed {rng_seed} -e {engine} {world_file}'.format(
rng_seed=self.rng_seed, engine=self.sim_config.physics_engine,
world_file=self.sim_config.world_model.resource_path.abs_path)
# If playback is specified, load the first log/world file in the recording at Gazebo launch
if self.sim_config.playback_path:
gzserver_args += ' --play {path}'.format(
path=os.path.join(self.sim_config.playback_path, 'gzserver/1.log'))
else:
# optional roslaunch support prior to Gazebo launch for non-playback simulations
if self.sim_config.ros_launch_abs_path is not None:
if self.sim_config.gzserver_host != 'local':
raise Exception('roslaunch is currently only supported on local installs.')
self._notify("Launching experiment ROS nodes and configuring parameters")
self.ros_launcher = ROSLaunch(self.sim_config.ros_launch_abs_path)
try:
logger.info("gzserver arguments: " + gzserver_args)
self.gzserver.start(ros_master_uri, extra_models, gzserver_args)
except XvfbXvnError as exception:
logger.error(exception)
error = "Recoverable error occurred. Please try again. Reason: {0}".format(exception)
raise Exception(error)
self._notify("Connecting to Gazebo robotic simulator")
self.robotManager.init_scene_handler()
self._notify("Connecting to Gazebo simulation recorder")
self.gazebo_recorder = GazeboSimulationRecorder(self.sim_config.sim_id)
self._notify("Starting Gazebo web client")
os.environ['GAZEBO_MASTER_URI'] = self.gzserver.gazebo_master_uri
self.__set_env_for_gzbridge()
# We do not know here in which state the previous user did let us gzweb.
self.gzweb = LocalGazeboBridgeInstance()
self.gzweb.restart()
def main():
param_names = rospy.get_param_names()
param_names.sort()
stefan_params = [
param for param in param_names if param.startswith("/stefan")
]
'''
for param in stefan_params[-6:]:
param_split = param.split("_")
xml = rospy.get_param(param)
name = param_split[1]
num = int(param_split[2])
for i in range(1, num+1):
model_name = "{}_{}".format(name, i)
spawn_model(model_name, xml, "", part1_pose, "world")
'''
part1_spawn(stefan_params[4])
part2_spawn(stefan_params[5])
part3_spawn(stefan_params[6])
part4_spawn(stefan_params[7])
part5_spawn(stefan_params[8])
part6_spawn(stefan_params[9])
_101350_spawn(stefan_params[0])
_104322_spawn(stefan_params[1])
_122620_spawn(stefan_params[2])
_122925_spawn(stefan_params[3])
def param_demo():
rospy.init_node("param_demo")
rate = rospy.Rate(1)
while(not rospy.is_shutdown()):
#get param
parameter1 = rospy.get_param("/param1")
parameter2 = rospy.get_param("/param2", default=222)
rospy.loginfo('Get param1 = %d', parameter1)
rospy.loginfo('Get param2 = %d', parameter2)
#delete param
rospy.delete_param('/param2')
#set param
rospy.set_param('/param2',2)
#check param
ifparam3 = rospy.has_param('/param3')
if(ifparam3):
rospy.loginfo('/param3 exists')
else:
rospy.loginfo('/param3 does not exist')
#get all param names
params = rospy.get_param_names()
rospy.loginfo('param list: %s', params)
rate.sleep()
def retrieve_params(self):
"""
called to update params from rospy.
CAREFUL : this can be called from another thread (subscriber callback)
"""
params = rospy.get_param_names()
self.reset_params(params)
def param_demo():
rospy.init_node("param_demo")
rate = rospy.Rate(1)
while (not rospy.is_shutdown()):
#get param
parameter1 = rospy.get_param("/param1")
parameter2 = rospy.get_param("/param2", default=222)
rospy.loginfo('Get param1 = %d', parameter1)
rospy.loginfo('Get param2 = %d', parameter2)
#delete param
rospy.delete_param('/param2')
#set param
rospy.set_param('/param2', 2)
#check param
ifparam3 = rospy.has_param('/param3')
if (ifparam3):
rospy.loginfo('/param3 exists')
else:
rospy.loginfo('/param3 does not exist')
#get all param names
params = rospy.get_param_names()
rospy.loginfo('param list: %s', params)
rate.sleep()
def save_params_callback(req):
"""Service to save all rosparam server param in a file (date_time.yaml)."""
file_name = datetime.datetime.now().strftime(
'%Y%m%d%H%M%S') + '_parameters.yaml'
file_name = params_path + '/' + file_name
print('Create file: ' + file_name)
param_file = open(file_name, "w")
try:
os.remove(params_path + "/latest_params.yaml")
except OSError:
pass
os.symlink(file_name, params_path + "/latest_params.yaml")
for name in rospy.get_param_names():
if not name.startswith('/robot_description'):
if not name.startswith('/roslaunch/uris'):
if not name.startswith('/rosbridge_websocket'):
if not name.startswith('/rosapi'):
value = rospy.get_param(name)
if isinstance(value, str):
line = "{:s}: '{}'\n".format(name, value)
else:
line = '{:s}: {}\n'.format(name, value)
param_file.write(line)
param_file.close()
return EmptyResponse()
def update(self):
self.listbox.delete(0, tk.END)
params = rospy.get_param_names()
for param in params:
self.listbox.insert(tk.END, param)
def __init__(self, context):
super(WorkspaceUI, self).__init__(context)
self.setObjectName('Instructor Workspace UI')
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
rospack = rospkg.RosPack()
ui_path = rospack.get_path('instructor_core') + '/ui/workspace.ui'
# Load the ui attributes into the main widget
loadUi(ui_path, self._widget)
self._widget.setObjectName('InstructorWorkspaceUI')
self._widget.setWindowTitle('Instructor Workspace UI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
title = self._widget.windowTitle() + (' (%d)' % context.serial_number())
self._widget.setWindowTitle(title)
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
# Hide things to start with
self._widget.fiducial_found_widget.hide()
self._widget.add_fiducial_widget.hide()
# Connect things
self._widget.add_ok_btn.clicked.connect(self.add_ok_event)
self._widget.add_cancel_btn.clicked.connect(self.add_cancel_event)
self._widget.fiducial_name_edit.textChanged.connect(self.name_entered_event)
# Add listener to detect AR markers
self.tf_listen = tf.TransformListener()
# Add predicator publisher
self.pub_predicates = rospy.Publisher('predicator/input', PredicateList)
# Clean params for landmarks
rospy.loginfo('Cleaning up previous workspace landmark params')
params = rospy.get_param_names()
landmarks = [p for p in params if "instructor_landmark" in p]
rospy.loginfo(landmarks)
for marker in landmarks:
rospy.loginfo('Removed '+str(marker))
rospy.delete_param(marker)
# Add timer
self.timer = QtCore.QTimer()
self.connect(self.timer, QtCore.SIGNAL("timeout()"), self.update)
self.timer.start(100)
def retrieve_system_state(self):
"""
This will retrieve the system state from ROS master if needed, and apply changes to local variable to keep
a local representation of the connections available up to date.
"""
try:
# we call the master only if we dont get system_state from connection cache
if self.enable_cache and self.connection_cache is not None:
publishers, subscribers, services = self.connection_cache.getSystemState(
)
topic_types = self.connection_cache.getTopicTypes()
try:
service_types = self.connection_cache.getServiceTypes()
# handling fallback here since master doesnt have the API
except rocon_python_comms.UnknownSystemState as exc:
service_types = []
else:
publishers, subscribers, services = self._master.getSystemState(
)[2]
topic_types = self._master.getTopicTypes()[2]
service_types = [] # master misses this API to be consistent
# Getting this doesnt depend on cache for now
params = set(rospy.get_param_names())
return publishers, subscribers, services, params, topic_types, service_types
except socket.error:
rospy.logerr(
"[{name}] couldn't get system state from the master ".format(
name=__name__))
def param_demo():
rospy.init_node("params_test")
rate = rospy.Rate(1)
while (not rospy.is_shutdown()):
#get param
'''parameter1 = rospy.get_param("~param1") #在node下的私有变量
parameter2 = rospy.get_param("/param2", default=222) #全局变量
print(type(parameter1)) #参数类型
print(type(parameter2)) #参数类型
rospy.loginfo('Get param1 = %s', parameter1)
rospy.loginfo('Get param2 = %d', parameter2)
#delete param 删除
rospy.delete_param('/param2')'''
#set param 设置
rospy.set_param('~param4', 4)
#check param 判断是否存在
'''ifparam3 = rospy.has_param('/param3')
if(ifparam3):
rospy.loginfo('/param3 exists')
else:
rospy.loginfo('/param3 does not exist')
'''
#批量获取参数
'''gain_param = rospy.get_param('gains')
p, i, d = gain_param['P'], gain_param['I'], gain_param['D']
rospy.loginfo("gains are %s, %s, %s", p, i, d)
'''
#get all param names
params = rospy.get_param_names()
rospy.loginfo('param list: %s', params)
rate.sleep()
def main():
client_spawn = rospy.ServiceProxy("gazebo/spawn_urdf_model", SpawnModel)
client_get_model = rospy.ServiceProxy("gazebo/get_model_state",
GetModelState)
client_delete = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
param_names = rospy.get_param_names()
object_param = [
param for param in param_names if param.startswith("/object")
][0]
object_xml = rospy.get_param(object_param)
object_xyz = Point(0, 0, 0)
rpy = np.array([0, 0, 0])
quat = tf.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
object_rpy = Quaternion(quat[0], quat[1], quat[2], quat[3])
object_pose = Pose(object_xyz, object_rpy)
object_name = "object"
object_namespace = ""
reference = "world"
already = client_get_model(object_name, "world").success
if not already:
pass
else:
print("'{}' already exists in gazebo, so delete and respawn this.".
format(object_name))
client_delete(object_name)
client_spawn(object_name, object_xml, object_namespace, object_pose,
reference)
def get(self):
items = {}
items['rosparams'] = rospy.get_param_names()
L = []
for p in items['rosparams']:
par = str(rospy.get_param(p))
tp = "String"
try:
float(par)
tp = "Number"
except:
tp = "String"
L.append([p, str(par), tp])
items['rosparams'] = L
# items['rosparams'] = [for p in items['rosparams']]
# for i in range(len(items['rosparams'])):
# paramname = items['rosparams'][i][0]
# pp = rospy.get_param(paramname)
# items['rosparams'][i][2] = str(pp)
items['roshubmainip'] = roshubmainip
self.render("roverctrl/templates/rosparams.html",
title="Ros Parameters",
items=items)
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.items():
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 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 boat_info_params():
global lisy_of_params
boat_info_array = []
params = rospy.get_param_names()
for i in range(len(params)):
if ("boat_name" in params[i]) and (params[i] not in list_of_params):
list_of_params.append(params[i])
for i in range(len(list_of_params)):
temp = BoatAuctionInfo()
if rospy.has_param("boat" + str(i + 1) + "/boat_speed"):
temp.speed = rospy.get_param("boat" + str(i + 1) + "/boat_speed")
#print "spped:" ,temp.speed
temp.capacity = rospy.get_param("boat" + str(i + 1) +
"/boat_capacity")
temp.boat_name = rospy.get_param("boat" + str(i + 1) +
"/boat_name")
temp.pose.position.x = rospy.get_param("boat" + str(i + 1) +
"/boat_location_x")
temp.pose.position.y = rospy.get_param("boat" + str(i + 1) +
"/boat_location_y")
temp.pose.position.z = rospy.get_param("boat" + str(i + 1) +
"/boat_location_z")
temp.pose.orientation.x = 0
temp.pose.orientation.y = 0
temp.pose.orientation.z = 0
temp.pose.orientation.w = 0
boat_info_array.append(temp)
#print boat_info_array
return boat_info_array
def setup_params(self):
"""
Reads the parameters that belong to the visual odometry pipeline and passes them to the instantiated visual
odometer
"""
# Get all parameters with the parameter file root in front
odometry_parameters = [
param for param in rospy.get_param_names()
if self.parameter_file_root in param
]
for parameter_name in odometry_parameters:
# Read parameter
parameter_value = rospy.get_param(parameter_name)
# String parameters have to be treated in a special way, appending \' \' at the two sides
s_parameter_value = [
"\'" + parameter_value + "\'" if isinstance(
parameter_value, basestring) else str(parameter_value)
][0]
# Remove the root from the parameter name, then pass it to the visual odometer
parameter_name = parameter_name.split(self.parameter_file_root)[1]
self.visual_odometer.set_parameter(parameter_name, parameter_value,
s_parameter_value)
self.log_info(parameter_name + "=" + s_parameter_value)
self.log_info("Parameters file loaded correctly")
def main():
rospy.set_param('Time_now',datetime.datetime.now())
rospy.sleep(10)
sec = rospy.get_param('Time_now')
rospy.loginfo(sec)
param_names = rospy.get_param_names()
rospy.loginfo(param_names)
def main():
logging.info("Running get_desktop path, setting dekstop path as ROS param")
desktop_path_current = desktop_path()
rospy.set_param("/DESKTOP_PATH", desktop_path_current)
logging.info("ROS Parameters are: {}", str(rospy.get_param_names()))
logging.info("Desktop path set to: {}".format(desktop_path_current))
return
def load_nl_command_map(self, control_param_name):
"""
Load the command map and create ros publishers for each topic.
:param control_param_name: The ROS parameter name to load.
:return: nothing, updates self._nl_command_mapping and
self._publisher_map
"""
rospy.loginfo('Looking for parameter: {}'.format(control_param_name))
if not rospy.has_param(control_param_name):
rospy.logerr(
'Cannot proceed without the control parameter: {}'.format(
control_param_name))
rospy.signal_shutdown('Cannot proceed without parameter.')
param = rospy.get_param(control_param_name)
assert 'topics' in param
# Convert a list of dictionaries to a list of tuples.
topics_and_types = [x.items()[0] for x in param['topics']]
rospy.loginfo('Desired topics & types: {}'.format(topics_and_types))
rospy.logdebug('Available parameters: {}'.format(
rospy.get_param_names()))
for (param_topic_name, topic_type_str) in topics_and_types:
if param_topic_name and param_topic_name[0] == '/':
rospy.logwarn(
'Parameter {} is a global parameter, '
'all parameters should be relative to: {}'.format(
param_topic_name, control_param_name))
continue
# The relative parameter (the one we look for) is under the
# control_param_name and will contain all the commands.
relative_param_name = control_param_name + '/' + param_topic_name
if not rospy.has_param(relative_param_name):
rospy.logerr(
'Missing ROS parameter {}'.format(relative_param_name))
continue # Do not die here, continue to next param.
rospy.loginfo(
'Getting all commands for topic {}'.format(param_topic_name))
command_mapping = rospy.get_param(relative_param_name)
# Note that the output topic is *not* relative.
cmd_map = LanguageToMessageTranslator.parse_command_mapping(
param_topic_name, topic_type_str, command_mapping)
self._nl_command_map.update(cmd_map)
pub_map = LanguageToMessageTranslator.get_publisher(
param_topic_name, topic_type_str)
self._publisher_map.update(pub_map)
rospy.loginfo('NL control running, listening to topic: {}.'.format(
self.nl_command_topic))
def get_fullscale():
params = rospy.get_param_names()
for param in params:
if 'spacenav' in param and 'full_scale' in param:
return int(rospy.get_param(param))
# assuming no full_scale since there wasn't a spacenav node with
# full_scale as a parameter
return 0
def loadParameters():
param_names = rospy.get_param_names()
config = {}
for key in param_names:
value = rospy.get_param(key)
_, key = key.rsplit('/', 1)
config.update({key: value})
return config
def does_param_exist(param):
try:
params = rospy.get_param_names() # list of params
if param in params:
return True
except:
pass
return False
def saveAllParams(filename):
with open(filename, 'w') as f:
params = rospy.get_param_names()
for param in params:
f.write(param)
f.write(" ; ")
f.write(str(rospy.get_param(param)))
f.write("\n")
def get_param(self, name):
try:
return rospy.get_param(name)
except KeyError as e:
params = ', '.join([ '"' + name + '"' for name in sorted(rospy.get_param_names()) ])
raise KeyError(
'Caught KeyError("%s") while getting parameter. Known parameters: %s' % (e, params)
)
def saveAllParams(filename):
with open(filename, 'w') as f:
params = rospy.get_param_names()
for param in params:
f.write(param)
f.write(" ; ")
f.write(str(rospy.get_param(param)))
f.write("\n")
def get_fullscale():
params = rospy.get_param_names()
for param in params:
if 'spacenav' in param and 'full_scale' in param:
return int(rospy.get_param(param))
# assuming no full_scale since there wasn't a spacenav node with
# full_scale as a parameter
return 0
def get_param_names(params_glob):
with param_server_lock:
if params_glob:
# If there is a parameter glob, filter by it.
return filter(lambda x: any(fnmatch.fnmatch(str(x), glob) for glob in params_glob), rospy.get_param_names())
else:
# If there is no parameter glob, don't filter.
return rospy.get_param_names()
def __init__(self):
rospy.init_node('goto_starting_pose', anonymous=False)
#starting_pose = PoseWithCovarianceStamped()
# What to do if shut down (e.g. Ctrl-C or failure)
rospy.on_shutdown(self.shutdown)
########################################################
self.goal_sent = False
starting_pose = PoseWithCovarianceStamped()
rospy.Subscriber("/pick_finished_alt_flag", Bool,
pick_finished_alternative)
initialPose_pub = rospy.Publisher("/initialpose",
PoseWithCovarianceStamped,
queue_size=10)
starting_pose.header.stamp = rospy.Time.now()
starting_pose.header.frame_id = "map"
#starting_pose.pose.pose.position.x = self.pose[0]
#starting_pose.pose.pose.position.y = self.pose[1]
if rospy.has_param("/initial_pose_x"):
alfa = rospy.get_param("/initial_pose_x")
print(alfa)
print(rospy.get_param_names())
#get parameters from the launch file
rospy.loginfo('Parameter %s has value %s',
rospy.resolve_name("/initial_pose_x"),
rospy.get_param("/initial_pose_x"))
rospy.loginfo('Parameter %s has value %s',
rospy.resolve_name("initial_pose_y"),
rospy.get_param("initial_pose_y"))
#get initial position from launchfile map_navigation_stage.launch
starting_pose.pose.pose.position.x = rospy.get_param(
"/initial_pose_x") #initial_pose_x
starting_pose.pose.pose.position.y = rospy.get_param(
"/initial_pose_y") #initial_pose_x
starting_pose.pose.pose.position.z = 0 # rospy.get_param('initial_pose_z') #initial_pose_x
# we extract the yaw component using tf.transformations..quaternion_from_euler(rot)
#that converts a RPY Euler notation into a quaternion ,
#The yaw is at index 2 of the euler array.
(starting_pose.pose.pose.orientation.x,
starting_pose.pose.pose.orientation.y,
starting_pose.pose.pose.orientation.z,
starting_pose.pose.pose.orientation.w
) = transformations.quaternion_from_euler(
0, 0, rospy.get_param('initial_pose_a')) #self.pose[2])
#Initial pose covariance (x*x), used to initialize filter with Gaussian distribution.
starting_pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5
#Initial pose covariance (y*y), used to initialize filter with Gaussian distribution.
starting_pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5
#starting_pose.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
#Initial pose covariance (yaw*yaw), used to initialize filter with Gaussian distribution.
starting_pose.pose.covariance[6 * 6 -
1] = math.pi / 12.0 * math.pi / 12.0
def checkParam(self, param_name):
params = rospy.get_param_names()
if any([x for x in params if x == param_name]):
rospy.loginfo('Checking param %s: OK' % param_name)
return 'succeeded'
else:
rospy.logerr('Checking param %s: Failed' % param_name)
self.ALL_OK = False
return 'aborted'
def get_current_ros_params():
params = dict()
param_names = rospy.get_param_names()
for name in param_names:
root_name = name[1:].split("/", 1)[0]
if root_name in params:
continue
params[root_name] = rospy.get_param("/" + root_name)
return params
def __check_params(self, param_name):
params = rospy.get_param_names()
if any([x for x in params if x == param_name]): # None
self._print_info("Checking params '%s': OK" % param_name)
return 'succeeded'
else:
self.ALL_OK = False
self._print_fatal("Checking params '%s': Failed" % param_name)
return 'aborted'
def __check_params(self, param_name):
params = rospy.get_param_names()
if any([x for x in params if x==param_name]): # None
self._print_info("Checking params '%s': OK" % param_name)
return 'succeeded'
else:
self.ALL_OK = False
self._print_fatal("Checking params '%s': Failed" % param_name)
return 'aborted'
def load_nl_command_map(self, control_param_name):
"""
Load the command map and create ros publishers for each topic.
:param control_param_name: The ROS parameter name to load.
:return: nothing, updates self._nl_command_mapping and
self._publisher_map
"""
rospy.loginfo('Looking for parameter: {}'.format(control_param_name))
if not rospy.has_param(control_param_name):
rospy.logerr('Cannot proceed without the control parameter: {}'.
format(control_param_name))
rospy.signal_shutdown('Cannot proceed without parameter.')
param = rospy.get_param(control_param_name)
assert 'topics' in param
# Convert a list of dictionaries to a list of tuples.
topics_and_types = [x.items()[0] for x in param['topics']]
rospy.loginfo('Desired topics & types: {}'.format(topics_and_types))
rospy.logdebug('Available parameters: {}'.format(
rospy.get_param_names()))
for (param_topic_name, topic_type_str) in topics_and_types:
if param_topic_name and param_topic_name[0] == '/':
rospy.logwarn('Parameter {} is a global parameter, '
'all parameters should be relative to: {}'.
format(param_topic_name, control_param_name))
continue
# The relative parameter (the one we look for) is under the
# control_param_name and will contain all the commands.
relative_param_name = control_param_name + '/' + param_topic_name
if not rospy.has_param(relative_param_name):
rospy.logerr('Missing ROS parameter {}'.format(
relative_param_name))
continue # Do not die here, continue to next param.
rospy.loginfo('Getting all commands for topic {}'.format(
param_topic_name))
command_mapping = rospy.get_param(relative_param_name)
# Note that the output topic is *not* relative.
cmd_map = LanguageToMessageTranslator.parse_command_mapping(
param_topic_name, topic_type_str, command_mapping)
self._nl_command_map.update(cmd_map)
pub_map = LanguageToMessageTranslator.get_publisher(
param_topic_name, topic_type_str)
self._publisher_map.update(pub_map)
rospy.loginfo('NL control running, listening to topic: {}.'.format(
self.nl_command_topic))
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 list_params():
"""
GET /api/<version>/param
GET a list of all available params from the ROS Parameter Server.
See http://wiki.ros.org/Parameter%20Server for more on the Parameter Server.
Parameter names are listed in the "results" field of the JSON response body.
"""
return jsonify({"results": rospy.get_param_names()})
def list_params():
"""
GET /api/<version>/param
GET a list of all available params from the ROS Parameter Server.
See http://wiki.ros.org/Parameter%20Server for more on the Parameter Server.
Parameter names are listed in the "results" field of the JSON response body.
"""
return jsonify({"results": rospy.get_param_names()})
def create_filter(req):
params = {}
prefix = "%s/%s/" % (rospy.get_name(), req.name)
for p in rospy.get_param_names():
if p.startswith(prefix):
params[p[len(prefix):]] = rospy.get_param(p)
fc.create_filter(req.name, req.type, req.order, params)
update_filter_topic()
return ros_vision.srv.CreateFilterResponse()
def fill_connections():
for param in rospy.get_param_names():
i = param.rfind('abtr_target')
if i != -1 :
ns = param[0:i-1]
source = Source(ns)
target = source.target()
if not target in connections :
connections[target] = [source]
else :
connections[target].append(source)
def load(self):
parameters = rospy.get_param_names()
self.fins = {}
finsNamespace = rospy.get_name()+"/fins"
for parameter in parameters:
match = re.match(finsNamespace+"/([^/]*)/.*", parameter)
if match and not match.group(1) in self.fins:
self.fins[match.group(1)] = Fin()
for fin in self.fins:
finNamespace = finsNamespace+"/"+fin
for actuator in self.actuators:
actuatorNamespace = finNamespace+"/"+actuator
actuator = getattr(self.fins[fin], actuator)
setattr(actuator, "servo",
rospy.get_param(actuatorNamespace+"/servo", -1))
for outputChannel in self.outputChannels:
outputChannelScale = self.outputChannels[outputChannel]
outputChannelNamespace = actuatorNamespace+"/"+outputChannel
outputChannel = getattr(actuator, outputChannel)
constant = rospy.get_param(outputChannelNamespace+"/constant", 0.0)
setattr(outputChannel, "constant", constant*outputChannelScale)
coefficients = {}
coefficientsNamespace = outputChannelNamespace+"/coefficients"
for parameter in parameters:
match = re.match(coefficientsNamespace+"/([^/]*)/.*", parameter)
if match and not match.group(1) in coefficients:
coefficients[match.group(1)] = Coefficient()
for coefficient in coefficients:
coefficientNamespace = coefficientsNamespace+"/"+coefficient
setattr(coefficients[coefficient], "input_channel",
rospy.get_param(coefficientNamespace+"/input_channel", 0))
transferFunction = rospy.get_param(
coefficientNamespace+"/transfer_function", "identity")
setattr(coefficients[coefficient], "transfer_function",
self.transferFunctions[transferFunction]);
setattr(coefficients[coefficient], "invert_arguments",
rospy.get_param(coefficientNamespace+"/invert_arguments", False))
setattr(coefficients[coefficient], "invert_values",
rospy.get_param(coefficientNamespace+"/invert_values", False))
setattr(outputChannel, "coefficients", coefficients.values())
def get_all_user_params():
""" Yields all the parameters loaded in the parameter server
except the ones already loaded by the ROSMaster.
Attributes:
:param ns: The namespace where to look for.
Yields:
:return param(param_full_name, param_value): yields A param"""
all_param_names = rospy.get_param_names()
for pn in all_param_names:
if any(map(pn.__contains__, MASTER_PARAMS)):
continue # skip parameters that are loaded by rosmaster
pvalue = rospy.get_param(pn)
yield Param(pn, pvalue)
def get_threshold(parameter_basename, prefix='bgr'):
possible_params = rospy.get_param_names()
bounds = []
for param in possible_params:
if param.startswith(parameter_basename) and (prefix in param):
param_value = rospy.get_param(param)
bounds.append(np.array(param_value))
if len(bounds) < 2:
raise KeyError
vals = np.vstack(bounds)
upper_bound = np.max(vals, axis=0)
lower_bound = np.min(vals, axis=0)
return np.vstack([lower_bound, upper_bound]).transpose()
def get_threshold(parameter_basename, prefix="bgr"):
possible_params = rospy.get_param_names()
bounds = []
for param in possible_params:
if param.startswith(parameter_basename) and (prefix in param):
param_value = rospy.get_param(param)
bounds.append(np.array(param_value))
if len(bounds) < 2:
raise (KeyError("Could not find parameter starting with {} with prefix {}".format(parameter_basename, prefix)))
vals = np.vstack(bounds)
upper_bound = np.max(vals, axis=0)
lower_bound = np.min(vals, axis=0)
return np.vstack([lower_bound, upper_bound]).transpose()
def show_usage():
"""Show usage information giving the possible motions to use."""
# Get the available motion names from param server
param_names = rospy.get_param_names()
motion_names = []
for param_name in param_names:
# If the parameter is like '/play_motion/motions/MOTION_NAME/joints'
if "/play_motion/motions" in param_name and '/joints' in param_name:
motion_name = param_name.replace('/play_motion/motions/', '')
motion_name = motion_name.replace('/joints', '')
motion_names.append(motion_name)
rospy.loginfo("""Usage:
\trosrun run_motion run_motion_python_node.py MOTION_NAME"
\twhere MOTION_NAME must be one of the motions listed in: """ + str(motion_names))
def update_client_list(self):
rospy.logdebug(self.namespace + ': Updating client list...')
param_list = rospy.get_param_names() # Get list of strings with all parameter names on the parameter server
for param in param_list:
if (param.endswith("server_started") and
not param.startswith(rospy.get_namespace()) and
rospy.get_param(param) is True):
# Parameter indicates action server exists, is a different robot, and has been started
server_name = param.replace("server_started", "external_pose_action")
rospy.logdebug(self.namespace + ': Found server with name: ' + server_name)
if server_name not in self.existing_clients:
# This server has not previously been added so let's make a new client for it
new_client = actionlib.SimpleActionClient(server_name, ExternalPoseAction)
self.client_list.append(new_client)
self.existing_clients.append(server_name)
rospy.logdebug(self.namespace + ': Added server with name ' + server_name + ' to client list.')
rospy.logdebug("Updated client list, it now contains %d robots", len(self.existing_clients))
def __init__(self):
rospy.init_node('dynamixel_joint_state_publisher', anonymous=True)
dynamixel_namespace = rospy.get_namespace()
rate = rospy.get_param('~rate', 10)
r = rospy.Rate(rate)
self.joints = list()
# Get all parameter names
parameters = rospy.get_param_names()
for parameter in parameters:
# Look for the parameters that name the joints.
if parameter.find(dynamixel_namespace) != -1 and parameter.find("joint_name") != -1:
self.joints.append(rospy.get_param(parameter))
self.servos = list()
self.controllers = list()
self.joint_states = dict({})
self.jnt_fudge = 0
for joint in self.joints:
# Remove "_joint" from the end of the joint name to get the controller names.
servo = joint.split("_joint")[0]
self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0)
# ARD
self.controllers.append(dynamixel_namespace + servo + "_controller")
# self.controllers.append(joint)
rospy.loginfo("Dynamixel Joint State Publisher " + joint)
# Start controller state subscribers
[rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
# Start publisher
self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2)
rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
while not rospy.is_shutdown():
self.publish_joint_states()
# With 23 servos, we go as fast as possible
r.sleep()
def check_landmarks(self):
params = rospy.get_param_names()
landmarks = [p for p in params if p.find('instructor_landmark')>=0]
#rospy.logwarn("landmarks: " + str(landmarks))
all_landmark_names =[]
for L in landmarks:
all_landmark_names.append(L.replace('/instructor_landmark/',''))
for L in landmarks:
landmark_name = L.replace('/instructor_landmark/','')
#rospy.logwarn(landmark_name)
try:
landmark_tf_name = rospy.get_param(L)
except KeyError:
rospy.logerr("Could not find landmark %s!"%(L))
continue
#rospy.logwarn(landmark_tf_name)
try:
self.listener_.waitForTransform('/world',landmark_tf_name, rospy.Time(), rospy.Duration(4.0))
try:
F = tf_c.fromTf(self.listener_.lookupTransform('/world',landmark_tf_name,rospy.Time(0)))
self.saved_landmark_frames[landmark_name] = F
except (tf.LookupException, tf.ConnectivityException) as e:
rospy.logerr('Frame ['+landmark_tf_name+'] not found')
return
if landmark_name not in self.landmarks.keys():
self.landmarks[landmark_name] = '/'+landmark_name
# short_name = 'landmark_' + landmark_name.split('_')[len(landmark_name.split('_'))-1:][0]
self.add_landmark_marker('/'+landmark_name,landmark_name)
else:
if landmark_name in self.stale_landmarks.keys():
self.stale_landmarks.pop(landmark_name)
self.add_landmark_marker('/'+landmark_name,landmark_name)
except tf.Exception, e:
rospy.logerr('Frame ['+landmark_tf_name+'] not found (TIMEOUT)')
if rospy.has_param(L):
rospy.delete_param(L)
def __init__(self, parent=None):
super(LineEditDialog, self).__init__()
self.value = None
vbox = QtGui.QVBoxLayout(self)
# combo box
model = QtGui.QStandardItemModel(self)
for elm in rospy.get_param_names():
model.setItem(model.rowCount(), 0, QtGui.QStandardItem(elm))
self.combo_box = QtGui.QComboBox(self)
self.line_edit = QtGui.QLineEdit()
self.combo_box.setLineEdit(self.line_edit)
self.combo_box.setCompleter(QtGui.QCompleter())
self.combo_box.setModel(model)
self.combo_box.completer().setModel(model)
self.combo_box.lineEdit().setText('')
vbox.addWidget(self.combo_box)
# button
button = QPushButton()
button.setText("Done")
button.clicked.connect(self.buttonCallback)
vbox.addWidget(button)
self.setLayout(vbox)
def test_param_server(self):
# this isn't a parameter server test, just checking that the rospy bindings work
import rospy
try:
rospy.get_param('not_a_param')
self.fail("should have raised KeyError")
except KeyError: pass
self.assertEquals('default_val', rospy.get_param('not_a_param', 'default_val') )
p = rospy.get_param('/param')
self.assertEquals("value", p)
p = rospy.get_param('param')
self.assertEquals("value", p)
p = rospy.get_param('/group/param')
self.assertEquals("group_value", p)
p = rospy.get_param('group/param')
self.assertEquals("group_value", p)
self.assertEquals('/param', rospy.search_param('param'))
names = rospy.get_param_names()
self.assert_('/param' in names)
self.assert_('/group/param' in names)
for p in ['/param', 'param', 'group/param', '/group/param']:
self.assert_(rospy.has_param(p))
rospy.set_param('param2', 'value2')
self.assert_(rospy.has_param('param2'))
self.assertEquals('value2', rospy.get_param('param2'))
rospy.delete_param('param2')
self.failIf(rospy.has_param('param2'))
try:
rospy.get_param('param2')
self.fail("should have raised KeyError")
except KeyError: pass
def delete_initial_landmarks(self):
params = rospy.get_param_names()
landmarks = [p for p in params if p.find('instructor_landmark')>=0]
for L in landmarks:
#print L
rospy.delete_param(L)
def get_param_names():
with param_server_lock:
return rospy.get_param_names()
def __init__(self):
name = rospy.get_name()
rospy.loginfo("get_node name " + name )
self.name = name
rospy.loginfo("FollowController " + name)
rospy.loginfo("init")
# parameters: rates and joints
self.rate = rospy.get_param('~controllers/'+ name +'/rate',50.0)
self.joints = list()
self.fudge_factor = list()
# Get all parameter names
parameters = rospy.get_param_names()
#for parameter in parameters:
# Look for the parameters that name the joints.
#if parameter.find("joint_name") != -1:
#self.joints.append(rospy.get_param(parameter))
self.joints = rospy.get_param('~controllers/'+name+'/joints')
self.controllers = list()
self.fudge_factor = list()
self.current_pos = list()
for joint in self.joints:
#self.device.servos[joint].controller = self
# Remove "_joint" from the end of the joint name to get the controller names.
servo_nm = joint.split("_joint")[0]
self.controllers.append(servo_nm + "_controller")
self.current_pos.append(0)
# action server for FollowController
name = rospy.get_param('~controllers/'+name +'/action_name','follow_joint_trajectory')
self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
rospy.loginfo("Started FollowController ("+name+"). Joints: " + str(self.joints) + " Controllers: " + str(self.controllers))
# /joint_states
rospy.Subscriber('joint_states', JointState, self.getJointState)
# Possible enhancement: subscribe to /joint_states
# Start controller state subscribers
self.position_pub = list()
self.torque_services = list()
self.speed_services = list()
self.last_speed = list()
self.max_speed = 0.5
self.max_speed_shoulder_pan = .2
for c in self.controllers:
if c != 'left_upper_arm_hinge_controller' and c != 'right_upper_arm_hinge_controller':
c_srv = rospy.Publisher(c + '/command', Float64)
self.position_pub.append(c_srv)
rospy.loginfo("Real pos pub " + c)
if c != 'left_shoulder_tilt_controller' and c != 'right_shoulder_tilt_controller':
speed_service = c + '/set_speed'
rospy.wait_for_service(speed_service)
srv = rospy.ServiceProxy(speed_service, SetSpeed)
self.speed_services.append(srv)
srv(self.max_speed)
self.last_speed.append(self.max_speed)
rospy.loginfo("Real speed " + c)
else:
# add dummy service so positions align
srv = self.speed_services[-1]
self.speed_services.append(srv)
self.last_speed.append(self.max_speed)
rospy.loginfo("Dummy speed " + c)
else:
# add dummy services so positions align
c_srv = self.position_pub[-1]
self.position_pub.append(c_srv)
srv = self.speed_services[-1]
self.speed_services.append(srv)
self.last_speed.append(self.max_speed)
rospy.loginfo("Dummy speed " + c)
rospy.loginfo("Dummy pos pub " + c)
rospy.loginfo("Starting " + c + "/command")
self.server.start()
def __init__(self):
myname = rospy.get_name()
#name = rospy.resolve_name()
rospy.loginfo("get_node name " + myname )
#name = myname[1:myname.find("follow_joint_trajectory")]
name = myname
self.name = name
rospy.loginfo("init")
# parameters: rates and joints
self.rate = rospy.get_param('~controllers/'+ name +'/rate',50.0)
self.joints = list()
self.fudge_factor = list()
# Get all parameter names
parameters = rospy.get_param_names()
#for parameter in parameters:
# Look for the parameters that name the joints.
#if parameter.find("joint_name") != -1:
#self.joints.append(rospy.get_param(parameter))
self.joints = rospy.get_param('~controllers/'+name+'/joints')
self.controllers = list()
self.fudge_factor = list()
self.current_pos = list()
self.current_pos_cnt = list()
self.last_speed = list()
# traj = goal.trajectory
self.trajectory_goal = deque()
self.cur_point = 0
self.jnt_fudge = 0
self.last_elbow_flex_pos = -1000
self.right_elbow_flex_fudge = 0
self.execute_joints = list()
for joint in self.joints:
#self.device.servos[joint].controller = self
# Remove "_joint" from the end of the joint name to get the controller names.
servo_nm = joint.split("_joint")[0]
self.controllers.append(servo_nm + "_controller")
self.current_pos.append(-10)
self.current_pos_cnt.append(0)
# action server for FollowController
name = rospy.get_param('~controllers/'+name +'/action_name','follow_joint_trajectory')
self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
rospy.loginfo("Started FollowController ("+name+"). Joints: " + str (self.joints) + " Controllers: " + str(self.controllers))
self.fudge_value = [rospy.get_param('~fudge_factor/' + joint + '/value', 0.0) for joint in self.joints]
# /joint_states
rospy.Subscriber('joint_states', JointState, self.getJointState)
# Possible enhancement: subscribe to /joint_states
# Start controller state subscribers
self.position_pub = list()
self.torque_services = list()
self.speed_services = list()
self.max_speed = 0.4
self.max_speed_shoulder_pan = .15
for c in self.controllers:
if c == 'torso_lift_controller':
# c_srv = rospy.Publisher('torso_lift_controller/position_joint_action', Float64)
c_srv = rospy.Publisher('torso_lift_controller/command', Float64)
self.position_pub.append(c_srv)
rospy.loginfo("Real pos pub:" + c)
# add dummy service so positions align
srv = self.speed_services[-1]
self.speed_services.append(srv)
self.last_speed.append(self.max_speed)
rospy.loginfo("Dummy speed:" + c)
elif c != 'left_upper_arm_hinge_controller' and c != 'right_upper_arm_hinge_controller':
c_srv = rospy.Publisher(c + '/command', Float64)
self.position_pub.append(c_srv)
rospy.loginfo("Real pos pub " + c)
if c != 'left_shoulder_tilt_controller' and c != 'right_shoulder_tilt_controller' and c != 'torso_lift_controller':
speed_service = c + '/set_speed'
rospy.wait_for_service(speed_service)
srv = rospy.ServiceProxy(speed_service, SetSpeed)
self.speed_services.append(srv)
if c == 'left_shoulder_pan_controller' or c == 'right_shoulder_pan_controller':
velocity = self.max_speed_shoulder_pan
else:
velocity = self.max_speed
srv(velocity)
self.last_speed.append(velocity)
rospy.loginfo("Real speed " + c + " " + str(velocity))
else:
# add dummy service so positions align
srv = self.speed_services[-1]
self.speed_services.append(srv)
self.last_speed.append(self.max_speed)
rospy.loginfo("Dummy speed " + c)
else:
# add dummy services so positions align
c_srv = self.position_pub[-1]
self.position_pub.append(c_srv)
srv = self.speed_services[-1]
self.speed_services.append(srv)
self.last_speed.append(self.max_speed)
rospy.loginfo("Dummy speed " + c)
rospy.loginfo("Dummy pos pub " + c)
rospy.loginfo("Starting " + c + "/command")
self.server.start()
