def __init__(self, node: Node, gait_name: str = "balanced_walk", default_walk: Gait = None): self.gait_name = gait_name self._node = node self._default_walk = default_walk self._constructing = False self.logger = Logger(self._node, __class__.__name__) self._current_subgait = None self._current_subgait_duration = Duration(0) self._start_time = None self._end_time = None self._current_time = None self.capture_point_event = Event() self.capture_point_result = None self._capture_point_service = { "left_leg": node.create_client(srv_name="/march/capture_point/foot_left", srv_type=CapturePointPose), "right_leg": node.create_client(srv_name="/march/capture_point/foot_right", srv_type=CapturePointPose), } self.moveit_event = Event() self.moveit_trajectory_result = None self._moveit_trajectory_service = node.create_client( srv_name="/march/moveit/get_trajectory", srv_type=GetMoveItTrajectory)
def main(args=None): rclpy.init(args=args) node = Node("task_power_rune2019_client") cli = node.create_client(SetMode, "task_power_rune/set_mode") while not cli.wait_for_service(timeout_sec=1.0): node.get_logger().info('service not available, waiting again...') print(msg) old_attr = termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) while rclpy.ok(): if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]: key=sys.stdin.read(1) print(key) if (key == 'q' or key == 'Q'): set_mode_req(cli,0x00) elif(key=='w' or key=='W'): set_mode_req(cli,0x01) elif(key=='e' or key=='E'): set_mode_req(cli,0x02) elif(key=='r' or key=='R'): set_mode_req(cli,0x03) elif(key=='a' or key=='A'): set_mode_req(cli,0x10) elif(key=='s' or key=='S'): set_mode_req(cli,0x11) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr) node.destroy_node() rclpy.shutdown()
def main(args=None): rclpy.init(args=args) node = Node("add_two_ints_no_oop") client = node.create_client(AddTwoInts, "add_two_ints") while not client.wait_for_service(1.0): node.get_logger().warn("Waiting for Server Add Two Ints...") request = AddTwoInts.Request() request.a = 3 request.b = 8 future = client.call_async(request) rclpy.spin_until_future_complete(node, future) try: response = future.result() node.get_logger().info( str(request.a) + " + " + str(request.b) + " = " + str(response.sum)) except Exception as e: node.get_logger().error("Service call failed %r" % (e, )) rclpy.shutdown()
def __init__(self, node:Node): self.node = node self.shut_down_request = False self.last_initialized = None self.initialized = False self.use_sim_time = self.node.get_parameter('use_sim_time').value # we only need tf in simulation. don't use it otherwise to safe performance if self.use_sim_time: self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=10)) self.tf_listener = tf2.TransformListener(self.tf_buffer, node) self.odom_frame = node.get_parameter('odom_frame').get_parameter_value().string_value self.base_footprint_frame = node.get_parameter('base_footprint_frame').get_parameter_value().string_value self.field_length = node.get_parameter('field_length').get_parameter_value().double_value # services self.reset_filter_proxy = node.create_client(ResetFilter, 'reset_localization') self.stop_filter_proxy = node.create_client(SetPaused, 'pause_localization') # Pose self.last_pose_update_time = None self.poseX = 0 self.poseY = 0 self.orientation = np.array([0, 0, 0, 1]) self.covariance = np.array([]) #GameState self.gamestate = GameStatusCapsule(node) #Robot Control State self.robot_control_state = None self.last_robot_control_state = None #Get up self.last_state_get_up = False #Picked up self.last_state_pickup = False #Last init action self.last_init_action_type = False self.last_init_odom_transform = None
def get_joint_names_from_service(node: Node) -> List[str]: """Get the joint names from the robot information node.""" joint_names_client = node.create_client( srv_type=GetJointNames, srv_name="/march/robot_information/get_joint_names", ) wait_for_service(node, joint_names_client) future = joint_names_client.call_async(request=GetJointNames.Request()) rclpy.spin_until_future_complete(node, future) return future.result().joint_names
def get_robots(node: Node): get_robots_client = node.create_client(GetRobots, "/GetRobots") req = GetRobots.Request() while not get_robots_client.wait_for_service(timeout_sec=3.0): node.get_logger().info( 'Parameter service not available, waiting again...') future = get_robots_client.call_async(req) rclpy.spin_until_future_complete(node, future) if future.result() is not None: robot_names = future.result().robots node.get_logger().info('Number of robots: %d' % (len(robot_names))) return robot_names else: node.get_logger().info('Service call failed %r' % (future.exception(), ))
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 main (args=None): rclpy.init(args=args) client=Node("My_client_node") client_obj=client.create_client(AddTwoInts,"First_server") while client_obj.wait_for_service(0.5)== False: client.get_logger().warn("wait for server node") req=AddTwoInts.Request() req.a=15 req.b=10 future_obj=client_obj.call_async(req) rclpy.spin_until_future_complete(client,future_obj) reponse=future_obj.result() client.get_logger().error(str(reponse.sum)) rclpy.shutdown()
def get_update_rate(node: Node) -> float: get_update_rate_client = node.create_client( GetGymUpdateRate, "/GetGymUpdateRate", qos_profile=qos_profile_services_default) req1 = GetGymUpdateRate.Request() while not get_update_rate_client.wait_for_service(timeout_sec=3.0): node.get_logger().warn( 'Parameter service not available, waiting again...') future = get_update_rate_client.call_async(req1) rclpy.spin_until_future_complete(node, future) if future.result() is not None: update_rate = future.result().update_rate node.get_logger().info(f'Gym update rate set to: {update_rate}Hz') return update_rate else: node.get_logger().warn(f'Service call failed {future.exception}')
def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None: super().__init__(name, config, 'buttons', 'axes') self.name = name service_name = config['service_name'] service_type = get_interface_type(config['interface_type'], 'srv') self.request = service_type.Request() if 'service_request' in config: # Set the message fields in the request in the constructor. This request will be used # during runtime, and has the side benefit of giving the user early feedback if the # config can't work. set_message_fields(self.request, config['service_request']) self.service_client = node.create_client(service_type, service_name) self.client_ready = False
def get_joints(node: Node, robot_name: str) -> Collection[float]: get_joints_client = node.create_client(GetAllJoints, "/GetAllControlJoints") req = GetAllJoints.Request() req.robot = robot_name while not get_joints_client.wait_for_service(timeout_sec=3.0): node.get_logger().info( 'Parameter service not available, waiting again...') future = get_joints_client.call_async(req) rclpy.spin_until_future_complete(node, future) if future.result() is not None: joint_names = future.result().joints node.get_logger().info('Number of joints: %d' % (len(joint_names))) return joint_names else: node.get_logger().info('Service call failed %r' % (future.exception(), )) return []
def main(args=None): rclpy.init(args=args) node = Node("add_nums_client") client = node.create_client(AddTwoInts, "add_nums_service") while not client.wait_for_service(1): node.get_logger().info("waiting for " + client.srv_name) request = AddTwoInts.Request() request.a = 1 request.b = 2 future = client.call_async(request) rclpy.spin_until_future_complete(node, future) try: response = future.result() node.get_logger().info("Got " + str(response.sum)) except Exception as e: node.get.get_logger().error("Service error %r" % (e,)) rclpy.shutdown()
def test_lifecycle_services(request): lc_node_name = 'test_lifecycle_services_lifecycle' lc_node = LifecycleNode(lc_node_name) client_node = Node('test_lifecycle_services_client') get_state_cli = client_node.create_client(lifecycle_msgs.srv.GetState, f'/{lc_node_name}/get_state') change_state_cli = client_node.create_client( lifecycle_msgs.srv.ChangeState, f'/{lc_node_name}/change_state') get_available_states_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableStates, f'/{lc_node_name}/get_available_states') get_available_transitions_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableTransitions, f'/{lc_node_name}/get_available_transitions') get_transition_graph_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableTransitions, f'/{lc_node_name}/get_transition_graph') for cli in ( get_state_cli, change_state_cli, get_available_states_cli, get_available_transitions_cli, get_transition_graph_cli, ): assert cli.wait_for_service(5.) # lunch a thread to spin the executor, so we can make sync service calls easily executor = SingleThreadedExecutor() executor.add_node(client_node) executor.add_node(lc_node) thread = Thread(target=executor.spin) thread.start() def cleanup(): # Stop executor and join thread. # This cleanup is run even if an assertion fails. executor.shutdown() thread.join() request.addfinalizer(cleanup) # test all services req = lifecycle_msgs.srv.GetState.Request() resp = get_state_cli.call(req) assert resp.current_state.label == 'unconfigured' req = lifecycle_msgs.srv.ChangeState.Request() req.transition.id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE resp = change_state_cli.call(req) assert resp.success req = lifecycle_msgs.srv.GetState.Request() resp = get_state_cli.call(req) assert resp.current_state.label == 'inactive' req = lifecycle_msgs.srv.GetAvailableStates.Request() resp = get_available_states_cli.call(req) states_labels = {state.label for state in resp.available_states} assert states_labels == { 'unknown', 'unconfigured', 'inactive', 'active', 'finalized', 'configuring', 'cleaningup', 'shuttingdown', 'activating', 'deactivating', 'errorprocessing', '' } req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_available_transitions_cli.call(req) transitions_labels = { transition_def.transition.label for transition_def in resp.available_transitions } assert transitions_labels == {'activate', 'cleanup', 'shutdown'} req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_transition_graph_cli.call(req) transitions_labels = { transition_def.transition.label for transition_def in resp.available_transitions } assert transitions_labels == { 'configure', 'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error', 'transition_failure', 'transition_success' }
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from std_srvs.srv import Empty from bitbots_msgs.msg import Buttons from rclpy.node import Node from rclpy.duration import Duration rclpy.init(args=None) node = Node('zero_on_button') zero_l = node.create_client(Empty, "/foot_pressure_left/set_foot_zero") zero_r = node.create_client(Empty, "/foot_pressure_right/set_foot_zero") button_prev_state = False press_time = node.get_clock().now() - Duration(seconds=1.0) def cb(msg): global button_prev_state, press_time print("New msg") print(msg.button1) print(not button_prev_state) print(node.get_clock().now() - press_time > Duration(seconds=1.0)) if msg.button3 and not button_prev_state and node.get_clock().now( ) - press_time > Duration(seconds=1.0): zero_l() zero_r() press_time = node.get_clock() button_prev_state = msg.button3
class Gazebo: def __init__(self, use_gui: bool = True, launch_description: Type[LaunchDescription] = None): # Launch gazebo with the arm in a new Process self.__pid_list = [] if launch_description is None: launch_description = ut_launch.generate_launch_description_lobot_arm( use_gui) self.launch_subp = ut_launch.startLaunchServiceProcess( launch_description) self.node = Node("openai_ros2_gazebo_node") self.node.set_parameters( [Parameter('use_sim_time', Parameter.Type.BOOL, True)]) self._reset_sim = self.node.create_client(Empty, '/reset_simulation') self._physics_pause_client = self.node.create_client( Empty, '/pause_physics') self._physics_unpause_client = self.node.create_client( Empty, '/unpause_physics') self._entity_delete = self.node.create_client(DeleteEntity, '/delete_entity') def pause_sim(self) -> None: while not self._physics_pause_client.wait_for_service(timeout_sec=1.0): self.node.get_logger().info( '/pause_physics service not available, waiting again...') pause_future = self._physics_pause_client.call_async(Empty.Request()) rclpy.spin_until_future_complete(self.node, pause_future) # print("Pausing simulation") def unpause_sim(self) -> None: while not self._physics_unpause_client.wait_for_service( timeout_sec=1.0): self.node.get_logger().info( '/unpause_physics service not available, waiting again...') unpause_future = self._physics_unpause_client.call_async( Empty.Request()) rclpy.spin_until_future_complete(self.node, unpause_future) # print("Unpausing simulation") def reset_sim(self) -> None: while not self._reset_sim.wait_for_service(timeout_sec=1.0): self.node.get_logger().info( '/reset_simulation service not available, waiting again...') reset_future = self._reset_sim.call_async(Empty.Request()) # print("Resetting simulation") rclpy.spin_until_future_complete(self.node, reset_future) def delete_entity(self, entity_name: str) -> bool: """ Deletes an entity from gazebo by name of the entity :param entity_name: :return: bool representing whether the delete is successful """ while not self._entity_delete.wait_for_service(timeout_sec=1.0): self.node.get_logger().info( '/delete_entity service not available, waiting again...') delete_future = self._entity_delete.call_async( DeleteEntity.Request(name=entity_name)) print("Deleting entity") rclpy.spin_until_future_complete(self.node, delete_future) if delete_future.done(): delete_result = delete_future.result() delete_success_status = delete_result.success delete_message = delete_result.status_message print(delete_message) return delete_success_status def __del__(self): # For some reason this is not called, but whatever print(f'Gazebo class destructor called, cleaning up processes...') current_pid = os.getpid() children = psutil.Process(current_pid).children(recursive=True) for child in children: name = child.name() if name == "gzserver" or name == "gzclient" or name == "params_server_exec": print(f'Killing process {child.name()} with pid {child.pid}') child.kill()
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from bitbots_msgs.srv import Leds from std_msgs.msg import ColorRGBA rclpy.init(args=None) from rclpy.node import Node node = Node('test_leds') client = node.create_client(Leds, "/set_leds") if not client.wait_for_service(timeout_sec=10): node.get_logger().fatal("Service not available") exit() request = Leds.Request() for i in range(3): request.leds.append(ColorRGBA()) request.leds[i].r = 1.0 request.leds[i].g = 0.0 request.leds[i].b = 0.0 request.leds[i].a = 1.0 client.call(request)
request.timeout.secs = 1 request.approximate = True request.look_at_goals.append(LookAtGoal()) request.look_at_goals[0].link_name = camera_frame request.look_at_goals[0].weight = 1 request.look_at_goals[0].axis.x = 1 pos_msg = JointCommand() pos_msg.joint_names = ["HeadPan", "HeadTilt"] pos_msg.positions = [0, 0] pos_msg.velocities = [1.5, 1.5] pos_msg.accelerations = [-1, -1] pos_msg.max_currents = [-1, -1] rclpy.wait_for_service("bio_ik/get_bio_ik") bio_ik = node.create_client(GetIK, 'bio_ik/get_bio_ik') publish_motor_goals = node.create_publisher(JointCommand, 'head_motor_goals', 10) while rclpy.ok(): x = float(input('x: ')) y = float(input('y: ')) point = PointStamped() point.header.stamp = node.get_clock().now() point.header.frame_id = base_footprint_frame point.point.x = x point.point.y = y point.point.z = 0 ###### Movement ######
time.sleep(1) done = False while not done: nodes = [((ns + "/") if ns != "/" else "/") + name for name, ns in node.get_node_names_and_namespaces()] nodes = [ fqn for fqn in nodes if "ros2cli" not in fqn and node.get_name() not in fqn ] if not nodes: print("No nodes found, trying again", end="\r") time.sleep(1) continue req_node = select_bullet("Select a node to examine parameters", nodes) list_params_client = node.create_client(ListParameters, req_node + "/list_parameters") get_params_client = node.create_client(GetParameters, req_node + "/get_parameters") set_params_client = node.create_client(SetParameters, req_node + "/set_parameters") time.sleep(0.2) if not list_params_client.service_is_ready(): print( f"Service {list_params_client.srv_name} is not ready, waiting a second" ) time.sleep(1) if not get_params_client.service_is_ready(): print( f"Service {get_params_client.srv_name} is not ready, waiting a second" ) time.sleep(1)
BLINK_DURATION = 0.2 ERROR_TIMEOUT = 1 rclpy.init(args=None) node = Node('led_error_blink') last_hardware_error_time = None # true means warning, false error warn_not_error = True currently_blinking = False leds_red = False led_set_time = None # set up service and prepare requests led_serv = node.create_client(Leds, "/set_leds") red_request = Leds.Request() red_leds_array = [] for i in range(3): red_led = ColorRGBA() red_led.r = 1.0 red_led.a = 1.0 red_leds_array.append(red_led) red_request.leds = red_leds_array orange_leds_array = [] for i in range(3): orange_led = ColorRGBA() orange_led.r = 1.0 orange_led.g = 0.5