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