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
