def __init__(self, optical_frame, tf_listener=None):
if tf_listener == None:
tf_listener = tf.TransformListener()
self.tf_listener = tf_listener
self.optical_frame = optical_frame
self.robot = pr2.PR2(self.tf_listener, base=True)
self.behaviors = ManipulationBehaviors('l', self.robot, tf_listener=self.tf_listener)
self.laser_scan = hd.LaserScanner('point_cloud_srv')
self.prosilica = rc.Prosilica('prosilica', 'polled')
self.wide_angle_camera_left = rc.ROSCamera('/wide_stereo/left/image_rect_color')
self.wide_angle_configure = dr.Client('wide_stereo_both')
#TODO: define start location in frame attached to torso instead of base_link
self.start_location_light_switch = (np.matrix([0.35, 0.30, 1.1]).T, np.matrix([0., 0., 0., 0.1]))
self.start_location_drawer = (np.matrix([0.20, 0.40, .8]).T,
np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
self.folded_pose = np.matrix([ 0.10134791, -0.29295995, 0.41193769]).T
self.driving_param = {'light_switch_up': {'coarse': .9, 'fine': .6, 'voi': .4},
'light_switch_down': {'coarse': .9, 'fine': .6, 'voi': .4},
'light_rocker_down': {'coarse': .9, 'fine': .6, 'voi': .4},
'light_rocker_up': {'coarse': .9, 'fine': .6, 'voi': .4},
'pull_drawer': {'coarse': .9, 'fine': .5, 'voi': .4},
'push_drawer': {'coarse': .9, 'fine': .5, 'voi': .4}}
self.create_arm_poses()
self.driving_posture('light_switch_down')
self.robot.projector.set(False)
def __init__(self):
rospy.init_node('arduino_servo_node', log_level=rospy.INFO)
self.dynamic_client = DynamicReconfig.Client("arduino_dynamic_reconfig",timeout=10,config_callback=self.dynamic_callBack)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
rospy.on_shutdown(self.shutdown)
# Get all param
self.getAllParams()
#subscrib topic to move servo
rospy.Subscriber(self.moveServoTopic, MoveServo, self.moveServoCB)
#define publisher to pub arduino firmware version
self.versionPub = rospy.Publisher(self.firmwareVerTopic, UInt16, queue_size=1)
#define all services
self.defineAllServices()
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
self.controller.connect()
rospy.loginfo("Connected to Arduino on port:" + self.port + " at " + str(self.baud) + " baud")
# Enable servo x and servo y
self.controller.servo_enable(self.servo_x_id, 1)
self.controller.servo_enable(self.servo_y_id, 1)
# while loop
while not rospy.is_shutdown():
rate = rospy.Rate(self.pubRate)
firmwareVersion = self.controller.get_firmware_version()
self.versionPub.publish(firmwareVersion)
rate.sleep()
def __init__(self, autostart=False):
trr_rpu.PeriodicNode.__init__(self, 'race_manager_node')
# this is our model
self.race_manager = trr_rm.RaceManager()
# we publish our status
self.status_pub = trr_rpu.RaceManagerStatusPublisher()
# we expose a service to be informed ( by state estimation) when landmarks are passed
self.lm_service = rospy.Service('LandmarkPassed',
two_d_guidance.srv.LandmarkPassed,
self.on_landmark_passed)
# we manipulate parameters exposed by the guidance node
guidance_client_name = "trr_guidance_node"
self.guidance_cfg_client = dyn_rec_clt.Client(
guidance_client_name,
timeout=30,
config_callback=self.guidance_cfg_callback)
rospy.loginfo(' guidance_client_name: {}'.format(guidance_client_name))
# we will expose some parameters to users
self.race_manager_cfg_srv = dyn_rec_srv.Server(
trr.cfg.race_managerConfig, self.race_manager_cfg_callback)
# we start either racing or idle
self.dyn_cfg_update_race_mode(
self.race_manager.mode_racing if autostart else self.race_manager.
mode_staging)
# we subscribe to state estimator and traffic light
self.state_est_sub = trr_rpu.TrrStateEstimationSubscriber(
what='race_manager')
self.traffic_light_sub = trr_rpu.TrrTrafficLightSubscriber()
# we expose a service for loading a velocity profile
self.lm_service = rospy.Service('RaceManagerLoadPath',
two_d_guidance.srv.GuidanceLoadVelProf,
self.on_load_path)
def get_curve():
global control_onoff
global reference_path
global step_time
rospy.init_node('curve_node', anonymous=True)
# published topics
pub = {}
pub['calibration'] = rospy.Publisher('calibration_channel', Float64, queue_size=10)
# communicate with the dynamic server
dyn_params = reconfig.Client('server', config_callback=gui_callback)
step_time = 60 # se eu alterar aqui eu preciso alterar no client_node step_t = 6
length_reference = 12000
new_freq = length_reference/step_time
rate = rospy.Rate(new_freq)
control_onoff = False
count = 0
loaded = 0
number_steps = 0
while not rospy.is_shutdown():
if control_onoff:
# load reference
if loaded == 0:
try:
reference_data = sio.loadmat(reference_path)
pw_ref = reference_data['pw_ref'][0]
loaded = 1
except:
print("Ooops, the file you tried to load is not in the folder.")
pw_ref = [0, 0, 0, 0]
if count > len(pw_ref) - 1:
count = 0
number_steps = number_steps + 1
# send messages
if number_steps == 0:
pub['calibration'].publish(pw_ref[count])
else:
pub['calibration'].publish(0)
# update counter
count = count + 1
else:
count = 0
number_steps = 0
loaded = 0
pub['calibration'].publish(0)
rate.sleep()
def __init__(self, hosp_graph):
self.current_pose = None
# Keeps track of current and prior node to check when it changes
self.current_node_msg = None
self.prior_node_msg = None
self.current_node_q = None
self.next_node_q = None
self.current_node_index = 0
# Parameters file
self.hosp_graph = hosp_graph
self.initial_pose = hosp_graph.graph['initial_pose']
# Publishes the robot's current node to this topic
self.node_pub = rospy.Publisher('node_in_graph', String, queue_size=1)
# self.human_status_pub = rospy.Publisher('human_status', String, queue_size=1)
# Which condition is the node / hall under
self.condition = None
# self.human_conditions = [[0.0, 1.0], # Humans present 0% of the time; speed at 100% normal
# [0.7, 0.9], # Humans present 70% of the time, speed at 90% normal
# [0.2, 0.75]] # Humans present 20% of the time, speed at 75% normal
self.human_conditions = [
[0.0, 1.0], # Humans present 0% of the time; speed at 100% normal
[0.4, 0.75], # Humans present 40% of the time, speed at 75% normal
[0.8, 0.50]
] # Humans present 80% of the time, speed at 50% normal
# Boolean for whether or not humans are present in the current node
self.human_status = None
# Default velocity of the turtlebot
self.turtlebot_default_vel = 0.22
# Publishers for velocity and human state
# self.human_pub = rospy.Publisher('humans_or_no', Bool, queue_size=10)
self.vel_pub = rospy.Publisher('velocity', Float64, queue_size=10)
# This is the magic sauce that allows us to change the max velocity on the fly
self.dynam_client = dynam.Client('move_base/DWAPlannerROS')
self.poses = None
self.plan = None
self.prior_plan = None
def _init_amcl(self, is_global=True):
"""
Initialize amcl
Parameters
----------
is_global: bool
flag to initialize global localization or not
"""
# publish initialpose for amcl
init_pose_msg = PoseWithCovarianceStamped()
init_pose_msg.header.stamp = rospy.get_rostime()
init_pose_msg.header.frame_id = 'map'
# position
init_pose_msg.pose.pose.position.x = 0.0 # pose_x
init_pose_msg.pose.pose.position.y = 0.0 # pose_y
init_pose_msg.pose.pose.position.z = 0.0
# orientation
quaternion = quaternion_from_euler(0.0, 0.0, 0.0) # pose_a
init_pose_msg.pose.pose.orientation.x = quaternion[0]
init_pose_msg.pose.pose.orientation.y = quaternion[1]
init_pose_msg.pose.pose.orientation.z = quaternion[2]
init_pose_msg.pose.pose.orientation.w = quaternion[3]
# covariance
covariance = [0.0] * 36 # 6x6 covariance
covariance[6 * 0 + 0] = 0.5 * 0.5 # cov_xx
covariance[6 * 1 + 1] = 0.5 * 0.5 # cov_yy
covariance[6 * 5 + 5] = (np.pi / 12.0) * (np.pi / 12.0) # cov_aa
init_pose_msg.pose.covariance = covariance
self._init_pose_pub.publish(init_pose_msg)
if is_global:
# dynamic reconfigure
particles = 10000 # Note: only max 10000 is getting accepted
client = dynamic_reconfig.Client('/amcl')
config_params = {
'max_particles': particles,
}
# hard coded directly in launch file
#config = client.update_configuration(config_params)
self._init_global_localization()
rospy.logdebug('status: amcl initialized')
def execute(self, userdata):
# Check if use parameters or input keys
ps_name = None
p_dict = None
if self.param_server_name == None or self.parameters_dict == None:
ps_name = userdata.param_server_name
p_dict = userdata.parameters_dict
else:
ps_name = self.param_server_name
p_dict = self.parameters_dict
rospy.loginfo("Trying to connect a service client to '" + ps_name +
"' dynamic reconfigure...")
dynparamclient = client.Client(ps_name)
rospy.loginfo("Got a client! Setting parameters.")
config = dynparamclient.update_configuration(p_dict)
# check if it was really set
#rospy.loginfo("Parameters set: " + str(config))
return 'succeeded'
def on_dropdown_change(self, widget, value):
# If we had a previous client, disconnect it
if self.dyn_rec_client is not None:
self.dyn_rec_client.close()
# Get new client
self.dyn_rec_client = drc.Client(value, timeout=10.0)
# Get a configuration which ensures we'll have the description too
curr_conf = self.dyn_rec_client.get_configuration()
self.gui_set_funcs_for_param = {}
params_list = self.dyn_rec_client.group_description['parameters']
if params_list is not None:
table = gui.Table()
row = gui.TableRow()
item = gui.TableTitle()
item.add_child(str(id(item)), 'Param name')
row.add_child(str(id(item)), item)
item = gui.TableTitle()
item.add_child(str(id(item)), 'Min')
row.add_child(str(id(item)), item)
item = gui.TableTitle()
item.add_child(str(id(item)), 'Edit')
row.add_child(str(id(item)), item)
item = gui.TableTitle()
item.add_child(str(id(item)), 'Max')
row.add_child(str(id(item)), item)
item = gui.TableTitle()
item.add_child(str(id(item)), 'Edit2')
row.add_child(str(id(item)), item)
table.add_child(str(id(row)), row)
for idx, param in enumerate(params_list):
if param['edit_method'] != '':
# Enum
param_name = param['name']
# WTF, really? the full enum dict is actually a string?
enum_dict_as_str = param['edit_method']
enum_dict = literal_eval(enum_dict_as_str)
description = enum_dict['enum_description']
current_value = curr_conf[param_name]
enums = enum_dict['enum']
# there must be at least one enum
enum_type = enums[0]['type']
# Create dynamically a method to be called
method_name, cb_method = self.add_cb_to_class_by_param_name_and_type(
param_name, 'enum', enum_type)
enum_wid, set_funcs = self.create_enum_row(
param_name, description, current_value, cb_method,
enums)
self.gui_set_funcs_for_param[param_name] = set_funcs
table.add_child(idx, enum_wid)
elif param['type'] == 'int' or param['type'] == 'double':
param_name = param['name']
range_min = param['min']
range_max = param['max']
description = param['description']
current_value = curr_conf[param_name]
if param['type'] == 'int':
step = (range_max - range_min) / 100
elif param['type'] == 'double':
step = (range_max - range_min) / 100.0
# Create dynamically a method to be called
method_name, cb_method = self.add_cb_to_class_by_param_name_and_type(
param_name, 'digit')
num_wid, set_funcs = self.create_num_row(
param_name, description, range_min, range_max,
current_value, cb_method, step)
self.gui_set_funcs_for_param[param_name] = set_funcs
table.add_child(idx, num_wid)
elif param['type'] == 'str':
param_name = param['name']
current_value = curr_conf[param_name]
description = param['description']
# Create dynamically a method to be called
method_name, cb_method = self.add_cb_to_class_by_param_name_and_type(
param_name, 'string')
str_wid, set_funcs = self.create_str_row(
param_name, description, current_value, cb_method)
self.gui_set_funcs_for_param[param_name] = set_funcs
table.add_child(idx, str_wid)
elif param['type'] == 'bool':
param_name = param['name']
current_value = curr_conf[param_name]
description = param['description']
# Create dynamically a method to be called
method_name, cb_method = self.add_cb_to_class_by_param_name_and_type(
param_name, 'bool')
bool_wid, set_funcs = self.create_bool_row(
param_name, description, current_value, cb_method)
self.gui_set_funcs_for_param[param_name] = set_funcs
table.add_child(idx, bool_wid)
self.wid.add_child(2, table)
# This must be done later on! HACK! (append sets a margin)
# This makes the table not stick left, but float in the middle
table.style['margin'] = '10px auto'
# Once the GUI is setup, setup the callback for new configs
self.dyn_rec_client.set_config_callback(self.dyn_rec_conf_callback)
def __init__(self, node_name, verbose=False):
self.client = drc.Client(node_name)
self.names = []
self.param_names = []
self.normalizers = []
self._verbose = verbose
#imports go here
import rospy
import numpy as np
import sensor_msgs.msg
from sensor_msgs.msg import LaserScan
from laser_geometry import LaserProjection
import sensor_msgs.point_cloud2 as pc2 #may want to use point cloud...not sure yet
import scipy.signal as sig
from rospy.numpy_msg import numpy_msg
from rospy_tutorials.msg import Floats
import matplotlib.pyplot as plt
from utils import nan_helper
import dynamic_reconfigure.client as dyn
tb_mode_client = dyn.Client('/cmd_vel_mux') # setup client instance of node to reconfigure
class LaserScan():
def __init__(self, scan_topic="/scan"):
#rospy.init_node('LaserScan')
#self.scan_pub = rospy.Publisher('/filtered_laserscan', numpy_msg(Floats), queue_size=10)
self.scan_sub = rospy.Subscriber(scan_topic, sensor_msgs.msg.LaserScan, self.on_scan, queue_size = 1)
self.laser_projector = LaserProjection()
self.in_msg = None
self.angle_min = -.547
self.angle_max = .547
self.angle_increment = 0.00171110546216
self.angles = np.arange(self.angle_min, self.angle_max, self.angle_increment)
self.out_msg = np.array([])
self.b_filt, self.a_filt = sig.butter(4, 0.03, analog=False)
def control_knee():
global t_control
global stimMsg
global update_values
global co_activation
global new_current_quad
global new_current_hams
global control_sel
global pw_min_h
global pw_min_q
global channels_sel
global control_onoff
global control_enable_quad
global control_enable_hams
global ilc_memory
global ilc_i
global Kp_now
global Ki_now
global Kd_now
global ES_A_now
global ES_omega_now
global ES_phase_now
global ES_K_now
global ES_RC_now
global ILC_alpha_now
global ILC_beta_now
global ILC_gama_now
global initTime
# init_variables
t_control = [0, 0]
# init control node
controller = control.Control(rospy.init_node('control', anonymous=False))
# communicate with the dynamic server
dyn_params = reconfig.Client('server', config_callback=server_callback)
# subscribed topics
sub = dict()
sub.update({
'pedal':
rospy.Subscriber('imu/pedal', Imu, callback=imu_callback),
'ref':
rospy.Subscriber('ref_channel', Float64, callback=reference_callback),
'steps':
rospy.Subscriber('steps_channel', Float64, callback=steps_callback),
'curve':
rospy.Subscriber('calibration_channel',
Float64,
callback=curve_callback)
})
# sub = {}
# sub['pedal'] = rospy.Subscriber('imu/pedal', Imu, callback=imu_callback)
# sub['ref'] = rospy.Subscriber('ref_channel', Float64, callback=reference_callback)
# sub['steps'] = rospy.Subscriber('steps_channel', Float64, callback=steps_callback)
# published topics
pub = dict()
pub.update({
'talker':
rospy.Publisher('chatter', String, queue_size=10),
'control':
rospy.Publisher('stimulator/ccl_update', Stimulator, queue_size=10),
'angle':
rospy.Publisher('control/angle', Float64, queue_size=10),
'signal':
rospy.Publisher('control/stimsignal', Int32MultiArray, queue_size=10)
})
# define loop rate (in hz)
rate = rospy.Rate(freq)
# build basic angle message
angleMsg = Float64()
# errMsg = Float64()
# build basic stimulator message
stimMsg = Stimulator()
stimMsg.mode = ['single', 'single', 'single', 'single']
stimMsg.pulse_width = [0, 0, 0, 0]
stimMsg.pulse_current = [0, 0, 0, 0]
stimMsg.channel = [1, 2, 3, 4]
control_sel = 0
pw_min_h = 0
pw_min_q = 0
channels_sel = 0
control_onoff = False
ESC_now = [0, 0, 0, 0, 0]
ILC_now = [0, 0, 0]
co_activation = False
initTime = tempo.time()
# load inverse dynamics
try:
ID_data = sio.loadmat(ID_path)
id_pw = ID_data['id_pw'][0]
id_angle = ID_data['id_angle'][0]
# knee_ref = reference_data['knee_ref'][0]
# loaded = 1
except:
print("Ooops, the file you tried to load is not in the folder.")
# knee_ref = [0, 0, 0, 0]
# ---------------------------------------------------- start
while not rospy.is_shutdown():
thisKnee = angle[-1]
thisError = err_angle[-1]
thisRef = refKnee[-1]
thisTime = tempo.time() - initTime
# ============================== >controllers start here
new_kp = Kp_vector[-1]
new_ki = Ki_vector[-1]
new_kd = Kd_vector[-1]
new_kp_hat = Kp_hat_vector[-1]
new_ki_hat = Ki_hat_vector[-1]
new_kd_hat = Kd_hat_vector[-1]
newIntegralError = 0
new_u = 0
jcost = controller.jfunction(thisError, freq) # cost function
if not co_activation:
co_act_h = 0
co_act_q = 0
else:
co_act_h = pw_min_h
co_act_q = pw_min_q
if control_onoff:
# Open-loop inverse dynamics
if control_sel == 0:
try:
res = next(x for x in id_angle if x > thisRef)
# print list(id_angle).index(res)
except StopIteration:
res = 0
print "Not in recruitment curve"
new_u = res / 500
# if thisError < 0:
# new_u = -1
# else:
# new_u = 1
# PID
elif control_sel == 1:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# PID-ILC
elif control_sel == 2:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
# first do PID
thisU_PID, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# then ILC
ilc_send[0] = ilc_memory[0][ilc_i]
ilc_send[1] = ilc_memory[1][ilc_i]
# only change new_u after the second cycle
if ilc_memory[0][ilc_i] == 0:
new_u = thisU_PID
else:
ILC_now[0] = ILC_alpha_now
ILC_now[1] = ILC_beta_now
ILC_now[2] = ILC_gama_now
new_u = controller.pid_ilc(ilc_send, ILC_now, thisU_PID,
ilc_i)
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update ilc_memory
ilc_memory[0][ilc_i] = thisError
ilc_memory[1][ilc_i] = new_u
# update counter
if ilc_i < len(ilc_memory[0]) - 1:
ilc_i = ilc_i + 1
else:
ilc_i = 0
# PID-ES
elif control_sel == 3:
# calculate new pid parameters using ES
ESC_now[0] = ES_A_now
ESC_now[1] = ES_omega_now
ESC_now[2] = ES_phase_now
ESC_now[3] = ES_RC_now
ESC_now[4] = ES_K_now
new_kp, new_kp_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kp_vector[-1], Kp_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kp
ESC_now[0] = ES_A_now / 4 # ES_A_now
# ESC_now[1] = ES_omega_now-2
ESC_now[2] = ES_phase_now + 0.1745
# ESC_now[3] = ES_RC_now/2
# ESC_now[4] = ES_K_now/2
new_ki, new_ki_hat = controller.pid_es2(
jcost, jcost_vector[-1], Ki_vector[-1], Ki_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new ki
ESC_now[0] = ES_A_now / 16 # ES_A_now
# ESC_now[1] = ES_omega_now-3
ESC_now[2] = ES_phase_now + 0.523
# ESC_now[3] = ES_RC_now/8
# ESC_now[4] = ES_K_now/8
new_kd, new_kd_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kd_vector[-1], Kd_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kd
# do PID
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# CR
elif control_sel == 4: # curve recruitment
if control_enable_quad == 1:
new_u = curveRecr[-1]
elif control_enable_hams == 1:
new_u = -curveRecr[-1]
# no controller
else:
print("No controller was selected.")
new_kp = 0
new_ki = 0
new_kd = 0
new_kp_hat = 0
new_ki_hat = 0
new_kd_hat = 0
jcost = 0
newIntegralError = 0
new_u = 0
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update vectors
Kp_vector.append(new_kp)
Ki_vector.append(new_ki)
Kd_vector.append(new_kd)
Kp_hat_vector.append(new_kp_hat)
Ki_hat_vector.append(new_ki_hat)
Kd_hat_vector.append(new_kd_hat)
jcost_vector.append(jcost)
integralError.append(newIntegralError)
u.append(new_u)
# print refKnee[-1]
# ============================== controllers end here
# u to pw
# new_pw = round(abs(u[-1] * 500) / 10) * 10
# define pw for muscles
if u[-1] < 0:
# new_pw = round(abs(u[-1]) * (500-pw_min_h) + pw_min_h) / 10 * 10 # u to pw
new_pw = round(abs(u[-1]) * (500 - pw_min_h) + pw_min_h) # u to pw
pw_h.append(new_pw)
pw_q.append(co_act_q)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (Q), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_q[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
else:
new_pw = round(abs(u[-1]) * (500 - pw_min_q) + pw_min_q) # u to pw
pw_q.append(new_pw)
pw_h.append(co_act_h)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (H), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_h[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
print(controlMsg)
# define current for muscles
current_q.append(new_current_quad)
current_h.append(new_current_hams)
# update topics
if channels_sel == 1: # right leg
stimMsg.pulse_width = [0, 0, pw_q[-1], pw_h[-1]]
stimMsg.pulse_current = [0, 0, current_q[-1], current_h[-1]]
else: # left leg
stimMsg.pulse_width = [pw_q[-1], pw_h[-1], 0, 0]
stimMsg.pulse_current = [current_q[-1], current_h[-1], 0, 0]
pub['control'].publish(stimMsg) # send stim update
angleMsg.data = thisKnee
pub['angle'].publish(angleMsg) # send imu update
# update timestamp
# ts = tempo.time()
t_control.append(thisTime)
# next iteration
rate.sleep()
# ---------------------------------------------------- save everything
# recreate vectors
angle_err_lists = [angle, err_angle, t_angle]
pid_lists = [Kp_vector, Ki_vector, Kd_vector, integralError, t_control]
stim_lists = [current_q, current_h, pw_q, pw_h, t_control]
ref_lists = [refKnee, t_ref, curveRecr, t_curve]
ilc_lists = [ilc_memory, ILC_now, t_control]
esc_lists = [
ESC_now, t_control, Kp_hat_vector, Ki_hat_vector, Kd_hat_vector,
jcost_vector
]
control_lists = [u, t_control]
gui_lists = ([
t_gui, gui_enable_c, gui_control_sel, gui_step_time, gui_kp, gui_ki,
gui_kd, gui_alpha, gui_beta, gui_gama, gui_A, gui_omega, gui_phase,
gui_channels_sel, gui_pw_min_q, gui_pw_min_h, gui_K, gui_RC
])
path_lists = ([gui_ref_param])
steps_lists = ([t_steps, count_steps])
# create name of the file
currentDT = datetime.datetime.now()
# path_str = "/home/bb8/Desktop/gait_"
path_str = gui_save_param[-1]
if channels_sel == 1: # right leg
leg_str = '_R_'
else: # left leg
leg_str = '_L_'
if control_sel == 0:
control_str = '0_'
elif control_sel == 1:
control_str = '1_'
elif control_sel == 2:
control_str = '2_'
elif control_sel == 3:
control_str = '3_'
elif control_sel == 4:
control_str = '4_'
else:
control_str = 'x_'
if not co_activation:
coact_str = "_noCA"
else:
coact_str = "_CA"
currentDT_str = str(currentDT)
name_file = path_str + coact_str + leg_str + control_str + currentDT_str + ".mat"
# save .mat
sio.savemat(
name_file, {
'angle_err_lists': angle_err_lists,
'pid_lists': pid_lists,
'stim_lists': stim_lists,
'ref_lists': ref_lists,
'ilc_lists': ilc_lists,
'control_lists': control_lists,
'esc_lists': esc_lists,
'gui_lists': gui_lists,
'path_lists': path_lists,
'steps_lists': steps_lists
})
if offsetIsSet:
tpose = calibration_offset
if markerIDset:
for tnode in tracking_points:
if tnode.tracker_id is calibration_marker_id and tnode.sensor_id is fixed_sensor:
tpose = tnode.pose
(trans, rot) = transform_frame(t, "s" + str(fixed_sensor), "world",
tpose)
quaternion = (rot.x, rot.y, rot.z, rot.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
client = reconf.Client(
rospy.get_param("/calibration_dyn_tf_node_name"))
client.update_configuration({
"x": trans.x,
"y": trans.y,
"z": trans.z,
"roll": euler[0],
"pitch": euler[1],
"yaw": euler[2]
})
# cameras transformed back by same Offset
(trans_offset,
rot_offset) = transform_frame(t, "s" + str(fixed_sensor),
origin_frame, tpose)
offset = fill_pose_from_msg(trans_offset, rot_offset)
add_frame(t, "new_origin", "world", offset)
#!/usr/bin/env python
import rospy
from dynamic_reconfigure import client as drc
from dynamic_reconfigure import DynamicReconfigureParameterException
if __name__ == '__main__':
rospy.init_node('loop_on_kp')
DYNAMIC_RECONFIGURE_SERVER = '/gazebo_test'
# Get new client, internally it connects and monitors the services/topics related
# to the dynamic reconfigure server, keep the client, don't create one every
# time you need to do a call
try:
dyn_rec_client = drc.Client(DYNAMIC_RECONFIGURE_SERVER, timeout=10.0)
except rospy.exceptions.ROSException as e:
rospy.logerr('Error while creating dynamic reconfigure client: ' +
str(e))
# Spawn the ramp
try:
dyn_rec_client.update_configuration({'click_to_spawn_ramp': True})
except DynamicReconfigureParameterException as e:
rospy.logerr(
'There was an exception trying to set up click_to_spawn_ramp new value: '
+ str(e))
rospy.sleep(3.0)
# Also, on the shell, this would be:
# rosrun dynamic_reconfigure dynparam set /gazebo_test click_to_spawn_ramp true
def __init__(self):
self.client = dc.Client("camera_synchronizer_node")
self.node_config = self.client.get_configuration()
Created on Jan 28 21:39:00 2014
@author: Sam Pfeiffer
Set openni dynamic params for the Asus Xtion of REEM.
We must lower the quantity of data sent.
"""
import rospy
from dynamic_reconfigure import client
if __name__ == '__main__':
rospy.init_node("set_xtion_dyn_params", anonymous=True)
rospy.loginfo(
"Trying to connect a service client to head_mount_xtion dynamic reconfigure..."
)
client = client.Client("/head_mount_xtion/driver") # xtion node
rospy.loginfo("Got a client! Setting parameters.")
params = {
'data_skip': 10,
'ir_mode': 6,
'color_mode': 6,
'depth_mode': 6
} # drop rate
config = client.update_configuration(params)
# check if it was really set
rospy.loginfo("Parameters set: " + str(config))
def __init__(self, nao_ip, nao_port):
#self.cmd_vel_topic = rospy.Publisher(CMD_VEL_TOPIC, Twist, queue_size=100)
self.move_base_as = actionlib.SimpleActionClient(
MOVE_BASE_AS_NAME, MoveBaseAction)
#rospy.loginfo(NODE_NAME + " waiting for underworlds start fact service server.")
#rospy.wait_for_service(START_FACT_SRV_NAME)
#rospy.loginfo(NODE_NAME + " found underworlds start fact service server.")
#rospy.loginfo(NODE_NAME + " waiting for underworlds stop fact service server.")
#rospy.wait_for_service(STOP_FACT_SRV_NAME)
#rospy.loginfo(NODE_NAME + " found underworlds stop fact service server.")
self.move_to_fact_id = None
self.approach_fact_id = None
self.rotate_fact_id = None
# rospy.loginfo(NODE_NAME + " waiting for multimodal human monitor service.")
# rospy.wait_for_service(TOGGLE_HUMAN_MONITORING_SRV)
# rospy.loginfo(NODE_NAME + " found multimodal human monitor service.")
self.toggle_human_monitor = rospy.ServiceProxy(
TOGGLE_HUMAN_MONITORING_SRV, SetBool)
self.go_to_stable_posture = rospy.ServiceProxy(NAOAI_JNT_SRV, Empty)
self.check_goal = rospy.Subscriber("/move_base/result",
MoveBaseActionResult,
self.go_to_posture_cb)
# self.nao_ip = nao_ip
# self.nao_port = nao_port
# self.al_motion_mtx = Lock()
# self.al_motion = ALProxy("ALMotion", nao_ip, nao_port)
#self.al_proxy = ALProxy.
self.tf_listener = tf.TransformListener()
rospy.loginfo(NODE_NAME + " waiting for move_base action server.")
self.move_base_as.wait_for_server()
rospy.loginfo(NODE_NAME + " found move_base action server.")
self._move_base_as_recfg_client = dyncfg.Client(
"/move_base/TebLocalPlannerROS")
self.as_move_to = actionlib.SimpleActionServer(
MOVE_TO_AS_NAME,
MoveBaseAction,
execute_cb=self.execute_move_to_cb,
auto_start=False)
self.need_final_rotation = False
self._feedback_move_to = None
self._feedback_rate = rospy.Rate(10)
self.as_move_to.start()
self.regex_frame_to_human_id = re.compile(REGEX_FRAME_TO_HUMAN_ID)
self.as_approach = actionlib.SimpleActionServer(
APPROACH_AS_NAME,
MoveBaseAction,
execute_cb=self.execute_approach_cb,
auto_start=False)
self._feedback_approach = None
self.as_approach.start()
self.as_rotate = actionlib.SimpleActionServer(
ROTATE_AS_NAME,
RotateAction,
execute_cb=self.execute_rotate_cb,
auto_start=False)
self._feedback_rotate = None
self.as_rotate.start()
self.register_pepper_sync = rospy.ServiceProxy(
REGISTER_PEPPER_SYNC, MetaStateMachineRegister)
rospy.loginfo(NODE_NAME + " server started.")
#rospy.Subscriber(INFEASIBLE_TRAJ_NAME, Bool, self.no_plan_cb)
self.running = False
def __init__(self):
rospy.init_node('face_location_node', log_level=rospy.INFO)
self.name = rospy.get_name()
rospy.on_shutdown(self.shutdown)
self.get_config_param()
self.dynamic_client = DynamicReconfig.Client(
"face_dynamic_reconfig",
timeout=10,
config_callback=self.dynamicCallBack)
# define move servo topic publisher
self.moveServoPub = rospy.Publisher(self.moveServoTopic,
MoveServo,
queue_size=2)
# Get a reference to webcam #0 (the default one)
self.video_capture = cv2.VideoCapture(self.camera_ID)
# Initialize some variables
face_locations = []
process_this_frame = 0
top = 0
right = 0
bottom = 0
left = 0
onceMoveOrgDeg = False
recordTime = 0.0
while not rospy.is_shutdown():
ret, frame = self.video_capture.read()
# Resize frame of video to 1/4 size for faster face recognition processing
small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
# Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
rgb_small_frame = small_frame[:, :, ::-1]
# Only process every other frame of video
if process_this_frame % self.face_tracker_speed == 0:
process_this_frame = 0
# Find all the faces in the current frame of video
face_locations = face_recognition.face_locations(
rgb_small_frame)
process_this_frame = process_this_frame + 1
if (len(face_locations) == 0 and (not onceMoveOrgDeg)):
checkTime = rospy.get_time()
if ((checkTime - recordTime) > self.face_detect_timeout):
self.moveServoOrgDeg()
onceMoveOrgDeg = True
# calculate face center ready to move servo
for (top, right, bottom, left) in face_locations:
onceMoveOrgDeg = False
recordTime = rospy.get_time()
# Scale back up face locations since the frame we detected in was scaled to 1/4 size
top *= 4
right *= 4
bottom *= 4
left *= 4
# print the face center postion
center_x = left + (right - left) / 2
center_y = top + (bottom - top) / 2
#rospy.loginfo("(%d,%d)" %(center_x, center_y))
self.check_face_move_servo(center_x, center_y)
break # at same time only recognize one face
