def set_param(cli, name, value):
req = SetParameters.Request()
param_value = ParameterValue(
double_value=value, type=ParameterType.PARAMETER_DOUBLE)
param = Parameter(name=name, value=param_value)
req.parameters.append(param)
cli.call_async(req)
def send_request(self, option, value):
self.req_ = SetParameters.Request()
# Create one parameter:
param = Parameter()
if (option == 1):
param.value.type = ParameterType.PARAMETER_STRING
param.value.string_value = value
else:
param.value.type = ParameterType.PARAMETER_BOOL
v = value == "True"
param.value.bool_value = v
if (option == 1):
param.name = "model_path"
elif (option == 2):
param.name = "display_masks"
elif (option == 3):
param.name = "display_bboxes"
elif (option == 4):
param.name = "display_text"
elif (option == 5):
param.name = "display_scores"
elif (option == 6):
param.name = "display_fps"
print(param.name)
# Append to de list:
self.req_.parameters.append(param)
self.future = self.client_srv_.call_async(self.req_)
def __set_string_param(self, node, name, value):
self.cli = self._node.create_client(SetParameters, self._node.get_namespace() + node + '/set_parameters')
self.cli.wait_for_service(timeout_sec=1)
req = SetParameters.Request()
param_value = ParameterValue(string_value=value, type=ParameterType.PARAMETER_STRING)
param = Parameter(name=name, value=param_value)
req.parameters.append(param)
self.cli.call_async(req)
def set_auv_node_params(self):
self.get_logger().debug('waiting for /auv_node/set_parameters server...')
self._set_param_auv_node_client.wait_for_service()
request = SetParameters.Request()
for param in self._experiments[self._idx].auv_params:
request.parameters.append(param)
self.get_logger().debug('setting auv_node params...')
self._set_auv_node_params_future = self._set_param_auv_node_client.call_async(request)
self._set_auv_node_params_future.add_done_callback(self.set_auv_node_params_done_cb)
def execution(self):
# Set calibration state to success
req = SetParameters.Request()
param = Parameter()
param.name = "calibration_state"
param.value.type = ParameterType.PARAMETER_INTEGER
param.value.integer_value = 4
req.parameters.append(param)
future = self.client_param.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def call_set_parameters(*, node, node_name, parameters):
# create client
client = node.create_client(SetParameters, f'{node_name}/set_parameters')
# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')
request = SetParameters.Request()
request.parameters = parameters
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
# handle response
response = future.result()
return response
def set_filter_node_params(self):
if len(self._experiments[self._idx].filter_params) > 0:
self.get_logger().debug('waiting for /filter_node/set_parameters server...')
self._set_param_filter_node_client.wait_for_service()
request = SetParameters.Request()
for param in self._experiments[self._idx].filter_params:
request.parameters.append(param)
self.get_logger().debug('setting filter_node params...')
self._set_filter_node_params_future = self._set_param_filter_node_client.call_async(
request)
self._set_filter_node_params_future.add_done_callback(
self.set_filter_node_params_done_cb)
else:
# Step 3
self.send_mission_goal()
def set_parameter(self, state=True, timeout=5.0):
parameters = [
rclpy.Parameter('demo_parameter_1',
value=state).to_parameter_msg()
]
client = self.create_client(SetParameters,
'demo_node_1/set_parameters')
ready = client.wait_for_service(timeout_sec=timeout)
if not ready:
raise RuntimeError('Wait for service timed out')
request = SetParameters.Request()
request.parameters = parameters
future = client.call_async(request)
rclpy.spin_until_future_complete(self, future, timeout_sec=timeout)
assert future.done(), 'Client request timed out'
response = future.result()
return response.results[0]
def call_set_parameters(*, node, node_name, parameters):
# create client
client = node.create_client(
SetParameters, '/{node_name}/set_parameters'.format_map(locals()))
# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')
request = SetParameters.Request()
request.parameters = parameters
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
# handle response
response = future.result()
if response is None:
e = future.exception()
raise RuntimeError('Exception while calling service of node '
"'{args.node_name}': {e}".format_map(locals()))
return response
def _call_set_parameters(node, node_name, parameters):
"""
Calls the SetParameters service to load the list of parameters
Returns True if all params were loaded succesfully and false otherwise
@param node: The node to create the clients that load the params
@param node_name: The name of the node which the params are loaded into
@param parameters: List of Parameters
"""
client = node.create_client(SetParameters, f'{node_name}/set_parameters')
while not client.wait_for_service(timeout_sec=1.0):
self.get_logger().warn(
'Parameter setting service not available, waiting again...')
req = SetParameters.Request()
req.parameters = parameters
future = client.call_async(req)
res = None
success = True
def callback(future):
nonlocal res
res = future.result()
future.add_done_callback(callback)
while rclpy.ok() and res is None:
time.sleep(0.1)
if len(res.results) != len(parameters):
e = future.exception()
node.get_logger().warn(
"Exception while calling set param service of node '{node_name}': {e}"
)
success = False
node.destroy_client(client)
return success
def set_parameters(self, parameters):
set_params_request = SetParameters.Request()
set_params_request.parameters = [
p.to_parameter_msg() for p in parameters
]
return self._set_params_client.call(set_params_request)
names_types[name] = t_str
names_values[name] = value
line = f"{name:{l}s}\t{t_str}\t{v}"
parameters_prompt.append(line)
req_params = select_bullet("Select desired parameter", parameters_prompt)
param_name = req_params.split()[0]
p = f"Enter new value for parameter '{param_name}' of type {names_types[param_name]} "
new_value_str = get_string(p)
valid, new_value = is_value_param_valid(new_value_str,
names_values[param_name])
# print(valid, new_value_str, new_value)
if valid:
print(
f"You've entered '{new_value_str}'' which translates to '{new_value}'"
)
req = SetParameters.Request()
param = Parameter()
param.name = param_name
param.value = get_parameter_value(names_values[param_name].type,
new_value)
# print("---------")
# print(param.value)
# print("---------")
req.parameters.append(param)
f = set_params_client.call_async(req)
req_time = time.time()
while not f.done():
rclpy.spin_once(node, timeout_sec=0.1)
if time.time() - req_time > 3.0:
print("Timedout requesting set_parameters")
break
