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)
