def leaveElevatorCallback(userdata, nav_goal): new_travel_speed_sfl = 0.1 new_point_b_sfl = -0.26 new_enable_oa = False node_to_reconfigure = "/move_by/move_base/PalLocalPlanner" client = dynamic_reconfigure.client.Client(node_to_reconfigure) old_config = client.update_configuration({}) rospy.loginfo(' Gathering planner parameters values to recover later...') userdata.old_travel_speed_sfl = old_config.travel_speed_sfl userdata.old_point_b_sfl = old_config.point_b_sfl userdata.old_enable_oa = old_config.enable_oa rospy.loginfo(' Tunning planner parameters values of the robot to leave elevator backwards...') new_params = {'travel_speed_sfl': new_travel_speed_sfl, 'point_b_sfl': new_point_b_sfl, 'enable_oa': new_enable_oa} new_config = client.update_configuration(new_params) rospy.loginfo(' Planner parameters values reconfigured. The new values are:') rospy.loginfo(' travel_speed_sfl : ' + str(new_travel_speed_sfl) + " => " + str(new_config.travel_speed_sfl)) rospy.loginfo(' point_b_sfl : ' + str(new_point_b_sfl) + " => " + str(new_config.point_b_sfl)) rospy.loginfo(' enable_oa : ' + str(new_enable_oa) + " => " + str(new_config.enable_oa)) leave_goal = MoveBaseGoal() leave_goal.target_pose.header.stamp = rospy.Time.now() leave_goal.target_pose.header.frame_id = "/base_link" leave_goal.target_pose.pose = userdata.final_pose rospy.loginfo(' Leaving elevator...i\'m going backwards to ' + str(userdata.final_pose)) return leave_goal
def cb(msg): global expected_n_cluster, reconfig_n_limit, reconfig_n_times global sub_kcluster, sub_ncluster with lock: if reconfig_n_times > reconfig_n_limit: sub_kcluster.unregister() sub_ncluster.unregister() return if expected_n_cluster is None: return cfg = client.get_configuration(timeout=None) tol_orig = cfg['tolerance'] delta = tol_orig * reconfig_eps if msg.data == expected_n_cluster: print('Expected/Actual n_cluster: {0}, Tolerance: {1}' .format(msg.data, tol_orig)) reconfig_n_times = reconfig_n_limit + 1 return elif msg.data > expected_n_cluster: cfg['tolerance'] += delta else: cfg['tolerance'] -= delta if cfg['tolerance'] < 0.001: print('Invalid tolerance, resetting') cfg['tolerance'] = 0.02 print('''\ Expected n_cluster: {0} Actual n_cluster: {1} tolerance: {2} -> {3} '''.format(expected_n_cluster, msg.data, tol_orig, cfg['tolerance'])) client.update_configuration(cfg) reconfig_n_times += 1
def testsimple(self): client = dynamic_reconfigure.client.Client("ref_server", timeout=5) config = client.get_configuration(timeout=5) self.assertEqual(0, config['int_']) self.assertEqual(0.0, config['double_']) self.assertEqual('foo', config['str_']) self.assertEqual(False, config['bool_']) self.assertEqual(1.0, config['double_no_minmax']) self.assertEqual(2.0, config['double_no_max']) self.assertEqual(-1.0, config['deep_var_double']) int_ = 7 double_ = 0.75 str_ = 'bar' bool_ = True double_no_max_ = 1e+300 double_no_minmax_ = -1e+300 client.update_configuration( {"int_": int_, "double_": double_, "str_": str_, "bool_": bool_, "double_no_max": double_no_max_, "double_no_minmax": double_no_minmax_} ) rospy.sleep(1.0) config = client.get_configuration(timeout=5) self.assertEqual(int_, config['int_']) self.assertEqual(double_, config['double_']) self.assertEqual(str_, config['str_']) self.assertEqual(type(str_), type(config['str_'])) self.assertEqual(bool_, config['bool_']) self.assertEqual(double_no_max_, config['double_no_max']) self.assertEqual(double_no_minmax_, config['double_no_minmax'])
def testmultibytestring(self): client = dynamic_reconfigure.client.Client("ref_server", timeout=5) config = client.get_configuration(timeout=5) self.assertEqual('bar', config['mstr_']) # Kanji for konnichi wa (hello) str_ = u"今日は" client.update_configuration( {"mstr_": str_} ) rospy.sleep(1.0) config = client.get_configuration(timeout=5) self.assertEqual(u"今日は", config['mstr_']) self.assertEqual(type(u"今日は"), type(config['mstr_'])) self.assertEqual(u"今日は", rospy.get_param('/ref_server/mstr_')) # Hiragana for konnichi wa (hello) str_ = u"こんにちは" client.update_configuration( {"mstr_": str_} ) rospy.sleep(1.0) config = client.get_configuration(timeout=5) self.assertEqual(u"こんにちは", config['mstr_']) self.assertEqual(type(u"こんにちは"), type(config['mstr_'])) self.assertEqual(u"こんにちは", rospy.get_param('/ref_server/mstr_'))
def testsimple(self): client = dynamic_reconfigure.client.Client("ref_server", timeout=5) config = client.get_configuration(timeout=5) self.assertEqual(0, config['int_']) self.assertEqual(0.0, config['double_']) self.assertEqual('foo', config['str_']) self.assertEqual(False, config['bool_']) int_ = 7 double_ = 0.75 str_ = 'bar' bool_ = True client.update_configuration( {"int_": int_, "double_": double_, "str_": str_, "bool_": bool_} ) rospy.sleep(1.0) config = client.get_configuration(timeout=5) self.assertEqual(int_, config['int_']) self.assertEqual(double_, config['double_']) self.assertEqual(str_, config['str_']) self.assertEqual(bool_, config['bool_'])
def update_parameter(self, param): client = dynamic_reconfigure.client.Client(self.node_name, timeout=2) try: client.update_configuration(param) except Exception as ex: logger.error("Updating parameter error: {}".format(ex)) return False return True
def tolerance_up(userdata): node_to_reconfigure = "/move_base/PalLocalPlanner" client = dynamic_reconfigure.client.Client(node_to_reconfigure) original_config = client.get_configuration() userdata.original_config = original_config.yaw_goal_tolerance new_params = {'yaw_goal_tolerance': 3.1416} client.update_configuration(new_params) return succeeded
def buttonImageResetClicked( self ): """ buttonImageResetClicked """ # Create a service. client = dynamic_reconfigure.client.Client( 'nao_blob_detection' ); client.update_configuration( { 'xOffset': 0, 'yOffset': 0, 'width': 0, 'height': 0 } ); self.__state = self.STATE_NONE;
def change_projector_mode(self, on): client = dynamic_reconfigure.client.Client("camera_synchronizer_node") node_config = client.get_configuration() node_config["projector_mode"] = 2 if on: node_config["narrow_stereo_trig_mode"] = 3 else: node_config["narrow_stereo_trig_mode"] = 4 client.update_configuration(node_config)
def main(): # change depth resolution to QVGA client = dynamic_reconfigure.client.Client("/camera/driver") client.update_configuration({"depth_mode":8, "data_skip":0}) rospy.Subscriber("/camera/depth/image_rect", Image, callback) rospy.Subscriber("/camera/obstacle_detection/enable", Bool, callback_enable) srv = Server(depth_analysisConfig, reconfigure_callback) rospy.spin()
def handleImageCrop( self, p1, p2 ): # Calculate settings. xOffset = min( p1.x(), p2.x() ); yOffset = min( p1.y(), p2.y() ); width = max( p1.x(), p2.x() ) - min( p1.x(), p2.x() ); height = max( p1.y(), p2.y() ) - min( p1.y(), p2.y() ); client = dynamic_reconfigure.client.Client( 'nao_blob_detection' ); client.update_configuration( { 'xOffset': xOffset, 'yOffset': yOffset, 'width': width, 'height': height } ); self.ImageWidget.clearRectangle(); self.reset();
def __init__(self): rospy.init_node("force_error_constants_dynamic_client") # Name this client rospy.loginfo("Connecting to force_controller_constants dyn_rec_client.") #pdb.set_trace() # debug # Client runs once at 10Hz and calls upate_configuration client = dynamic_reconfigure.client.Client("force_error_constants", timeout=30, config_callback=self.callback) r = rospy.Rate(0.1) while not rospy.is_shutdown(): #client.update_configuration({"kf0":kf0, "kf1":kf1, "kf2":kf2, "km0":km0, "km1":km1, "km2":km2}) client.update_configuration() r.sleep()
def listener(command): # Get the ~private namespace parameters from command line or launch file. # topic = rospy.get_param('~topic', 'chatter') ## Next structure is a pythonic "select case" options = {"show_accel" : ('/imu', Imu), "show_tilt" : ('/cur_tilt_angle', Float64), "show_status" : ('/cur_tilt_status', UInt8), "update_tilt" : ('/tilt_angle', Float64), "image_mode" : ('/camera/driver/image_mode', int), "depth_mode" : ('~depth_mode', int), "depth_registration" : ('~depth_registration', int), "data_skip" : ('data_skip', int), } topic, dataType = options[command] if command == "update_tilt": pub = rospy.Publisher(topic, dataType) pub.publish("10") rospy.spinOnce(); return if command == "data_skip": client = dynamic_reconfigure.client.Client('/camera/driver') params = { topic : int(sys.argv[2]) } config = client.update_configuration(params) return timeout = 100 # Create a subscriber with appropriate topic, custom message and name of callback function. # rospy.Subscriber(topic, Imu, callback) print "Reading topic %s"%topic rcvdMsg = rospy.wait_for_message(topic, dataType, timeout) if dataType == Imu: callback(rcvdMsg) else: print rcvdMsg
def setVelocity(velocity, type): client = dynamic_reconfigure.client.Client('move_base/DWAPlannerROS') if type == 'LINEAR': params = {'min_vel_x' : velocity, 'max_vel_x' : velocity, 'max_trans_vel': velocity, 'min_trans_vel':velocity} else: params = {'max_rot_vel': velocity, 'min_rot_vel':velocity} config = client.update_configuration(params) rospy.sleep(0.2) rospy.loginfo("Velocity set to " + str(velocity))
def setVelocity(velocity, type): client = dynamic_reconfigure.client.Client('move_base/DWAPlannerROS') if type == 'LINEAR': params = {'max_vel_x' : velocity, 'max_trans_vel': velocity} else: params = {'max_rot_vel': velocity, 'min_rot_vel':velocity} config = client.update_configuration(params) rospy.sleep(0.2) rospy.loginfo("Velocity set to " + str(velocity))
def laser_reconfiguration(angles): client = dynamic_reconfigure.client.Client('hokuyo_node') if angles[0] <= -math.pi: print "First angle out of range" if angles[1] >= math.pi: print "Last angle out of range" new_config = { 'min_ang' : angles[0], 'max_ang' : angles[1] } rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config) config = client.update_configuration(new_config)
def set_table_filter_limits(table, clients): for field, client in clients.iteritems(): params = dict(filter_field_name=field) if field in ("x", "y"): # params['input_frame'] = table.pose.header.frame_id params["input_frame"] = "/table" params["output_frame"] = "/table" params["filter_limit_min"] = table.__getattribute__("%s_min" % field) params["filter_limit_max"] = table.__getattribute__("%s_max" % field) else: params["input_frame"] = "/table" params["output_frame"] = "/table" params["filter_limit_min"] = -0.01 params["filter_limit_max"] = 0.01 # params['filter_limit_min'] = -5 # params['filter_limit_max'] = 5 client.update_configuration(params)
def laser_reconfiguration(angles): client = dynamic_reconfigure.client.Client('hokuyo_node') if angles[0] <= -math.pi: print "First angle out of range" if angles[1] >= math.pi: print "Last angle out of range" new_config = {'min_ang': angles[0], 'max_ang': angles[1]} rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config) config = client.update_configuration(new_config)
def local_path_callback(data): global nowG if len(data.poses) > 100: data.poses = data.poses[:100] if len(data.poses) > 1: w = np.array([it.orientation.w for it in data.poses]) x = np.array([it.orientation.x for it in data.poses]) y = np.array([it.orientation.y for it in data.poses]) z = np.array([it.orientation.z for it in data.poses]) yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y)) dyaw = np.abs(np.diff(yaw)) q2 = q[:len(dyaw)] res = np.mean(dyaw * q2) r = res / 4 + globalP vel = k / r + min_vel if vel > max_vel: vel = max_vel vels.append(vel) if len(vels) > 5: vels.pop(0) vel = np.mean(vels) # print(vel) '''if length < 40: vel *= 0.3 if length < 30: vel *= 0.15 if length < 20: vel *= 0.05''' if dyawG > 2: nowG = slow_down_time msg = Float64() msg.data = nowG / 3 pub2.publish(msg) if nowG > 0: vel *= slow_down_rate print('slow down', nowG) if stop: vel = 0 print('stop') msg = Float64() msg.data = vel pub3.publish(msg) client.update_configuration({'max_vel_x': vel})
def callback(data): node = '/d435i/RGB_Camera' #if data.data == 1 # value = True #else value = data.data[0] ##value1 = data.data[1] value1 = data.data[1] ##rospy.loginfo(data.data) ##if value < 1500: ## values_dict = { 'Exposure' : value, 'Gain' : value1} ##else: ## values_dict = { 'Exposure' : value, 'Gain' : value1} values_dict = {'Exposure': value, 'Gain': 64} client = dynamic_reconfigure.client.Client(node, None) try: client.update_configuration(values_dict) except dynamic_reconfigure.DynamicReconfigureParameterException as e: print('error updating parameters: ' + str(e))
def execute(self, userdata): start = time.time() print(userdata.task_input) temp_task_list = list(userdata.task_input) temp_task = temp_task_list.pop(0) userdata.task_output = temp_task_list userdata.current_task_output = temp_task client = dynamic_reconfigure.client.Client( "agv1_move_base/DWAPlannerROS") client.update_configuration({ 'xy_goal_tolerance': 0.1, 'yaw_goal_tolerance': 0.5 }) print("Parameters Updated") position = temp_task["Waypoint"]["position"] yaw = temp_task["Waypoint"]["yaw"] if docking_states.docking_val: docking_states.docking_val = False else: client_2 = dynamic_reconfigure.client.Client( "agv1_move_base/local_costmap/inflation_layer") client_2.update_configuration({'inflation_radius': 0.2}) agv_nav.nav_dynamic_target_position_and_yaw(position, yaw) success = self.odom_compare(position) done = time.time() time_diff = done - start idle_standby_state.time_variable += time_diff idle_standby_state.time_list.append(time_diff) if success: return 'succeeded' else: return 'aborted'
def change_costmap_params(robot_radius=0.18, inflation_radius=0.3): client = dynamic_reconfigure.client.Client("move_base/global_costmap", timeout=10) client.update_configuration({"robot_radius": robot_radius}) client = dynamic_reconfigure.client.Client( "move_base/global_costmap/inflation_layer", timeout=10) client.update_configuration({"inflation_radius": inflation_radius}) client = dynamic_reconfigure.client.Client("move_base/local_costmap", timeout=10) client.update_configuration({"robot_radius": robot_radius}) client = dynamic_reconfigure.client.Client( "move_base/local_costmap/inflation_layer", timeout=10) client.update_configuration({"inflation_radius": inflation_radius})
def set_table_filter_limits(table, clients): for field, client in clients.iteritems(): params = dict(filter_field_name=field) if field in ('x', 'y'): # params['input_frame'] = table.pose.header.frame_id params['input_frame'] = '/table' params['output_frame'] = '/table' params['filter_limit_min'] = table.__getattribute__('%s_min' % field) params['filter_limit_max'] = table.__getattribute__('%s_max' % field) else: params['input_frame'] = '/table' params['output_frame'] = '/table' params['filter_limit_min'] = -0.01 params['filter_limit_max'] = 0.01 # params['filter_limit_min'] = -5 # params['filter_limit_max'] = 5 client.update_configuration(params)
def execute(self, userdata): print(userdata.task_input) temp_task_list = list(userdata.task_input) temp_task = temp_task_list.pop(0) userdata.task_output = temp_task_list userdata.current_task_output = temp_task waypoint_list = [temp_task["Waypoint"]] print("\n\nWaypoint - " + str(waypoint_list) + " - is starting to work.\n\n") print("Parametre Degisecek") client = dynamic_reconfigure.client.Client( "agv1_move_base/DWAPlannerROS") client.update_configuration({ 'xy_goal_tolerance': 0.15, 'yaw_goal_tolerance': 0.15 }) client_2 = dynamic_reconfigure.client.Client( "agv1_move_base/local_costmap/inflation_layer") client_2.update_configuration({'inflation_radius': 0.2}) print("Parametre Degisti") nav = agv_nav.GoToPose() success, position = agv_nav.dynamic_waypoint(nav, waypoint_list, self.coordinate) time.sleep(3) success = True if success: return 'succeeded' else: return 'aborted'
def recoverOldPlannerParameters(userdata): rospy.loginfo(' Recovering old planner parameters values...') client = dynamic_reconfigure.client.Client(node_to_reconfigure) params = {'travel_speed_sfl': userdata.old_travel_speed_sfl, 'point_b_sfl': userdata.old_point_b_sfl, 'enable_oa': userdata.old_enable_oa} new_config = client.update_configuration(params) rospy.loginfo(' Planner parameters values reconfigured. The new values are:') rospy.loginfo(' travel_speed_sfl : ' + str(userdata.old_travel_speed_sfl) + " => " + str(new_config.travel_speed_sfl)) rospy.loginfo(' point_b_sfl : ' + str(userdata.old_point_b_sfl) + " => " + str(new_config.point_b_sfl)) rospy.loginfo(' enable_oa : ' + str(userdata.old_enable_oa) + " => " + str(new_config.enable_oa)) return succeeded
def reconfigureNBV(self): mapToSet = { "enableCropBoxFiltering": True, "enableIntermediateObjectWeighting": False } rospy.loginfo( "Dynamic reconfigure of NBV: enableCropBoxFiltering=True and enableIntermediateObjectWeighting=False" ) if self.newMOmegaUtility is not None: rospy.loginfo("Dynamic reconfigure of NBV: mOmegaUtility=" + str(self.newMOmegaUtility)) mapToSet.update({"mOmegaUtility": self.newMOmegaUtility}) if self.newMOmegaBase is not None: rospy.loginfo("Dynamic reconfigure of NBV: mOmegaBase=" + str(self.newMOmegaBase)) mapToSet.update({"mOmegaBase": self.newMOmegaBase}) client = dynamic_reconfigure.client.Client( "nbv", timeout=30, config_callback=self.reconfigureNBVCallback) client.update_configuration(mapToSet)
def talker(): global cur_rate pub = rospy.Publisher('chatter', env, queue_size=10) rospy.init_node('talker', anonymous=True) server_ip = "192.168.1.100" rospy.set_param("img_pub_rate", 10) client = dynamic_reconfigure.client.Client("/camera/left/image/compressed") info = client.get_configuration() params = {"jpeg_quality": 10} client.update_configuration(params) print info while not rospy.is_shutdown(): img_pub_rate = rospy.get_param("img_pub_rate") rate = rospy.Rate(img_pub_rate) value = env() value.atmo = 1 value.temp = 2 value.hum = 3 pub.publish(value) # rate.sleep() time.sleep(2)
def run(self): client = dynamic_reconfigure.client.Client("odometry_benchmarker", timeout=30, config_callback=self.config_callback) client.update_configuration({"pose_correction":0.0, "phase_offset":0.0}) r = rospy.Rate(5) plt.figure() while not rospy.is_shutdown(): xs = [pt[0] for pt in self.pts] ys = [pt[1] for pt in self.pts] yaws = [pt[2] for pt in self.pts] min_dim = min(len(xs), len(ys), len(yaws)) xs = xs[:min_dim] ys = ys[:min_dim] yaws = yaws[:min_dim] if len(xs) > 5: xc,yc,R,residu = leastsq_circle(np.asarray(xs),np.asarray(ys)) phase_offset = self.get_phase_offset(xc,yc,R,xs,ys,yaws) plot_data_circle(xs,ys,xc,yc,R) plt.pause(.05) r.sleep() client.update_configuration({"pose_correction":R, "phase_offset":phase_offset})
def __init__(self): rospy.init_node("dynamic_client") rospy.loginfo("Connecting to battery simulator node...") client = dynamic_reconfigure.client.Client("battery_simulator", timeout=30, config_callback=self.callback) r = rospy.Rate(0.1) charge = True while not rospy.is_shutdown(): if charge: level = 100 else: level = 0 charge = not charge client.update_configuration({"new_battery_level": level}) r.sleep()
def testsimple(self): client = dynamic_reconfigure.client.Client("ref_server", timeout=5) config = client.get_configuration(timeout=5) self.assertEqual(0, config['int_']) self.assertEqual(0.0, config['double_']) self.assertEqual('foo', config['str_']) self.assertEqual(False, config['bool_']) self.assertEqual(1.0, config['double_no_minmax']) self.assertEqual(2.0, config['double_no_max']) self.assertEqual(-1.0, config['deep_var_double']) int_ = 7 double_ = 0.75 str_ = 'bar' bool_ = True double_no_max_ = 1e+300 double_no_minmax_ = -1e+300 client.update_configuration({ "int_": int_, "double_": double_, "str_": str_, "bool_": bool_, "double_no_max": double_no_max_, "double_no_minmax": double_no_minmax_ }) rospy.sleep(1.0) config = client.get_configuration(timeout=5) self.assertEqual(int_, config['int_']) self.assertEqual(double_, config['double_']) self.assertEqual(str_, config['str_']) self.assertEqual(type(str_), type(config['str_'])) self.assertEqual(bool_, config['bool_']) self.assertEqual(double_no_max_, config['double_no_max']) self.assertEqual(double_no_minmax_, config['double_no_minmax'])
def evaluate_parameters(self, iss_model_resolution, pfhrgb_radius_search, counter): self.rates = {} # Update the paramters to the next setting client = dynamic_reconfigure.client.Client("quasimodo_retrieval_node", timeout=30) client.update_configuration({"iss_model_resolution": iss_model_resolution, "pfhrgb_radius_search": pfhrgb_radius_search}) for (bag, label) in zip(self.bags, self.labels): self.current_label = label goal = quasimodo_optimization.msg.rosbag_playGoal(command='start', file=bag) self.client.send_goal(goal) self.client.wait_for_result() total_correct = 0 total = 0 for key, value in self.rates.iteritems(): total_correct = total_correct + value[0] total = total + value[1] self.rates['total'] = (total_correct, total, float(total_correct)/float(total)) summary = (iss_model_resolution, pfhrgb_radius_search, self.rates) with open(str(counter) + '.json', 'w') as f: json.dump(summary, f)
def set_dynamic_navigation_params(param_type): ''' Sets a list of dynamic reconfigure parameters defined by the string `param_type` args: param_type: str -- name of list of parameters specified in the `dynamic_navigation_params` file ''' node_name = '/maneuver_navigation/TebLocalPlannerROS' try: client = dynamic_reconfigure.client.Client(node_name, timeout=1.5) except Exception as e: rospy.logerr("Service {0} does not exist".format(node_name + '/set_parameters')) return False params_file = rospy.get_param('~dynamic_navigation_params', 'ros/config/dynamic_navigation_params.yaml') params = yaml.load(open(params_file)) try: client.update_configuration(params[param_type]) except Exception as e: rospy.logerr("Failed to set dynamic reconfigure params for " + node_name) rospy.logerr(e.message)
def __init__(self): rospy.init_node("dynamic_client") rospy.loginfo("Connecting to battery simulator node...") client = dynamic_reconfigure.client.Client( "battery_simulator", timeout=30, config_callback=self.callback) r = rospy.Rate(0.1) charge = True while not rospy.is_shutdown(): if charge: level = 100 else: level = 0 charge = not charge client.update_configuration({"new_battery_level": level}) r.sleep()
def setPlannerParameters(userdata): debugPrint(' Setting planner parameters values for following the operator...', 4) node_to_reconfigure = "/move_by/move_base/PalLocalPlanner" client = dynamic_reconfigure.client.Client(node_to_reconfigure) params = { 'acc_lim_x': 0.65, 'acc_lim_th': 1.5, 'max_vel_x': 0.5, 'max_vel_th': 0.5} new_config = client.update_configuration(params) debugPrint("New configuration returned by the dynamic reconfigure server:\n" + str(new_config), 4) return succeeded
def handle_request(req): vel = req.max_vel acc = req.max_acc jerk= req.max_jerk print "Setting motion parameters (vel, acc, jerk).. {0:4.2f} {1:4.2f} {2:4.2f}".format(vel, acc, jerk) params = { 'max_trans_vel' : vel, 'max_vel_x' : vel, 'acc_lim_x' : acc, # TODO consider angular velocity! 'acc_lim_y' : acc, 'acc_lim_theta' : acc+0.7, # angular limit is by default higher than linear } config = client.update_configuration(params) return True
def handle_set_camera_param(self,req): """ Handles requests to set a given cameras parameters. Currenty, brightness, shutter, and gain can be set. """ response = SetCameraParamResponse() response.flag = True response.message = '' node = get_node_from_camera_name(req.camera_name) if node is not None: params = { 'brightness': req.brightness, 'shutter': req.shutter, 'gain': req.gain, } client = dynamic_reconfigure.client.Client(node) client.update_configuration(params) else: response.flag = False response.message = 'camera, {0}, not found'.format(req.camera_name) return response
def update_reached_precision(self, goal_with_prirority): return tolerance_param = 'xy_goal_tolerance' priority_idx = int(goal_with_prirority.priority / 100) precision = self.priority_to_precision[priority_idx] # http://wiki.ros.org/base_local_planner#Goal_Tolerance_Parameters client = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS") old_config = client.get_configuration(0.05) if old_config is None or old_config[tolerance_param] != precision: params = { tolerance_param : precision } new_config = client.update_configuration(params) rospy.loginfo("Goal xy tolerance set to %s", str(precision)) else: rospy.loginfo("Goal tolerance not updated")
def run(self): client = dynamic_reconfigure.client.Client("star_center_positioning_node", timeout=30, config_callback=self.config_callback) client.update_configuration({"pose_correction":0.0, "phase_offset":0.0}) r = rospy.Rate(5) plt.figure() while not rospy.is_shutdown(): xs = [pt[0] for pt in self.pts] ys = [pt[1] for pt in self.pts] yaws = [pt[2] for pt in self.pts] min_dim = min(len(xs), len(ys), len(yaws)) xs = xs[:min_dim] ys = ys[:min_dim] yaws = yaws[:min_dim] if len(xs) > 5: xc,yc,R,residu = leastsq_circle(np.asarray(xs),np.asarray(ys)) phase_offset = self.get_phase_offset(xc,yc,R,xs,ys,yaws) print "rosrun my_pf star_center_position_revised.py _pose_correction:="+str(R) + " _phase_offset:="+str(phase_offset) plot_data_circle(xs,ys,xc,yc,R) plt.pause(.05) r.sleep() client.update_configuration({"pose_correction":R, "phase_offset":phase_offset})
def __init__(self): rospy.init_node('parameter_set', anonymous=True) control_sub = rospy.Subscriber( "/navigation_velocity_smoother/parameter_updates", Config, self.controller_callback) control_pub = rospy.Publisher( "/navigation_velocity_smoother/parameter_updates", Config, queue_size=1) client = dynamic_reconfigure.client.Client( "/navigation_velocity_smoother") params = {'speed_lim_w': 0.3, 'accel_lim_w': 0.2, 'speed_lim_v': 0.5} config = client.update_configuration(params)
def updateConfig(self, goal, depth_kp, heading_kp, depth_motor_kp, heading_motor_kp, depth_startup_time, heading_startup_time): #Update Config file parameters client = dynamic_reconfigure.client.Client('controller') pidparams = { 'goal': goal, 'depth_kp': depth_kp, 'heading_kp': heading_kp, 'depth_motor_kp': depth_motor_kp, 'heading_motor_kp': heading_motor_kp, 'depth_startup_time': depth_startup_time, 'heading_startup_time': heading_startup_time } config = client.update_configuration(pidparams)
def update_reached_precision(self, goal_with_prirority): return tolerance_param = 'xy_goal_tolerance' priority_idx = int(goal_with_prirority.priority / 100) precision = self.priority_to_precision[priority_idx] # http://wiki.ros.org/base_local_planner#Goal_Tolerance_Parameters client = dynamic_reconfigure.client.Client( "/move_base/TrajectoryPlannerROS") old_config = client.get_configuration(0.05) if old_config is None or old_config[tolerance_param] != precision: params = {tolerance_param: precision} new_config = client.update_configuration(params) rospy.loginfo("Goal xy tolerance set to %s", str(precision)) else: rospy.loginfo("Goal tolerance not updated")
def callback_control_change(data): control_level = data.data print "Data: ", control_level try: if control_level==4 or control_level==1: # client.update_configuration({"max_vel_x":0.5, "max_rot_vel":5.0, "min_rot_vel":0.4}) client.update_configuration({"max_vel_x":0.2, "max_vel_theta":1.0, "min_vel_theta":-1.0}) elif control_level==3: # client.update_configuration({"max_vel_x":0.3, "max_rot_vel":3.0, "min_rot_vel":0.4}) client.update_configuration({"max_vel_x":0.15, "max_vel_theta":0.8, "min_vel_theta":-0.8}) elif control_level==2: # client.update_configuration({"max_vel_x":0.2, "max_rot_vel":2.0, "min_rot_vel":0.4}) client.update_configuration({"max_vel_x":0.1, "max_vel_theta":0.6, "min_vel_theta":-0.6}) else: client.update_configuration({"max_vel_x":0.0, "max_vel_theta":0.0, "min_vel_theta":0.0}) except Exception as e: print "Param client error: ", e
def performAction(self, action): # Set LIDAR to pan at specified speed pan_param_msg = Int16MultiArray(data=[self.lidar_ID, -1, -1, self.action_space[action]*1023]) self.pan_param_pub.publish(pan_param_msg) self.lidar_pan_speed = self.action_space[action]; # Scale driving speed so the robot drives more slowly when it is panning more slowly # TODO: Make it so two dithering nodes can be active and coordinate which one controls the driving linear_velocity_limit_new = self.max_linear_velocity*(0.5+2*self.action_space[action]) client = dynamic_reconfigure.client.Client('move_base/TrajectoryPlannerROS') params = {'max_vel_x' : linear_velocity_limit_new} config = client.update_configuration(params) rospy.loginfo("Set LIDAR %s to %s%% Speed\n", self.lidar_ID, self.action_space[action]*100) rospy.loginfo('Set max autonomous driving speed to %s m/s.\n', linear_velocity_limit_new) return
def handle_emphasis_request(req): #print "min vel: %s"%min_vel #print "max_vel: %s"%max_vel val=float(req.action) #print "value: %f"%val if (val==-1): print "reset..." val=default_speed if (val<min_vel): val=min_vel elif (val>max_vel): val=max_vel print "Squeeze value: %s <--> Robot speed value: %f" % (req.action,val) params={'max_rot_vel':val, 'max_trans_vel':val,'max_vel_x':val,'max_vel_y':val,'min_vel_x':-val,'min_vel_y':-val} config=client.update_configuration(params); return 1
def handle_emphasis_request(req): val = float(req.action) if val < MIN_VEL: val = MIN_VEL elif val > MAX_VEL: val = MAX_VEL print "Squeeze value: %s <--> Robot speed value: %s" % (req.action, val) params = { "max_rot_vel": val, "max_trans_vel": val, "max_vel_x": val, "max_vel_y": val, "min_vel_x": -val, "min_vel_y": -val, } config = client.update_configuration(params) return 1
def gdist_scale(self, dist): client = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS") params = { 'gdist_scale' : dist} config = client.update_configuration(params) rospy.loginfo("gdist_scale set to %s", str(dist))
def set_max_vel_x(self, velocity): client = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS") params = { 'max_vel_x' : velocity} config = client.update_configuration(params) rospy.loginfo("max_vel_x set to %s", str(velocity))
def test(msg): if len(predictor.global_path) == 0: return scan = msg.ranges policy.forward([scan, predictor.global_path]) sub_robot = rospy.Subscriber("/odometry/filtered", Odometry, predictor.update_status) sub_gp = rospy.Subscriber("/move_base/TrajectoryPlannerROS/global_plan", Path, predictor.update_global_path, queue_size=1) sub_scan = rospy.Subscriber("/front/scan", LaserScan, test, queue_size=1) client = dynamic_reconfigure.client.Client( 'move_base/TrajectoryPlannerROS') client2 = dynamic_reconfigure.client.Client( 'move_base/local_costmap/inflater_layer') while not rospy.is_shutdown(): try: ct = predictor.context_type params = env_params[ct] infla = env_inflates[ct] config = client.update_configuration(params) config2 = client2.update_configuration({'inflation_radius': infla}) except dynamic_reconfigure.DynamicReconfigureCallbackException: continue except rospy.exceptions.ROSInterruptException: break
def callback(config): rospy.loginfo( "Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}" .format(**config)) if __name__ == "__main__": rospy.init_node("dynamic_client") client = dynamic_reconfigure.client.Client("dynamic_tutorials", timeout=30, config_callback=callback) r = rospy.Rate(0.1) x = 0 b = False while not rospy.is_shutdown(): x = x + 1 if x > 10: x = 0 b = not b client.update_configuration({ "int_param": x, "double_param": (1 / x + 1), "str_param": str(rospy.get_rostime()), "bool_param": b, "size": 1 }) r.sleep()
def subCallback(data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) client.update_configuration({"maximun_speed": data.data})
def set_max_vel_x(self, velocity): client = dynamic_reconfigure.client.Client( "/move_base/TrajectoryPlannerROS") params = {'max_vel_x': velocity} config = client.update_configuration(params) rospy.loginfo("max_vel_x set to %s", str(velocity))
client = dynamic_reconfigure.client.Client('tilt_hokuyo_node') global old_config old_config = client.get_configuration(2.0) new_config = { 'skip': 0, 'intensity': 0, 'min_ang': -1.57, 'max_ang': 1.57, 'calibrate_time': 1, 'cluster': 1, 'time_offset': 0.0 } rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config) client.update_configuration(new_config) if __name__ == '__main__': name = 'setting_laser' rospy.init_node(name) #create a client to the tilt laser rospy.wait_for_service('laser_tilt_controller/set_traj_cmd') tilt_profile_client = rospy.ServiceProxy( 'laser_tilt_controller/set_traj_cmd', SetLaserTrajCmd) ## set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]) ## original set_tilt_profile([1.05, -.7, 1.05], [0.0, 2.4, 2.4 + .2125 + .3]) configure_laser()
def execute_cb(self, goal): client = dynamic_reconfigure.client.Client("thruster_controller", timeout=30) # Get the mass and COM with open(rospy.get_param('vehicle_file'), 'r') as stream: vehicle = yaml.safe_load(stream) mass = vehicle['mass'] com = vehicle['com'] rospy.loginfo("Starting calibration") # Reset parameters to default volume = mass / WATER_DENSITY cobX = com[0] cobY = com[1] cobZ = com[2] client.update_configuration({ "Volume": volume, "Buoyancy_X_POS": cobX, "Buoyancy_Y_POS": cobY, "Buoyancy_Z_POS": cobZ }) self.depthPub.publish(True, -1.5) self.rollPub.publish(0, AttitudeCommand.POSITION) self.pitchPub.publish(0, AttitudeCommand.POSITION) rospy.sleep(3) # Recalibrate until it converges lastAdjustment = 0 converged = False while not converged: rospy.sleep(3) # Average 10 samples volumeAverage = 0 for _ in range(10): forceMsg = rospy.wait_for_message("command/force_depth", Vector3Stamped).vector force = math.sqrt(forceMsg.x**2 + forceMsg.y**2 + forceMsg.z**2) if forceMsg.z < 0: force *= -1 volumeAdjust = force / GRAVITY / WATER_DENSITY volumeAverage += volumeAdjust / 10 # Adjust in the right direction volume -= volumeAverage * .9 client.update_configuration({ "Volume": volume, "Buoyancy_X_POS": cobX, "Buoyancy_Y_POS": cobY, "Buoyancy_Z_POS": cobZ }) # If the direction of adjustment changed, stop if lastAdjustment * volumeAverage < 0: converged = True lastAdjustment = volumeAverage if self._as.is_preempt_requested(): rospy.loginfo('Preempted Calibration') self.cleanup() self._as.set_preempted() return Fb = volume * GRAVITY * WATER_DENSITY rospy.loginfo("Buoyant force calibration complete") lastAdjustmentX = 0 convergedX = False lastAdjustmentY = 0 convergedY = False while not convergedX or not convergedY: rospy.sleep(3) cobYAverage = 0 cobXAverage = 0 for _ in range(10): momentMsg = rospy.wait_for_message("command/moment", Vector3Stamped).vector cobYAverage += momentMsg.x / Fb * 2**.5 * 0.1 cobXAverage += momentMsg.y / Fb * 2**.5 * 0.1 rospy.loginfo(cobYAverage) cobY -= cobYAverage cobX += cobXAverage client.update_configuration({ "Volume": volume, "Buoyancy_X_POS": cobX, "Buoyancy_Y_POS": cobY, "Buoyancy_Z_POS": cobZ }) # If the direction of adjustment changed, stop if lastAdjustmentX * cobXAverage < 0: convergedX = True if lastAdjustmentY * cobYAverage < 0: convergedY = True lastAdjustmentX = cobXAverage lastAdjustmentY = cobYAverage if self._as.is_preempt_requested(): rospy.loginfo('Preempted Calibration') self.cleanup() self._as.set_preempted() return rospy.loginfo("Buoyancy XY calibration complete") self.rollPub.publish(45, AttitudeCommand.POSITION) rospy.sleep(3) lastAdjustment = 0 converged = False while not converged: rospy.sleep(3) cobZAverage = 0 for _ in range(10): momentMsg = rospy.wait_for_message("command/moment", Vector3Stamped).vector cobZAverage += momentMsg.x / Fb * 2**.5 * 0.1 cobZ += cobZAverage client.update_configuration({ "Volume": volume, "Buoyancy_X_POS": cobX, "Buoyancy_Y_POS": cobY, "Buoyancy_Z_POS": cobZ }) # If the direction of adjustment changed, stop if lastAdjustment * cobZAverage < 0: converged = True lastAdjustment = cobZAverage if self._as.is_preempt_requested(): rospy.loginfo('Preempted Calibration') self.cleanup() self._as.set_preempted() return rospy.loginfo("Calibration complete") self.cleanup() self._as.set_succeeded()
class DynamicReconfigureClient(object): """ Sets dynamic reconifgure parameters for sets of nodes Loads named configurations from a yaml file Sets parameters for each node in the named configuration published on ~configuration_name Configuration file can be loaded during runtime if empty message is publish to ~reload_config """ def __init__(self): self.configuration_name = None self.event = None self.config_file_name = rospy.get_param('~config_file') self.named_configurations = yaml.load(open(self.config_file_name, 'r')) # Node cycle rate (in hz) self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 10)) # Publishers self.event_out = rospy.Publisher("~event_out", std_msgs.msg.String) # Subscribers rospy.Subscriber('~event_in', std_msgs.msg.String, self.event_in_cb) rospy.Subscriber('~configuration_name', std_msgs.msg.String, self.configuration_name_cb) rospy.Subscriber('~reload_config', std_msgs.msg.Empty, self.reload_config_cb) def start(self): """ Starts the state machine """ rospy.loginfo("Ready to start...") state = 'INIT' while not rospy.is_shutdown(): if state == 'INIT': state = self.init_state() elif state == 'IDLE': state = self.idle_state() elif state == 'RUNNING': state = self.running_state() rospy.logdebug("State: {0}".format(state)) self.loop_rate.sleep() def configuration_name_cb(self, msg): """ Obtains configuration name to be set """ self.configuration_name = msg.data def event_in_cb(self, msg): """ Obtains an event for dynamic reconfiguration """ self.event = msg.data def reload_config_cb(self, msg): """ Reloads named configurations from file """ self.named_configurations = yaml.load(open(self.config_file_name, 'r')) def init_state(self): """ Executes the INIT state of the state machine. :return: The updated state. :rtype: str """ if self.event == 'e_start': return 'IDLE' else: return 'INIT' def idle_state(self): """ Executes the IDLE state of the state machine. :return: The updated state. :rtype: str """ if self.event == 'e_stop': self.event_out.publish("e_stopped") self.event = None self.configuration_name = None return 'INIT' elif self.configuration_name: return 'RUNNING' else: return 'IDLE' def running_state(self): """ Executes the RUNNING state of the state machine. :return: The updated state. :rtype: str """ if self.event == 'e_stop': self.event = None self.configuration_name = None self.event_out.publish('e_stopped') return 'INIT' else: if self.configuration_name in self.named_configurations.keys(): if self.set_all_parameters( self.named_configurations[self.configuration_name]): self.event_out.publish("e_success") else: self.event_out.publish("e_failure") self.event = None self.configuration_name = None return 'INIT' else: self.event_out.publish("e_failure") self.event = None self.configuration_name = None return 'INIT' self.event = None self.configuration_name = None return 'IDLE' def set_all_parameters(self, params): """ set dynamic reconfigure parameters for all nodes listed under the named configuration """ for key, value in params.items(): if not self.set_node_parameters(key, value): rospy.logerr("Could not set parameters for {0}".format(key)) return False return True def set_node_parameters(self, node_name, params): """ create dynamic reconfigure client and set params for a single node """ try: client = dynamic_reconfigure.client.Client(node_name, timeout=1.5) except Exception, e: rospy.logerr( "Service {0} does not exist".format(node_name + '/set_parameters')) return False try: config = client.update_configuration(params) except Exception as e: rospy.logerr("Error: %s", str(e)) return False return True
import sys import yaml import rospy import dynamic_reconfigure.client if __name__ == '__main__': rospy.init_node('param_loader') filtered_argv=rospy.myargv(argv=sys.argv) if len(filtered_argv)<2: rospy.logwarn("No config yaml file provided.") elif len(filtered_argv)>2: rospy.logwarn("Too many arguments.") else: file_name=filtered_argv[1] with open(file_name, 'r') as stream: yaml_config=yaml.load(stream) print yaml_config for key, val in yaml_config.iteritems(): print key print val client = dynamic_reconfigure.client.Client("/move_base/" + key) client.update_configuration(val) print "UPDATED"
#!/usr/bin/env python import rospy import tf from PyKDL import Rotation import dynamic_reconfigure.client # find the rotation between the glass frame and the face detection frame, and # rotate the face detection frame by its inverse if __name__ == '__main__': rospy.init_node('frame_adjust') client = dynamic_reconfigure.client.Client(rospy.myargv()[1]) listener = tf.TransformListener() listener.waitForTransform('face_detection', 'glass', rospy.Time(), rospy.Duration(10)) trans, rot = listener.lookupTransform('glass', 'face_detection', rospy.Time(0)) rot = Rotation.Quaternion(*rot) r, p, y = rot.Inverse().GetRPY() config = client.get_configuration() config.yaw -= y # config.pitch -= p # config.roll -= r client.update_configuration(config)
#! /usr/bin/env python # -*- coding: utf-8 -*- """ 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' : 4, '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 gdist_scale(self, dist): client = dynamic_reconfigure.client.Client( "/move_base/TrajectoryPlannerROS") params = {'gdist_scale': dist} config = client.update_configuration(params) rospy.loginfo("gdist_scale set to %s", str(dist))
rospy.wait_for_service("/move_base/DWAPlannerROS/set_parameters", 5.0) client = dynamic_reconfigure.client.Client( "/move_base/DWAPlannerROS", timeout=30) client.update_configuration({"yaw_goal_tolerance": 3.1416}) client.update_configuration({"xy_goal_tolerance": xy_tol}) rospy.loginfo("[NC] DWAPlannerROS Update tolerance DONE") except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("[NC] DWAPlannerROS Service call failed: %s" % (e, )) else: try: rospy.wait_for_service("/move_base/DWAPlannerROS/set_parameters", 5.0) client = dynamic_reconfigure.client.Client( "/move_base/DWAPlannerROS", timeout=30) client.update_configuration({"yaw_goal_tolerance": 0.1}) client.update_configuration({"xy_goal_tolerance": xy_tol}) rospy.loginfo("[NC] DWAPlannerROS Update tolerance DONE") except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("[NC] DWAPlannerROS Service call failed: %s" % (e, )) def move_base_agent(): global status # Loading the destination coordinate # Wait for Move_base's confirmation while True: if status == 'moving_reached': pose_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, pose_agent)