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 export_to_file(self):
doc = {}
if self.error_outside_testblock:
doc["error"] = "An error occured outside monitored testblocks. Aborted analysis..."
else:
for item in self.testblocks:
name = item.testblock_name
test_status = TestStatus()
test_status.test_name = self.test_name
test_status.status_analysing = 1
testblock_status = TestblockStatus()
testblock_status.name = item.testblock_name
testblock_status.status = item.get_state()
test_status.testblock.append(testblock_status)
test_status.total = self.number_of_tests
self.test_status_publisher.publish(test_status)
if name in self.testblock_error:
doc.update({name: {"status": "error"}})
else:
for metric in item.metrics:
result = metric.get_result()
if result is not False:
(m, data) = result
if name not in doc:
doc.update({name: {m: data}})
else:
if m not in doc[name]:
doc[name].update({m: data})
else:
doc[name][m].update(data)
else:
item.exit()
break
test_status = TestStatus()
test_status.test_name = self.test_name
test_status.status_analysing = 3
test_status.total = self.number_of_tests
self.test_status_publisher.publish(test_status)
filename = rosparam.get_param("/analysing/result_json_output") + self.test_name + ".json"
stream = file(filename, 'w')
json.dump(copy(doc), stream)
filename = rosparam.get_param("/analysing/result_yaml_output") + self.test_name + ".yaml"
if not filename == "":
stream = file(filename, 'w')
yaml.dump(doc, stream, default_flow_style=False)
def __init__(self):
self.ns = "/atf/"
self.parsing_error_message = ""
# get full config from parameter server (test_config = list of all test configs)
self.config = rosparam.get_param(self.ns)
def __init__(self):
params = rosparam.get_param("/arm_pose_move")
self.pose_dict = params['poses']
self.traj_dict = params['trajectories']
self.apm_ctrl = TrajPlayback(CTRL_NAME_LOW, PARAMS_FILE_LOW)
rospy.Subscriber(HEARTBEAT_TOPIC, Bool, self.heartbeat_cb)
rospy.Subscriber(COMMAND_TOPIC, ArmPoseMoveCmd, self.cmd_cb)
def publish_transform():
optitrak_params = rosparam.get_param("optitrak_calibration")
remap_mat = PoseConverter.to_homo_mat(optitrak_params["position"], optitrak_params["orientation"]) ** -1
tf_list = tf.TransformListener()
tf_broad = tf.TransformBroadcaster()
small_dur = rospy.Duration(0.001)
robot_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
opti_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
while not rospy.is_shutdown():
try:
now = rospy.Time(0.0)
(opti_pos, opti_rot) = tf_list.lookupTransform("/optitrak", "/pr2_antenna", now)
opti_mat = PoseConverter.to_homo_mat(opti_pos, opti_rot)
now = rospy.Time(0.0)
(robot_pos, robot_rot) = tf_list.lookupTransform("/wide_stereo_link", "/base_footprint", now)
robot_mat = PoseConverter.to_homo_mat(robot_pos, robot_rot)
odom_mat = opti_mat * remap_mat * robot_mat
odom_pos, odom_rot = PoseConverter.to_pos_quat(odom_mat)
tf_broad.sendTransform(odom_pos, odom_rot, rospy.Time.now(), "/base_footprint", "/optitrak")
rospy.sleep(0.001)
except Exception as e:
print e
def create_test_list(self):
if not rosparam.get_param("/analysing/result_yaml_output") == "":
if not os.path.exists(rosparam.get_param("/analysing/result_yaml_output")):
os.makedirs(rosparam.get_param("/analysing/result_yaml_output"))
if not os.path.exists(rosparam.get_param("/analysing/result_json_output")):
os.makedirs(rosparam.get_param("/analysing/result_json_output"))
test_config_path = rosparam.get_param("/analysing/test_config_file")
config_data = self.load_data(test_config_path)[rosparam.get_param("/analysing/test_config")]
metrics_data = self.load_data(rospkg.RosPack().get_path("atf_metrics") + "/config/metrics.yaml")
testblock_list = []
for testblock_name in config_data:
metrics = []
for metric in config_data[testblock_name]:
metric_return = getattr(atf_metrics, metrics_data[metric]["handler"])() \
.parse_parameter(config_data[testblock_name][metric])
if type(metric_return) == list:
for value in metric_return:
metrics.append(value)
else:
metrics.append(metric_return)
testblock_list.append(Testblock(testblock_name, metrics))
return testblock_list
def __init__(self):
self.bag_name = rosparam.get_param("/test_name")
self.number_of_tests = rosparam.get_param("/number_of_tests")
self.robot_config_file = self.load_data(rosparam.get_param("/robot_config"))
if not os.path.exists(rosparam.get_param(rospy.get_name() + "/bagfile_output")):
os.makedirs(rosparam.get_param(rospy.get_name() + "/bagfile_output"))
self.topic = "/atf/"
self.lock_write = Lock()
self.bag = rosbag.Bag(rosparam.get_param(rospy.get_name() + "/bagfile_output") + self.bag_name + ".bag", 'w')
self.test_config = self.load_data(rosparam.get_param(rospy.get_name() + "/test_config_file")
)[rosparam.get_param("/test_config")]
recorder_config = self.load_data(rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
self.BfW = BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
for (key, value) in recorder_config.iteritems():
self.recorder_plugin_list.append(getattr(atf_recorder_plugins, value)(self.topic,
self.test_config,
self.robot_config_file,
self.lock_write,
self.bag))
self.topic_pipeline = []
self.active_sections = []
self.requested_topics = []
self.testblock_list = self.create_testblock_list()
for topic in self.get_topics():
msg_type = rostopic.get_topic_class(topic, blocking=True)[0]
rospy.Subscriber(topic, msg_type, self.global_topic_callback, queue_size=5, callback_args=topic)
self.test_status_publisher = rospy.Publisher(self.topic + "test_status", TestStatus, queue_size=10)
rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
# Wait for subscriber
num_subscriber = self.test_status_publisher.get_num_connections()
while num_subscriber == 0:
num_subscriber = self.test_status_publisher.get_num_connections()
test_status = TestStatus()
test_status.test_name = self.bag_name
test_status.status_recording = 1
test_status.status_analysing = 0
test_status.total = self.number_of_tests
self.test_status_publisher.publish(test_status)
rospy.loginfo(rospy.get_name() + ": Node started!")
def __init__(self):
super(ArmPoseMoveGUIFrame, self).__init__()
params = rosparam.get_param("/arm_pose_move")
self.pose_dict = params['poses']
self.traj_dict = params['trajectories']
self.heartbeat_pub = rospy.Publisher(HEARTBEAT_TOPIC, Bool)
self.cmd_pub = rospy.Publisher(COMMAND_TOPIC, ArmPoseMoveCmd)
self.init_ui()
def get(self, node):
print(self.current_user)
user_data = self.get_current_user()
if self.current_user:
try:
params = rosparam.get_param("/arom/node/"+node)
self.render("../template/node.hbs", user_data=user_data, NavItems=[], NodeParams = params, FeatureParams = None)
except Exception as e:
self.render("../template/nonode.hbs", user_data=user_data, NavItems=[], NodeParams = None, FeatureParams = None)
else:
self.render("../template/loggedout.hbs")
def __init__(self, testblocks):
self.testblocks = testblocks
self.error = False
self.error_outside_testblock = False
self.testblock_error = {}
self.test_name = rosparam.get_param("/analysing/test_name")
self.number_of_tests = rosparam.get_param("/number_of_tests")
self.test_status_publisher = rospy.Publisher("atf/test_status", TestStatus, queue_size=10)
# Wait for subscriber
num_subscriber = self.test_status_publisher.get_num_connections()
while num_subscriber == 0:
num_subscriber = self.test_status_publisher.get_num_connections()
test_status = TestStatus()
test_status.test_name = self.test_name
test_status.status_analysing = 1
test_status.total = self.number_of_tests
self.test_status_publisher.publish(test_status)
def __init__(self):
self.test_status_list = rosparam.get_param("status_list")
self.yaml_lock = Lock()
sub = rospy.Subscriber("/atf/test_status", TestStatus, self.status_update_callback, queue_size=10)
# Wait for publisher
num_subscriber = sub.get_num_connections()
while num_subscriber == 0:
num_subscriber = sub.get_num_connections()
rospy.Service("atf/get_test_status", GetTestStatus, self.status_service_callback)
rospy.loginfo("ATF server started!")
def refresh_loadable_ctrlers(self):
if self.cm_namespace_combo.count() == 0:
# no controller managers found so there are no loadable controllers
# remove old loadables
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
return
ctrlman_ns = self.cm_namespace_combo.currentText()
if self.ctrlman_ns_cur != ctrlman_ns:
# new controller manager selected
# remove old loadables from list from last CM
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
rospy.wait_for_service(ctrlman_ns + '/controller_manager/list_controller_types', 0.2)
try:
resp = self.list_types[ctrlman_ns].call(ListControllerTypesRequest())
except rospy.ServiceException as e:
# TODO: display warning somehow
return
ctrler_types = resp.types
loadable_params_cur = []
all_params = rosparam.list_params('/')
# for every parameter
for pname in all_params:
# remove the controller manager namespace
if ctrlman_ns == '/':
pname_sub = pname
else:
pname_sub = pname[len(ctrlman_ns):]
psplit = pname_sub.split('/')
if len(psplit) > 2 and psplit[2] == 'type':
loadable_type = rosparam.get_param(pname)
if loadable_type in ctrler_types:
load_text = pname[:-5] + ' - ' + loadable_type
loadable_params_cur.append(load_text)
if load_text not in self.loadable_params:
self.loadable_params[load_text] = psplit[1]
self.load_ctrl_combo.addItem(load_text)
# remove loadable parameters no longer in the parameter server
for load_text_old in self.loadable_params.keys():
if load_text_old not in loadable_params_cur:
self.remove_loadable_from_list(load_text_old)
def get_params(ns):
"""Get the params of each POI and it's location"""
pois_dict = rosparam.get_param(ns)
# Looks like:
# {'numberOfSubMaps': 1,
# 'poi': {'submap_0': {'avoid_that': ['submap_0',
# 'avoid_that',
# -7.298,
# 5.911,
# -2.252],
# 'fetch_and_carry': ['submap_0', 'fetch_and_carry', -2.0, -2.0, 0],
# ... etc, it has submembers called poi, numberOfSubMaps and vo, at least, using mmap
poi_dicts_to_remove = []
for poi_dict_name in pois_dict:
if poi_dict_name != "poi": # and poi_dict_name != "vo": # we only want the pois on a subspace called poi or vo # we dont really want vo
poi_dicts_to_remove.append(poi_dict_name)
for poi_dict_name in poi_dicts_to_remove:
pois_dict.pop(poi_dict_name)
return pois_dict
def get_robot_description_ns_list():
result = []
for param in rosparam.list_params('/'):
value = rosparam.get_param(param)
if not isinstance(value, str):
continue
if '<?xml' not in value:
continue
try:
assert etree.fromstring(value).xpath('/robot') != None
except:
traceback.print_exc()
continue
result.append(param)
return result
def __init__(self, bot_name):
# bot name
self.name = bot_name
self.side_color = rosparam.get_param("/searchRun/side")
# robot state
"""
PENDING=0 ACTIVE=1 PREEMPTED=2 SUCCEEDED=3 ABORTED=4
REJECTED=5 PREEMPTING=6 RECALLING=7 RECALLED=8 LOST=9
"""
self.move_base_state = 0
self.fUpdateRoute = False
# robot pose
self.pose_x = 0
self.pose_y = 0
self.th = 0
# speed [m/s]
self.speed = 0.20
self.target_markers = []
self.current_target_marker = None
self.detected_player_markers = []
self.scan = []
# EnemyDetector
self.enemy_detector = EnemyDetector()
self.enemy_detect_timer = 0.0
# actionlib
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# publisher
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
# subscriber
self.pose_sub = rospy.Subscriber('amcl_pose',
PoseWithCovarianceStamped,
self.poseCallback)
self.war_state_sub = rospy.Subscriber('war_state', String,
self.warstateCallback)
self.lidar_sub = rospy.Subscriber('scan', LaserScan,
self.lidarCallback)
def get_params(ns):
"""Get the params of each POI and it's location"""
pois_dict = rosparam.get_param(ns)
# Looks like:
# {'numberOfSubMaps': 1,
# 'poi': {'submap_0': {'avoid_that': ['submap_0',
# 'avoid_that',
# -7.298,
# 5.911,
# -2.252],
# 'fetch_and_carry': ['submap_0', 'fetch_and_carry', -2.0, -2.0, 0],
# ... etc, it has submembers called poi, numberOfSubMaps and vo, at least, using mmap
poi_dicts_to_remove = []
for poi_dict_name in pois_dict:
if poi_dict_name != "poi": # and poi_dict_name != "vo": # we only want the pois on a subspace called poi or vo # we dont really want vo
poi_dicts_to_remove.append(poi_dict_name)
for poi_dict_name in poi_dicts_to_remove:
pois_dict.pop(poi_dict_name)
return pois_dict
def get(self, node):
print(self.current_user)
user_data = self.get_current_user()
if self.current_user:
try:
params = rosparam.get_param("/arom/node/" + node)
self.render("../template/node.hbs",
user_data=user_data,
NavItems=[],
NodeParams=params,
FeatureParams=None)
except Exception as e:
self.render("../template/nonode.hbs",
user_data=user_data,
NavItems=[],
NodeParams=None,
FeatureParams=None)
else:
self.render("../template/loggedout.hbs")
def __init__(self):
rospy.loginfo("start init")
self.loop_rate = rospy.Rate(1)
self.h_idx = np.zeros(1)
self.publisher = rospy.Publisher('hypothesis', String, queue_size=1)
self.TRAIN_DATA_FILES = [
'dead_end', 'left', 'right', 'straight', 'threeway_left',
'threeway_center', 'threeway_right'
]
NUM_CLASSES = len(self.TRAIN_DATA_FILES)
self.MAX_LASER_DISTANCE = 30.0
rosparam.set_param(
"model_full_path",
"/home/docker/catkin_ws/src/intersection_ws/model/NN.h5")
model_full_path = rosparam.get_param("tensorflow/model_full_path")
self.model = keras.models.load_model(model_full_path)
rospy.loginfo("finish init")
def handle_rwt_rosparam_marc_load(req):
paramResponse = []
paramList = []
ns = ''
for doc in yaml.load_all(req.params):
paramList.append((doc, ns))
for params, ns in paramList:
rosparam.upload_params(ns, params)
# TODO return test
tree = rosparam.get_param(script_resolve_name('rosparam', ns))
dump = yaml.dump(tree)
if dump.endswith('\n...\n'):
dump = dump[:-5]
paramResponse.append("%s\n" % (dump))
return LoadParamsResponse(paramResponse)
def processing(msg):
move_flag = rosparam.get_param("/gpsr/do_move")
if not msg:
rospy.logerr("GPSR : Move -> No destination")
# command_analyzerで既に登録してあるlocationのリストを読み込む
location_list = rosparam.load_file(
"/home/sobit-x3/catkin_ws/src/command_analyzer/dataForGPSR/location_list_gpsr.yaml"
)
rospy.wait_for_service('/waypoint_nav/move_ctrl')
if move_flag == False:
rospy.logwarn("GPSR : skipped moving")
return "SUCCESS"
else:
# dynamixelを動かすnodeへ角度をpub 2048が並行 2200が斜め下を向く
pub_joint_pose = rospy.Publisher("/joint_goal",
dynamixel_pos_id,
queue_size=10)
joint = dynamixel_pos_id()
joint.position = 2400
joint.id = 3
#print joint
rospy.sleep(1)
pub_joint_pose.publish(joint)
try:
move_ctrl = rospy.ServiceProxy('/waypoint_nav/move_ctrl',
waypoint_nav)
location_name = String()
location_pose = Pose()
flag = move_ctrl(msg, location_pose)
return flag.result_text
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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 handler(self,data,args):
second = time.time()
#second = rospy.get_time()
self.avg_array[args]= [second-self.first]+self.avg_array[args]
N = len(self.avg_array[args])
# np.convolve used for calculating moving average of hertz
freq = 1/(np.convolve(self.avg_array[args], np.ones((N,))/N, mode='valid'))[0]
# starts evaluating HZ once 'avg_array' is completely full.
if self.avg_array[args][-1] != 0.0:
desired_parameter = rosparam.get_param(self.topic_names[args]+'/timeSync')
if freq <= desired_parameter-0.1*desired_parameter or freq >= desired_parameter+0.1*desired_parameter:
print("topic: '"+str(self.topic_names[args])+"' delayed")
else:
print("on time")
self.avg_array[args].pop()
self.first = second
def __init__(self, label):
client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
rate = rospy.Rate(1)
goal=MoveBaseGoal()
goal_tmp = Pose()
self._ctrl_c = False
rospy.on_shutdown(self.shutdownhook)
tag = label
while not self._ctrl_c:
goal_tmp.position.x=rosparam.get_param(tag+'/position/x')
goal_tmp.position.y=rosparam.get_param(tag+'/position/y')
goal_tmp.position.z=rosparam.get_param(tag+'/position/z')
goal_tmp.orientation.x=rosparam.get_param(tag+'/orientation/x')
goal_tmp.orientation.y=rosparam.get_param(tag+'/orientation/y')
goal_tmp.orientation.z=rosparam.get_param(tag+'/orientation/z')
goal_tmp.orientation.w=rosparam.get_param(tag+'/orientation/w')
goal.target_pose.pose=goal_tmp
goal.target_pose.header.frame_id='odom'
client.wait_for_server()
rospy.loginfo('Going to spot='+str(label))
rospy.loginfo('sending goal')
client.send_goal(goal, feedback_cb=self.callback)
rospy.loginfo('waiting for result')
client.wait_for_result()
rospy.loginfo('get state')
result=client.get_state()
#print result
if result==3:
print('successfuly reached point '+str(label))
self.shutdownhook()
def speechCallback(self, message):
rospy.loginfo("(-O-) Task start (-O-)")
# initialize
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = rosparam.get_param("/controller_joint_names")
# start_state.name = rosparam.get_param("/sia5_controller/joints")
for i in range(len(start_state.name)):
start_state.position.append(0.)
get_num_from_pepper = int(message.data)
# do the planning depending on the order number
if get_num_from_pepper == 99:
rospy.loginfo("Called order 99")
trans = []
box_num = self.initial_box_num
rospy.loginfo("%d objects detected...", box_num)
for x in xrange(1, box_num + 1):
trans.append(self.get_tf_data(x))
for x in xrange(0, box_num):
state = self.run(1, (x + 1) % 2, start_state, trans[x])
if rospy.is_shutdown():
rospy.on_shutdown(self.shutdown)
break
rospy.loginfo("No.%i task finished.", x + 1)
start_state = state
rospy.loginfo("(^O^) All task finished (^O^)")
else:
object_num = get_num_from_pepper / 10
box_num = get_num_from_pepper % 10 - 1
trans = self.get_tf_data(object_num)
self.run(object_num, box_num, start_state, trans)
if rospy.is_shutdown():
rospy.on_shutdown(self.shutdown)
rospy.loginfo("(^O^) All task finished (^O^)")
def speechCallback(self, message):
rospy.loginfo("(-O-) Task start (-O-)")
# initialize
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = rosparam.get_param("/controller_joint_names")
# start_state.name = rosparam.get_param("/sia5_controller/joints")
for i in range(len(start_state.name)):
start_state.position.append(0.)
# do the planning depending on the order number
object_num = message.num
box_num = 1
object_trans = self.get_object_tf_data(object_num)
moving_x = message.xm
moving_y = message.ym
goal_trans = self.get_goal_tf_data(object_trans, moving_x,
moving_y) #offset x, y
if goal_trans is False:
return
print
print "target: " + message.tag
print "↓ offset_x[m] : " + str(moving_x)
print "→ offse_ty[m] : " + str(moving_y)
# print "input key A to continue."
# while(1):
# key = raw_input('>>> ')
# if key == "a":
# break
self.run(object_num, box_num, start_state, object_trans, goal_trans)
if rospy.is_shutdown():
rospy.on_shutdown(self.shutdown)
rospy.loginfo("(^O^) All task finished (^O^)")
def setup_kinematic_server():
"""
Setup a unity TCP server which proxy the messages from and to Unity.
It automatically setup a list of proxies for kinematic relevant ROS topics.
/joint_states is used as the default joint configuration publisher for all controllers
For each controller which is configured in the unity_moveit_controller_manager
it sets up a controller topic with the following path
/move_group/unity_trajectory/<CONTROLLER_NAME>
This script is intended to work with unity_moveit_manager.cpp
"""
ros_node_name = rospy.get_param(TCP_NODE_NAME_PARAM, 'TCPServer')
buffer_size = rospy.get_param(TCP_BUFFER_SIZE_PARAM, 1024)
connections = rospy.get_param(TCP_CONNECTIONS_PARAM, 10)
tcp_server = TcpServer(ros_node_name, buffer_size, connections)
rospy.init_node(ros_node_name, anonymous=True, log_level=rospy.INFO)
rospy.loginfo('Advertising /joint_states for the joint configuration')
topics = {
'joint_states': RosPublisher(ROS_JOINT_STATES_TOPIC, JointState),
}
for controller in rosparam.get_param(UNITY_CONTROLLER_LIST_PARAM):
controller_name = controller['name']
topics[controller_name] = RosSubscriber(UNITY_CONTROLLER_BASE + controller_name,
JointTrajectory,
tcp_server)
rospy.loginfo(f'Listening to {UNITY_CONTROLLER_BASE + controller_name} '
f'for the trajectory execution')
tcp_server.start(topics)
rospy.spin()
def init_communications(self):
self.publisher_to_egos = ar.get.publisher(
topic_name=get_param("TOPIC_PC"), data_class=msg.PerceptionConcept)
self.subscriber_to_devices = ar.get.subscriber(
topic_name=get_param("TOPIC_DEV_DATA"),
data_class=msg.DeviceData,
callback=self.device_data_handler)
self.subscriber_to_emotion_params = ar.get.subscriber(
topic_name=get_param("TOPIC_EPARAM"),
data_class=msg.EmotionParams,
callback=self.handler_emotion_params)
self.client_of_emotion_core_write = ar.get.client(
srv_name=get_param("SRV_ECORE_W"), service=srv.EmotionCoreWrite)
self.client_of_emotion_data_descriptor = ar.get.client(
srv_name=get_param("SRV_ECORE_DDSC"),
service=srv.EmotionCoreDataDescriptor)
self.server_of_perception_concept_dsc = ar.get.server(
name=get_param("SRV_D2C_PCDSC"),
service=srv.PerceptionConceptDescriptor,
handler=self.handler_perception_concetps_dsc)
def __init__(self):
self.image_pub = rospy.Publisher("debug_image",Image, queue_size=1)
self.map_pub = rospy.Publisher("world_lane",Image, queue_size=1)
self.object_pub = rospy.Publisher("objects", Detection2DArray, queue_size=1)
self.loc_pub = rospy.Publisher("vehicle_loc",Lanepoints, queue_size=1)
self.bridge = CvBridge()
# camera parameters
# rosparam.load_file('/home/aj/Desktop/barc_calibrationdata/ost.yaml')
self.cam_mtx = rosparam.get_param('camera_matrix/data')
self.cam_rows = rosparam.get_param('camera_matrix/rows')
self.cam_cols = rosparam.get_param('camera_matrix/cols')
self.cam_mtx = np.array(np.array(self.cam_mtx).reshape(self.cam_rows, self.cam_cols))
self.dst_mtx = rosparam.get_param('distortion_coefficients/data')
(self.dst_rows, self.dst_cols) = (rosparam.get_param('distortion_coefficients/rows'),
rosparam.get_param('distortion_coefficients/cols'))
self.dst_mtx = np.array(np.array(self.dst_mtx).reshape(self.dst_rows, self.dst_cols))
# ~camera paramters
self.image_sub = rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage,
self.image_cb, queue_size=1, buff_size=2**24)
self.sess = tf.Session(graph=detection_graph,config=config)
# intrinsic matrix is transposed as monocular class expects a transposed matrix
# monocular class can be modified to make the matrix transposed
self.m = mono.Monocular(self.cam_mtx.T,
1.06, 0.0, 0.0, 0.0, np.array([0.0, 0.0])) # (0.0762, 2.0) is height,pitch for barc real time
def lobby():
"""
Lobby.
@return next_room: int
"""
# start = time.time()
def read_clue():
# stitchImage = stitch_image.StitchImage()
# stitchImage.DEBUG = True
# stitchImage.preStitch()
# stitchImage.stitch()
print("=== Report === Read from png")
readClue = clue.ReadClue()
next_room = readClue.lobby() # replace this line
return next_room
next_room = read_clue()
while input("\n\nThe clue reads {}. Is this correct?\n".format(
next_room)) == "n":
if input(
"\n\nWould you like to attempt to use opencv to read the clue again\n?"
) == "y":
next_room = read_clue()
else:
print("\n\nManually obtaining the clue from the parameter server.")
next_room = rosparam.get_param("/competition2_server/shapes_room")
print("Shapes room is room {}.".format(next_room))
# TODO read map
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
numbered_locations = read_map()
print("\n\nroom assignments:")
print(numbered_locations, "\n")
while input("\nAre the numbered room assignments correct?\n") == "n":
if input(
"\n\nWould you like to attempt to use opencv to read the map again?\n"
) == "y":
numbered_locations = read_map()
else:
input_locations = input(
"\n\nManually enter the locations. Please enter them in the from '1a,2b,3c,4d,...', with no spacing.\n"
)
input_locations = input_locations.split(",")
numbered_locations = {}
for location in input_locations:
if location == "0lobby":
number = 0
letter = "lobby"
else:
number = int(location[:-1])
letter = location[-1]
numbered_locations[number] = letter
print("\n\nroom assignments:")
print(numbered_locations, "\n")
# save to file
with open(
str(Path.home()) +
"/catkin_ws/src/competition2/yaml/numbered_locations.yaml",
"w") as f:
yaml.dump(numbered_locations, f)
# reload yaml into send_goal_client class
send_goal_client.numbered_locations = numbered_locations
# determine next room using clue
numbered_locations_keys = numbered_locations.keys()
if next_room == "highest":
next_room = max(numbered_locations)
elif next_room == "lowest":
next_room = min(numbered_locations)
# end = time.time()
# completion_time = str(datetime.timedelta(seconds=(end - start))).split(".")[0]
# print("\n\ntime to complete lobby: {} h:mm:ss\n".format(completion_time))
return next_room
filename_user = '******' + str(n_user) + '.yaml'
filename_robot = 'robot.yaml'
pathfile_user = path_data + filename_user
pathfile_robot = path_data + filename_robot
try:
stream_user = file(pathfile_user, 'r')
stream_robot = file(pathfile_robot, 'r')
except IOError as e:
rospy.logerr(e)
user = yaml.load(stream_user)
robot = yaml.load(stream_robot)
# Context
context = {'user': user, 'robot': robot}
# Set context
rospy.set_param('context', context)
# Print parameters
print('Context:')
print(rosparam.get_param('context'))
print('')
print(type(rospy.get_param('context/user/birth_date')))
print('')
print(rospy.get_param('context/user/surname'))
print('')
except rospy.ROSInterruptException:
pass
waypoints = yaml.load(f_waypoint)
#書き込み用のグラフを用意する。
graph = nx.Graph()
GraphBilder = Graph_Bilde(graph)
#読み込んだwaypointをグラフのノードに追加する
for i, point in enumerate(waypoints["waypoints"]): #waypoint一覧を取り出す。
#ノード名：waypointの要素番号
GraphBilder.add_node_point(i, point)
#finishsposeを追加する。
GraphBilder.add_node_pose("finish_pose", waypoints["finish_pose"])
edge_list = yaml.load(f_edge)
for i in range(len(edge_list)):
GraphBilder.add_edge(edge_list[i][0], edge_list[i][1])
GraphFile.make_file(graph, graph_file_name)
f_waypoint.close()
if __name__ == "__main__":
rospy.init_node("waypoint_to_graph_node")
waypoint_file = rosparam.get_param("/graph_maker/waypoints_file")
edge_file = rosparam.get_param("/graph_maker/edge_file")
graph_file = rosparam.get_param("/graph_maker/graph_file")
test = a()
test.generate_graph_file(waypoint_file, edge_file, graph_file)
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!'
#!/usr/bin/env python
import rosparam
import rospy
import numpy as np
from sdpp_experiments.agent_action_client import AgentActionClient
if __name__ == '__main__':
#TODO (45) fully load configs from yamls
rospy.init_node("move_agents")
experiment_info = rosparam.get_param("experiment_info")
agent_list = []
for agent_info in experiment_info:
agent_list.append(AgentActionClient(**agent_info))
while not rospy.is_shutdown():
pass
import rospy
import rosparam
from cv2 import aruco
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from vision.ArucoDetector import ArucoDetector
from vision.Point import Point
from fields_objects.Robot import Robot
from fields_objects.Gate import Gate
from soccer.msg import RobotsOnField, Robot as Robot_msg, MarkersOnField,\
PathsList, AllGates as GatesMsg, Point2d, FinishPoint, GameStatus
from std_msgs.msg import Bool
robot_size = 40
field_w = rosparam.get_param("sector_width")
field_h = rosparam.get_param("sector_height")
dx = rosparam.get_param("sectors_x_crossing")
dy = rosparam.get_param("sectors_y_crossing")
out = rosparam.get_param("out")
robot_size = rosparam.get_param("robot_size")
path_eps = rosparam.get_param("path_eps")
angle_eps = rosparam.get_param("angle_eps")
marker_size = rosparam.get_param("marker_size")
from_center_to_front_side = rosparam.get_param("from_center_to_front_side")
from_center_to_back_side = rosparam.get_param("from_center_to_back_side")
from_center_to_left_side = rosparam.get_param("from_center_to_left_side")
from_center_to_right_side = rosparam.get_param("from_center_to_right_side")
@author: marianne
"""
#Simplified virtual camera
#For extracting reduced central FOV from camera with wide-FOV lens
import numpy as np
import rospy
import cv2
import rosparam
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from utils import recv_image_msg, disp_img, crop_img
import VirtualCam
image_topic_name = rosparam.get_param('virtualcam/camera_topic')
output_RPY = rosparam.get_param('virtualcam/RPY')
input_camera_model = rosparam.get_param(
'virtualcam/input_model'
) #(r"/home/marianne/catkin_ws/src/trailnet_on_thorvald/trail_net/scripts/input_cam_model_campus_2018-09-21.xml")
output_camera_model = rosparam.get_param(
'virtualcam/output_model'
) #(r"/home/marianne/catkin_ws/src/trailnet_on_thorvald/trail_net/scripts/output_cam_model.xml")
remap_image_dims = rosparam.get_param('virtualcam/output_image_dims')
#image_topic_name = "/image_folder_publisher/image" #"/pylon_camera_node/image_raw"
#Initialize node
rospy.init_node('virtualcam')
#Load camera models and set output angle
'''
VirtualCam.LoadCameraModels(
def __init__(self, *args, **kwargs):
agent_id = 1
all_names = rospy.get_param_names()
if '/agent_id' in all_names:
rospy.logwarn('retrieve agent_id from param')
agent_id = rospy.get_param('/agent_id')
rospy.logwarn('agent : %s' % agent_id)
# initialize super class
GoalSubscriber.__init__(self)
RobotSubscriber.__init__(
self, '/nubot' + str(agent_id) + '/omnivision/OmniVisionInfo',
agent_id)
### create nav path subscriber
self.sub = {
'pos': TrajectorySubscriber('robosoccer_trajectory_pos'),
'vel': TrajectorySubscriber('robosoccer_trajectory_vel')
}
self.sub['pos'].register_callback(self.trajectory_callback)
# self.pub = rospy.Publisher('/nubot'+str(agent_id)+'/nubotcontrol/velcmd', nubotmsg.VelCmd, queue_size=3)
self.pub = rospy.Publisher('/nubot' + str(agent_id) +
'/nubotcontrol/velcmd',
nubotmsg.VelCmd,
queue_size=3)
self.info_pub = rospy.Publisher('/tracker_info',
trackmsg.ControlInfo,
queue_size=3)
self.error = {
'x': .0,
'y': .0,
'w': .0,
'sum': {
# sum for compute integral part of control
'x': .0,
'y': .0,
'w': .0,
}
}
self.control = {
'x': .0,
'y': .0,
'w': .0,
'com': np.array([0., 0., 0.])
}
self.pid = {
# initial value, will be overwritten
'p': np.diag([1., 1., 1.]),
'i': np.diag([.05, .05, .05])
}
# characteristic polynomial for computing gain
## default value
poly = {'p': [.5, .5, .5], 'i': [.05, .05, .05]}
## check if we have in rosparam
params = rosparam.list_params(rospy.get_name())
rospy.loginfo('parameter in %s : %s' % (rospy.get_name(), params))
if any('char_poly' in x for x in params):
rospy.logwarn('loading char poly from param')
poly = rosparam.get_param(rospy.get_name() + '/char_poly')
rospy.logwarn('poly : %s' % poly)
try:
self.enable_tracking = rosparam.get_param(rospy.get_name() +
'/enable')
rospy.logwarn('tracking enable : %s' % self.enable_tracking)
except:
rospy.logwarn('tracking not set, default to false')
self.enable_tracking = False
self.char_poly = {
# some tuning parameter
# 'p' : np.diag([0., 0., 0.]),
# 'i' : np.diag([0., 0., 0.])
'p': np.diag(poly['p']) * -1.,
'i': np.diag(poly['i']) * -1.,
# 'p' : np.diag([-.5, -.5, -.5]),
# 'i' : np.diag([-.05, -.05, -.05])
}
# command (or reference if you like)
self.command = {
# 'pos' : {'x' : .0, 'y' : .0, 'w' : .0,},
# 'vel' : {'x' : .0, 'y' : .0, 'w' : .0,}
'pos': np.array([0., 0., 0.]),
'vel': np.array([0., 0., 0.]),
}
self.last_update = None
# this enable is for waiting first goal to be published
self.enable = False
# saturation
self.saturate = {
'enabled': True,
'saturated': False,
'limit': {
'trans': 3.,
'rot': 1.5
}
}
self.ref_time = rospy.get_time()
self.ctrl_time = rospy.get_time()
rospy.logwarn('READY')
def get_joint_names( node_name ):
return rosparam.get_param( ''.join( (node_name, '/joints') ) )
goal = createHandGoal(side, thumb_joint, middle_joint, index_joint)
rospy.loginfo("Will move " + side + " hand to poses: " + str(thumb_joint) + " " + str(middle_joint) + " " + str(index_joint))
elif len(sys.argv) == 3:
if sys.argv[1].startswith('l'):
side = "left"
elif sys.argv[1].startswith('r'):
side = "right"
if sys.argv[2].startswith('p'): # pre-grasp position
pose_to_move = "pregrasp"
goal = createHandGoal(side, 1.5, 0.1, 0.1)
elif sys.argv[2].startswith('o'): # open position
pose_to_move = "open"
goal = createHandGoal(side, 0.1, 0.1, 0.1)
elif sys.argv[2].startswith('g') or sys.argv[2].startswith('c'): # close position
pose_to_move = "closed"
if rosparam.get_param('/use_sim_time'):
goal = createHandGoal(side, 1.5, 2.0, 2.0)
else:
goal = createHandGoal(side, 1.5, 4.0, 4.0)
rospy.loginfo("Will move " + side + " hand to pose: " + pose_to_move)
else:
rospy.loginfo("Incorrect number of parameters, usage:")
rospy.loginfo("move_reem_hands.py <right/left> <thumb_joint> <middle_joint> <index_joint> ")
rospy.loginfo("move_reem_hands.py <right/left> <open/pregrasp/closed> ")
rospy.loginfo('Also accepts shortcuts like "l = left", "r = right", "o = open", "c = closed", "p = pregrasp"')
exit(0)
hand_as = actionlib.SimpleActionClient('/' + side + '_hand_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
rospy.loginfo("Connecting to " + side + " hand AS...")
hand_as.wait_for_server()
rospy.sleep(1.0)
rospy.loginfo("Connected, sending goal.")
rospy.init_node('State_Publisher')
# Name of the joints in the robotic arm
# Name of motor ID [1,2,3,4,5,6] is specified in the list.
joints = [
"Rjoint0", "Rjoint1", "Rjoint2", "Rjoint3", "Rjoint4", "Rjoint5", "Rjoint6"
]
#Dynamixel Motors will not be in Position 0 when switched ON. Dynamixel motors will have some initial position value.
#In the URDF it is assumed that initial joint value is 0 radian.
#Hence offset is required to match the URDF and the real robot.
#Offset for motor id [10,11,13,14,15,16,17] is specified in the list.
start_ID = 10
# offset = [512,512,512,512,512,512,512]
j0_init = rosparam.get_param("/RArm/joint0_controller/motor/init")
j1_init = rosparam.get_param("/RArm/joint1_controller/motor_master/init")
j2_init = rosparam.get_param("/RArm/joint2_controller/motor/init")
j3_init = rosparam.get_param("/RArm/joint3_controller/motor/init")
j4_init = rosparam.get_param("/RArm/joint4_controller/motor/init")
j5_init = rosparam.get_param("/RArm/joint5_controller/motor/init")
j6_init = rosparam.get_param("/RArm/joint6_controller/motor/init")
offset = [j0_init, j1_init, j2_init, j3_init, j4_init, j5_init, j6_init]
print('R: ', offset)
# offset = [512,2047,512,512,512,512,512]
'''
Function: process(). Callback for subscriber of raw data from dynamixel motor.
Logic: Dynamixel position = 0 for 0 degree and Dynamixel position = 1023 for 300 degree.
Current position can be calculated by (position*(300.0/1023))*(pi/180) radian.
def main(argv):
rospy.init_node('csv_proc')
volt_values=[]
time_values=[]
sg = savitzky.savitzky_golay(window_size=901, order=3)
####################
# Parameters for the Python script
####################
filename = rosparam.get_param("/csv_proc/file_name")
robot_name = rosparam.get_param("/csv_proc/robot_name")
mode = rosparam.get_param("/csv_proc/mode")
yaml_file = open("voltage_filter.yaml", "w")
yl = {}
if(mode=="update"):
abcd = rosparam.get_param("/csv_proc/abcd")
try:
opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="])
except getopt.GetoptError:
print 'time_volt.py -f <filename> -r <robotname>'
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print 'time_volt.py -f <filename> -r <robotname>'
sys.exit()
elif opt in ("-f", "--ifile"):
filename = arg
elif opt in ("-r", "--irobot"):
robot_name = arg
####################
# Opening the csv file and getting the values for time and voltage
####################
with open(filename, 'rb') as csvfile:
csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|')
for row in csvreader:
row = row[0].split(',')
volt_v = (float)(row[1]) * 1000.0
if(volt_v < 48000 and volt_v > 44000):
time_values.append((float)(row[0]))
volt_values.append(volt_v)
time_values[:] = [x - time_values[0] for x in time_values]
time_values = time_values[::-1]
####################
# Plotting graphics for the Voltage vs Time
####################
pylab.figure(1)
pylab.plot(volt_values, time_values)
pylab.ylabel("Time elapsed(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
secArray = np.asarray(time_values)
voltArray = np.asarray(volt_values)
# Polyfit for the voltage values
z1 = np.polyfit(voltArray, secArray,1)
z2 = np.polyfit(voltArray, secArray,2)
z3 = np.polyfit(voltArray, secArray,3)
# Linear space for better visualization
xp = np.linspace(49000, 43000, 100)
# Generating the polynomial function from the fit
p1 = np.poly1d(z1)
p2 = np.poly1d(z2)
p3 = np.poly1d(z3)
pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')
pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))
pylab.savefig(filename.replace('.csv',''), format="pdf")
#pylab.show()
####################
# Residuals Analysis
####################
pylab.figure(2)
pylab.ylabel("Residuals(s)")
pylab.xlabel("Voltage(mV)")
pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))
#Evaluating the polynomial through all the volt values
z1_val = np.polyval(z1, volt_values)
z2_val = np.polyval(z2, volt_values)
z3_val = np.polyval(z3, volt_values)
# Getting the residuals from the fit functions compared to the real values
z1_res = time_values - z1_val
z2_res = time_values - z2_val
z3_res = time_values - z3_val
pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)
pylab.grid()
pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))
pylab.savefig(filename.replace('.csv','')+'_res', format="pdf")
###################
# Savitzky Golay Filter Applied to the Battery Voltage
###################
values_filt = sg.filter(voltArray)
pylab.figure(3)
pylab.subplot(211)
pylab.plot(voltArray, time_values)
pylab.grid(True)
pylab.title('Comparison between real and filtered data')
pylab.ylabel('Real Values(mV)')
pylab.subplot(212)
pylab.plot(values_filt, time_values)
pylab.grid(True)
pylab.ylabel('Filtered Values(mV)')
pylab.xlabel('Time(s)')
#####
###################
# Filtered fits
###################
pylab.figure(4)
pylab.plot(values_filt, time_values)
pylab.ylabel("Time elapsed(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
secArray = np.asarray(time_values)
# Polyfit for the voltage values
z1 = np.polyfit(values_filt, secArray,1)
z2 = np.polyfit(values_filt, secArray,2)
z3 = np.polyfit(values_filt, secArray,3)
if (mode=="initial"):
z3_l = []
for el in z3:
z3_l.append((float)(el))
abcd = z3_l
yl["abcd"] = z3_l
yl["theta"] = 0.
yl["off_y"] = 0.
# Linear space for better visualization
xp = np.linspace(49000, 43000, 100)
# Generating the polynomial function from the fit
p1 = np.poly1d(z1)
p2 = np.poly1d(z2)
p3 = np.poly1d(z3)
pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')
pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))
####################
# Residuals Analysis for the filtered Signal
####################
pylab.figure(5)
pylab.ylabel("Residuals(s)")
pylab.xlabel("Voltage(mV)")
pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))
#Evaluating the polynomial through all the time values
z1_val = np.polyval(z1, values_filt)
z2_val = np.polyval(z2, values_filt)
z3_val = np.polyval(z3, values_filt)
# Getting the residuals from the fit functions compared to the real values
z1_res = time_values - z1_val
z2_res = time_values - z2_val
z3_res = time_values - z3_val
pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)
pylab.grid()
pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))
####################
# Polynomial Evaluation for the filtered signal and the function from the non-moving case
####################
if(mode == "update"):
pylab.figure(6)
poly_vals = np.polyval(abcd, values_filt)
pylab.plot(values_filt, poly_vals, values_filt, time_values)
pylab.legend(('Polynomial', 'Real'))
pylab.grid()
#####
pylab.figure(7)
pylab.ylabel("Time available(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
poly_vals = np.polyval(abcd, values_filt)
ss = lambda y1, y2: ((y1-y2)**2).sum()
#theta = -0.07
#new_x = values_filt*cos(theta) - poly_vals*sin(theta)
#new_y = values_filt*sin(theta) + poly_vals*cos(theta)
theta = -0.2
theta_values = []
cost_values = []
off_values = []
while theta < 0.2:
theta +=0.01
new_x = values_filt*cos(theta) - poly_vals*sin(theta)
new_y = values_filt*sin(theta) + poly_vals*cos(theta)
off_y = -6000
cost_values_i =[]
off_y_values_i=[]
while off_y < 6000:
off_y +=200
new_y_temp = new_y
new_y_temp = new_y_temp + off_y
ss1=ss(time_values,new_y_temp)
print ss1, off_y
cost_values_i.append(ss1)
off_y_values_i.append(off_y)
#ss1=ss(time_values,new_y)
#print ss1, theta
#cost_values.append(ss1)
theta_values.append(theta)
cost_min = min(cost_values_i)
cost_min_index = cost_values_i.index(cost_min)
cost_values.append(cost_values_i[cost_min_index])
off_values.append(off_y_values_i[cost_min_index])
cost_min = min(cost_values)
cost_min_index = cost_values.index(cost_min)
theta = theta_values[cost_min_index]
off_y = off_values[cost_min_index]
new_x = values_filt*cos(theta) - poly_vals*sin(theta)
new_y = values_filt*sin(theta) + poly_vals*cos(theta)
new_y = new_y + off_y
yl["abcd"] = abcd
yl["theta"] = theta
yl["off_y"] = off_y
yl["maximum_time"] = (float)(new_y[0])
pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt)
pylab.legend(('Poly not moving', 'Real', 'Shifted Fit'))
yaml.safe_dump(yl, yaml_file,default_flow_style=False)
yaml_file.close()
pylab.show()
def main(argv):
rospy.init_node('csv_proc')
volt_values=[]
time_values=[]
sg = savitzky.savitzky_golay(window_size=901, order=3)
####################
# Parameters for the Python script
####################
filename = rosparam.get_param("/csv_proc/file_name")
robot_name = rosparam.get_param("/csv_proc/robot_name")
mode = rosparam.get_param("/csv_proc/mode")
yaml_file = open("voltage_filter.yaml", "w")
yl = {}
if(mode=="update"):
abcd = rosparam.get_param("/csv_proc/abcd")
try:
opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="])
except getopt.GetoptError:
print 'time_volt.py -f <filename> -r <robotname>'
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print 'time_volt.py -f <filename> -r <robotname>'
sys.exit()
elif opt in ("-f", "--ifile"):
filename = arg
elif opt in ("-r", "--irobot"):
robot_name = arg
####################
# Opening the csv file and getting the values for time and voltage
####################
with open(filename, 'rb') as csvfile:
csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|')
for row in csvreader:
row = row[0].split(',')
volt_v = (float)(row[1]) * 1000.0
if(volt_v < 48000 and volt_v > 44000):
time_values.append((float)(row[0]))
volt_values.append(volt_v)
time_values[:] = [x - time_values[0] for x in time_values]
time_values = time_values[::-1]
####################
# Plotting graphics for the Voltage vs Time
####################
pylab.figure(1)
pylab.plot(volt_values, time_values)
pylab.ylabel("Time elapsed(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
secArray = np.asarray(time_values)
voltArray = np.asarray(volt_values)
# Polyfit for the voltage values
z1 = np.polyfit(voltArray, secArray,1)
z2 = np.polyfit(voltArray, secArray,2)
z3 = np.polyfit(voltArray, secArray,3)
# Linear space for better visualization
xp = np.linspace(49000, 43000, 100)
# Generating the polynomial function from the fit
p1 = np.poly1d(z1)
p2 = np.poly1d(z2)
p3 = np.poly1d(z3)
pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')
pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))
pylab.savefig(filename.replace('.csv',''), format="pdf")
#pylab.show()
####################
# Residuals Analysis
####################
pylab.figure(2)
pylab.ylabel("Residuals(s)")
pylab.xlabel("Voltage(mV)")
pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))
#Evaluating the polynomial through all the volt values
z1_val = np.polyval(z1, volt_values)
z2_val = np.polyval(z2, volt_values)
z3_val = np.polyval(z3, volt_values)
# Getting the residuals from the fit functions compared to the real values
z1_res = time_values - z1_val
z2_res = time_values - z2_val
z3_res = time_values - z3_val
pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)
pylab.grid()
pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))
pylab.savefig(filename.replace('.csv','')+'_res', format="pdf")
###################
# Savitzky Golay Filter Applied to the Battery Voltage
###################
values_filt = sg.filter(voltArray)
pylab.figure(3)
pylab.subplot(211)
pylab.plot(voltArray, time_values)
pylab.grid(True)
pylab.title('Comparison between real and filtered data')
pylab.ylabel('Real Values(mV)')
pylab.subplot(212)
pylab.plot(values_filt, time_values)
pylab.grid(True)
pylab.ylabel('Filtered Values(mV)')
pylab.xlabel('Time(s)')
#####
###################
# Filtered fits
###################
pylab.figure(4)
pylab.plot(values_filt, time_values)
pylab.ylabel("Time elapsed(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
secArray = np.asarray(time_values)
# Polyfit for the voltage values
z1 = np.polyfit(values_filt, secArray,1)
z2 = np.polyfit(values_filt, secArray,2)
z3 = np.polyfit(values_filt, secArray,3)
if (mode=="initial"):
z3_l = []
for el in z3:
z3_l.append((float)(el))
abcd = z3_l
yl["abcd"] = z3_l
yl["theta"] = 0.
yl["off_y"] = 0.
# Linear space for better visualization
xp = np.linspace(49000, 43000, 100)
# Generating the polynomial function from the fit
p1 = np.poly1d(z1)
p2 = np.poly1d(z2)
p3 = np.poly1d(z3)
pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')
pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))
####################
# Residuals Analysis for the filtered Signal
####################
pylab.figure(5)
pylab.ylabel("Residuals(s)")
pylab.xlabel("Voltage(mV)")
pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))
#Evaluating the polynomial through all the time values
z1_val = np.polyval(z1, values_filt)
z2_val = np.polyval(z2, values_filt)
z3_val = np.polyval(z3, values_filt)
# Getting the residuals from the fit functions compared to the real values
z1_res = time_values - z1_val
z2_res = time_values - z2_val
z3_res = time_values - z3_val
pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)
pylab.grid()
pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))
####################
# Polynomial Evaluation for the filtered signal and the function from the non-moving case
####################
if(mode == "update"):
pylab.figure(6)
poly_vals = np.polyval(abcd, values_filt)
pylab.plot(values_filt, poly_vals, values_filt, time_values)
pylab.legend(('Polynomial', 'Real'))
pylab.grid()
#####
pylab.figure(7)
pylab.ylabel("Time available(seconds)")
pylab.xlabel("Voltage(mV)")
pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
pylab.grid(True)
poly_vals = np.polyval(abcd, values_filt)
ss = lambda y1, y2: ((y1-y2)**2).sum()
#theta = -0.07
#new_x = values_filt*cos(theta) - poly_vals*sin(theta)
#new_y = values_filt*sin(theta) + poly_vals*cos(theta)
theta = -0.2
theta_values = []
cost_values = []
off_values = []
while theta < 0.2:
theta +=0.01
new_x = values_filt*cos(theta) - poly_vals*sin(theta)
new_y = values_filt*sin(theta) + poly_vals*cos(theta)
off_y = -6000
cost_values_i =[]
off_y_values_i=[]
while off_y < 6000:
off_y +=200
new_y_temp = new_y
new_y_temp = new_y_temp + off_y
ss1=ss(time_values,new_y_temp)
print ss1, off_y
cost_values_i.append(ss1)
off_y_values_i.append(off_y)
#ss1=ss(time_values,new_y)
#print ss1, theta
#cost_values.append(ss1)
theta_values.append(theta)
cost_min = min(cost_values_i)
cost_min_index = cost_values_i.index(cost_min)
cost_values.append(cost_values_i[cost_min_index])
off_values.append(off_y_values_i[cost_min_index])
cost_min = min(cost_values)
cost_min_index = cost_values.index(cost_min)
theta = theta_values[cost_min_index]
off_y = off_values[cost_min_index]
new_x = values_filt*cos(theta) - poly_vals*sin(theta)
new_y = values_filt*sin(theta) + poly_vals*cos(theta)
new_y = new_y + off_y
yl["abcd"] = abcd
yl["theta"] = theta
yl["off_y"] = off_y
yl["maximum_time"] = (float)(new_y[0])
pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt)
pylab.legend(('Poly not moving', 'Real', 'Shifted Fit'))
yaml.safe_dump(yl, yaml_file,default_flow_style=False)
yaml_file.close()
pylab.show()
# parameters checking
if args.calibrate_only and not (
rospy.has_param("/reha_exercise/calibration_duration")
and rospy.has_param("/reha_exercise/camera_width")
and rospy.has_param("/reha_exercise/camera_height")
and rospy.has_param("/reha_exercise/robot_position")
and rospy.has_param("/reha_exercise/rgb_colors")
and rospy.has_param("/reha_exercise/motion_type")
and rospy.has_param("/reha_exercise/rotation_type")):
print(
"ERROR: not all required parameters are set on the parameter server for calibration! Aborting."
)
sys.exit(1)
else:
ros_motion_type = rosparam.get_param("/reha_exercise/motion_type")
ros_rotation_type = rosparam.get_param("/reha_exercise/rotation_type")
if not args.calibrate_only and not (
rospy.has_param("/reha_exercise/quantitative_frequency")
or rospy.has_param("/reha_exercise/qualitative_frequency")
or rospy.has_param("/reha_exercise/number_of_blocks") or
rospy.has_param("/reha_exercise/calibration_points_left_arm")
or
rospy.has_param("/reha_exercise/calibration_points_right_arm")
or rospy.has_param("/reha_exercise/emotional_feedback_list")):
print(
"ERROR: not all required parameters are set on the parameter server for the exercise! Aborting."
)
sys.exit(1)
elif not (ros_motion_type == 0 and ros_rotation_type > 0
or ros_motion_type > 0 and ros_rotation_type == 0):
def load_params(self, namespace):
if not rosparam.list_params(namespace):
print "> No data to load"
else:
data = rosparam.get_param(namespace)
self.load_data(data)
def __init__(self,
number_of_blocks=1,
repetitions_limit=10,
calibration_duration=10,
camera_resolution=(640, 480),
time_limit=0,
robot_position=RobotPosition.LEFT):
rospy.init_node('reha_exercise', anonymous=True)
self.repetitions_limit = repetitions_limit
self.calibration_duration = calibration_duration
if calibration_duration > 0:
self._calibration_points_left_arm = []
self._calibration_points_right_arm = []
self.camera_resolution = camera_resolution
self.limb = Limb.LEFT_ARM
self._number_of_blocks = number_of_blocks
self.robot_position = robot_position
self.time_limit = time_limit
self._session_timer = None
self._kill_session = False
temp_quant_freq = 0
temp_quali_freq = 0
temp_emotional_feedbacks = []
# define behaviour when SIGINT or SIGTERM received
signal.signal(signal.SIGINT, self.exit_gracefully)
signal.signal(signal.SIGTERM, self.exit_gracefully)
# retrieve settings from parameter server
if rospy.has_param('/reha_exercise/camera_width') and rospy.has_param(
'/reha_exercise/camera_height'):
self.camera_resolution = (
rosparam.get_param("/reha_exercise/camera_width"),
rosparam.get_param("/reha_exercise/camera_height"))
if rospy.has_param('/reha_exercise/robot_position'):
self._robot_position = rosparam.get_param(
"/reha_exercise/robot_position")
if rospy.has_param('/reha_exercise/rgb_colors'):
rgb_colors = rosparam.get_param("/reha_exercise/rgb_colors")
else:
rospy.logerr(
"Required parameter rgb_colors is not set on server! Aborting."
)
sys.exit()
if rospy.has_param('/reha_exercise/number_of_blocks'):
self._number_of_blocks = rosparam.get_param(
"/reha_exercise/number_of_blocks")
if rospy.has_param('/reha_exercise/repetitions_limit'):
self._repetitions_limit = rosparam.get_param(
"/reha_exercise/repetitions_limit")
if rospy.has_param('/reha_exercise/time_limit'):
self._time_limit = rosparam.get_param("/reha_exercise/time_limit")
if rospy.has_param('/reha_exercise/calibration_duration'):
self._calibration_duration = rosparam.get_param(
"/reha_exercise/calibration_duration")
if rospy.has_param('/reha_exercise/quantitative_frequency'):
temp_quant_freq = rosparam.get_param(
"/reha_exercise/quantitative_frequency")
if rospy.has_param('/reha_exercise/qualitative_frequency'):
temp_quali_freq = rosparam.get_param(
"/reha_exercise/qualitative_frequency")
if rospy.has_param('/reha_exercise/calibration_points_left_arm'
) and rospy.has_param(
'/reha_exercise/calibration_points_left_arm'):
self._calibration_points_left_arm = rosparam.get_param(
"/reha_exercise/calibration_points_left_arm")
self._calibration_points_right_arm = rosparam.get_param(
"/reha_exercise/calibration_points_right_arm")
if rospy.has_param('/reha_exercise/emotional_feedback_list'):
temp_emotional_feedbacks = rosparam.get_param(
"/reha_exercise/emotional_feedback_list")
# define tolerance values for both axis when calibrating manually
self._tolerance_x = camera_resolution[0] / 12
self._tolerance_y = camera_resolution[1] / 12
self._video_reader = USBCamReader(rgb_colors, camera_resolution,
self._calibration_points_left_arm,
self._tolerance_x, self._tolerance_y)
#self._video_reader.start()
self._encourager = EncouragerUnit(
self._number_of_blocks,
self.repetitions_limit,
quantitative_frequency=temp_quant_freq,
qualitative_frequency=temp_quali_freq,
emotional_feedbacks=temp_emotional_feedbacks)
self._rate = rospy.Rate(30) # 30hz
sleep(
0.2
) # wait some miliseconds until the video reader grabs its first frame from the capture device
if __name__ == "__main__":
if len(sys.argv) < 2:
print("One argument required:")
print("rosrun benchmark_runner run.py <config_file_name>")
exit()
else:
config_file = sys.argv[1]
rospy.init_node("execute_benchmark_run")
rospack = rospkg.RosPack()
# Open a connection to database for logging
DB = LogDB(collection="screencast_example")
filepath = rosparam.get_param("/planning_task_path")
config_file_path = rospack.get_path("benchmark_runner") + "/config/"
with open(config_file_path + config_file) as file:
config = json.load(file)
base_config = config["base_config"]
print("Base configurations:")
print("--------------------")
pprint.pprint(base_config)
print("Start with planner sweeps")
print("-------------------------")
timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S")
import rosparam
from std_msgs.msg import String
from soccer.msg import single_camera, stack_of_cams
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import cv2
import numpy as np
from vision.Camera import Camera
rospy.init_node("cameras_node")
cameras_publisher = rospy.Publisher("stack_of_cams",
stack_of_cams,
queue_size=5)
map_publisher = rospy.Publisher("map", Image, queue_size=5)
cam_params = rosparam.get_param("cam_params")
field_w = rosparam.get_param("sector_width")
field_h = rosparam.get_param("sector_height")
dx = rosparam.get_param("sectors_x_crossing")
dy = rosparam.get_param("sectors_y_crossing")
img_w = int(field_w * 2 - dx)
img_h = int(field_h * 2 - dy)
valid_cameras_ids = [0, 2]
cameras = []
for cam_id in cam_params:
if int(cam_id) in valid_cameras_ids:
cameras.append(Camera(int(cam_id)))
str(index_joint))
elif len(sys.argv) == 3:
if sys.argv[1].startswith('l'):
side = "left"
elif sys.argv[1].startswith('r'):
side = "right"
if sys.argv[2].startswith('p'): # pre-grasp position
pose_to_move = "pregrasp"
goal = createHandGoal(side, 1.5, 0.1, 0.1)
elif sys.argv[2].startswith('o'): # open position
pose_to_move = "open"
goal = createHandGoal(side, 0.1, 0.1, 0.1)
elif sys.argv[2].startswith('g') or sys.argv[2].startswith(
'c'): # close position
pose_to_move = "closed"
if rosparam.get_param('/use_sim_time'):
goal = createHandGoal(side, 1.5, 2.0, 2.0)
else:
goal = createHandGoal(side, 1.5, 4.0, 4.0)
rospy.loginfo("Will move " + side + " hand to pose: " + pose_to_move)
else:
rospy.loginfo("Incorrect number of parameters, usage:")
rospy.loginfo(
"move_reem_hands.py <right/left> <thumb_joint> <middle_joint> <index_joint> "
)
rospy.loginfo(
"move_reem_hands.py <right/left> <open/pregrasp/closed> ")
rospy.loginfo(
'Also accepts shortcuts like "l = left", "r = right", "o = open", "c = closed", "p = pregrasp"'
)
exit(0)
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 setup_ros_parameters():
'''
Returns the gateway parameters from the ros param server.
Most of these should be fairly self explanatory.
'''
param = {}
# Check if there is a user provided custom configuration
try:
custom_configuration_file = rospy.get_param('~custom_rules_file')
if custom_configuration_file:
if os.path.isfile(custom_configuration_file):
loaded_parameters = rosparam.load_file(
custom_configuration_file,
default_namespace=rospy.resolve_name(
rospy.get_name())) # this sets the private namespace
for params, ns in loaded_parameters:
# need to merge whatever default rules are already on the param server with these settings
existing_parameters = rosparam.get_param(ns)
for p, v in params.iteritems():
if p in [
'default_flips', 'default_advertisements',
'default_pulls'
] and p in existing_parameters:
existing_parameters[p] += v
else:
existing_parameters[p] = v
rosparam.upload_params(ns,
existing_parameters,
verbose=True)
else:
rospy.logwarn(
"Gateway : user provided configuration file could not be found [%s]"
% custom_configuration_file)
except KeyError:
# Not an error, just no need to load a custom configuration
pass
# Hub
param['hub_uri'] = rospy.get_param('~hub_uri', '')
# Convert these back to accepting lists once https://github.com/ros/ros_comm/pull/218
# goes through, for now we use semi-colon separated lists.
param['hub_whitelist'] = rospy.get_param('~hub_whitelist', [])
param['hub_blacklist'] = rospy.get_param('~hub_blacklist', [])
# Gateway
param['name'] = rospy.get_param('~name', 'gateway')
param['watch_loop_period'] = rospy.get_param('~watch_loop_period',
10) # in seconds
# Blacklist used for advertise all, flip all and pull all commands
param['default_blacklist'] = rospy.get_param('~default_blacklist',
[]) # list of Rule objects
# Used to block/permit remote gateway's from flipping to this gateway.
param['firewall'] = rospy.get_param('~firewall', True)
# The gateway can automagically detect zeroconf, but sometimes you want to force it off
param['disable_zeroconf'] = rospy.get_param('~disable_zeroconf', False)
# The gateway uses uui'd to guarantee uniqueness, but this can be disabled
# if you want clean names without uuid's (but you have to manually
# guarantee uniqueness)
param['disable_uuids'] = rospy.get_param('~disable_uuids', False)
# Make everything publicly available (excepting the default blacklist)
param['advertise_all'] = rospy.get_param('~advertise_all', []) # boolean
# Topics and services for pre-initialisation/configuration
param['default_advertisements'] = rospy.get_param(
'~default_advertisements', []) # list of Rule objects
param['default_flips'] = rospy.get_param('~default_flips',
[]) # list of RemoteRule objects
param['default_pulls'] = rospy.get_param('~default_pulls',
[]) # list of RemoteRule objects
# Network interface name (to be used when there are multiple active interfaces))
param['network_interface'] = rospy.get_param('~network_interface',
'') # string
return param
def getJointNames( controller_name ):
return rosparam.get_param( ''.join( (controller_name, '/joints') ) )
