def main(args=None):
# print(sys.argv)
rclcpp_init('')
rclpy.init(args=args)
node = Node('moveit_py_demo')
node.get_logger().info('moveit2 python interface demo')
rclcpp_shutdown()
def wait_for_messages(self, themes):
# tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}}
self.func_data = dict([[theme_name, {}] for theme_name in themes])
node = Node('wait_for_messages')
for theme_name in themes:
theme = self.themes[theme_name]
node.get_logger().info('Subscribing %s on topic: %s' %
(theme_name, theme['topic']))
self.func_data[theme_name]['sub'] = node.create_subscription(
theme['msg_type'], theme['topic'],
theme['callback'](theme_name), qos.qos_profile_sensor_data)
self.tfBuffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node)
self.prev_time = time.time()
break_timeout = False
while not break_timeout:
rclpy.spin_once(node, timeout_sec=1)
if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
break_timeout = True
self.unregister_all(node, self.func_data)
node.destroy_node()
return self.func_data
def init(node: Node):
node.get_logger().info("Starting localization handler")
global_params_dict = get_parameters_from_other_node(
node, "parameter_blackboard", ["field_length", "team_id"])
global_params = [("field_length", global_params_dict["field_length"]),
("team_id", global_params_dict["team_id"])]
node.declare_parameters(parameters=global_params, namespace="")
blackboard = LocalizationBlackboard(node)
dirname = get_package_share_directory("bitbots_localization_handler")
node.get_logger().info(dirname)
dsd = DSD(blackboard, "debug/dsd/localization", node)
dsd.register_actions(os.path.join(dirname, 'actions'))
dsd.register_decisions(os.path.join(dirname, 'decisions'))
dsd.load_behavior(os.path.join(dirname, 'localization.dsd'))
node.create_subscription(PoseWithCovarianceStamped, "pose_with_covariance",
blackboard._callback_pose, 1)
node.create_subscription(GameState, "gamestate",
blackboard.gamestate.gamestate_callback, 1)
node.create_subscription(RobotControlState, "robot_state",
blackboard._callback_robot_control_state, 1)
return dsd
def __init__(self,
node: Node,
topic_name: str,
message_type: str,
expected_values: Dict[str, str],
callback: Callable[[bool, Dict[str, str], Dict[str, str]],
None] = default_callback):
self.__callback = callback
self.__expected_values = expected_values
self.__message_type = message_type
self.__node = node
latching_qos = QoSProfile(depth=1,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
node.get_logger().info("Subscribing to \"{}\" of type \"{}\"".format(
topic_name, message_type))
node.get_logger().info(
"Expecting to recieve message with value \"{}\"".format(
str(expected_values)))
self.__sub = node.create_subscription(get_message(message_type),
topic_name,
self.__sub_callback,
qos_profile=latching_qos)
def subscriber(node: Node,
topic_name: str,
message_type: MsgType,
callback: Callable[[MsgType], Any],
qos_profile: QoSProfile,
report_lost_messages: bool,
future=None,
timeout=None) -> Optional[str]:
"""Initialize a node with a single subscription and spin."""
event_callbacks = None
if report_lost_messages:
event_callbacks = SubscriptionEventCallbacks(
message_lost=message_lost_event_callback)
try:
node.create_subscription(message_type,
topic_name,
callback,
qos_profile,
event_callbacks=event_callbacks)
except UnsupportedEventTypeError:
assert report_lost_messages
print(f"The rmw implementation '{get_rmw_implementation_identifier()}'"
' does not support reporting lost messages')
rclpy.spin_until_future_complete(node, future, timeout)
if future is not None:
if not future.done():
node.get_logger().info('Timeout occured')
def init():
"""
Initialize new components needed for head_behavior:
blackboard, dsd, rostopic subscriber
"""
rclpy.init(args=None)
node = Node("head_node2")
# This is a general purpose initialization function provided by moved
# It is used to correctly initialize roscpp which is used in the collision checker module
roscpp_init('collision_checker', [])
blackboard = HeadBlackboard()
rclpy.Subscriber('head_mode', HeadModeMsg, blackboard.head_capsule.head_mode_callback, queue_size=1)
rclpy.Subscriber("ball_position_relative_filtered", PoseWithCovarianceStamped, blackboard.world_model.ball_filtered_callback)
rclpy.Subscriber('joint_states', JointState, blackboard.head_capsule.joint_state_callback)
blackboard.head_capsule.position_publisher = self.create_publisher(JointCommand, "head_motor_goals", 10)
blackboard.head_capsule.visual_compass_record_trigger = self.create_publisher(Header, blackboard.config['visual_compass_trigger_topic'], 5)
dirname = os.path.dirname(os.path.realpath(__file__))
dsd = DSD(blackboard, 'debug/dsd/head_behavior')
dsd.register_actions(os.path.join(dirname, 'actions'))
dsd.register_decisions(os.path.join(dirname, 'decisions'))
dsd.load_behavior(os.path.join(dirname, 'head_behavior.dsd'))
node.get_logger().debug("Head Behavior completely loaded")
return dsd
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 wait_for_message(self, params, msg_type=msg_Image):
topic = params['topic']
print ('connect to ROS with name: %s' % self.node_name)
rclpy.init()
node = Node(self.node_name)
out_filename = params.get('filename', None)
if (out_filename):
self.fout = open(out_filename, 'w')
if msg_type is msg_Imu:
col_w = 20
print ('Writing to file: %s' % out_filename)
columns = ['frame_number', 'frame_time(sec)', 'accel.x', 'accel.y', 'accel.z', 'gyro.x', 'gyro.y', 'gyro.z']
line = ('{:<%d}'*len(columns) % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(*columns) + '\n'
sys.stdout.write(line)
self.fout.write(line)
node.get_logger().info('Subscribing on topic: %s' % topic)
sub = node.create_subscription(msg_type, topic, self.callback, qos.qos_profile_sensor_data)
self.prev_time = time.time()
break_timeout = False
while not any([(not rclpy.ok()), break_timeout, self.result]):
rclpy.spin_once(node)
if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
break_timeout = True
node.destroy_subscription(sub)
return self.result
def main(args=None):
rclpy.init(args=args)
node = Node("py_test")
node.get_logger().info("Hello ROS2")
rclpy.spin(node)
rclpy.shutdown()
def wait_for_service(
node: Node, client: Client, timeout: Optional[float] = SERVICE_TIMEOUT
):
"""
Wait for a service to become available.
:param node: Node to use for logging
:param client: Client of the service.
:param timeout: Optional timeout to wait before logging again
"""
while not client.wait_for_service(timeout_sec=timeout):
node.get_logger().info(f"Waiting for {client.srv_name} to become available")
class Pause(object):
"""
Always go in and out of pause by manual penalty.
Go in and out of pause by game controller, if manual penalty is not active.
"""
def __init__(self):
rclpy.init(args=None)
self.node = Node("Pause")
self.penalty_manual = False
self.game_controller_penalty = False
self.pause = False
self.manual_penalize_service = self.node.create_service(
ManualPenalize, "manual_penalize", self.manual_update)
self.pause_publisher = self.node.create_publisher(Bool, "pause",
10) # todo latch
self.speak_publisher = self.node.create_publisher(Audio, "speak", 10)
while rclpy.ok():
try:
rclpy.spin_once(self.node)
except (ExternalShutdownException, KeyboardInterrupt):
exit(0)
def manual_update(self, req):
if req.penalize == 0:
# off
self.penalty_manual = False
elif req.penalize == 1:
# on
self.penalty_manual = True
elif req.penalize == 2:
# switch
self.penalty_manual = not self.penalty_manual
else:
self.node.get_logger().error(
"Manual penalize call with unspecified request")
self.set_pause(self.penalty_manual)
return True
def set_pause(self, state):
self.pause = state
if state:
text = "Pause!"
else:
text = "Unpause!"
self.node.get_logger().warn(text)
speak(text, self.speak_publisher, speaking_active=True, priority=90)
self.pause_publisher.publish(Bool(data=state))
def __init__(
self,
node: Node,
topic_and_expected_values_pairs: List[TopicAndValuesPair],
eqaulity_type: EqualityType,
callback: Callable[
[bool, Dict[str, bool], List[TopicAndValuesPair], EqualityType],
None] = default_multi_message_callback):
self.__equality_type = eqaulity_type
self.__callback = callback
self.__topic_and_expected_values_pairs = topic_and_expected_values_pairs
node.get_logger().info("Setting up subscriptions")
for topic_and_expected_values_pair in topic_and_expected_values_pairs:
#Create MessageEqualityTester that store the result in __message_store when a new message
#is received, and then perform the predicate check (which calls the callback)
def cb(val: bool,
actual_msg: Dict,
expected_values: Dict,
topic_and_expected_values_pair=topic_and_expected_values_pair
):
#Store the value of that message
self.__message_store[
topic_and_expected_values_pair.topic_name] = val
node.get_logger().info("Message received ({}/{})".format(
len(self.__message_store),
len(topic_and_expected_values_pairs)))
if (len(self.__message_store) == len(
topic_and_expected_values_pairs)):
node.get_logger().info(
"All messages received, performing check")
self.__perform_check()
tester = MessageEqualityTester(
node, topic_and_expected_values_pair.topic_name,
topic_and_expected_values_pair.topic_type,
topic_and_expected_values_pair.values, cb)
self.__testers.append(tester)
node.get_logger().info(
"Waiting for at least one message on each topic...")
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 main(args=None):
rclpy.init(args=args)
parser = argparse.ArgumentParser()
parser.add_argument('controller_name', help='Name of the controller')
parser.add_argument('-c',
'--controller-manager',
help='Name of the controller manager ROS node',
default='/controller_manager',
required=False)
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
controller_name = args.controller_name
controller_manager_name = args.controller_manager
node = Node('unspawner_' + controller_name)
try:
# Ignore returncode, because message is already printed and we'll try to unload anyway
ret = switch_controllers(node, controller_manager_name,
[controller_name], [], True, True, 5.0)
node.get_logger().info('Stopped controller')
ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().info('Failed to unload controller')
return 1
node.get_logger().info('Unloaded controller')
return 0
finally:
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 run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
# The logic for responding to this joystick press is:
# 1. Save off the current state of active.
# 2. Update the current state of active based on buttons and axes.
# 3. If this command is currently not active, return without publishing.
# 4. If this is a msg_value, and the value of the previous active is the same as now,
# debounce and return without publishing.
# 5. In all other cases, publish. This means that this is a msg_value and the button
# transitioned from 0 -> 1, or it means that this is an axis mapping and data should
# continue to be published without debouncing.
last_active = self.active
self.update_active_from_buttons_and_axes(joy_state)
if not self.active:
return
if self.msg_value is not None and last_active == self.active:
return
if self.msg_value is not None:
# This is the case for a static message.
msg = self.msg_value
else:
# This is the case to forward along mappings.
msg = self.topic_type()
for mapping, values in self.axis_mappings.items():
if 'axis' in values:
if len(joy_state.axes) > values['axis']:
val = joy_state.axes[values['axis']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} axes (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.axes), values['axis']))
val = 0.0
elif 'button' in values:
if len(joy_state.buttons) > values['button']:
val = joy_state.buttons[values['button']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} buttons (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.buttons), values['button']))
val = 0.0
elif 'value' in values:
# Pass on the value as its Python-implicit type
val = values.get('value')
else:
node.get_logger().error(
'No Supported axis_mappings type found in: {}'.format(mapping))
val = 0.0
set_member(msg, mapping, val)
# If there is a stamp field, fill it with now().
if hasattr(msg, 'header'):
msg.header.stamp = node.get_clock().now().to_msg()
self.pub.publish(msg)
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 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 __init__(self,
node: Node,
action_name: str,
delay: float,
future: Optional[Future] = None):
self.__actsrvr = ActionServer(
node,
Trigger,
action_name,
execute_callback=self.__execute_callback,
cancel_callback=lambda cancel_request: (print(''), node.get_logger(
).info("Canceling goal..."), CancelResponse.ACCEPT)[-1],
goal_callback=lambda goal_request:
(inprog := self.__goal_handle is not None, self.__node.get_logger(
).info("Rejecting as action allready in progress")
if inprog else None, GoalResponse.REJECT
if inprog else GoalResponse.ACCEPT)[-1])
self.__node = node
self.__future = future
self.__delay = delay
self.__action_name = action_name
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 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 decorated_function():
#SETUP
print("Setup fixture")
rclpy.init()
node = Node('minimal_publisher')
pub1 = node.create_publisher(String, 'topic1', latching_qos)
msg1 = String()
msg1.data = "hello1"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello1", "topic1"))
pub1.publish(msg1)
pub2 = node.create_publisher(String, 'topic2', latching_qos)
msg2 = String()
msg2.data = "hello2"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello2", "topic2"))
pub2.publish(msg2)
pub3 = node.create_publisher(String, 'topic3', latching_qos)
msg3 = String()
msg3.data = "hello3"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello3", "topic3"))
pub3.publish(msg3)
exe = SingleThreadedExecutor()
exe.add_node(node)
future_message = Future()
#TEST
result = function(future_message, exe)
#TEARDOWN
print("Teardown fixture")
node.destroy_node()
rclpy.shutdown()
time.sleep(0.2)
return result
def run():
node = Node("testHeadBehaviour")
pub_ball = node.create_publisher(BallInImage, "ball_in_image", 1)
pub_hmg = node.create_publisher(JointTrajectory, "head_motor_goals", 1)
hmg = JointTrajectory()
goal = JointTrajectoryPoint()
goal.positions = [0, 0]
goal.velocities = [0, 0]
hmg.points = [goal]
counter = 320
direction = 1
node.get_logger().info("Create Test")
rclpy.init(args=None)
pub_hmg.publish(hmg)
rate = node.create_rate(4)
node.get_logger().debug("Laeuft...")
while rclpy.ok():
# Ball in Image
ball = BallInImage()
ball.center.x = counter
if(counter > 340 or counter < 300):
direction *= -1
counter += direction
else:
counter += direction
ball.center.y = 200
ball.diameter = 10
ball.confidence = 1
balls = BallInImageArray()
balls.candidates.append(ball)
pub_ball.publish(balls)
node.get_logger().info("Published ball: %s" % counter)
rate.sleep()
timer.cancel()
rclpy.init(args=sys.argv)
node = Node("diag_listener", namespace=namespace)
if namespace == "/":
time.sleep(1)
nns = node.get_node_names_and_namespaces()
discovered_ns = {ns for (_, ns) in nns if ns != "/"}
if discovered_ns:
ns_selection = {
i + 1: ns
for (i, ns) in enumerate(sorted(discovered_ns))
}
node.get_logger().warn(
f"Namespace not specified, use '__ns:=/XXXX' argument for specific namespace"
)
print("Discovered namespaces:")
for i, ns in ns_selection.items():
print(f"\t{i}: {ns}")
try:
selection = input("Please enter desired namespace number: ")
except KeyboardInterrupt:
node.destroy_node()
sys.exit(0)
if int(selection) not in ns_selection.keys():
print("WRONG")
node.destroy_node()
sys.exit(0)
ns = ns_selection[int(selection)]
print(f"Restarting in namespace: {ns}")
class HardwareControlManager:
def __init__(self):
rclpy.init(args=None)
self.node = Node("hcm",
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
multi_executor = MultiThreadedExecutor()
multi_executor.add_node(self.node)
self.spin_thread = threading.Thread(target=multi_executor.spin,
args=(),
daemon=True)
#self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
self.spin_thread.start()
self.blackboard = None
# --- Initialize Node ---
# Otherwise messages will get lost, bc the init is not finished
self.node.get_clock().sleep_for(Duration(seconds=0.1))
self.node.get_logger().debug("Starting hcm")
self.simulation_active = self.node.get_parameter("simulation_active")
# dsd
self.blackboard = HcmBlackboard(self.node)
self.blackboard.animation_action_client = ActionClient(
self.node, PlayAnimation, 'animation')
self.blackboard.dynup_action_client = ActionClient(
self.node, Dynup, 'dynup')
dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd"
self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node)
self.dsd.register_actions(os.path.join(dirname, 'actions'))
self.dsd.register_decisions(os.path.join(dirname, 'decisions'))
self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd'))
self.hcm_deactivated = False
# Publisher / subscriber
self.joint_goal_publisher = self.node.create_publisher(
JointCommand, 'DynamixelController/command', 1)
self.hcm_state_publisher = self.node.create_publisher(
RobotControlState, 'robot_state', 1) # todo latch
self.blackboard.speak_publisher = self.node.create_publisher(
Audio, 'speak', 1)
# important to make sure the connection to the speaker is established, for next line
self.node.get_clock().sleep_for(Duration(seconds=0.1))
speak("Starting hcm", self.blackboard.speak_publisher, priority=50)
self.node.create_subscription(Imu, "imu/data", self.update_imu, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_left/filtered",
self.update_pressure_left, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_right/filtered",
self.update_pressure_right, 1)
self.node.create_subscription(JointCommand, "walking_motor_goals",
self.walking_goal_callback, 1)
self.node.create_subscription(AnimationMsg, "animation",
self.animation_callback, 1)
self.node.create_subscription(JointCommand, "dynup_motor_goals",
self.dynup_callback, 1)
self.node.create_subscription(JointCommand, "head_motor_goals",
self.head_goal_callback, 1)
self.node.create_subscription(JointCommand, "record_motor_goals",
self.record_goal_callback, 1)
self.node.create_subscription(JointCommand, "kick_motor_goals",
self.kick_goal_callback, 1)
self.node.create_subscription(Bool, "pause", self.pause, 1)
self.node.create_subscription(JointState, "joint_states",
self.joint_state_callback, 1)
self.node.create_subscription(PointStamped, "cop_l", self.cop_l_cb, 1)
self.node.create_subscription(PointStamped, "cop_r", self.cop_r_cb, 1)
self.node.create_subscription(Bool, "core/power_switch_status",
self.power_cb, 1)
self.node.create_subscription(Bool, "hcm_deactivate",
self.deactivate_cb, 1)
self.node.create_subscription(DiagnosticArray, "diagnostics_agg",
self.blackboard.diag_cb, 1)
self.node.add_on_set_parameters_callback(self.on_set_parameters)
self.main_loop()
def deactivate_cb(self, msg):
self.hcm_deactivated = msg.data
def pause(self, msg):
""" Updates the stop state for the state machine"""
self.blackboard.stopped = msg.data
def update_imu(self, msg):
"""Gets new IMU values and computes the smoothed values of these"""
self.blackboard.last_imu_update_time = msg.header.stamp
self.blackboard.accel = numpy.array([
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
])
self.blackboard.gyro = numpy.array([
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
])
self.blackboard.quaternion = numpy.array(([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]))
self.blackboard.smooth_gyro = numpy.multiply(
self.blackboard.smooth_gyro, 0.95) + numpy.multiply(
self.blackboard.gyro, 0.05)
self.blackboard.smooth_accel = numpy.multiply(
self.blackboard.smooth_accel, 0.99) + numpy.multiply(
self.blackboard.accel, 0.01)
self.blackboard.not_much_smoothed_gyro = numpy.multiply(
self.blackboard.not_much_smoothed_gyro, 0.5) + numpy.multiply(
self.blackboard.gyro, 0.5)
self.blackboard.imu_msg = msg
def update_pressure_left(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[0] = msg.left_front
self.blackboard.pressures[1] = msg.left_back
self.blackboard.pressures[2] = msg.right_front
self.blackboard.pressures[3] = msg.right_back
def update_pressure_right(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[4] = msg.left_front
self.blackboard.pressures[5] = msg.left_back
self.blackboard.pressures[6] = msg.right_front
self.blackboard.pressures[7] = msg.right_back
def on_set_parameters(self, config, level):
""" Dynamic reconfigure of the fall checker values."""
# just pass on to the StandupHandler, as all the variables are located there
self.blackboard.fall_checker.update_reconfigurable_values(
config, level)
self.blackboard.pickup_accel_threshold = config[
"pick_up_accel_threshold"]
return config
def walking_goal_callback(self, msg):
self.blackboard.last_walking_goal_time = self.node.get_clock().now()
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
self.joint_goal_publisher.publish(msg)
def dynup_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.STARTUP, RobotControlState.FALLEN,
RobotControlState.GETTING_UP, RobotControlState.CONTROLLABLE
]:
self.joint_goal_publisher.publish(msg)
def head_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
# we can move our head
self.joint_goal_publisher.publish(msg)
def record_goal_callback(self, msg):
if msg.joint_names == []:
# record tells us that its finished
self.blackboard.record_active = False
else:
self.blackboard.record_active = True
self.joint_goal_publisher.publish(msg)
def kick_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.KICKING, RobotControlState.CONTROLLABLE
]:
# we can perform a kick
self.joint_goal_publisher.publish(msg)
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp
if msg.request:
self.node.get_logger().info(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# dsd should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# coming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# coming from outside
# check if we can run an animation now
if self.blackboard.current_state != RobotControlState.CONTROLLABLE:
self.node.get_logger().warn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(
msg.position.points
) > 0 and self.blackboard.current_state != RobotControlState.GETTING_UP:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1.0] * len(out_msg.joint_names)
out_msg.velocities = [-1.0] * len(out_msg.joint_names)
out_msg.max_currents = [-1.0] * len(out_msg.joint_names)
if msg.position.points[0].effort:
out_msg.max_currents = [
-x for x in msg.position.points[0].effort
]
if self.blackboard.shut_down_request:
# there are sometimes transmission errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def joint_state_callback(self, msg: JointState):
self.blackboard.last_motor_update_time = msg.header.stamp
self.blackboard.previous_joint_state = self.blackboard.current_joint_state
self.blackboard.current_joint_state = msg
def cop_l_cb(self, msg):
self.blackboard.cop_l_msg = msg
def cop_r_cb(self, msg):
self.blackboard.cop_r_msg = msg
def power_cb(self, msg):
self.blackboard.is_power_on = msg.data
def main_loop(self):
""" Keeps updating the DSD and publish its current state.
All the forwarding of joint goals is directly done in the callbacks to reduce latency. """
last_loop_start_time = self.node.get_clock().now()
while rclpy.ok() and not self.blackboard.shut_down_request:
try:
loop_start_time = self.node.get_clock().now()
#can happen in simulation due to bad implementation in rclpy
if (last_loop_start_time != loop_start_time):
last_loop_start_time = loop_start_time
if self.hcm_deactivated:
self.blackboard.current_state = RobotControlState.CONTROLLABLE
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
else:
self.blackboard.current_time = self.node.get_clock(
).now()
try:
self.dsd.update()
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
except IndexError:
# this error will happen during shutdown procedure, just ignore it
pass
self.node.get_clock().sleep_until(loop_start_time +
Duration(seconds=1 / 500.0))
except (ExternalShutdownException, KeyboardInterrupt):
if not self.simulation_active:
self.on_shutdown_hook()
exit(0)
if not self.simulation_active:
self.on_shutdown_hook()
self.spin_thread.join()
def on_shutdown_hook(self):
if not self.blackboard:
return
# we got external shutdown, tell it to the DSD, it will handle it
self.blackboard.shut_down_request = True
self.node.get_logger().warn(
"You're stopping the HCM. The robot will sit down and power off its motors."
)
speak("Stopping HCM", self.blackboard.speak_publisher, priority=50)
# now wait for it finishing the shutdown procedure
while not self.blackboard.current_state == RobotControlState.HCM_OFF:
# we still have to update everything
self.blackboard.current_time = self.node.get_clock().now()
self.dsd.update()
self.hcm_state_publisher.publish(self.blackboard.current_state)
self.node.get_clock().sleep_for(Duration(seconds=0.01))
#!/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)
def main(args=None):
rclpy.init(args=args)
parser = argparse.ArgumentParser()
parser.add_argument('controller_name', help='Name of the controller')
parser.add_argument('-c',
'--controller-manager',
help='Name of the controller manager ROS node',
default='/controller_manager',
required=False)
parser.add_argument(
'-p',
'--param-file',
help=
'Controller param file to be loaded into controller node before configure',
required=False)
parser.add_argument(
'--stopped',
help='Load and configure the controller, however do not start them',
action='store_true',
required=False)
parser.add_argument(
'-t',
'--controller-type',
help=
'If not provided it should exist in the controller manager namespace',
default=None,
required=False)
parser.add_argument(
'-u',
'--unload-on-kill',
help='Wait until this application is interrupted and unload controller',
action='store_true')
parser.add_argument('--controller-manager-timeout',
help='Time to wait for the controller manager',
required=False,
default=10,
type=int)
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
controller_name = args.controller_name
controller_manager_name = make_absolute(args.controller_manager)
param_file = args.param_file
controller_type = args.controller_type
controller_manager_timeout = args.controller_manager_timeout
if param_file and not os.path.isfile(param_file):
raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT),
param_file)
node = Node('spawner_' + controller_name)
try:
if not wait_for_controller_manager(node, controller_manager_name,
controller_manager_timeout):
node.get_logger().error('Controller manager not available')
return 1
if is_controller_loaded(node, controller_manager_name,
controller_name):
node.get_logger().info(
'Controller already loaded, skipping load_controller')
else:
if controller_type:
ret = subprocess.run([
'ros2', 'param', 'set', controller_manager_name,
controller_name + '.type', controller_type
])
ret = load_controller(node, controller_manager_name,
controller_name)
if not ret.ok:
# Error message printed by ros2 control
return 1
node.get_logger().info('Loaded ' + controller_name)
if param_file:
ret = subprocess.run(
['ros2', 'param', 'load', controller_name, param_file])
if ret.returncode != 0:
# Error message printed by ros2 param
return ret.returncode
node.get_logger().info('Loaded ' + param_file + ' into ' +
controller_name)
ret = configure_controller(node, controller_manager_name,
controller_name)
if not ret.ok:
node.get_logger().info('Failed to configure controller')
return 1
if not args.stopped:
ret = switch_controllers(node, controller_manager_name, [],
[controller_name], True, True, 5.0)
if not ret.ok:
node.get_logger().info('Failed to start controller')
return 1
node.get_logger().info('Configured and started ' + controller_name)
if not args.unload_on_kill:
return 0
try:
node.get_logger().info(
'Waiting until interrupt to unload controllers')
while True:
time.sleep(1)
except KeyboardInterrupt:
if not args.stopped:
node.get_logger().info(
'Interrupt captured, stopping and unloading controller')
ret = switch_controllers(node, controller_manager_name,
[controller_name], [], True, True,
5.0)
if not ret.ok:
node.get_logger().info('Failed to stop controller')
return 1
node.get_logger().info('Stopped controller')
ret = unload_controller(node, controller_manager_name,
controller_name)
if not ret.ok:
node.get_logger().info('Failed to unload controller')
return 1
node.get_logger().info('Unloaded controller')
return 0
finally:
rclpy.shutdown()
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()
def main(args=None):
rclpy.init(args=args)
node = Node("no_oop")
node.get_logger().info("This is no oop node.")
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = Node('my_node_name')
node.get_logger().info("Hello ROS2")
rclpy.spin(node)
rclpy.shutdown()
