def subscribe_and_spin(
self,
node: Node,
topic_name: str,
message_type: MsgType,
qos_profile: QoSProfile,
no_report_lost_messages: bool,
raw: bool
) -> Optional[str]:
"""Initialize a node with a single subscription and spin."""
event_callbacks = None
if not no_report_lost_messages:
event_callbacks = SubscriptionEventCallbacks(
message_lost=_message_lost_event_callback)
try:
node.create_subscription(
message_type,
topic_name,
self._subscriber_callback,
qos_profile,
event_callbacks=event_callbacks,
raw=raw)
except UnsupportedEventTypeError:
assert not no_report_lost_messages
node.create_subscription(
message_type,
topic_name,
self._subscriber_callback,
qos_profile,
event_callbacks=None,
raw=raw)
rclpy.spin(node)
def subscriber(node: Node, topic_name: str, message_type: MsgType,
callback: Callable[[MsgType], Any],
qos_profile: QoSProfile) -> Optional[str]:
"""Initialize a node with a single subscription and spin."""
if message_type is None:
topic_names_and_types = get_topic_names_and_types(
node=node, include_hidden_topics=True)
try:
expanded_name = expand_topic_name(topic_name, node.get_name(),
node.get_namespace())
except ValueError as e:
raise RuntimeError(e)
try:
validate_full_topic_name(expanded_name)
except rclpy.exceptions.InvalidTopicNameException as e:
raise RuntimeError(e)
for n, t in topic_names_and_types:
if n == expanded_name:
if len(t) > 1:
raise RuntimeError(
"Cannot echo topic '%s', as it contains more than one type: [%s]"
% (topic_name, ', '.join(t)))
message_type = t[0]
break
else:
raise RuntimeError(
'Could not determine the type for the passed topic')
msg_module = get_message(message_type)
node.create_subscription(msg_module, topic_name, callback, qos_profile)
rclpy.spin(node)
def subscriber(node: Node, topic_name: str, message_type: MsgType,
callback: Callable[[MsgType], Any],
qos_profile: QoSProfile) -> Optional[str]:
"""Initialize a node with a single subscription and spin."""
node.create_subscription(message_type, topic_name, callback, qos_profile)
rclpy.spin(node)
def __init__(self, node: Node) -> None:
self.node = node
self.waypoints = []
self.balls = []
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer, self.node)
self.pubBackDoor = node.create_publisher(Wrench, "/force_back_door",
qos_profile_services_default)
self.pubFrontDoor = node.create_publisher(
Wrench, "/force_front_door", qos_profile_services_default)
self.pubVel = node.create_publisher(Float32, "/vel",
qos_profile_services_default)
self.pubPointA = node.create_publisher(Point, "/pointA",
qos_profile_services_default)
self.pubPointB = node.create_publisher(Point, "/pointB",
qos_profile_services_default)
self.pubTarget = node.create_publisher(Point, "/target",
qos_profile_services_default)
node.create_subscription(Float32MultiArray, "/balls_coords",
self.__balls_callback,
qos_profile_sensor_data)
node.create_subscription(Float64MultiArray, "/waypoints",
self.__wp_callback, qos_profile_sensor_data)
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__(self,
buffer: Buffer,
node: Node,
*,
spin_thread: bool = False,
qos: Optional[Union[QoSProfile, int]] = None,
static_qos: Optional[Union[QoSProfile, int]] = None) -> None:
"""
Constructor.
:param buffer: The buffer to propagate changes to when tf info updates.
:param node: The ROS2 node.
:param spin_thread: Whether to create a dedidcated thread to spin this node.
:param qos: A QoSProfile or a history depth to apply to subscribers.
:param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers.
"""
if qos is None:
qos = QoSProfile(
depth=100,
durability=DurabilityPolicy.VOLATILE,
history=HistoryPolicy.KEEP_LAST,
)
if static_qos is None:
static_qos = QoSProfile(
depth=100,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
)
self.buffer = buffer
self.node = node
# Default callback group is mutually exclusive, which would prevent waiting for transforms
# from another callback in the same group.
self.group = ReentrantCallbackGroup()
self.tf_sub = node.create_subscription(TFMessage,
'/tf',
self.callback,
qos,
callback_group=self.group)
self.tf_static_sub = node.create_subscription(
TFMessage,
'/tf_static',
self.static_callback,
static_qos,
callback_group=self.group)
if spin_thread:
self.executor = SingleThreadedExecutor()
def run_func():
self.executor.add_node(self.node)
self.executor.spin()
self.executor.remove_node(self.node)
self.dedicated_listener_thread = Thread(target=run_func)
self.dedicated_listener_thread.start()
def __init__(
self,
node: Node,
topic: str,
message_type: type,
updater: Updater,
frequency: float,
):
self._frequency_params = FrequencyStatusParam({"min": frequency})
self._updater = updater
self._diagnostics = {}
node.create_subscription(message_type, topic, self._cb, qos_profile=10)
def Robotiq2FGripperStatusListener():
"""Initialize the node and subscribe to the Robotiq2FGripperRobotInput topic."""
rclpy.init()
node = Node('Robotiq2FGripperStatusListener')
node.create_subscription(inputMsg, "Robotiq2FGripperRobotInput",
printStatus, 1)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.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__(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 __init__(self, node: Node, joint_name: str, topic: str,
msg_type: type):
self.node = node
self.joint_name = joint_name
node.create_subscription(msg_type, topic, self._cb, qos_profile=10)
(
self._default_threshold,
self._thresholds_warning,
self._thresholds_non_fatal,
self._thresholds_fatal,
) = self._get_temperature_thresholds()
self._timestamp = None
self._temperature = None
def mainLoop(device):
#Gripper is a 2F with a TCP connection
gripper = robotiq_2f_gripper_control.baseRobotiq2FGripper.robotiqbaseRobotiq2FGripper()
gripper.client = robotiq_modbus_rtu.comModbusRtu.communication()
#We connect to the address received as an argument
gripper.client.connectToDevice(device)
node = Node('robotiq2FGripper')
#The Gripper status is published on the topic named 'Robotiq2FGripperRobotInput'
pub = node.create_publisher(inputMsg, 'Robotiq2FGripperRobotInput', 1)
#The Gripper command is received from the topic named 'Robotiq2FGripperRobotOutput'
sud = node.create_subscription(outputMsg, 'Robotiq2FGripperRobotOutput', gripper.refreshCommand, 1)
#We loop
while rclpy.ok():
#Get and publish the Gripper status
status = gripper.getStatus()
pub.publish(status)
#Wait a little
#rospy.sleep(0.05)
#Send the most recent command
gripper.sendCommand()
#Wait a little
#rospy.sleep(0.05)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
topic = '/ifm3d/camera/unit_vectors'
topic_type = Image
rclpy.init(args=args)
qos_profile = QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
)
node = Node('uvec_listener')
sub = node.create_subscription(
topic_type,
topic,
lambda msg: print(msg),
qos_profile)
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.destroy_node()
rclpy.shutdown()
finally:
return 0
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 make_mock_subscription(namespace, topic_name, cli_args=None):
node = Node('node_name', namespace=namespace, cli_args=cli_args)
return node.create_subscription(
msg_type=Empty,
topic=topic_name,
callback=lambda _: None,
qos_profile=10,
)
def rosinit(self):
node_name = 'circle'
rclpy.init()
node = Node(node_name)
msg = Float32()
print('Spinning: {}'.format(node_name))
self.publeft = node.create_publisher(Float32, '/wheel_left')
self.pubright = node.create_publisher(Float32, '/wheel_right')
if self.problem == 'triangle':
self.goal_pt = next(self.tri_pts)
self.goal_pt = next(self.tri_pts)
self.goal_theta = next(self.tri_thetas)
self.goal_theta = next(self.tri_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.tri_callback)
elif self.problem == 'square':
self.goal_pt = next(self.sqr_pts)
self.goal_pt = next(self.sqr_pts)
self.goal_theta = next(self.sqr_thetas)
self.goal_theta = next(self.sqr_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.sqr_callback)
else:
# Circle
msg.data = 5.0
self.publeft.publish(msg)
msg.data = msg.data * (15.0 + 1.2) / 15
self.pubright.publish(msg)
try:
rclpy.spin(node)
except KeyboardInterrupt:
print('Shutting down...')
msg.data = 0.0
self.publeft.publish(msg)
self.pubright.publish(msg)
node.destroy_node()
rclpy.shutdown()
def main():
args = sys.argv
if len(args) != 2 and len(args) != 3:
print("Usage: joystick_differential {topic-in} [topic-out]")
print("Joystick should be {topic-in}/joystick")
print(
"Wheels will be {topic-out}/steer, {topic-out}/left_wheel, and {topic-out}/right_wheel"
)
print("If no {topic-out} given, {topic-in} will be used for both")
return
channelin = channelout = args[1]
if len(args) == 3:
channelout = args[2]
channelin = channelin + "/joystick"
rclpy.init(args=args)
node = Node("joystick_differential")
publeft = node.create_publisher(Float32, channelout + "/left_wheel")
pubright = node.create_publisher(Float32, channelout + "/right_wheel")
pubsteer = node.create_publisher(Float32, channelout + "/steer")
def joystick_callback(msg):
if len(msg.axes) < 2:
print("Not enough joystick axes")
return
speed = msg.axes[1]
steer = -msg.axes[0]
phi1 = phi2 = 1 / R * speed * SPEED
phi3 = math.pi / 4.0 * steer
publishWheelVelocity(publeft, pubright, phi1, phi2)
publishWheelSteer(pubsteer, phi3)
node.create_subscription(Joy, channelin, joystick_callback)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def test_get_subscription_topic_name_after_remapping(topic_name, namespace, cli_args, expected):
node = Node('node_name', namespace=namespace, cli_args=cli_args)
sub = node.create_subscription(
msg_type=Empty,
topic=topic_name,
callback=lambda _: None,
qos_profile=10,
)
assert sub.topic_name == expected
def init(self, node: Node, yellow_: bool) -> None:
self.node_ = node
self.yellow_ = yellow_
# Robots
self.p_allies_ = node.create_subscription(Robots, 'allies',
self.update_allies, 10)
self.p_opponents_ = node.create_subscription(Robots, 'opponents',
self.update_opponents, 10)
# Ball
self.p_ball_ = node.create_subscription(Ball, 'ball', self.update_ball,
10)
# GameController
self.p_gc_ = node.create_subscription(Referee, 'gc', self.update_gc,
10)
def main():
args = sys.argv
if len(args) != 2:
print("Usage: joystick_listener {topic}")
return
channel = str(args[1])
print("Listening on topic \'" + channel + "\'")
rclpy.init(args=args)
node = Node("joystick_snooper")
node.create_subscription(Joy, channel, joystick_callback)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def main():
args = sys.argv
if len(args) != 2:
print("Usage: lidar_listener topic")
return
channel = str(args[1])
print("Listening on topic \'" + channel + "\'")
rclpy.init(args=args)
node = Node("lidar_snooper")
node.create_subscription(LaserScan, channel, lidar_callback)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def __init__(self, node: Node, topic: str, msg_type: type):
node.create_subscription(msg_type, topic, self.cb, qos_profile=10)
# callback variables
self._timestamp: Optional[Time] = None
self._joint_names: Optional[List[str]] = None
self._position: Optional[List[float]] = None
self._velocity: Optional[List[float]] = None
self._effort: Optional[List[float]] = None
# robot properties
self._lower_soft_limits = {}
self._upper_soft_limits = {}
self._velocity_limits = {}
self._effort_limits = {}
robot = get_robot_urdf_from_service(node)
for joint in robot.joints:
self.set_joint_limits(joint)
def subscriber(node: Node, topic_name: str, message_type: MsgType,
callback: Callable[[MsgType], Any], qos_profile: QoSProfile,
report_lost_messages: bool, raw: bool) -> 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,
raw=raw)
except UnsupportedEventTypeError:
assert report_lost_messages
print(f"The rmw implementation '{get_rmw_implementation_identifier()}'"
' does not support reporting lost messages')
rclpy.spin(node)
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, updater: Updater):
"""Initializes an gait diagnostic which analyzes gait and subgait states.
:type updater: diagnostic_updater.Updater
"""
self._goal_sub = node.create_subscription(
topic="/march/gait_selection/current_gait",
msg_type=CurrentGait,
callback=self._cb_goal,
qos_profile=10,
)
self._gait_msg = None
updater.add("Gait", self._diagnostics)
def __init__(self, node: Node, msg_type: MsgType, topic: str,
callback: Callable, print_incoming_msg: bool = False) -> None:
# Check if a ros2 node is provided.
if (not isinstance(node, Node) or not issubclass(type(node), Node)):
raise TypeError(
"Input argument 'node' is not of type rclpy.node.Node!")
# Check if topic already created.
for operating_subscription in node.subscriptions:
if topic[0] != "/":
if "/" + topic is operating_subscription.topic:
raise AttributeError(
f"Subscription for topic, /{topic}, already created!")
if topic is operating_subscription.topic:
raise AttributeError(
f"Subscription for topic, {topic}, already created!")
self.node = node
self.topic = topic
self.msg_type = msg_type
self.data = None
self.__thread_state = False
self.__widgets = {
"start_btn": widgets.Button(description='Start'),
"stop_btn": widgets.Button(description='Stop'),
"out": widgets.Output(layout={'border': '1px solid gray'}),
}
if print_incoming_msg:
self.__subscription = node.create_subscription(
msg_type, topic, self.__print_msg(self.__data_msg(callback)),
10)
else:
self.__subscription = node.create_subscription(
msg_type, topic, self.__data_msg(callback), 10)
def __init__(self, node: Node, updater: Updater, joint_names: List[str]):
"""Initialize an MotorController diagnostic which analyzes MotorController states.
:type updater: diagnostic_updater.Updater
"""
self.node = node
self._sub = node.create_subscription(
msg_type=MotorControllerState,
topic="/march/motor_controller_states",
callback=self._cb,
qos_profile=10,
)
self._motor_controller_state = None
for i, joint_name in enumerate(joint_names):
updater.add(f"MotorController {joint_name}", self._diagnostic(i))
def main():
if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv:
print ('USAGE:')
print('echo_metadata.py <topic>')
print('Demo for listening on given metadata topic.')
print('App subscribes on given topic')
print('App then prints metadata from messages')
print('')
print('Example: echo_metadata.py /camera/depth/metadata')
print('')
exit(-1)
topic = sys.argv[1]
rclpy.init()
node = Node('metadata_tester')
depth_sub = node.create_subscription(Metadata, topic, metadata_cb, qos.qos_profile_sensor_data)
rclpy.spin(node)
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))
class ROS2TopicManager(object):
"""
"""
##
# @if jp
# @brief コンストラクタ
#
# コンストラクタ
#
# @param self
#
# @else
# @brief Constructor
#
# @param self
#
# @endif
def __init__(self):
self._thread = None
self._loop = True
#mgr = OpenRTM_aist.Manager.instance()
# mgr.addManagerActionListener(ManagerActionListener(self))
#self._rtcout = mgr.getLogbuf("ROS2TopicManager")
##
# @if jp
# @brief デストラクタ
#
#
# @param self
#
# @else
#
# @brief self
#
# @endif
def __del__(self):
pass
##
# @if jp
# @brief ROS2初期化
#
# @param self
# @param args rclpy.initの引数
#
# @else
#
# @brief
#
# @param self
# @param args
#
# @endif
def start(self, args=[]):
rclpy.init(args=args)
self._node = Node("openrtm")
def spin():
while self._loop:
rclpy.spin_once(self._node, timeout_sec=0.01)
self._thread = threading.Thread(target=spin)
self._thread.daemon = True
self._thread.start()
##
# @if jp
# @brief 終了処理
#
# @param self
#
# @else
#
# @brief
#
# @param self
#
# @endif
def shutdown(self):
if self._node:
self._loop = False
self._node.destroy_node()
# rclpy.try_shutdown()
# if self._thread:
# self._thread.join()
##
# @if jp
# @brief Publisherオブジェクト生成
#
# @param self
# @param msgtype メッセージ型
# @param topic トピック名
# @return Publisherオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @return
#
# @endif
def createPublisher(self, msgtype, topic):
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_publisher(msgtype, topic)
return None
##
# @if jp
# @brief Subscriberオブジェクト生成
#
# @param self
# @param msgtype メッセージ型
# @param topic トピック名
# @param listener コールバック関数
# @return Subscriberオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @param listener
# @return
#
# @endif
def createSubscriber(self, msgtype, topic, listener):
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_subscription(msgtype, topic, listener)
return None
def deletePublisher(self, pub):
pass
def deleteSubscriber(self, sub):
pass
##
# @if jp
# @brief インスタンス取得
#
# @return インスタンス
#
# @else
#
# @brief
#
# @return インスタンス
#
# @endif
def instance(args=[]):
global manager
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if manager is None:
manager = ROS2TopicManager()
manager.start(args)
return manager
instance = staticmethod(instance)
##
# @if jp
# @brief ROS2TopicManagerを初期化している場合に終了処理を呼び出す
#
#
# @else
#
# @brief
#
#
# @endif
def shutdown_global():
global manager
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if manager is not None:
manager.shutdown()
manager = None
shutdown_global = staticmethod(shutdown_global)
