def check_users_exist(self):
gone = []
for uid, user in self.users_.items():
if user.exists_ == False:
gone.append(uid)
for g in gone:
if rospy.has_param("wallframe/user_data/" + str(g)):
rospy.delete_param("wallframe/user_data/" + str(g))
rospy.logwarn("User [wallframe UID: " + str(g) +
"] lost, removing from list")
self.toast_pub_.publish(String("User " + str(g) + " Lost"))
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'user_left'
self.user_event_pub_.publish(msg)
del self.users_[g]
if len(self.users_) == 0:
if self.no_users_ == False:
# Send all users left message
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'all_users_left'
self.user_event_pub_.publish(msg)
self.no_users_ = True
def __init__(self):
try:
self._map = rospy.get_param("~map")
except KeyError as e:
rospy.logfatal("map param not set!: %s " % e)
raise MapSetupException("map param not set")
# Clean/delete params:
try:
rospy.delete_param("mmap")
except:
pass
# Publish map_in_use (latched):
self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True)
self._map_in_use_pub.publish("submap_0")
# Set params:
rospy.set_param("map_package_in_use" , "deprecated")
rospy.set_param("nice_map_in_use" , os.path.join(self._map, "map.bmp"))
rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml"))
# Load
paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap")
for param, ns in paramlist:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
def __init__(self):
rospy.logwarn('clearing parameters')
rospy.delete_param('~rplidar_scan_topic')
rospy.delete_param('~rplidar_port_name')
rospy.delete_param('~rplidar_frame')
rospy.delete_param('~rplidar_range_min')
rospy.delete_param('~rplidar_range_max')
def cb_rem_master_model(f):
print "##### start cb_remove_master_model " +datetime.datetime.today().strftime("%Y_%m_%d_%H%M%S")
# error_code,messageの初期化
publish_retCode_and_message("","")
ret = remove_master_model()
if(ret == 501):
publish_retCode_and_message(str(ret),str("REMOVE MASTER:recipe_id not registerd"))
elif(ret == 502):
publish_retCode_and_message(str(ret),str("REMOVE MASTER:remove master failed"))
# 削除したマスターが現在選択中の場合には、/mt/masterパラメータを削除する
# パラメータ取得
# レシピID
remove_recipe_id = rospy.get_param('/mt/master/recipe_id',None)
if(remove_recipe_id == None): # recipe_idが無い場合はそのまま終了
publish_retCode_and_message(str(0),str("REMOVE MASTER:Success"))
return
else:
if(len(recipe_id) == 0): # recipe_idが無い場合はそのまま終了
publish_retCode_and_message(str(0),str("REMOVE MASTER:Success"))
return
# /mt/masterパラメータを削除する
if rospy.has_param('/mt/master'):
rospy.delete_param('/mt/master')
publish_retCode_and_message(str(0),str("REMOVE MASTER:Success"))
print "##### end cb_remove_master_model " +datetime.datetime.today().strftime("%Y_%m_%d_%H%M%S")
return
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 main():
try:
# For testing purposes
try:
rospy.delete_param("is_initialized")
except:
pass
# Unpause the physics
rospy.loginfo("Unpausing Gazebo...")
rospy.wait_for_service('/gazebo/unpause_physics')
unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
resp = unpause_gazebo()
rospy.loginfo("Unpaused Gazebo.")
example = ExampleInitializeGazeboRobot()
success = example.is_init_success
if success:
success &= example.home_the_robot()
except Exception as e:
print (e)
success = False
# For testing purposes
rospy.set_param("is_initialized", success)
if not success:
rospy.logerr("The Gazebo initialization encountered an error.")
else:
rospy.loginfo("The Gazebo initialization executed without fail.")
def __init__(self):
try:
self._map = rospy.get_param("~map")
except KeyError as e:
rospy.logfatal("map param not set!: %s " % e)
raise MapSetupException("map param not set")
# Clean/delete params:
try:
rospy.delete_param("mmap")
except:
pass
# Publish map_in_use (latched):
self._map_in_use_pub = rospy.Publisher("map_in_use",
String,
queue_size=1,
latch=True)
self._map_in_use_pub.publish("submap_0")
# Set params:
rospy.set_param("map_package_in_use", "deprecated")
rospy.set_param("nice_map_in_use", os.path.join(self._map, "map.bmp"))
rospy.set_param("map_transformation_in_use",
os.path.join(self._map, "transformation.xml"))
# Load
paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"),
"mmap")
for param, ns in paramlist:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
def clear_params(node_name, params):
for k in params:
try:
rospy.delete_param("{}/{}".format(node_name, k))
except KeyError:
print("value {} not set".format(k))
return True
def main():
rospy.init_node('encoders_acq_node', anonymous=True)
"""Module initialization"""
for joint_name in joints:
if rospy.has_param("traction_hw/joints/{}".format(joint_name)):
joint_hw = rospy.get_param("traction_hw/joints/{}".format(joint_name))
exec("{} = EncoderAcq('{}')".format(joint_name, joint_name))
rate_param = rospy.get_param("traction_hw/sampling_frequency", 100)
rate = rospy.Rate(rate_param)
rospy.loginfo("[Encoders] Reading sensor data...")
while not rospy.is_shutdown():
if 'right_wheel' in locals(): right_wheel.get_sensor_data()
if 'left_wheel' in locals(): left_wheel.get_sensor_data()
rate.sleep()
for joint_name in joints:
if 'right_wheel' in locals(): right_wheel.spi_bus.close()
if 'left_wheel' in locals(): left_wheel.spi_bus.close()
"""Clean up ROS parameter server"""
try:
rospy.delete_param("exo_traction")
rospy.delete_param("spi_comm")
except KeyError:
pass
def shutdown(self):
""" Delete any remaining parameters so they don't affect next launch """
for param in [self._device_name_param, self._lm_param, self._dic_param]:
if rospy.has_param(param):
rospy.delete_param(param)
""" Shutdown the GTK thread. """
def remote_destroy(self):
""" Method should be called to delete the Parameter from the parameter
server.
"""
if self._registered:
try:
rospy.delete_param(self._name)
except rospy.ROSException:
pass
self._registered = False
if self._owner:
self._owner.unregisterParameter(self)
self._owner = None
if self._status:
def eb(failure):
if not failure.check(PBConnectionLost):
log.err(failure)
try:
self._status.callRemote('died').addErrback(eb)
except (DeadReferenceError, PBConnectionLost):
pass
self._status = None
def check_users_exist(self):
gone = []
for uid, user in self.users_.items():
if user.exists_ == False:
gone.append(uid)
for g in gone:
if rospy.has_param("wallframe/user_data/"+str(g)):
rospy.delete_param("wallframe/user_data/"+str(g))
rospy.logwarn("User [wallframe UID: " + str(g) + "] lost, removing from list")
self.toast_pub_.publish(String("User "+str(g)+" Lost"))
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'user_left'
self.user_event_pub_.publish(msg)
del self.users_[g]
if len(self.users_) == 0:
if self.no_users_ == False:
# Send all users left message
msg = user_event_msg()
msg.user_id = g
msg.event_id = 'workspace_event'
msg.message = 'all_users_left'
self.user_event_pub_.publish(msg)
self.no_users_ = True
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 main():
rospy.init_node('elevation_acq_node', anonymous=True)
"""Module initialization"""
if rospy.has_param("elevation_hw/joints/elevation"):
joint_hw = rospy.get_param("elevation_hw/joints/elevation")
can_id = rospy.get_param("elevation_hw/joints/elevation/can_id")
has_hw = []
for hw_component in hw_names:
has_hw.append(joint_hw[hw_component])
if True in has_hw:
elevation = SensorAcq('elevation', has_hw)
# CAN
can_channel = rospy.get_param("can_comm/elevation_port", "can1")
can_bus = CANbus(channel=can_channel)
rate_param = rospy.get_param("elevation_hw/sampling_frequency", 100)
rate = rospy.Rate(rate_param)
rospy.loginfo("[Elevation Acquisition] Reading sensor data...")
while not rospy.is_shutdown():
can_bus.send_command()
msg = can_bus.receive_data()
if msg is not None:
elevation.get_sensor_data(msg)
rate.sleep()
"""Clean up ROS parameter server"""
try:
rospy.delete_param("exo_hw")
rospy.delete_param("can_comm")
except KeyError:
pass
def reset_demo(self):
"""Reset the environment for demo collection episodes.
same functionality as "reset" method.
"""
# Desired goal is given here.
# Should wait until reset finishes.
if rospy.has_param('vel_calc'):
rospy.delete_param('vel_calc')
self.control_command[-1] = float(0)
if not self.isReal: # for simulated env.
des_goal = self._reset_gazebo()
else:
des_goal = self._reset_real()
_joint_pos, _joint_vels, _joint_effos = self.get_joints_states()
_color_obs = self.get_color_observation()
_targ_obj_obs = self.get_target_obj_obs()
_gripper_pos = self.get_gripper_position()
_ee_pose = self.get_end_effector_pose()
obs = dict()
obs['full_state'] = [_joint_pos, _joint_vels, _joint_effos] # default observations
if self.isGripper:
obs['full_state'].append([_gripper_pos])
if self.isCartesian:
obs['full_state'].append(_ee_pose)
if self.isPOMDP:
obs['color_obs'] = _color_obs
rospy.set_param('vel_calc','true')
return {'observation': obs,'desired_goal':des_goal, 'auxiliary':_targ_obj_obs}
def test_one_hand_no_mapping_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "")
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1,
"It should be only one mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"rh_", "It should be only rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1,
"It should be only one mapping")
print(hand_finder.get_hand_joints())
self.assertIn("1", hand_finder.get_hand_joints(),
"Serial should be in the joints result")
joints = hand_finder.get_hand_joints()['1'] # use serial
self.assertEqual(len(joints), 24, "Joint number should be 24")
self.assertIn("rh_FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should be in the joints list")
def main():
try:
# For testing purposes
try:
rospy.delete_param("is_initialized")
except:
pass
# Unpause the physics
# This will let MoveIt finish its initialization
rospy.loginfo("Unpausing")
rospy.wait_for_service('/gazebo/unpause_physics')
unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
resp = unpause_gazebo()
rospy.loginfo("Unpaused")
example = ExampleInitializeGazeboRobot()
rospy.loginfo("Created example")
success = example.is_init_success
rospy.loginfo("success = {}".format(success))
if success:
success &= example.home_the_robot()
except:
success = False
# For testing purposes
rospy.set_param("is_initialized", success)
if not success:
rospy.logerr("The Gazebo initialization encountered an error.")
else:
rospy.loginfo("The Gazebo initialization executed without fail.")
def __init__(self):
rospy.delete_param('~fussion_use_rplidar_topic')
rospy.delete_param('~fussion_use_asus_topic')
rospy.delete_param('~fussion_scan_topic')
rospy.delete_param('~fussion_PublishFrequency')
rospy.delete_param('~fussion_asus_max_range')
def spin(self):
#-----sync-----
now = rospy.get_rostime()
s, ns = now.secs, now.nsecs
target = self.sync_time
ns = ns * 10**(-9)
diff = target - (s + ns)
print "diff", diff
#sleep until sync time
rospy.sleep(diff)
self.pid.clear()
self.last_time = rospy.get_time()
self.latest_msg = [rospy.get_time()]*self.nr_cars
rate = rospy.Rate(config.rate)
first = True
while not rospy.is_shutdown():
rate.sleep()
self.update()
if self.id == 0 and first:
rospy.delete_param("sync_time")
rospy.delete_param("random")
first = False
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 main(self):
# For testing purposes
success = self.is_init_success
try:
rospy.delete_param(
"/kortex_examples_test_results/actuator_configuration_python")
except:
pass
if success:
#-------------------------------------------------------------
# Find actuators and set which one we want to configure
success &= self.example_find_actuators_and_set_device_id()
#-------------------------------------------------------------
# Clear the faults on a specific actuator
success &= self.example_clear_actuator_faults()
#-------------------------------------------------------------
# Get the control loop parameters on a specific actuator
control_loop_parameters = None
success &= self.example_get_control_loop_parameters(
control_loop_parameters)
# For testing purposes
rospy.set_param(
"/kortex_examples_test_results/actuator_configuration_python",
success)
if not success:
rospy.logerr("The example encountered an error.")
def set_lane(self, next):
# to next node
if next:
self.last_node += 1
else:
self.last_node -= 1
if self.last_node >= len(self.guid_path) - 1:
rospy.loginfo("It's the last lane.")
self.last_node -= 1
return
elif self.last_node < 0:
rospy.loginfo("Back to first lane.")
self.last_node = 0
# set last, next node (anchor)
rospy.set_param("/guid_lane_last",
self.all_anchor_ids[self.guid_path[self.last_node]])
rospy.set_param(
"/guid_lane_next",
self.all_anchor_ids[self.guid_path[self.last_node + 1]])
# set pozyx ranging tag id
try:
rospy.delete_param("/anchorballs")
except KeyError:
rospy.logerr("No such param /anchorballs")
pozyx_id = {}
pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[
self.guid_path[self.last_node]]
pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[
self.guid_path[self.last_node + 1]]
rospy.set_param("/anchorballs", pozyx_id)
def _set_param_and(self):
params = {
'countermeasure': {
'config': {
'storage_timeout': 10,
'reaction_autonomy_level': 50},
'constraints': {
'john': {
'reactions': {
'two': {
'action': 'publish',
'node': 'node1',
'message': 'node1 has a problem',
'loglevel': 'info',
'autonomy_level': 13},
'one': {
'action': 'stop',
'node': 'node1',
'autonomy_level': 100}},
'min_reaction_interval': 5,
'constraint': {
'or': {
'n!node2': {'ram_usage_mean': 'high'},
'n!node1': {'cpu_usage_mean': 'high'}}}}}}}
if rospy.has_param("/arni"):
rospy.delete_param("/arni")
rospy.set_param("/arni", params)
def ros():
try:
logging.loginfo(u'deleting tmp test folder')
except Exception:
pass
logging.loginfo(u'init ros')
rospy.init_node(u'tests')
tf_init(60)
launch = roslaunch.scriptapi.ROSLaunch()
launch.start()
rospy.set_param('/joint_trajectory_splitter/state_topics', [
'/whole_body_controller/base/state',
'/whole_body_controller/body/state'
])
rospy.set_param('/joint_trajectory_splitter/client_topics', [
'/whole_body_controller/base/follow_joint_trajectory',
'/whole_body_controller/body/follow_joint_trajectory'
])
node = roslaunch.core.Node('giskardpy',
'joint_trajectory_splitter.py',
name='joint_trajectory_splitter')
joint_trajectory_splitter = launch.launch(node)
yield
joint_trajectory_splitter.stop()
rospy.delete_param('/joint_trajectory_splitter/state_topics')
rospy.delete_param('/joint_trajectory_splitter/client_topics')
logging.loginfo(u'shutdown ros')
rospy.signal_shutdown(u'die')
try:
logging.loginfo(u'deleting tmp test folder')
except Exception:
pass
def test_no_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
0, "correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0,
"correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_joints()), 0,
"correct joints without a hand")
self.assertEqual(len(hand_finder.get_calibration_path()), 0,
"correct calibration path without a hand")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().friction_compensation),
0, "correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().host_control), 0,
"correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().motor_control), 0,
"correct tuning without a hands")
def datastore_callback(self, name, value):
if value is not None:
rospy.loginfo('got new value of %r from Datastore', name)
rospy.set_param(name, value)
else:
rospy.loginfo('parameter %r deleted from Datastore', name)
rospy.delete_param(name, value)
def shutdown(self):
""" Delete any remaining parameters so they don't affect next launch """
for param in [self._fsg_param]:
if rospy.has_param(param):
rospy.delete_param(param)
""" Shutdown the GTK thread. """
gtk.main_quit()
def __init__(self):
rospy.loginfo('Cleaning Parameters...')
rospy.delete_param('~PlanTopic')
rospy.delete_param('~OdomTopic')
rospy.delete_param('~PositionFree')
rospy.delete_param('~ActionPlanTopic')
rospy.delete_param('~GoalTopic')
def test_param_withhold_update_disappear(self):
"""
Test param witholding functionality for a param which doesnt exists anymore in
the ros environment. Normal usecase.
Sequence : (-> UPDATE ?) -> WITHHOLD -> UPDATE -> DISAPPEAR (-> UPDATE ?)
:return:
"""
paramname = '/test/absentparam1'
# every added param should be in the list of args
self.assertTrue(paramname not in self.param_if_pool.params_args)
# the backend should not have been created
self.assertTrue(paramname not in self.param_if_pool.params.keys())
self.param_if_pool.expose_params([paramname])
# every added param should be in the list of args
self.assertTrue(paramname in self.param_if_pool.params_args)
# service backend has NOT been created yet
self.assertTrue(paramname not in self.param_if_pool.params.keys())
# create the param and then try updating again, simulating
# it coming online after expose call.
rospy.set_param(paramname, 'param_value')
try:
params = self.get_system_state()
dt = self.param_if_pool.update(params)
self.assertTrue(paramname in dt.added) # paramname added
self.assertEqual(dt.removed, []) # nothing removed
# every withhold param should STILL be in the list of args
self.assertTrue(paramname in self.param_if_pool.params_args)
# param backend has been created
self.assertTrue(paramname in self.param_if_pool.params.keys())
dt = self.param_if_pool.expose_params([])
self.assertEqual(dt.added, []) # nothing added
self.assertTrue(paramname in dt.removed) # paramname removed
# every withhold param should NOT be in the list of args
self.assertTrue(paramname not in self.param_if_pool.params_args)
# param backend should be GONE
self.assertTrue(paramname not in self.param_if_pool.params.keys())
params = self.get_system_state()
dt = self.param_if_pool.update(params)
self.assertEqual(dt.added, []) # nothing added
self.assertEqual(dt.removed, []) # nothing removed
# every withhold param should STILL NOT be in the list of args
self.assertTrue(paramname not in self.param_if_pool.params)
# param backend should be GONE
self.assertTrue(paramname not in self.param_if_pool.params.keys())
finally:
rospy.delete_param(paramname)
params = self.get_system_state()
dt = self.param_if_pool.update(params)
self.assertEqual(dt.added, []) # nothing added
self.assertEqual(dt.removed, []) # nonexistent_srv already removed
# every withhold service should STILL NOT be in the list of args
self.assertTrue(paramname not in self.param_if_pool.params_args)
# service backend should be GONE
self.assertTrue(paramname not in self.param_if_pool.params.keys())
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 run():
global goal_pub
global frontier_pub
global getGlobalPath
rospy.init_node('exploration_node')
navType = rospy.get_param('~nav', default='rbe')
if navType != 'rbe': rospy.delete_param('~nav')
if navType == 'rbe':
print("Using rbe nav")
goal_pub = rospy.Publisher('/rbe_goal', PoseStamped, queue_size=1)
else:
print
"using gMapping nav"
goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1)
planner.init()
nav.init()
frontier_pub = rospy.Publisher('/frontier/frontier', GridCells, queue_size=1)
getGlobalPath = rospy.ServiceProxy('global_path', CalcPath)
# status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, statusCallback, queue_size=1)
map_sub = rospy.Subscriber('/map', OccupancyGrid, mapCallback, queue_size=1)
exploreMap()
def main():
example = ExampleMoveItTrajectories()
# For testing purposes
success = example.is_init_success
try:
rospy.delete_param("/kortex_examples_test_results/moveit_general_python")
except:
pass
if success:
actual_pose = example.get_cartesian_pose()
#Try to use the MoveIt function to reach a cartesian pose
rospy.loginfo("Reaching cartesian pose...")
success &= example.go_to_pose_goal()
# rospy.loginfo("Reaching Named Target Vertical...")
# success &= example.reach_named_position("vertical")
# rospy.loginfo("Reaching Joint Angles...")
# success &= example.reach_joint_angles(tolerance=0.01) #rad
# rospy.loginfo("Reaching Named Target Home...")
# success &= example.reach_named_position("home")
# rospy.loginfo("Reaching Cartesian Pose...")
actual_pose = example.get_cartesian_pose()
actual_pose.position.z += 0.2
def param_talker():
rospy.init_node('param_talker')
# Fetch values from the Parameter Server. In this example, we fetch
# parameters from three different namespaces:
#
# 1) global (/global_example)
# 2) parent (/foo/utterance)
# 3) private (/foo/param_talker/topic_name)
# fetch a /global parameter
global_example = rospy.get_param("/global_example")
rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'),
global_example)
# fetch the utterance parameter from our parent namespace
utterance = rospy.get_param('utterance')
rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance)
# fetch topic_name from the ~private namespace
topic_name = rospy.get_param('~topic_name')
rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name)
# fetch a parameter, using 'default_value' if it doesn't exist
default_param = rospy.get_param('default_param', 'default_value')
rospy.loginfo('%s is %s', rospy.resolve_name('default_param'),
default_param)
# fetch a group (dictionary) of parameters
gains = rospy.get_param('gains')
p, i, d = gains['P'], gains['I'], gains['D']
rospy.loginfo("gains are %s, %s, %s", p, i, d)
# set some parameters
rospy.loginfo('setting parameters...')
rospy.set_param('list_of_floats', [1., 2., 3., 4.])
rospy.set_param('bool_True', True)
rospy.set_param('~private_bar', 1 + 2)
rospy.set_param('to_delete', 'baz')
rospy.loginfo('...parameters have been set')
# delete a parameter
if rospy.has_param('to_delete'):
rospy.delete_param('to_delete')
rospy.loginfo("deleted %s parameter" % rospy.resolve_name('to_delete'))
else:
rospy.loginfo('parameter %s was already deleted' %
rospy.resolve_name('to_delete'))
# search for a parameter
param_name = rospy.search_param('global_example')
rospy.loginfo('found global_example parameter under key: %s' % param_name)
# publish the value of utterance repeatedly
pub = rospy.Publisher(topic_name, String, queue_size=10)
while not rospy.is_shutdown():
pub.publish(utterance)
rospy.loginfo(utterance)
rospy.sleep(1)
def check_delete_params(self):
if self.param_delete:
for param in self.param_delete:
if (rospy.get_param(param, False)):
rospy.delete_param(param)
else:
self._print_warning("Not deleting any param")
def check_delete_params(self):
if self.param_delete:
for param in self.param_delete:
if (rospy.get_param(param,False)):
rospy.delete_param(param)
else:
self._print_warning("Not deleting any param")
def clean(self):
# on shutdown
velocity_param_name = rospy.search_param('velocity')
if velocity_param_name != None:
rospy.delete_param(velocity_param_name)
def set_params(self, m):
for ns in self.namespaces:
if rospy.has_param(ns):
rospy.delete_param(ns)
self.set_params_(self.fixed_params)
self.set_params_(m)
self.scenario.parameterize(m)
rospy.set_param('/nav_experiments/scenario', self.scenario.get_scenario())
def reset(self):
self.agentMap = {}
self.obsMap = {}
self.nextObsNum = 60
self.nextAgentNum = 1
self.agentVel = {}
rospy.delete_param('/spher_swarm/connected')
self.update()
def test_servo_vel(self, time_step):
dynamic_pose = self.dynamic_object(step=time_step)
dynamic_pose.position.z += 0.1
rospy.set_param('vel_calc','true')
self.servo_vel(dynamic_pose, num_wp=2)
if rospy.has_param('vel_calc'):
rospy.logwarn('Initialized pose.')
rospy.delete_param('vel_calc')
def shutdown_all_apps(self):
for full_app_name,app_process in self.active_app_launchers_.items():
app_process.terminate()
while app_process.poll() == None:
pass
if rospy.has_param("/wallframe/core/apps/running/" + full_app_name):
rospy.delete_param("/wallframe/core/apps/running/" + full_app_name)
rospy.logwarn("WallframeAppManager: App [" + full_app_name + "] shutdown successfully")
self.active_app_launchers_.clear()
def set_params(self, m):
for ns in self.namespaces:
if rospy.has_param(ns):
rospy.delete_param(ns)
self.set_params_(self.fixed_params)
self.set_params_(m)
self.scenario.parameterize(m)
rospy.set_param('/nav_experiments/scenario',
self.scenario.get_scenario())
def shutdown(retval):
"""
We need to delete the parameter that the simulator sets, or else other
nodes might still think this node is running. (Maybe there's a better
way to do this...)
"""
rospy.loginfo("Shutting down...")
rospy.delete_param(PARAM_NAME)
exit(retval)
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 update(self):
all_frames = self.listener_.getFrameStrings()
ar_frames = [f for f in all_frames if f.find('ar_marker')>=0]
un_filtered = [f for f in ar_frames if f.find('filtered')<0]
# Decay each frame count at every timestep
for f in self.frames.keys():
self.frames[f]['no_frames'] -= 1
if self.frames[f]['no_frames'] <= 0:
short_name = 'landmark_' + f.split('_')[len(f.split('_'))-1:][0]
rospy.delete_param('instructor_landmark/'+short_name)
rospy.logwarn('Deleted:' +short_name)
self.frames.pop(f)
for frame in un_filtered:
try:
F = tf_c.fromTf(self.listener_.lookupTransform('/world',frame,rospy.Time(0)))
except (tf.LookupException, tf.ConnectivityException) as e:
rospy.logerr('Frame ['+frame+'] not found')
return
except tf.ExtrapolationException as e:
# The frame we're seeing is old
return
if frame not in self.frames.keys():
self.frames[frame] = {'no_frames':0, 'poses':[], 'average_pose':None}
rospy.logwarn('New Frame:' + frame)
# rospy.logwarn(self.frames[frame]['no_frames'])
if self.frames[frame]['no_frames'] < self.buffer:
self.frames[frame]['no_frames'] += 2
self.frames[frame]['poses'].append(F)
else:
self.frames[frame]['poses'].pop(0)
self.frames[frame]['poses'].pop(0)
self.frames[frame]['poses'].append(F)
for frame in self.frames.keys():
# Get all stored frame positions/rotations
sum_xyz = [tf_c.toTf(f)[0] for f in self.frames[frame]['poses']]
sum_rpy = [f.M.GetRPY() for f in self.frames[frame]['poses']]
if len(sum_xyz) > 2:
xyz = np.mean(sum_xyz, 0)
rpy = np.mean(sum_rpy, 0)
F_avg = PyKDL.Frame()
F_avg.p = PyKDL.Vector(*xyz)
F_avg.M = PyKDL.Rotation.RPY(*rpy)
self.broadcaster_.sendTransform(tuple(F_avg.p),tuple(F_avg.M.GetQuaternion()),rospy.Time.now(), '/filtered/'+frame, '/world')
# rosparam_marker_name = "instructor_landmark/{}".format(str('filtered/'+frame))
# rospy.set_param(rosparam_marker_name, str('filtered/'+frame))
short_name = 'landmark_' + frame.split('_')[len(frame.split('_'))-1:][0]
rospy.set_param('instructor_landmark/'+short_name, str('filtered/'+frame))
def clean_up(self):
for app_id,app in self.apps_.items():
if rospy.has_param("/wallframe/core/available_app/" + app_id):
rospy.delete_param("/wallframe/core/available_apps/" + app_id)
print("App parameters for [" + app_id + "] cleaned up")
if rospy.has_param("/wallframe/core/available_apps"):
rospy.delete_param("/wallframe/core/available_apps")
print("Remaining parameters cleaned up")
pass
def shutdown_app(self,app_name):
full_app_name = "wallframe_app_" + app_name
app_process = self.active_app_launchers_[full_app_name]
app_process.terminate()
while app_process.poll() == None:
pass
if rospy.has_param("/wallframe/core/apps/running/" + full_app_name):
rospy.delete_param("/wallframe/core/apps/running/" + full_app_name)
del self.active_app_launchers_[full_app_name]
rospy.logwarn("WallframeAppManager: App [" + full_app_name + "] shutdown successfully")
def delete_param(name, params_glob):
if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob):
# If the glob list is not empty and there are no glob matches,
# stop the attempt to delete the parameter.
return
# If the glob list is empty (i.e. false) or the parameter matches
# one of the glob strings, continue to delete the parameter.
if has_param(name, params_glob):
with param_server_lock:
rospy.delete_param(name)
def set_param(self, param_name, val):
# Set private var value
setattr(self, '_{}'.format(param_name), val)
if val == None:
full_param_name = '/needybot/blackboard/{}'.format(param_name)
if rospy.has_param(full_param_name):
rospy.delete_param(full_param_name)
self.publish("delete_{}".format(param_name))
else:
rospy.set_param('/needybot/blackboard/{}'.format(param_name), val)
self.publish("set_{}".format(param_name))
def remove(self):
""" Method which is used to remove the parameter from the parameter
server.
"""
if self._registered:
try:
rospy.delete_param(self._name)
except rospy.ROSException:
pass
self._registered = False
def on_stopButton_clicked(self):
try:
rospy.delete_param('/tracking/scenario')
except KeyError:
pass
msg1 = SimControl()
msg1.mode = "stop"
self.pub_control.publish(msg1)
msg2 = Bool()
msg2.data = True
self.pub_reset.publish(msg2)
def __init__(self, default_update_rate=50.0):
rospy.init_node('sphero_swarm')
self.update_rate = default_update_rate
self.sampling_divisor = int(400/self.update_rate)
self.sphero_dict = {}
self.sphero_connected = {}
self._init_pubsub()
self._init_params()
self.last_diagnostics_time = rospy.Time.now()
self.mutual_lock = threading.Lock()
rospy.delete_param('/sphero_swarm/team')
def test_spec3(self):
'''
Values within their limits will be rated with 3 (OK).
'''
rospy.set_param(spec_namespace, mockSpecs)
sh = SpecificationHandler()
res = sh.compare(exampleMessages["h!127.0.0.1"], "h!127.0.0.1")
self.assertEqual(res.get_value('cpu_temp_stddev')['state'], 3)
self.assertEqual(res.get_value('cpu_temp_core')['state'], [3, 3])
try:
rospy.delete_param(spec_namespace)
except KeyError:
pass
def delete_map(self, request):
rospy.logdebug("Service call : delete map %s" % request.map_id)
response = DeleteMapResponse()
if rospy.has_param('~last_map_id') and rospy.get_param('~last_map_id') == request.map_id:
rospy.delete_param('~last_map_id')
if self.last_map == request.map_id:
self.last_map = None
if self.map_collection.remove({'uuid': {'$in': [request.map_id]}}) == 0:
return None
return response
def test_spec2(self):
'''
Values without specifications will be rated with 2 (OK).
'''
rospy.set_param(spec_namespace, mockSpecs)
sh = SpecificationHandler()
res = sh.compare(exampleMessages["h!127.0.0.1"], "h!127.0.0.1")
self.assertEqual(res.get_value('ram_usage_max')['state'], 2)
self.assertEqual(res.get_value('cpu_usage_core_max')['state'], [2, 2])
try:
rospy.delete_param(spec_namespace)
except KeyError:
pass
def remote_destroy(self):
""" Method should be called to delete the Parameter from the parameter
server.
"""
if self._registered:
try:
rospy.delete_param(self._name)
except rospy.ROSException:
pass
self._registered = False
if self._owner:
self._owner.unregisterParameter(self)
self._owner = None
def test_spec10(self):
'''
Values below their limits will be rated with 1 (Low).
Values above their limits will be rated with 0 (High).
'''
rospy.set_param(spec_namespace, mockSpecs)
msg = exampleMessages["h!127.0.0.1"]
msg.cpu_temp_core_mean[0] -= 30
msg.cpu_temp_core_mean[1] += 10
sh = SpecificationHandler()
res = sh.compare(msg, "h!127.0.0.1")
self.assertEqual(res.get_value('cpu_temp_core_mean')['state'], [1, 0])
try:
rospy.delete_param(spec_namespace)
except KeyError:
pass
def handle_msg(msg, who):
assert isinstance(msg, Odometry)
global last_cap_r, last_cap_f, last_cap_yaw, current_time
now = rospy.get_rostime()
if now<current_time :
reset()
try:
rospy.delete_param('obs_centroid')
except KeyError:
pass
current_time = now
if who == 'cap_r':
last_cap_r = rtk_position_to_numpy(msg)
elif who == 'cap_f' and last_cap_r is not None:
cap_f = rtk_position_to_numpy(msg)
cap_r = last_cap_r
last_cap_f = cap_f
last_cap_yaw = get_yaw(cap_f, cap_r)
elif who == 'obs_r' and last_cap_f is not None and last_cap_yaw is not None:
md = None
for obs in metadata:
if obs['obstacle_name'] == 'obs1':
md = obs
assert md, 'obs1 metadata not found'
# find obstacle rear RTK to centroid vector
lrg_to_gps = [md['gps_l'], -md['gps_w'], md['gps_h']]
lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.]
obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps)
# in the fixed GPS frame
cap_f = last_cap_f
obs_r = rtk_position_to_numpy(msg)
# in the capture vehicle front RTK frame
cap_to_obs = np.dot(rotMatZ(-last_cap_yaw), obs_r - cap_f)
cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid
velo_to_front = np.array([-1.0922, 0, -0.0508])
cap_to_obs_centroid += velo_to_front
rospy.set_param('obs_centroid', cap_to_obs_centroid.tolist())
def main(args):
parser = argparse.ArgumentParser()
parser.add_argument('params', metavar='param', nargs='+', help='Parameters to delete on shutdown')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('on_shutdown_param_cleaner', anonymous=True)
print('Will delete params %s on shutdown' % args.params)
try:
while not rospy.is_shutdown():
rospy.sleep(1)
except rospy.exceptions.ROSInterruptException:
pass
for param in args.params:
if rospy.has_param(param):
rospy.delete_param(param)
def stop(self):
print "stop sphero swarm"
#tell the ball to stop moving before quiting
for name in self.sphero_dict:
sphero = self.sphero_dict[name]
sphero.robot.roll(int(0), int(0), 1, False)
sphero.robot.shutdown = True
rospy.sleep(1.0)
for name in self.sphero_dict:
sphero = self.sphero_dict[name]
sphero.is_connected = sphero.robot.disconnect()
sphero.robot.join()
del self.sphero_dict[name]
del self.team_info[name]
del self.sphero_connected[name]
#rospy.set_param('/sphero_swarm/team', self.team_info)
rospy.delete_param('/sphero_swarm/team')
rospy.set_param('/sphero_swarm/connected', self.sphero_connected)
