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