def test_concurrent_calls_to_service(self):
cli = self.node.create_client(GetParameters, 'get/parameters')
srv = self.node.create_service(GetParameters, 'get/parameters',
lambda request, response: response)
try:
self.assertTrue(cli.wait_for_service(timeout_sec=20))
future1 = cli.call_async(GetParameters.Request())
future2 = cli.call_async(GetParameters.Request())
rclpy.spin_until_future_complete(self.node, future1)
rclpy.spin_until_future_complete(self.node, future2)
self.assertTrue(future1.result() is not None)
self.assertTrue(future2.result() is not None)
finally:
self.node.destroy_client(cli)
self.node.destroy_service(srv)
def _should_use_current_time(self) -> bool:
"""Determine whether the rqt_note_taker should use the current time
when creating a new entry or the timestamp of the log msg.
This is determined based on whether the use_sim_time parameter is True.
When this parameter is true, the log msg will not have
an useful timestamp, meaning the current time should be used.
If the node is launched via the launch file, then the parameter can
simply be retrieved from the node's parameter server.
If the node is launch via rqt_gui, it will not have the parameter
correctly set in its own parameter server, therefore the parameter
is retrieved from the march_monitor node.
:return Returns a boolean, indicating whether the current time should
be used when creating a new entry.
"""
client = self._node.create_client(GetParameters,
"/march_monitor/get_parameters")
if client.service_is_ready():
future = client.call_async(
GetParameters.Request(names=["use_sim_time"]))
rclpy.spin_until_future_complete(self._node, future)
client.destroy()
return future.result().values[0].bool_value
else:
return (self._node.get_parameter(
"use_sim_time").get_parameter_value().bool_value)
def __init__(self):
super().__init__('dynamixel_joint_state_publisher')
self.controllers = self.declare_parameter('dynamixel_controllers', []).value
self.joint_states = dict({})
for controller in sorted(self.controllers):
self.get_logger().info('Getting joint name from %s' % controller)
client = self.create_client(GetParameters, '/{}/get_parameters'.format(controller))
client.wait_for_service()
future = client.call_async(GetParameters.Request(names=['joint_name']))
rclpy.spin_until_future_complete(self, future)
joint = None
if future.result():
joint = future.result().values[0].string_value
if joint is None:
self.get_logger().error('Could not get joint name from %S' % controller)
continue
self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0)
self.create_subscription(JointStateDynamixel, '/{}/state'.format(controller), self.controller_state_handler, 1)
# Start publisher
self.joint_states_pub = self.create_publisher(JointState, '/joint_states', 1)
rate = self.declare_parameter('rate', 20).value
self.get_logger().info("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
self.timer = self.create_timer(1./rate, self.publish_joint_states)
def test_all_thresholds_set(self):
"""Test that all expected temperature thresholds are set."""
parameter_names = []
for threshold_type in THRESHOLD_TYPES:
for joint_name in JOINT_NAMES:
parameter_names.append(
f"temperature_thresholds_{threshold_type}.{joint_name}"
)
self.assertTrue(
self.wait_for_parameter_availability(
set(
parameter_names
+ [
"use_sim_time",
"default_temperature_threshold",
"send_errors_interval",
]
)
),
msg="Parameters were not set within timeout duration",
)
client = self.node.create_client(
srv_type=GetParameters, srv_name="/march/safety_node/get_parameters"
)
future = client.call_async(GetParameters.Request(names=parameter_names))
rclpy.spin_until_future_complete(self.node, future)
expected_values = [
self.get_threshold(*parameter.split(".")) for parameter in parameter_names
]
actual_values = [value.double_value for value in future.result().values]
self.assertEqual(expected_values, actual_values)
def get_parameters(self, names):
get_params_request = GetParameters.Request()
get_params_request.names = names
get_params_response = self._get_params_client.call(get_params_request)
return [
Parameter.from_parameter_msg(ParameterMsg(name=name, value=value))
for name, value in zip(names, get_params_response.values)
]
def __init__(self):
super().__init__("queue_client")
self.logger = self.get_logger()
self._cli = self.create_client(GetParameters, "get_parameters")
while not self._cli.wait_for_service(timeout_sec=1.0):
self.logger.info("service not available, waiting again...")
self.future = None
self.req = GetParameters.Request()
self.logger.info("----- [queue_client]Start! -----")
def test_robot_description(self):
client = self.node.create_client(
GetParameters, 'robot_state_publisher/get_parameters')
assert client.wait_for_service(5.)
request = GetParameters.Request(names=['robot_description'])
recv_robot_description = client.call_async(request)
rclpy.spin_until_future_complete(self.node,
recv_robot_description,
timeout_sec=5.)
assert recv_robot_description.done()
def get_params(node, node_name, param_names, wfs_timeout=5.0):
client = node.create_client(GetParameters, '/{name}/get_parameters'.format(name=node_name))
resp = None
with BackgroundExecutor(node):
assert client.wait_for_service(timeout_sec=wfs_timeout)
request = GetParameters.Request()
request.names = param_names
resp = client.call(request)
# Don't destroy client while spinning ros2/rmw_fastrtps#205
node.destroy_client(client)
return resp
def get_parameters(node, parameter_names, timeout_sec=10.0):
# create client
client = node.create_client(GetParameters,
'consai2r2_description/get_parameters')
# call as soon as ready
ready = client.wait_for_service(timeout_sec)
if not ready:
raise RuntimeError('Wait for service timed out')
request = GetParameters.Request()
request.names = parameter_names
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(
"Failed to get parameters form node 'consai2r2_description'")
return_values = {}
# https://github.com/ros2/ros2cli/blob/780923c046f8e537e884d18bef33a2338f2d409c/ros2param/ros2param/api/__init__.py#L54
for i, pvalue in enumerate(response.values):
if pvalue.type == ParameterType.PARAMETER_BOOL:
value = pvalue.bool_value
elif pvalue.type == ParameterType.PARAMETER_INTEGER:
value = pvalue.integer_value
elif pvalue.type == ParameterType.PARAMETER_DOUBLE:
value = pvalue.double_value
elif pvalue.type == ParameterType.PARAMETER_STRING:
value = pvalue.string_value
elif pvalue.type == ParameterType.PARAMETER_BYTE_ARRAY:
value = pvalue.byte_array_value
elif pvalue.type == ParameterType.PARAMETER_BOOL_ARRAY:
value = pvalue.bool_array_value
elif pvalue.type == ParameterType.PARAMETER_INTEGER_ARRAY:
value = pvalue.integer_array_value
elif pvalue.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
value = pvalue.double_array_value
elif pvalue.type == ParameterType.PARAMETER_STRING_ARRAY:
value = pvalue.string_array_value
elif pvalue.type == ParameterType.PARAMETER_NOT_SET:
value = None
else:
raise RuntimeError(
"Unknown parameter type '{pvalue.type}'".format_map(locals()))
return_values[parameter_names[i]] = value
return return_values
def get_robot_urdf_from_service(node: Node) -> urdf.Robot:
"""Get the robot description from the robot state publisher.
:param node Node to use for making the request.
"""
robot_description_client = node.create_client(
srv_type=GetParameters,
srv_name="/march/robot_state_publisher/get_parameters",
)
wait_for_service(node, robot_description_client, 2)
robot_future = robot_description_client.call_async(
request=GetParameters.Request(names=["robot_description"])
)
rclpy.spin_until_future_complete(node, robot_future)
return urdf.Robot.from_xml_string(robot_future.result().values[0].string_value)
def call_get_parameters(*, node, node_name, parameter_names):
# create client
client = node.create_client(GetParameters, f'{node_name}/get_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 = GetParameters.Request()
request.names = parameter_names
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
# handle response
response = future.result()
return response
def _get_temperature_thresholds(self) -> List[float]:
"""Get the temperature thresholds for this joint.
:return Returns a tuple containing all temperature thresholds
"""
names = ["/march/safety_node/default_temperature_threshold"] + [
f"march/safety_node/temperature_thresholds_{threshold_type}/{self.joint_name}"
for threshold_type in THRESHOLD_TYPES
]
client = self.node.create_client(
srv_type=GetParameters,
srv_name="/march/safety_node/get_parameters")
wait_for_service(self.node, client)
future = client.call_async(GetParameters.Request(names=names))
rclpy.spin_until_future_complete(self.node, future)
return [value.double_value for value in future.result().value]
def test_different_type_raises(self):
cli = self.node.create_client(GetParameters, 'get/parameters')
srv = self.node.create_service(
GetParameters, 'get/parameters',
lambda request, response: 'different response type')
try:
with self.assertRaises(TypeError):
cli.call('different request type')
with self.assertRaises(TypeError):
cli.call_async('different request type')
self.assertTrue(cli.wait_for_service(timeout_sec=20))
future = cli.call_async(GetParameters.Request())
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
with self.assertRaises(TypeError):
rclpy.spin_until_future_complete(self.node, future, executor=executor)
finally:
self.node.destroy_client(cli)
self.node.destroy_service(srv)
def get_dynamixel_parameter(self, name, default_value=None):
try:
req = GetParameters.Request()
req.names.append(name)
self.get_logger().info('Getting dynamixel parameter %s' % name)
future = self.dynamixel_parameter_client.call_async(req)
#rclpy.get_global_executor().spin_until_future_complete(future)
rclpy.spin_until_future_complete(self, future)
res = future.result()
if res is not None:
value = None
pvalue = res.values[0]
if pvalue.type == ParameterType.PARAMETER_BOOL:
value = pvalue.bool_value
elif pvalue.type == ParameterType.PARAMETER_INTEGER:
value = pvalue.integer_value
elif pvalue.type == ParameterType.PARAMETER_DOUBLE:
value = pvalue.double_value
elif pvalue.type == ParameterType.PARAMETER_STRING:
value = pvalue.string_value
elif pvalue.type == ParameterType.PARAMETER_BYTE_ARRAY:
value = pvalue.byte_array_value
elif pvalue.type == ParameterType.PARAMETER_BOOL_ARRAY:
value = pvalue.bool_array_value
elif pvalue.type == ParameterType.PARAMETER_INTEGER_ARRAY:
value = pvalue.integer_array_value
elif pvalue.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
value = pvalue.double_array_value
elif pvalue.type == ParameterType.PARAMETER_STRING_ARRAY:
value = pvalue.string_array_value
elif pvalue.type == ParameterType.PARAMETER_NOT_SET:
value = None
if value is not None:
return value
if default_value is not None:
return default_value
raise RuntimeError()
except Exception as e:
self.get_logger().error('Could not get dynamixel parameter %s' %
name)
raise
def get_nn2depth_sync(self, device_id):
req = GetParameters.Request()
req.names = ['nn2depth' + device_id]
client = self.create_client(GetParameters,
'/depthai_publisher/get_parameters')
while not client.wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'service not available, nn2depth specified so depthai_wrapper needs to be running before starting a listener.'
)
self.future = client.call_async(req)
rclpy.spin_until_future_complete(self, self.future)
if self.future.done():
try:
response = self.future.result()
except Exception as e:
self.get_logger().info('Service call failed %r' % (e, ))
else:
return response.values[0].string_value
return ""
def test_service_timestamps(self):
cli = self.node.create_client(GetParameters, 'get/parameters')
srv = self.node.create_service(GetParameters, 'get/parameters',
lambda request, response: response)
try:
self.assertTrue(cli.wait_for_service(timeout_sec=20))
cli.call_async(GetParameters.Request())
cycle_count = 0
while cycle_count < 5:
with srv.handle:
result = srv.handle.service_take_request(
srv.srv_type.Request)
if result is not None:
request, header = result
self.assertNotEqual(0, header.source_timestamp)
return
else:
time.sleep(0.1)
self.fail('Did not get a request in time')
finally:
self.node.destroy_client(cli)
self.node.destroy_service(srv)
def call_get_parameters(*, node, node_name, parameter_names):
# create client
client = node.create_client(
GetParameters, '/{node_name}/get_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 = GetParameters.Request()
request.names = parameter_names
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 make_get_parameters_request(self, node: str,
names: List[str]) -> List[ParameterValue]:
"""
Make a request to a GetParameters service of a node.
:param node: Node to make the request to.
:param names: Parameter names to request from the node.
:return: Returns the values that are retrieved from the service call.
"""
srv_name = f"{node}/get_parameters"
if srv_name not in self.get_parameter_clients:
client = self.create_client(GetParameters, srv_name)
self.get_parameter_clients[srv_name] = client
else:
client = self.get_parameter_clients[srv_name]
while not client.wait_for_service(timeout_sec=1):
self.get_logger().info(
f"Waiting for {srv_name} to become available.")
future = client.call_async(GetParameters.Request(names=names))
rclpy.spin_until_future_complete(self, future)
return future.result().values
def __is_sim_time_node(timeout_sec=10, return_param=True, default_value=False):
"""
Sends a request to the global sim time node and returns the value of the
use_sim_time parameter. If a 'no_global_sim_time' parameter has been
specified to the node, the function returns the default value.
:param timeout_sec: timeout in seconds before returning the default value
:param return_param: True to return a parameter, False to return the raw
boolean
:param default_value: default value to return
:return A boolean or a parameter, both indicating if Plankton is using
sim time
"""
try:
def get_value(value):
if return_param:
return rclpy.parameter.Parameter(
'use_sim_time',
rclpy.Parameter.Type.BOOL,
value
)
else:
return value
import random
import os
import string
node = None
LENGTH = 5
if not rclpy.ok():
raise RuntimeError('rclpy has not been initialized. Initialize rclpy first')
letter_pool = string.ascii_letters
random_name = 'test_sim_time_' + ''.join(random.choice(letter_pool) for i in range(LENGTH))
random_name += '_%d' % os.getpid()
node = rclpy.create_node(random_name)
if node.has_parameter('no_global_sim_time'):
return get_value(default_value)
start_time = time.time()
sim_time_srv = node.create_client(GetParameters, '/plankton_global_sim_time/get_parameters')
if not sim_time_srv.wait_for_service(timeout_sec=timeout_sec):
node.get_logger().info('service %s not available' % sim_time_srv.srv_name)
return get_value(default_value)
# Compute the remaining time
timeout_sec -= time.time() - start_time
req = GetParameters.Request()
req.names = ['use_sim_time']
future = sim_time_srv.call_async(req)
start_time = time.time()
while not future.done():
rclpy.spin_once(node)
if time.time() - start_time > timeout_sec:
return get_value(default_value)
resp = future.result().values[0].bool_value
return get_value(resp)
except Exception as e:
print('Caught exception: ' + repr(e))
finally:
if node is not None:
node.destroy_node()
node = None
def start_run(self):
print_info("preparing to start run")
# wait to receive sensor data from the environment (e.g., a simulator may need time to startup)
waiting_time = 0.0
waiting_period = 0.5
while not self.received_first_scan and rclpy.ok():
time.sleep(waiting_period)
rclpy.spin_once(self)
waiting_time += waiting_period
if waiting_time > 5.0:
self.get_logger().warning(
'still waiting to receive first sensor message from environment'
)
waiting_time = 0.0
# get the parameter robot_radius from the global costmap
parameters_request = GetParameters.Request(names=['robot_radius'])
parameters_response = self.call_service(
self.global_costmap_get_parameters_service_client,
parameters_request)
self.robot_radius = parameters_response.values[0].double_value
print_info("got robot radius")
# get deleaved reduced Voronoi graph from ground truth map
voronoi_graph = self.ground_truth_map.deleaved_reduced_voronoi_graph(
minimum_radius=2 * self.robot_radius).copy()
minimum_length_paths = nx.all_pairs_dijkstra_path(
voronoi_graph, weight='voronoi_path_distance')
minimum_length_costs = dict(
nx.all_pairs_dijkstra_path_length(voronoi_graph,
weight='voronoi_path_distance'))
costs = defaultdict(dict)
for i, paths_dict in minimum_length_paths:
for j in paths_dict.keys():
if i != j:
costs[i][j] = minimum_length_costs[i][j]
# in case the graph has multiple unconnected components, remove the components with less than two nodes
too_small_voronoi_graph_components = list(
filter(lambda component: len(component) < 2,
nx.connected_components(voronoi_graph)))
for graph_component in too_small_voronoi_graph_components:
voronoi_graph.remove_nodes_from(graph_component)
if len(voronoi_graph.nodes) < 2:
self.write_event(
self.get_clock().now(),
'insufficient_number_of_nodes_in_deleaved_reduced_voronoi_graph'
)
raise RunFailException(
"insufficient number of nodes in deleaved_reduced_voronoi_graph, can not generate traversal path"
)
# get greedy path traversing the whole graph starting from a random node
traversal_path_indices = list()
current_node = random.choice(list(voronoi_graph.nodes))
nodes_queue = set(
nx.node_connected_component(voronoi_graph, current_node))
while len(nodes_queue):
candidates = list(
filter(lambda node_cost: node_cost[0] in nodes_queue,
costs[current_node].items()))
candidate_nodes, candidate_costs = zip(*candidates)
next_node = candidate_nodes[int(np.argmin(candidate_costs))]
traversal_path_indices.append(next_node)
current_node = next_node
nodes_queue.remove(next_node)
# convert path of nodes to list of poses (as deque so they can be popped)
self.traversal_path_poses = deque()
for node_index in traversal_path_indices:
pose = Pose()
pose.position.x, pose.position.y = voronoi_graph.nodes[node_index][
'vertex']
q = pyquaternion.Quaternion(axis=[0, 0, 1],
radians=np.random.uniform(
-np.pi, np.pi))
pose.orientation = Quaternion(w=q.w, x=q.x, y=q.y, z=q.z)
self.traversal_path_poses.append(pose)
# publish the traversal path for visualization
traversal_path_msg = Path()
traversal_path_msg.header.frame_id = self.fixed_frame
traversal_path_msg.header.stamp = self.get_clock().now().to_msg()
for traversal_pose in self.traversal_path_poses:
traversal_pose_stamped = PoseStamped()
traversal_pose_stamped.header = traversal_path_msg.header
traversal_pose_stamped.pose = traversal_pose
traversal_path_msg.poses.append(traversal_pose_stamped)
self.traversal_path_publisher.publish(traversal_path_msg)
# pop the first pose from traversal_path_poses and set it as initial pose
self.initial_pose = PoseWithCovarianceStamped()
self.initial_pose.header.frame_id = self.fixed_frame
self.initial_pose.header.stamp = self.get_clock().now().to_msg()
self.initial_pose.pose.pose = self.traversal_path_poses.popleft()
self.initial_pose.pose.covariance = list(
self.initial_pose_covariance_matrix.flat)
self.num_goals = len(self.traversal_path_poses)
# set the position of the robot in the simulator
self.call_service(self.pause_physics_service_client, Empty.Request())
print_info("called pause_physics_service")
time.sleep(1.0)
robot_entity_state = EntityState(name=self.robot_entity_name,
pose=self.initial_pose.pose.pose)
set_entity_state_response = self.call_service(
self.set_entity_state_service_client,
SetEntityState.Request(state=robot_entity_state))
if not set_entity_state_response.success:
self.write_event(self.get_clock().now(),
'failed_to_set_entity_state')
raise RunFailException("could not set robot entity state")
print_info("called set_entity_state_service")
time.sleep(1.0)
self.call_service(self.unpause_physics_service_client, Empty.Request())
print_info("called unpause_physics_service")
time.sleep(1.0)
# ask lifecycle_manager to startup all its managed nodes
startup_request = ManageLifecycleNodes.Request(
command=ManageLifecycleNodes.Request.STARTUP)
startup_response: ManageLifecycleNodes.Response = self.call_service(
self.lifecycle_manager_service_client, startup_request)
if not startup_response.success:
self.write_event(self.get_clock().now(), 'failed_to_startup_nodes')
raise RunFailException("lifecycle manager could not startup nodes")
self.write_event(self.get_clock().now(), 'run_start')
self.run_started = True
self.send_goal()
# result_params = result_params.result
params_found = False
while not params_found:
query_param_name = get_string(
"Enter a parameter name (or part of it): ")
if query_param_name:
filtered_params = [
p for p in all_param_names if query_param_name in p
]
else:
filtered_params = all_param_names
if filtered_params:
params_found = True
else:
print(f"Failed to found paramteres with mask *{query_param_name}*")
req = GetParameters.Request()
req.names = filtered_params
f = get_params_client.call_async(req)
while not f.done():
rclpy.spin_once(node, timeout_sec=0.1)
all_param_values = f.result().values
parameters_prompt = []
l = 0
for p_name in filtered_params:
if len(p_name) > l:
l = len(p_name)
names_values = {}
names_types = {}
for name, value in zip(filtered_params, all_param_values):
t_str, v = get_param_type_and_value(value)
names_types[name] = t_str