def histogram_of_stop_point_elbow(self, subjects, labels):
fig1 = plt.figure(4)
fig2 = plt.figure(5)
labels = ['good']
stop_locations = []
arm_lengths = []
for num, label in enumerate(labels):
for subj_num, subject in enumerate(subjects):
subject_stop_locations = []
paramlist = rosparam.load_file(''.join([data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')/100.
# fig1 = plt.figure(2*num+1)
ax1 = fig1.add_subplot(111)
ax1.set_xlim(0.2, .4)
# ax1.set_ylim(-10.0, 1.0)
ax1.set_xlabel('Stop Position (m)', fontsize=20)
ax1.set_ylabel('Number of trials', fontsize=20)
ax1.set_title(''.join(['Difference between arm length and stop position at the elbow']), fontsize=20)
ax1.tick_params(axis='x', labelsize=20)
ax1.tick_params(axis='y', labelsize=20)
ax2 = fig2.add_subplot(431+subj_num)
ax2.set_xlim(0.2, .4)
# ax1.set_ylim(-10.0, 1.0)
ax2.set_xlabel('Position (m)')
ax2.set_ylabel('Number of trials')
ax2.set_title(''.join(['Stop position for "Good" outcome']), fontsize=20)
vel = 0.1
directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/'])
force_file_list = os.listdir(directory)
for file_name in force_file_list:
# print directory+file_name
loaded_data = load_pickle(directory+file_name)
stop_locations.append(np.max(loaded_data[:,1])-arm_length)
subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length)
arm_lengths.append(arm_length)
vel = 0.15
directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/'])
force_file_list = os.listdir(directory)
for file_name in force_file_list:
loaded_data = load_pickle(directory+file_name)
stop_locations.append(np.max(loaded_data[:,1]-arm_length))
subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length)
ax2.hist(subject_stop_locations)
mu = np.mean(stop_locations)
sigma = np.std(stop_locations)
print 'The minimum arm length is: ', np.min(arm_lengths)
print 'The max arm length is: ', np.max(arm_lengths)
print 'The mean arm length is: ', np.mean(arm_lengths)
print 'The mean of the stop location is: ', mu
print arm_lengths
print 'The standard deviation of the stop location is: ', sigma
n, bins, patches = ax1.hist(stop_locations, 10, color="green", alpha=0.75)
points = np.arange(0, 10, 0.001)
y = mlab.normpdf(points, mu, sigma)
l = ax1.plot(points, y, 'r--', linewidth=2)
def __init__(self):
self.client = actionlib.SimpleActionClient("/move_base",
MoveBaseAction)
self.client.wait_for_server()
self.locations = rosparam.load_file(
str(Path.home()) +
"/catkin_ws/src/competition2/yaml/locations.yaml")[0][0]
self.numbered_locations = rosparam.load_file(
str(Path.home()) +
"/catkin_ws/src/competition2/yaml/numbered_locations.yaml")[0][0]
self.doorway_locations = rosparam.load_file(
str(Path.home()) +
"/catkin_ws/src/competition2/yaml/doorway_locations.yaml")[0][0]
self.cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.twist = Twist()
self.amcl_pose = rospy.Subscriber("/amcl_pose",
PoseWithCovarianceStamped,
self.amcl_pose_callback)
self.covariance = {}
# wait for covariance subscriber to receive data
while len(self.covariance) != 3:
pass
self.initial_pose = rospy.Publisher("/initialpose",
PoseWithCovarianceStamped,
queue_size=10)
self.move = Move()
def __init__(self, name):
rospy.loginfo("Starting %s" % name)
a_list = rospy.get_param("han_action_dispatcher")["han_actions"]
param_file = rospy.get_param("~param_file")
for a in a_list.keys():
rosparam.load_file(param_file, a, True)
rospy.loginfo("done")
def fillAliasCombo(self):
# Fill ComboBox with saved positions
positions_dict = rosparam.load_file(ALIAS_FILE)[0][0]
positions = sorted(positions_dict.keys())
for position in positions:
self._widget.alias_combo.addItem(position)
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 update(self):
self.actions = []
rospack = rospkg.RosPack()
packages = rospack.get_depends_on(self.name, implicit=False)
for package in packages:
manifest = rospack.get_manifest(package)
file_path = manifest.get_export(self.name, 'actions')
if not file_path:
continue
elif len(file_path) != 1:
rospy.logwarn("Cannot load actions [%s]: invalid 'actions' attribute."%(pkg))
continue
file_path = file_path[0]
try:
if not os.path.isfile(file_path):
rospy.logwarn('Actions parameter file with path "' + file_path + '" does not exists.')
continue
package_path = rospack.get_path(package)
parameters = load_file(file_path)
for action_parameters in parameters[0][0]['actions']:
entry = ActionEntry(package_path, action_parameters['action'])
self.actions.append(entry)
except Exception:
rospy.logwarn("Unable to load actions [%s] from package [%s]."%(file_path, package))
self.actions = sorted(self.actions, key = lambda x: (x.id))
return True
def load_params(self, req):
''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE:
paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
'''
resp = LoadResponse()
if len(req.type) == 0:
resp.status.result = Status.FAILURE
resp.status.error = Status.TYPE_MISSING
resp.status.info = "No type provided!"
else:
path = join(self._root, req.type)
filename = join(path, req.id)
if not os.path.exists(path):
resp.status.result = Status.FAILURE
resp.status.error = Status.NO_SUCH_TYPE
resp.status.info = "Type %s does not exist!"%(req.type)
elif not os.path.exists(filename):
resp.status.result = Status.FAILURE
resp.status.error = Status.FILE_MISSING
resp.status.info = "File %s does not exist as a member of type %s!"%(req.id, req.type)
else:
paramlist=rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns,params)
resp.status.result = Status.SUCCESS
return resp
def loadYaml(self, file, namespace):
'''
Load all the parameters on the parameter server
@return: True if OK, False otherwise
'''
parameters = rosparam.load_file("file")
rosparam.upload_params(namespace, parameters[0][0][namespace])
def load_thruster_ports(self, ports_layout, thruster_definitions):
''' Loads a dictionary ThrusterPort objects '''
self.ports = {} # ThrusterPort objects
self.thruster_to_port_map = {} # node_id to ThrusterPort
rospack = rospkg.RosPack()
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'")
# Instantiate thruster comms port
for port_info in ports_layout:
port_name = port_info['port']
self.ports[port_name] = thruster_comm_factory(port_info, thruster_definitions, fake=self.make_fake)
# Add the thrusters to the thruster dict and configure if present
for thruster_name in port_info['thruster_names']:
self.thruster_to_port_map[thruster_name] = port_info['port']
if thruster_name not in self.ports[port_name].online_thruster_names:
rospy.logerr("ThrusterDriver: {} IS MISSING!".format(thruster_name))
else:
rospy.loginfo("ThrusterDriver: {} registered".format(thruster_name))
# Set firmware settings
port = self.ports[port_name]
node_id = thruster_definitions[thruster_name]['node_id']
config_path = (rospack.get_path('sub8_videoray_m5_thruster') + '/config/firmware_settings/' +
thruster_name + '.yaml')
rospy.loginfo('Configuring {} with settings specified in {}.'.format(thruster_name,
config_path))
port.set_registers_from_dict(node_id=node_id,
reg_dict=rosparam.load_file(config_path)[0][0])
port.reboot_thruster(node_id) # Necessary for some settings to take effect
def load_params(self, req):
''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE:
paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
'''
resp = LoadResponse()
if len(req.type) == 0:
resp.status.result = Status.FAILURE
resp.status.error = Status.TYPE_MISSING
resp.status.info = "No type provided!"
else:
path = join(self._root, req.type)
filename = join(path, req.id)
if not os.path.exists(path):
resp.status.result = Status.FAILURE
resp.status.error = Status.NO_SUCH_TYPE
resp.status.info = "Type %s does not exist!" % (req.type)
elif not os.path.exists(filename):
resp.status.result = Status.FAILURE
resp.status.error = Status.FILE_MISSING
resp.status.info = "File %s does not exist as a member of type %s!" % (
req.id, req.type)
else:
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
resp.status.result = Status.SUCCESS
return resp
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():
try:
data = rosparam.load_file("smach.yaml", default_namespace="smach")
print "load yaml file success"
#update to server
for params, ns in data:
rosparam.upload_params(ns, params)
time.sleep(0.5)
except:
print "load yaml file faile"
sys.exit(0)
try:
slam = int(rospy.get_param('~/smach/slam'))
navigation = int(rospy.get_param('~/smach/navigation'))
control = int(rospy.get_param('~/smach/control'))
print "load param success"
time.sleep(0.5)
print "**********************************************"
print "value:"
print("slam { %d }" % (slam)) #0:none, 1:open
print("navigation { %d }" % (navigation)
) #0:none, 1:location1, 2:location2 ,etc
print("control { %d }" % (control)
) #0:none, 1:front, 2:back, 3:left, 2:right
print "**********************************************"
time.sleep(0.1)
except:
print "load param faile"
sys.exit(0)
def launch_platform_controls_spawner(self):
rospack = rospkg.RosPack()
srvss_bobcat_pkg_path = rospack.get_path('srvss_bobcat')
srvss_bobcat_controllers_yaml = srvss_bobcat_pkg_path + "/controllers/srvss_bobcat_controllers.yaml"
paramlist = rosparam.load_file(srvss_bobcat_controllers_yaml,
default_namespace='',
verbose=True)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
# if the controllers do not load it possible to increase the time of waiting for the server in the spawner
# it located in /home/userws3/gz_ros_cws/src/ros_control/controller_manager/scripts
node = ROSNode("controller_manager",
"spawner",
name="platform_controllers_spawner",
namespace="/srvss_bobcat",
output="screen",
respawn="false",
args="joint_state_controller \
front_right_wheel_velocity_controller \
front_left_wheel_velocity_controller \
back_right_wheel_velocity_controller \
back_left_wheel_velocity_controller")
self.launcher.launch(node)
time.sleep(10)
node = ROSNode("srvss_bobcat",
"srvss_bobcat_rate2effort_node",
name="srvss_bobcat_RateToEffort_node",
cwd="node",
output="screen")
self.launcher.launch(node)
time.sleep(3)
def load_rois(self):
"""
"""
import rosparam
# TODO also check in current directory?
#files = glob.glob('compressor_rois_*.yaml')
files = glob.glob(os.path.join(rospy.get_param('source_directory'),
'compressor_rois_*.yaml'))
if len(files) < 1:
rospy.logfatal(
'Did not find any files matching compressor_rois_*.yaml')
return []
elif len(files) > 1:
rospy.logfatal(
'Found too many files matching compressor_rois_*.yaml')
return []
filename = os.path.abspath(files[0])
# get the parameters in the namespace of the name we want
# TODO find roi specifiers wherever they are, in the future
paramlist = rosparam.load_file(filename)
ns = 'delta_compressor'
ns_param_dict = self.find_roi_namespace(ns, paramlist)
if ns_param_dict is None:
rospy.logfatal('could not find parameter namespace: ' + ns)
return
rois = self.find_and_call_function('load_', 'parameter dump loading',
params=ns_param_dict)
rospy.logwarn('loaded rois:' + str(rois))
self.launch_queue.put(rois)
def __init__(self):
''' connect to midi device and set up ros
'''
pygame.midi.init()
devices = pygame.midi.get_count()
if devices < 1:
rospy.logerr("No MIDI devices detected")
exit(-1)
rospy.loginfo("Found %d MIDI devices" % devices)
input_dev = int(rospy.get_param("~input_dev",
pygame.midi.get_default_input_id))
rospy.loginfo("Using input device %d" % input_dev)
self.controller = pygame.midi.Input(input_dev)
# load in default parameters if not set
if not rospy.has_param('~modes'):
rospack = rospkg.RosPack()
paramlist = rosparam.load_file(rospack.get_path('korg_nanokontrol') +
'/config/nanokontrol_config.yaml')
for params, ns in paramlist:
rosparam.upload_params(ns, params)
self.modes = rospy.get_param('~modes')
# determine how axis should be interpreted
self.center_axis = rospy.get_param('~center_axis', True)
self.msg = Joy()
self.mode = None
self.pub = rospy.Publisher('joy', Joy, queue_size=10, latch=True)
def load_device_map(self, fname):
try:
params = rosparam.load_file(fname)[0][0]
if 'lights' in params:
lights = params
for k, v in params['lights'].items():
try:
light_id = int(k, 16)
alias = v['alias_id']
except ValueError:
rospy.logerr(
f'Failed to load device map config: device ID should be hexadecimal, but \'{k}\' value was found instead'
)
return False
except KeyError:
rospy.logerr(
f'Failed to load device map config: missing alias_id key'
)
return False
rosparam.upload_params(rospy.get_name(), lights)
else:
rospy.logerr(
f'Failed to load device map config: unable to find \'lights\' parameter'
)
return False
except Exception as e:
rospy.logerr(f'Failed to load device map config: {e}')
return False
return True
def rosparam(*args):
"""
Load rosparam YAML file. This can either be called with
rosparam(filename) or rosparam(pkg, filename)
@return: loaded data
"""
#TODO: should this do a search to find filename
if len(args) == 2:
pkg, filename = args
d = roslib.packages.get_pkg_dir(pkg)
matches = roslib.packages.find_resource(pkg, filename)
if matches:
filename = matches[0]
else:
filename = None
elif len(args) == 1:
filename = args[0]
else:
raise ValueError(args)
if not filename:
#TODO: what should the error behavior of ROSH be?
return None
#TODO: this strip any namespace in the file
return _rosparam.load_file(filename)[0][0]
def srv_callback(self, request):
label = request.label
response = SaveSpotServiceMessageResponse()
"""
---
bool navigation_successfull
string message # Direction
"""
os.chdir("/home/zulhafiz/catkin_ws/src/kasimx_navigation/spots")
paramlist = rosparam.load_file("spots.yaml", default_namespace=None)
for params, ns in paramlist: #ns,param
for key, value in params.items():
if key == request.label:
rosparam.upload_params(ns, params) #ns,param
response.message = "Correctly uploaded parameters"
send_coordinates = SendCoordinates(request.label)
response.navigation_successfull = True
return response
def __init__(self):
# Parameters
# global settings
self.cfg_ = rospy.get_param('~cfg', rosparam.load_file(
os.path.join(iarc_root(), 'config', 'perception.yaml'))[0][0] )
print 'cfg'
print '==============='
print self.cfg_
print '==============='
# parameter overrides
self.src_ = rospy.get_param('~src', self.cfg_['src'])
self.dbg_ = rospy.get_param('~dbg', self.cfg_['dbg'])
self.root_ = rospy.get_param('~root', os.path.join(iarc_root(), 'data'))
self.max_dt_ = rospy.get_param('~max_dt', self.cfg_['max_dt'])
self.dfac_ = DetectorFactory()
# Data
self.data_ = {}
# ROS Handles
self.cvbr_ = CvBridge()
self.tf_buf_ = tf2_ros.Buffer()
self.tfl_ = tf2_ros.TransformListener(self.tf_buf_)
self.cam_ = {k : CameraHandle(k, self.cvbr_, self.data_cb) for k in self.src_}
self.modules_ = {m : self.dfac_(m['model'], self.cfg_[m]) for m in self.cfg_['modules']}
def carefree_switch(self,
arm,
new_controller,
param_file=None,
possible_controllers=None):
if '%s' in new_controller:
new_controller = new_controller % arm
if param_file is not None:
params = rosparam.load_file(
roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
if possible_controllers is None:
possible_controllers = POSSIBLE_ARM_CONTROLLERS
check_arm_controllers = [
controller % arm for controller in possible_controllers
]
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_controller], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[
i] == 'running':
stop_controllers.append(controller)
self.load_controller(new_controller)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" %
(start_controllers[0]) + " and stopping controllers: [" +
",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers,
1)
return resp.ok
def __init__(
self,
output_topic="/bounding_box_2d_monitor",
rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color",
depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw",
camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info",
yolo_yaml="yolov3.yaml",
save_image=True,
z_offset=0.1,
MEMO="/spacoty/get_dataset"):
super(hsr_SpaCoTyGetDataset,
self).__init__(outcomes=['completed', 'failed'],
input_keys=['save_folder'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = {
"img": "bounding_box_2d",
"sw": "/spacoty/get_dataset",
"wrd": "/spacoty/place_name"
}
self._output_topic = output_topic
self._depth_topic = depth_topic
self._rgb_topic = rgb_topic
self._camera_info_topic = camera_info_topic
self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/"
self._save_image = save_image
self.camera_info = None
self.rgb_image = None
self.depth_image = None
self.z_offset = z_offset
# get object labels
# self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml
# obj_yaml = rosparam.load_file(self._yolo_yaml_path)
# self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"]
self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml
obj_yaml = rosparam.load_file(self._yolo_yaml_path)
self.obj_class = obj_yaml[0][0].values()
# pass required clients as dict (topic: type)
self._client = ProxyActionClient(
{self._topic["img"]: hsr_common.msg.BoundingBox2DAction})
self.bridge = cv_bridge.CvBridge()
# Subscriber
self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool})
self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String})
# define the dimension
self._data_dim = {"pose": 3, "object": len(self.obj_class), "word": 0}
# It may happen that the action client fails to send the action goal.
self._error = False
self._get_image_error = False
def __init__(self, name, configFilePath):
params = rosparam.load_file(configFilePath)
for param, ns in params:
try:
rosparam.upload_params(ns, param)
except ROSParamException, e:
print >> sys.stderr, "ERROR: " + str(e)
pass # ignore empty params
def set_goal_from_file(self, file_path):
"""Set the goal from a YAML file containing the action combination."""
from rosparam import load_file
if not os.path.isfile(file_path):
rospy.logerr('File with path "' + file_path + '" does not exists.')
self.set_state(ActionState.ERROR)
return
self.set_goal_from_yaml(load_file(file_path))
def set_goal_from_file(self, file_path):
"""Set the goal from a YAML file containing the action combination."""
from rosparam import load_file
if not os.path.isfile(file_path):
rospy.logerr('File with path "' + file_path + '" does not exists.')
self.set_state(ActionState.ERROR)
return
self.set_goal_from_yaml(load_file(file_path))
def load_markers():
publisher = rospy.Publisher('~marker', MarkerArray, queue_size=1, latch=True)
markerArray = MarkerArray();
file_path = rospy.get_param('~world_file')
if not file_path:
rospy.loginfo('World description file path is empty.')
return
if not isfile(file_path):
rospy.logwarn('World description file with path "' + file_path + '" does not exists.')
return
parameters = load_file(file_path)
if not parameters:
return
for model in parameters[0][0]['models']:
marker = Marker();
if 'box' in model:
marker.type = Marker.CUBE;
model = model['box']
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
elif 'cylinder' in model:
marker.type = Marker.CYLINDER;
model = model['cylinder']
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
elif 'mesh' in model:
marker.type = Marker.MESH_RESOURCE;
model = model['mesh']
marker.mesh_resource = model['mesh_resource']
marker.mesh_use_embedded_materials = True
else:
continue
marker.ns = model['name']
marker.header.frame_id = model['frame_id']
# marker.id = i # TODO Required?
marker.action = Marker.ADD
marker.pose.position.x = model['position']['x']
marker.pose.position.y = model['position']['y']
marker.pose.position.z = model['position']['z']
marker.pose.orientation.x = model['orientation']['x']
marker.pose.orientation.y = model['orientation']['y']
marker.pose.orientation.z = model['orientation']['z']
marker.pose.orientation.w = model['orientation']['w']
marker.scale.x = model['scale']['x']
marker.scale.y = model['scale']['y']
marker.scale.z = model['scale']['z']
markerArray.markers.append(marker)
rospy.loginfo("World `%s` is published as marker.", marker.ns)
publisher.publish(markerArray)
rospy.spin()
def rosmodel2owl(configFilePath):
# the file path should be given as argument, alternatively a ns can be added
# params = rosparam.load_file(rospack.get_path('mros1_reasoner')+'/config/nav_config.yaml', '')
# loads individuals to specify a single function and QalityAttributeTypes from the domain_ontology_file
params = rosparam.load_file(configFilePath)
for param, ns in params:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
ontology_pkg = rospy.get_param('ontology_pkg')
ontology_path = rospy.get_param('ontology_path')
ontology_file = os.path.join(
rospack.get_path(ontology_pkg) + '/' + ontology_path)
domain_ontology_pkg = rospy.get_param('domain_ontology_pkg')
domain_ontology_path = rospy.get_param('domain_ontology_path')
domain_ontology_file = os.path.join(
rospack.get_path(domain_ontology_pkg) + '/' + domain_ontology_path)
function = rospy.get_param('function')
configs = rospy.get_param('configs')
result_file = rospy.get_param('result_file')
tomasys = get_ontology(ontology_file).load()
onto = get_ontology(domain_ontology_file).load()
rospy.loginfo('Loaded domain ontology: ' + domain_ontology_file)
function_ = onto.search_one(iri='*' + function + '*')
if function_ == None:
print('The domain ontology provided does not contain a Function ',
function)
sys.exit(0)
for config in configs:
config_name = config
file_name = config_name + '.rossystem'
file_path = os.path.join(rospack.get_path(config_name), file_name)
parser = RosSystemModelParser(file_path)
model = parser.parse()
sys_name = model.system_name[0]
# create a FunctionDesign
fd = tomasys.FunctionDesign(sys_name,
namespace=onto,
solvesF=function_)
# create a QualityAttribute expected value for the FunctionDesign with the type indicated by the name of the param in the RosSystem and the value of the param (e.g. 0.5)
for qa_param in model.global_parameters:
qa_string = qa_param.param_name[0].replace('qa_', '')
value = qa_param.param_value[0]
qa = tomasys.QAvalue('{0}_{1}'.format(qa_param.param_name[0],
sys_name),
namespace=onto,
isQAtype=onto.search_one(iri='*' + qa_string),
hasValue=value)
fd.hasQAestimation.append(qa)
# save the ontology to a file
onto.save(file=result_file, format='rdfxml')
def __init__(self):
rospy.loginfo("Sarting info terminal reconfigure tasks")
paramlist = rosparam.load_file(rospy.get_param("~config_file"))[0][0]
self.server_list = []
for k, v in paramlist.items():
self.server_list.append(
Server(name=k,
location=v["location"],
parameters=v["parameters"]))
def load_joint_position_file(self, filename):
'''Returns the dict with the positions previously saved in a yaml file'''
try:
data = rosparam.load_file(filename)
except rosparam.RosParamException:
# If not found, check again in the current package
try:
filename = rospkg.RosPack().get_path('iris_sami') + '/yaml/' + filename
data = rosparam.load_file(filename)
except Exception as e:
self.error_msg = "Can't load positions from'" + filename + "'\n" + str(e)
rospy.logerr(self.error_msg)
return False
self.joint_positions = data[0][0]
self.positions_file = filename
return data[0][0]
def load_action_from_file_and_transform(file_path, source_frame_id='', position=[0, 0, 0], orientation=[0, 0, 0, 1]):
from rosparam import load_file
import os
if not os.path.isfile(file_path):
rospy.logerr('File with path "' + file_path + '" does not exists.')
return None
return parse_action(load_file(file_path), source_frame_id, position, orientation)
def __load_params(self):
try:
rospy.loginfo('Loading parameters')
res = rosparam.load_file(self.config_filepath, self.ns, True)[0]
rospy.loginfo('Loading done')
return True
except:
rospy.logwarn('Failed to read data from config')
return False
def load_action_from_file_and_transform(file_path, source_frame_id='', position=[0, 0, 0], orientation=[0, 0, 0, 1]):
from rosparam import load_file
import os
if not os.path.isfile(file_path):
rospy.logerr('File with path "' + file_path + '" does not exists.')
return None
return parse_action(load_file(file_path), source_frame_id, position, orientation)
def file_open(self):
os.chdir("/home/user/catkin_ws/src/navigation_exam/params")
paramlist = rosparam.load_file("task2.yaml", default_namespace=None)
for params, ns in paramlist: #ns,param
for key, value in params.items():
rosparam.upload_params(ns, params)
return self.point(key, value)
def __init__(self):
########### load yaml file ###########
try:
self.data = rosparam.load_file("location.yaml",
default_namespace="artag")
print "load yaml file success"
#update to server
for params, ns in self.data:
rosparam.upload_params(ns, params)
time.sleep(1)
except:
print "load yaml file faile"
sys.exit(0)
########### load param ###########
try:
self.local1_con = int(
rospy.get_param('~/artag/location1/Confidence'))
self.local1_x = float(rospy.get_param('~/artag/location1/x'))
self.local1_y = float(rospy.get_param('~/artag/location1/y'))
self.local2_con = int(
rospy.get_param('~/artag/location2/Confidence'))
self.local2_x = float(rospy.get_param('~/artag/location2/x'))
self.local2_y = float(rospy.get_param('~/artag/location2/y'))
self.local3_con = int(
rospy.get_param('~/artag/location3/Confidence'))
self.local3_x = float(rospy.get_param('~/artag/location3/x'))
self.local3_y = float(rospy.get_param('~/artag/location3/y'))
self.local4_con = int(
rospy.get_param('~/artag/location4/Confidence'))
self.local4_x = float(rospy.get_param('~/artag/location4/x'))
self.local4_y = float(rospy.get_param('~/artag/location4/y'))
print "load param success"
time.sleep(1)
print "**********************************************"
print "value:"
print("local1 { con %d , x %.4f , y %.4f }" %
(self.local1_con, self.local1_x, self.local1_y))
print("local2 { con %d , x %.4f , y %.4f }" %
(self.local2_con, self.local2_x, self.local2_y))
print("local3 { con %d , x %.4f , y %.4f }" %
(self.local3_con, self.local3_x, self.local3_y))
print("local4 { con %d , x %.4f , y %.4f }" %
(self.local4_con, self.local4_x, self.local4_y))
print "**********************************************"
time.sleep(1)
except:
print "load param faile"
sys.exit(0)
self.listener = tf.TransformListener()
self.listener2 = tf.TransformListener()
self.listener3 = tf.TransformListener()
self.listener4 = tf.TransformListener()
def _get_map_params(self):
paramlist=rosparam.load_file(self.map_path + ".yaml",default_namespace="map_params")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
origin = rospy.get_param('/map_params/origin')
resolution = rospy.get_param('/map_params/resolution')
return (origin, resolution)
def main():
rospy.init_node("optitrak_odom_remap")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f',
'--file',
dest="filename",
default="",
help="YAML file to save parameters in.")
p.add_option('-l',
'--load',
dest="is_load",
action="store_true",
default=False,
help="Load parameters from file.")
p.add_option(
'-t',
'--train',
dest="is_train",
action="store_true",
default=False,
help=
"Train by moving the head to different poses and capturing the relative positions."
)
p.add_option(
'-p',
'--publish',
dest="is_pub",
action="store_true",
default=False,
help="Publish the transformation from optitrak to base_footprint.")
(opts, args) = p.parse_args()
if opts.filename == "":
print "Bad command line parameters"
p.print_help()
return
if opts.is_load:
print "hi"
params = rosparam.load_file(opts.filename,
"optitrak_calibration")[0][0]
rosparam.upload_params("optitrak_calibration", params)
rospy.sleep(0.1)
if opts.is_pub:
publish_transform()
return
if opts.is_train:
opti_mat, robot_mat = capture_data()
opti_mat = np.mat(opti_mat).T
robot_mat = np.mat(robot_mat).T
pos, rot = umeyama_method(opti_mat, robot_mat)
print np.linalg.norm(robot_mat - (rot * opti_mat + pos))
save_params(pos, rot, opts.filename)
return
def main():
rospy.init_node("netft_gravity_zeroing")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--file', dest="filename", default="",
help="YAML file to save parameters in.")
p.add_option('-l', '--load', dest="is_load",
action="store_true", default=False,
help="Load parameters from file.")
p.add_option('-r', '--run', dest="is_run",
action="store_true", default=False,
help="Publish a zeroed wrench.")
p.add_option('-t', '--train', dest="is_train",
action="store_true", default=False,
help="Train by moving the gripper to different poses and measuring the wrenches.")
p.add_option('-p', '--pr2', dest="is_pr2",
action="store_true", default=False,
help="Will run automated data collection if this is the PR2.")
p.add_option('-z', '--start_zero', dest="start_zero",
action="store_true", default=False,
help="Use the first value in to zero the sensor.")
p.add_option('-n', '--ntrials', dest="n_trials",
default="6,6,4",
help="Number of trials for each of the last 3 joint angles to move through. (default: 6,6,4)")
(opts, args) = p.parse_args()
try:
n_4, n_5, n_6 = [int(n_str) for n_str in opts.n_trials.split(',')]
except:
print "Bad --ntrials parameter (format: --ntrials 6,6,4)"
p.print_help()
return
if opts.is_load:
params = rosparam.load_file(opts.filename, rospy.get_name())[0][0]
rosparam.upload_params(rospy.get_name(), params)
if opts.is_run:
rospy.sleep(0.1)
nft_z = NetFTZeroer(start_zero=opts.start_zero, is_pr2=opts.is_pr2)
rospy.spin()
return
if opts.is_train:
if opts.is_pr2:
data = collect_data_pr2(n_4, n_5, n_6)
param_vector = process_data(data, True)
save_params(param_vector, opts.filename)
return
else:
data = collect_data_tool()
param_vector = process_data(data, False)
save_params(param_vector, opts.filename)
return
def __init__(self, name):
rospy.loginfo("Starting node: %s" % name)
self.current_edge = 'none'
self.current_node = 'none'
self.alive = False
self.white_list_edges = []
white_list_file = rospy.get_param("~white_list_file", '')
if white_list_file == '':
rospy.logfatal("No white list for logging specified")
raise rospy.ROSException("No white list for logging specified")
return
white_list = rosparam.load_file(white_list_file)[0][0]
rospy.loginfo("Getting topological map name...")
self.map_name = None
self.map_sub = rospy.Subscriber(
rospy.get_param("~topo_map_topic", "/topological_map"),
TopologicalMap, self.get_map_name)
while not self.map_name and not rospy.is_shutdown():
rospy.logwarn("Topologial map is not being published. Waiting...")
rospy.sleep(1)
rospy.loginfo(" ... got topological map: %s" % self.map_name)
rospy.loginfo("Getting allowed nodes from file...")
self.white_list_nodes = white_list["nodes"]
rospy.loginfo(" ... got nodes: %s" % self.white_list_nodes)
if not [x for x in self.white_list_nodes if "ALL" in x]:
while not rospy.is_shutdown():
try:
rospy.loginfo("Creating edge look-up service...")
s = rospy.ServiceProxy(
'/topological_map_manager/get_edges_between_nodes',
GetEdgesBetweenNodes)
rospy.loginfo("... waiting for edge look-up service")
s.wait_for_service()
rospy.loginfo("... found edge look-up service.")
for idx, nodea in enumerate(self.white_list_nodes):
for nodeb in self.white_list_nodes[idx + 1:]:
rospy.logdebug("%s <-> %s" % (nodea, nodeb))
req = GetEdgesBetweenNodesRequest()
req.nodea = nodea
req.nodeb = nodeb
resp = s(req)
if resp.ids_a_to_b and resp.ids_b_to_a:
self.white_list_edges.append(
str(resp.ids_a_to_b[0]) + '--' +
self.map_name)
self.white_list_edges.append(
str(resp.ids_b_to_a[0]) + '--' +
self.map_name)
break
except rospy.ServiceException, e:
rospy.logwarn("Service call failed: %s. Retrying." % e)
rospy.sleep(1)
def read_map():
"""
Extract room numbers from the map.
@return numbered_locations: dict {int: string}
"""
numbered_locations = rosparam.load_file(
str(Path.home()) +
"/catkin_ws/src/competition2/yaml/numbered_locations.yaml")[0][0]
return numbered_locations
def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name):
rospack = rospkg.RosPack()
pkg_path = rospack.get_path(rospackage_name)
config_dir = os.path.join(pkg_path, rel_path_from_package_to_file)
path_config_file = os.path.join(config_dir, yaml_file_name)
paramlist=rosparam.load_file(path_config_file)
for params, ns in paramlist:
rosparam.upload_params(ns,params)
def __init__(self, name):
rospy.loginfo("Starting node: %s" % name)
self.current_edge = 'none'
self.current_node = 'none'
self.alive = False
self.white_list_edges = []
white_list_file = rospy.get_param("~white_list_file", '')
if white_list_file == '':
rospy.logfatal("No white list for logging specified")
raise rospy.ROSException("No white list for logging specified")
return
white_list = rosparam.load_file(white_list_file)[0][0]
rospy.loginfo("Getting topological map name...")
self.map_name = None
self.map_sub = rospy.Subscriber(
rospy.get_param("~topo_map_topic", "/topological_map"),
TopologicalMap,
self.get_map_name
)
while not self.map_name and not rospy.is_shutdown():
rospy.logwarn("Topologial map is not being published. Waiting...")
rospy.sleep(1)
rospy.loginfo(" ... got topological map: %s" % self.map_name)
rospy.loginfo("Getting allowed nodes from file...")
self.white_list_nodes = white_list["nodes"]
rospy.loginfo(" ... got nodes: %s" % self.white_list_nodes)
if not [x for x in self.white_list_nodes if "ALL" in x]:
while not rospy.is_shutdown():
try:
rospy.loginfo("Creating edge look-up service...")
s = rospy.ServiceProxy(
'/topological_map_manager/get_edges_between_nodes',
GetEdgesBetweenNodes
)
rospy.loginfo("... waiting for edge look-up service")
s.wait_for_service()
rospy.loginfo("... found edge look-up service.")
for idx, nodea in enumerate(self.white_list_nodes):
for nodeb in self.white_list_nodes[idx+1:]:
rospy.logdebug("%s <-> %s" % (nodea, nodeb))
req = GetEdgesBetweenNodesRequest()
req.nodea = nodea
req.nodeb = nodeb
resp = s(req)
if resp.ids_a_to_b and resp.ids_b_to_a:
self.white_list_edges.append(str(resp.ids_a_to_b[0])+'--'+self.map_name)
self.white_list_edges.append(str(resp.ids_b_to_a[0])+'--'+self.map_name)
break
except rospy.ServiceException, e:
rospy.logwarn("Service call failed: %s. Retrying." % e)
rospy.sleep(1)
def __init__(self):
try:
self.no_frames = rospy.get_param('/camera_calibration/frames');
except:
self.no_frames = 1000
print self.no_frames
self.pattern_size = (6, 8)
self.pattern_points = np.zeros((np.prod(self.pattern_size), 3), np.float32)
self.pattern_points[:, :2] = np.indices(self.pattern_size).T.reshape(-1, 2)
self.pattern_points[:, 0] = -self.pattern_points[:, 0]
self.pattern_points *= 0.05
self.count = [1, 1]
self.left_midXYZ = [0, 0, 0]
self.left_midQ = [0, 0, 0, 0]
self.right_midXYZ = [0, 0, 0]
self.right_midQ = [0, 0, 0, 0]
self.final_left_xyz = [0, 0, 0]
self.final_left_q = [0, 0, 0, 1]
self.final_right_xyz = [0, 0, 0]
self.final_right_q = [0, 0, 0, 1]
self.still_calibrating = True
self.bridge = CvBridge()
self.left_image = rospy.Subscriber('/left/rgb/image_color', Image, self.left_callback)
self.right_image = rospy.Subscriber('/right/rgb/image_color', Image, self.right_callback)
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.pt = {'previous_transform': {'left': {'orientation': [0.0, 0.0, 0.0, 0.0],
'position': [0.0, 0.0, 0.0]},
'right': {'orientation': [0.0, 0.0, 0.0, 0.0],
'position': [0.0, 0.0, 0.0]}}}
self.rospack = rospkg.RosPack()
try:
self.pt = rosparam.load_file(self.rospack.get_path('camera_setup') + '/config/previous_transform.yaml')[0][0]
except:
self.still_calibrating = True
if rospy.get_param('~quick'):
self.final_left_xyz = self.pt['previous_transform']['left']['position']
self.final_left_q = self.pt['previous_transform']['left']['orientation']
self.final_right_xyz = self.pt['previous_transform']['right']['position']
self.final_right_q = self.pt['previous_transform']['right']['orientation']
self.still_calibrating = False
self.publish_feedback()
def main():
rospy.init_node("optitrak_odom_remap")
from optparse import OptionParser
p = OptionParser()
p.add_option("-f", "--file", dest="filename", default="", help="YAML file to save parameters in.")
p.add_option("-l", "--load", dest="is_load", action="store_true", default=False, help="Load parameters from file.")
p.add_option(
"-t",
"--train",
dest="is_train",
action="store_true",
default=False,
help="Train by moving the head to different poses and capturing the relative positions.",
)
p.add_option(
"-p",
"--publish",
dest="is_pub",
action="store_true",
default=False,
help="Publish the transformation from optitrak to base_footprint.",
)
(opts, args) = p.parse_args()
if opts.filename == "":
print "Bad command line parameters"
p.print_help()
return
if opts.is_load:
print "hi"
params = rosparam.load_file(opts.filename, "optitrak_calibration")[0][0]
rosparam.upload_params("optitrak_calibration", params)
rospy.sleep(0.1)
if opts.is_pub:
publish_transform()
return
if opts.is_train:
opti_mat, robot_mat = capture_data()
opti_mat = np.mat(opti_mat).T
robot_mat = np.mat(robot_mat).T
pos, rot = umeyama_method(opti_mat, robot_mat)
print np.linalg.norm(robot_mat - (rot * opti_mat + pos))
save_params(pos, rot, opts.filename)
return
def write_and_start(filename, buffer_string):
f = open(filename, "w+")
f.write(buffer_string)
f.close()
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
if ON:
# send start command to smach!
start = rospy.ServiceProxy("/start_SMACH", Empty)
try:
rospy.wait_for_service('/start_SMACH')
start()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self, *args):
super(TestAddAnalyzer, self).__init__(*args)
rospy.init_node('test_add_analyzer')
self.namespace = rospy.get_name()
paramlist = rosparam.load_file(rospy.myargv()[1])
# expect to receive these paths in the added analyzers
self.expected = [paramlist[0][1] + analyzer['path'] for name, analyzer in paramlist[0][0]['analyzers'].iteritems()]
self._mutex = threading.Lock()
self.agg_msgs = {}
# put parameters in the node namespace so they can be read by the aggregator
for params, ns in paramlist:
rosparam.upload_params(rospy.get_name() + '/' + ns, params)
rospy.Subscriber('/diagnostics_agg', DiagnosticArray, self.agg_cb)
self.pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def switch(self, old_controller, new_controller, param_file=None):
if param_file is None:
self.load_controller(new_controller)
resp = self.switch_controller_srv([new_controller], [old_controller], 1)
self.unload_controller(old_controller)
return resp.ok
else:
params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
self.switch_controller_srv([], [old_controller], 1)
self.unload_controller(old_controller)
if old_controller != new_controller:
self.unload_controller(new_controller)
self.load_controller(old_controller)
if old_controller != new_controller:
self.load_controller(new_controller)
resp = self.switch_controller_srv([new_controller], [], 1)
return resp.ok
def carefree_switch(self, arm, new_controller, param_file=None, possible_controllers=None):
if '%s' in new_controller:
new_controller = new_controller % arm
if param_file is not None:
params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
if possible_controllers is None:
possible_controllers = POSSIBLE_ARM_CONTROLLERS
check_arm_controllers = [controller % arm for controller in possible_controllers]
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_controller], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[i] == 'running':
stop_controllers.append(controller)
self.load_controller(new_controller)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) +
" and stopping controllers: [" + ",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers, 1)
return resp.ok
def update(self):
self.collections = []
self.collections_to_merge = []
self.collections_to_ignore = []
rospack = rospkg.RosPack()
packages = rospack.get_depends_on(self.name, implicit=False)
for package in packages:
manifest = rospack.get_manifest(package)
file_path = manifest.get_export(self.name, 'collections')
if not file_path:
continue
elif len(file_path) != 1:
rospy.logwarn("Cannot load collections [%s]: invalid 'collections' attribute."%(pkg))
continue
file_path = file_path[0]
try:
if not os.path.isfile(file_path):
rospy.logwarn('Collections parameter file with path "' + file_path + '" does not exists.')
continue
parameters = load_file(file_path)
for collections_parameters in parameters[0][0]['collections']:
if 'collection' in collections_parameters:
collection = Collection(collections_parameters['collection'])
self.collections.append(collection)
elif 'add_to_collection' in collections_parameters:
collection = Collection(collections_parameters['add_to_collection'])
self.collections_to_merge.append(collection)
elif 'ignore_collection' in collections_parameters:
collection = Collection(collections_parameters['ignore_collection'])
self.collections_to_ignore.append(collection)
except Exception:
rospy.logwarn("Unable to load collections [%s] from package [%s]."%(file_path, package))
self._merge_collections()
self._ignore_collections()
self.collections = sorted(self.collections, key = lambda x: (x.id))
return True
def carefree_switch(self, arm, new_controller, param_file=None, reset=True):
if '%s' in new_controller:
new_ctrl = new_controller % arm
else:
new_ctrl = new_controller
if param_file is not None:
params = rosparam.load_file(roslaunch.substitution_args.resolve_args(param_file))
if new_ctrl not in params[0][0]:
rospy.logwarn("[pr2_controller_switcher] Controller not in parameter file.")
return
else:
rosparam.upload_params("", {new_ctrl : params[0][0][new_ctrl]})
possible_controllers = rosparam.get_param(LOADED_CTRLS_PARAMS[arm])
if new_ctrl not in possible_controllers:
possible_controllers.append(new_ctrl)
rosparam.set_param_raw(LOADED_CTRLS_PARAMS[arm], possible_controllers)
check_arm_controllers = []
for controller in possible_controllers:
if '%s' in controller:
controller = controller % arm
if controller[0] == arm:
check_arm_controllers.append(controller)
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_ctrl], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[i] == 'running':
stop_controllers.append(controller)
if controller == new_ctrl:
if resp.state[i] == 'running':
if not reset:
rospy.loginfo("[pr2_controller_switcher] Specified controller is already running.")
return True
self.switch_controller_srv([], [new_ctrl], 1)
self.unload_controller(new_ctrl)
self.load_controller(new_ctrl)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) +
" and stopping controllers: [" + ",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers, 1)
return resp.ok
def load_action_from_file(file_path, placeholders=None):
import os
from rosparam import load_file
if not os.path.isfile(file_path):
rospy.logerr('File with path "' + file_path + '" does not exists.')
return None
parameters = load_file(file_path)
# Replace placeholders.
if placeholders is not None:
replace_placeholders(parameters[0][0], placeholders)
# Adapt coordinates.
is_adapt = False
source_frame_id = ''
target_frame_id = ''
position = [0, 0, 0]
orientation = [0, 0, 0, 1]
if 'adapt_coordinates' in parameters[0][0]:
is_adapt = True
adapt_parameters = parameters[0][0]['adapt_coordinates'][0]['transform']
source_frame_id = adapt_parameters['source_frame']
target_frame_id = adapt_parameters['target_frame']
if 'transform_in_source_frame' in adapt_parameters:
if 'position' in adapt_parameters['transform_in_source_frame']:
position = adapt_parameters['transform_in_source_frame']['position']
if 'orientation' in adapt_parameters['transform_in_source_frame']:
orientation = adapt_parameters['transform_in_source_frame']['orientation']
if len(orientation) == 3:
orientation = quaternion_from_euler(orientation[0], orientation[1], orientation[2])
if is_adapt:
try:
(position, orientation) = transform_coordinates(source_frame_id, target_frame_id, position, orientation)
except TypeError:
return None
return parse_action(parameters, source_frame_id, target_frame_id, position, orientation)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
now, rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
from util_states.image_to_cv import image_converter
import os
import shutil
import subprocess
from geometry_msgs.msg._PoseWithCovarianceStamped import PoseWithCovarianceStamped
# Loading the parameters depending on which robot we are using.
# Reduce the size of image file
robot = os.environ.get('PAL_ROBOT')
if robot == 'rh2c' or robot == 'rh2m':
IMAGE_PATH = roslib.packages.get_pkg_dir('reemh2_maps') + '/config/'
RH2_PATH = roslib.packages.get_pkg_dir('reemh2_maps')
paramlist = rosparam.load_file(RH2_PATH+"/config/map.yaml", default_namespace="emergency_situation")
for params, ns in paramlist:
rosparam.upload_params(ns, params)
elif robot == 'reemh3c' or robot == 'reemh3m':
IMAGE_PATH = roslib.packages.get_pkg_dir('reem_maps') + '/config/'
REEMH3_PATH = roslib.packages.get_pkg_dir('reem_maps')
paramlist = rosparam.load_file(REEMH3_PATH+'/config/map.yaml', default_namespace="emergency_situation")
for params, ns in paramlist:
rosparam.upload_params(ns, params)
else:
#IMAGE_PATH = roslib.packages.get_pkg_dir('robocup_iworlds') + '/navigation/'
IMAGE_PATH = roslib.packages.get_pkg_dir('emergency_situation') + '/config/'
#ROBOCUP_PATH = roslib.packages.get_pkg_dir('robocup_worlds')
#paramlist = rosparam.load_file(ROBOCUP_PATH+"/navigation/subMap1.yaml", default_namespace="emergency_situation")
def reconfigure(self):
try:
yaml = rospy.get_param('/tracking/scenario')
except KeyError:
print("could not find parameter: tracking/scenario on server")
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
try:
param,ns = rosparam.load_file(yaml)[0]
except rosparam.RosParamException:
print("Failed to load %s" %yaml)
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
print("Loaded %s" %yaml)
try:
self.entities = [ Entity(**p) for p in param['entities'] ]
except NameError as e:
print("Couldn't instantiate entities.")
print("Something seems wrong with the entities section of your yaml file:")
print("\t%s"%e)
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
except TypeError as e:
print("Couldn't instantiate entities.")
print("Something seems wrong with the entities section of your yaml file:")
print("\t%s"%e)
print(Entity.__doc__)
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
except:
print(sys.exc_info())
raise
print("Entity creation successful. (%s entities loaded)" % len(self.entities))
try:
self.world = World(**param['world'])
except NameError as e:
print("Couldn't instantiate simulation.")
print("Something seems wrong with the world section of your yaml file.")
print("\t%s"%e)
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
except TypeError as e:
print("Couldn't instantiate simulation.")
print("Something seems wrong with the world section of your yaml file.")
print("\t%s"%e)
print(World.__doc__)
print("Waiting for reset message (/tracking/reset_all) ...")
self.mode = "stop"
return
except:
print(sys.exc_info())
raise
self.mode = "play"
print("Simulator creation successful.")
print("Default output points [mlr_msgs::Point2dArray], topic is: tracking/lk2d/points")
print("Default output images [sensor_msgs::Image], topic is: camera/rgb/image_color")
self.world.set_entities(self.entities)
print("Running...")
def format_data_three_categories(self, subject, result):
# print subject
# print result
print 'Now editing files to insert position data and storing them in labeled folders.'
paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')
fist_length = arm_length - rosparam.get_param('crook_to_wrist')
print 'Fist length: ', fist_length, ' cm'
position_of_initial_contact = (44.0 - arm_length)/100.
position_profile = None
for ind_i in xrange(len(result)):
for ind_j in xrange(len(result[0])):
if ind_j == 2:
continue
else:
if result[ind_i][ind_j] is not None:
if ind_i < len(result)/2:
load_num = ind_i
vel = 0.1
else:
load_num = ind_i - len(result)/2
vel = 0.15
if vel == 0.1:
# print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
# print 'Position profile loaded!'
elif vel == 0.15:
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']))
# print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])
# print 'Position profile loaded!'
else:
print 'There is no saved position profile for this velocity! Something has gone wrong!'
return None
ft_threshold_was_exceeded = False
# current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(load_num), '.log']))])
# while os.path.isfile(''.join([self.pkg_path, '/data/', subject, '/',str(vel),'mps/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])):
ft_threshold_was_exceeded = False
# print ''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])
current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))])
del_index = []
for k in xrange(len(current_data)):
if current_data[k][0] < 5.0:
del_index.append(k)
current_data = np.delete(current_data, del_index, 0)
position_of_stop = 0.
del_index = []
time_of_initial_contact = None
# current_data = load_pickle(''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl']))
# if np.max(current_data[:, 2]) >= 10. or np.max(current_data[:, 3]) >= 10. \
# or np.max(current_data[:, 4]) >= 10.:
# ft_threshold_was_exceeded = True
position_of_stop = 0.
del_index = []
for num, j in enumerate(current_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8:
del_index.append(num)
elif new_position < position_of_initial_contact:
del_index.append(num)
else:
if time_of_initial_contact is None:
time_of_initial_contact = j[0]
position_of_stop = new_position
# if result[ind_i][ind_j] == 'good'
current_data = np.delete(current_data, del_index, 0)
output_data = []
for item in current_data:
output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]])
output_data = np.array(output_data)
save_number = 0
if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other':
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl'])
else:
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl'])
while os.path.isfile(save_file):
save_number += 1
if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other':
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl'])
else:
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl'])
print 'Saving with file name', save_file
save_pickle(output_data, save_file)
print 'Done editing files!'
def _parse(self, file_name):
param_list = rosparam.load_file(file_name, default_namespace='/')
super(Yaml, self).__init__(param_list[0][0])
def __init__(self):
rospy.init_node("config_manager")
rospy.on_shutdown(self._on_node_shutdown)
if not ros_datacentre.util.wait_for_mongo():
sys.exit(1)
self._mongo_client = pymongo.MongoClient(rospy.get_param("datacentre_host","localhost"),
int(rospy.get_param("datacentre_port")))
# Load the default settings from the defaults/ folder
path = os.path.join(roslib.packages.get_pkg_dir('ros_datacentre'),"defaults")
files = os.listdir(path)
defaults=[] # a list of 3-tuples, (param, val, originating_filename)
def flatten(d, c="", f_name="" ):
l=[]
for k, v in d.iteritems():
if isinstance(v, collections.Mapping):
l.extend(flatten(v,c+"/"+k, f_name))
else:
l.append((c+"/"+k, v, f_name))
return l
for f in files:
if not f.endswith(".yaml"):
continue
params = rosparam.load_file(os.path.join(path,f))
rospy.loginfo("Found default parameter file %s" % f)
for p, n in params:
defaults.extend(flatten(p,c="",f_name=f))
# Copy the defaults into the DB if not there already
defaults_collection = self._mongo_client.config.defaults
for param,val,filename in defaults:
existing = defaults_collection.find_one({"path":param})
if existing is None:
rospy.loginfo("New default parameter for %s"%param)
defaults_collection.insert({"path":param,
"value":val,
"from_file":filename})
elif existing["from_file"]!=filename:
rospy.logerr("Two defaults parameter files have the same key:\n%s and %s, key %s"%
(existing["from_file"],filename,param))
# Delete the entry so that it can be fixed...
defaults_collection.remove(existing)
rospy.signal_shutdown("Default parameter set error")
elif existing["value"]!=val:
rospy.loginfo("Updating stored default for %s"%param)
defaults_collection.update(existing,{"$set":{"value":val}})
# Load the settings onto the ros parameter server
defaults_collection = self._mongo_client.config.defaults
local_collection = self._mongo_client.config.local
for param in defaults_collection.find():
name=param["path"]
val=param["value"]
if local_collection.find_one({"path":name}) is None:
rospy.set_param(name,val)
for param in local_collection.find():
name=param["path"]
val=param["value"]
rospy.set_param(name,val)
# Advertise ros services for parameter setting / getting
self._getparam_srv = rospy.Service("/config_manager/get_param",
GetParam,
self._getparam_srv_cb)
self._setparam_srv = rospy.Service("/config_manager/set_param",
SetParam,
self._setparam_srv_cb)
self._saveparam_srv = rospy.Service("/config_manager/save_param",
SetParam,
self._saveparam_srv_cb)
#self._list_params()
# Start the main loop
rospy.spin()
def autolabel_and_set_position(self, subject, result):
print 'Now inserting position data and labeling results automatically according to where they stopped.'
paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')
fist_length = arm_length - rosparam.get_param('crook_to_wrist')
print 'Fist length: ', fist_length, ' cm'
position_of_initial_contact = (44.0 - arm_length)/100.
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
vel = 0.1
ft_threshold_was_exceeded = False
reference_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height3/ft_sleeve_0.log']))])
del_index = []
for k in xrange(len(reference_data)):
if reference_data[k][0] < 5.0:
del_index.append(k)
reference_data = np.delete(reference_data, del_index, 0)
del_index = []
for num, j in enumerate(reference_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
success_position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
print 'The stop position when the sleeve reaches the elbow is: ', success_position_of_stop
position_profile = None
for ind_i in xrange(len(result)):
for ind_j in xrange(len(result[0])):
if result[ind_i][ind_j] is not None:
if ind_i < len(result)/2:
load_num = ind_i
vel = 0.1
else:
load_num = ind_i - len(result)/2
vel = 0.15
if vel == 0.1:
# print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
# print 'Position profile loaded!'
elif vel == 0.15:
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']))
# print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])
# print 'Position profile loaded!'
else:
print 'There is no saved position profile for this velocity! Something has gone wrong!'
return None
ft_threshold_was_exceeded = False
current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))])
del_index = []
for k in xrange(len(current_data)):
if current_data[k][0] < 5.0:
del_index.append(k)
current_data = np.delete(current_data, del_index, 0)
position_of_stop = 0.
del_index = []
time_of_initial_contact = None
for num, j in enumerate(current_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8:
del_index.append(num)
elif new_position < position_of_initial_contact:
del_index.append(num)
else:
if time_of_initial_contact is None:
time_of_initial_contact = j[0]
position_of_stop = new_position
# if num > 8 and 0.05 < new_position < 0.4 and position_of_initial_contact is None:
# prev_f_x_average = np.mean(current_data[num-7:num,2])
# curr_f_x_average = np.mean(current_data[num-6:num+1,2])
# prev_f_z_average = np.mean(current_data[num-7:num,4])
# curr_f_z_average = np.mean(current_data[num-6:num+1,4])
# if curr_f_x_average < -0.08 and curr_f_x_average < prev_f_x_average and curr_f_z_average < prev_f_z_average and curr_f_z_average < -0.03:
# position_of_initial_contact = copy.copy(new_position)
# if position_of_initial_contact is None:
# print 'Position of initial contact was not detected for ', subject, ' and trial ', ind_j, 'at height ', ind_i
# position_of_initial_contact = 0.
# # return
# else:
# print 'Position of initial contact is: ', position_of_initial_contact
# current_data = np.delete(current_data, del_index, 0)
# del_index = []
# for i in xrange(len(current_data)):
# if current_data[i, 1] < position_of_initial_contact:
# del_index.append(i)
current_data = np.delete(current_data, del_index, 0)
output_data = []
for item in current_data:
output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]])
output_data = np.array(output_data)
print 'This trial failed at: ', position_of_stop
this_label = None
if not ft_threshold_was_exceeded:
this_label = 'missed'
elif abs(position_of_stop - success_position_of_stop) < 0.015 or position_of_stop >= success_position_of_stop:
this_label = 'good'
else:
if abs(position_of_stop - success_position_of_stop) + fist_length/100. + 0.05 >= arm_length/100.:
this_label = 'caught_fist'
else:
this_label = 'caught_other'
save_number = 0
save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl'])
while os.path.isfile(save_file):
save_number += 1
save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl'])
print 'Saving with file name', save_file
save_pickle(output_data, save_file)
print 'Done editing files!'
def setUp(self):
sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper')
self.thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0]
s = list(thruster_port.online_thruster_names)
fprint(msg.reset.text('\n\tName: ').set_yellow.text(port).reset
.text('\n\tMotor id\'s on port: ').set_cyan.bold(s).reset)
except SubjuGatorException:
pass
return port_dict
rospy.init_node('thruster_spinner')
# Connect to thrusters
sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper')
thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0]
thruster_ports = ports_from_layout(thruster_layout)
if thruster_ports == {}:
fprint('Unable to connect to any thruster ports. Quitting.')
sys.exit()
help_msg = \
'''
Welcome to David's ThrusterSpinner.\n
Help:
* press 'i' to toggle Individual mode on or off
+ Individual mode allows setting thrust values for individual thrusters:
- press a motor_id to select a thruster
- press up or down to change the thrust for that specific thruster
- press '*' to clear reset all thrusts to 0
+ Having Individual mode off allows all thrusters to be commanded with a single thrust value:
print "\nWelcome to David's M5 Thruster Shell\n"
import rospy
print 'Imported rospy'
import sub8_thruster_comm.thruster_comm as tc
print 'Imported sub8_thruster_comm.thruster_comm as tc'
import sub8_thruster_comm.protocol as tp
print 'Imported sub8_thruster_comm.protocol as tp'
import rosparam
print 'Imported rosparam'
import numpy as np
print 'Imported numpy as np'
import os
print 'Imported os'
rospy.init_node('m5_debug_shell')
rosparam.load_file(os.environ['CATKIN_DIR'] + '/src/SubjuGator/gnc/sub8_thruster_mapper/config/thruster_layout.yaml')[0][0]
layout = rosparam.load_file(os.environ['CATKIN_DIR'] + '/src/SubjuGator/gnc/sub8_thruster_mapper/config/thruster_layout.yaml')[0][0]
port_defs, thruster_defs = layout['thruster_ports'], layout['thrusters']
print "Port definitions and thruster definitions available as 'port_defs' and 'thruster_defs' respectively"
tports = []
for i in range(4):
t = None
try:
t = tc.ThrusterPort(port_defs[i], thruster_defs)
except tc.VRCSRException as e:
print e
tports.append(t)
t0, t1, t2, t3 = tports
