Exemplo n.º 1
0
 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)
Exemplo n.º 2
0
    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)
Exemplo n.º 5
0
 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)
     ]
Exemplo n.º 6
0
    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()
Exemplo n.º 8
0
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
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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)
Exemplo n.º 11
0
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
Exemplo n.º 12
0
    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]
Exemplo n.º 13
0
 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)
Exemplo n.º 14
0
 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 ""
Exemplo n.º 16
0
 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)
Exemplo n.º 17
0
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
Exemplo n.º 18
0
    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
Exemplo n.º 19
0
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()
Exemplo n.º 21
0
 # 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