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 main():
rclpy.init()
node = Node("talker")
pub = node.create_publisher(Pose2D, 'robot0/target_velocity')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 10
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
x, y = speedScale * math.sin(
simTime.global_time()), speedScale * 2 * math.cos(
2 * simTime.global_time())
t = 0.
print(x, y)
# Publish velocities
publishTargetVelocity(pub, x, y, t)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
node = Node("talker")
publeft = node.create_publisher(Float32, 'robot0/left_wheel')
pubright = node.create_publisher(Float32, 'robot0/right_wheel')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 2
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
phi1, phi2 = DD_IK(x_t, y_t, simTime.global_time() + 2*math.pi, speedScale)
print(phi1, phi2)
# Publish velocities
publishWheelVelocity(publeft, pubright, phi1, phi2)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
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 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 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('referee_system')
referee_system = SimpleRefereeSystem(node)
rclpy.spin(node)
referee_system
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = Node("simple_node")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
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 main():
rclpy.init()
node = Node('cartesian_compute')
try:
c = CartesianCompute(node)
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
return 0
def main():
rclpy.init()
node = Node('Controller')
pub = node.create_publisher(String, 'DIRECTION', 10)
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
msg = String()
while ip != 'Z':
if (ip == "W" or ip == "S" or ip == "A" or ip == "D" or ip == "Q"):
msg.data = ip
pub.publish(msg)
else:
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=None)
# needed to init rclcpp ros for moveit_bindings
node = Node("bitbots_localization_handler",
automatically_declare_parameters_from_overrides=True)
dsd = init(node)
node.create_timer(1 / 25.0, dsd.update)
multi_executor = MultiThreadedExecutor()
multi_executor.add_node(node)
try:
multi_executor.spin()
except KeyboardInterrupt:
pass
node.destroy_node()
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 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 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 __init__(self):
node = Node('ros2top')
# spin on another thread
thread = Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
self.model = ScenesModel(node)
start_scene = None
while rclpy.ok():
try:
Screen.wrapper(self.screen_main, arguments=[start_scene])
break
except ResizeScreenError as e:
start_scene = e.scene
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
nh = Node('webserver')
nh.declare_parameter('port')
os.chdir(
os.path.dirname(__file__) +
'/../../../../share/pimouse_control2/contents')
param = nh.get_parameter_or('port')
if param.type_ == Parameter.Type.NOT_SET:
port = 8000
else:
port = param.value
handler = http.server.SimpleHTTPRequestHandler
with socketserver.TCPServer(('', port), handler) as httpd:
print('serving at port', port)
httpd.serve_forever()
nh.destroy_node()
rclpy.shutdown()
def publisher(args=None):
"""Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic."""
rclpy.init(args=args)
node = Node('Robotiq2FGripperSimpleController')
pub = node.create_publisher(outputMsg, 'Robotiq2FGripperRobotOutput', 1)
command = outputMsg()
try:
while rclpy.ok():
command = genCommand(askForCommand(command), command)
pub.publish(command)
sleep(0.1)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
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)
def get_hit(msg):
hits = msg.data
for i, hit in enumerate(hits):
hit = struct.unpack('b', hit)[0]
if hit:
print(f'Contact on sensor: {i}')
ros.init()
node = Node('Circle')
left_pub = node.create_publisher(Float32, 'robot0/left_wheel')
right_pub = node.create_publisher(Float32, 'robot0/right_wheel')
contact_sub = node.create_subscription(ByteMultiArray, 'robot0/touches', get_hit)
msg = Float32()
msg.data = 5.0
left_pub.publish(msg)
msg.data = 10.0
right_pub.publish(msg)
try:
ros.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
ros.shutdown()
class Ui(QtWidgets.QMainWindow):
def __init__(self):
super(Ui, self).__init__()
# Initialize UI
self.load_ui()
self.reset_ui_state()
# Init state
self.recording_enabled_target = False
# Create ROS
self.node = Node("asv_status")
# Init ROS Entities
self.init_ros()
# Create Qt timer to poll ROS2 node for events
self.ros_timer = QtCore.QTimer(self)
self.ros_timer.timeout.connect(self.handle_ros)
self.ros_timer.start(10)
# Connect Checkbox signal to publish ROS message with updated state
self.checkbox_recording_enabled.stateChanged.connect(
self.update_recording_enable_target)
# Display window
self.show()
def update_recording_enable_target(self, state):
if (QtCore.Qt.Checked == state):
self.recording_enabled_target = True
else:
self.recording_enabled_target = False
msg = Bool()
msg.data = self.recording_enabled_target
self.pub_req_enable_recording.publish(msg)
def init_ros(self):
# Create ROS Timer for vehicle heartbeat
self.heartbeat_received = False
self.last_heartbeat_time = datetime.datetime.now()
self.heartbeat_timer = self.node.create_timer(1,
self.heartbeat_callback)
# Create QOS profiles
qos_be = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
qos_klr_v = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
qos_klr_tl = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
# Create Subscribers
self.sub_heartbeat = self.node.create_subscription(
msg_type=String,
topic='heartbeat',
callback=self.update_heartbeat,
qos_profile=qos_klr_v)
self.sub_camera_status = self.node.create_subscription(
msg_type=String,
topic='camera_status',
callback=self.update_camera_status,
qos_profile=qos_klr_tl)
self.sub_gps_status = self.node.create_subscription(
msg_type=String,
topic='gps_status',
callback=self.update_gps_status,
qos_profile=qos_klr_tl)
self.sub_storage_status = self.node.create_subscription(
msg_type=String,
topic='storage_status',
callback=self.update_storage_status,
qos_profile=qos_klr_tl)
self.sub_system_time = self.node.create_subscription(
msg_type=String,
topic='system_time',
callback=self.update_system_time,
qos_profile=qos_klr_tl)
self.sub_recording_status = self.node.create_subscription(
msg_type=String,
topic='recording_status',
callback=self.update_recording_status,
qos_profile=qos_klr_tl)
self.sub_recorded_samples = self.node.create_subscription(
msg_type=String,
topic='recorded_samples',
callback=self.update_recorded_samples,
qos_profile=qos_klr_tl)
self.sub_video_left = self.node.create_subscription(
msg_type=CompressedImage,
topic='camera_left/image_raw/compressed',
callback=self.update_video_image,
qos_profile=qos_be)
# Create publisher
self.pub_req_enable_recording = self.node.create_publisher(
msg_type=Bool,
topic='recording_enabled_target',
qos_profile=qos_klr_tl)
# Set initial recording enable target
msg = Bool()
msg.data = self.recording_enabled_target
self.pub_req_enable_recording.publish(msg)
def load_ui(self):
# Get path to Qt UI file
package_share_directory = get_package_share_directory('asv_status')
ui_path = os.path.join(package_share_directory, "ui/asv_status.ui")
# Load UI
uic.loadUi(ui_path, self)
# Find UI elements
self.label_camera_status = self.findChild(QtWidgets.QLabel,
'label_camera_status')
self.label_gps_status = self.findChild(QtWidgets.QLabel,
'label_gps_status')
self.label_storage_status = self.findChild(QtWidgets.QLabel,
'label_storage_status')
self.label_system_time = self.findChild(QtWidgets.QLabel,
'label_system_time')
self.label_recording_status = self.findChild(QtWidgets.QLabel,
'label_recording_status')
self.label_recorded_samples = self.findChild(QtWidgets.QLabel,
'label_recorded_samples')
self.label_video = self.findChild(QtWidgets.QLabel, 'label_video')
self.checkbox_recording_enabled = self.findChild(
QtWidgets.QCheckBox, 'checkbox_recording_enabled')
def reset_ui_state(self):
# Set starting values for labels
self.update_camera_status(String(data="UNKNOWN"))
self.update_gps_status(String(data="UNKNOWN"))
self.update_storage_status(String(data="UNKNOWN"))
self.update_system_time(String(data="UNKNOWN"))
self.update_recording_status(String(data="NOT READY"))
self.update_recorded_samples(String(data="0"))
# Reset video label
self.label_video.clear()
self.label_video.setAlignment(QtCore.Qt.AlignRight
| QtCore.Qt.AlignBottom)
self.label_video.setScaledContents(True)
self.label_video.setText("DISCONNECTED")
def cleanup(self):
self.node.destroy_node()
def handle_ros(self):
rclpy.spin_once(node=self.node, timeout_sec=0)
def heartbeat_callback(self):
# If we haven't received a new heartbeat message in the last 3 seconds, clear the video feed
elapsed_ms = (datetime.datetime.now() -
self.last_heartbeat_time).total_seconds() * 1000
if (elapsed_ms > 3000):
self.last_heartbeat_time = datetime.datetime.now()
if self.heartbeat_received is False:
self.label_video.clear()
self.label_video.setText("DISCONNECTED")
else:
self.label_video.setText("")
self.heartbeat_received = False
# Message handlers
def update_camera_status(self, msg):
self.camera_status = msg.data
self.label_camera_status.setText(msg.data)
def update_gps_status(self, msg):
self.gps_status = msg.data
self.label_gps_status.setText(msg.data)
def update_storage_status(self, msg):
self.storage_status = msg.data
self.label_storage_status.setText(msg.data)
def update_system_time(self, msg):
self.system_time = msg.data
self.label_system_time.setText(msg.data)
def update_recording_status(self, msg):
self.recording_status = msg.data
self.label_recording_status.setText(msg.data)
def update_recorded_samples(self, msg):
self.recorded_samples = msg.data
self.label_recorded_samples.setText(msg.data)
def update_heartbeat(self, msg):
self.heartbeat_received = True
def update_video_image(self, msg):
qp = QPixmap()
if qp.loadFromData(msg.data):
self.label_video.setPixmap(qp)
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 トピック名
# @param qos QoSProfile
# @return Publisherオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @param qos
# @return
#
# @endif
def createPublisher(self, msgtype, topic, qos=None):
global mutex
if qos is None:
qos = QoSProfile(depth=10)
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_publisher(msgtype, topic, qos)
return None
##
# @if jp
# @brief Subscriberオブジェクト生成
#
# @param self
# @param msgtype メッセージ型
# @param topic トピック名
# @param listener コールバック関数
# @param qos QoSProfile
# @return Subscriberオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @param listener
# @param qos
# @return
#
# @endif
def createSubscriber(self, msgtype, topic, listener, qos=None):
global mutex
if qos is None:
qos = QoSProfile(depth=10)
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_subscription(msgtype, topic, listener,
qos)
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)
##
# @if jp
# @brief プロパティからQoSProfileを設定する
# @param prop プロパティ
# @return QoSProfile
#
# @else
#
# @brief
# @param prop
# @return QoSProfile
#
# @endif
def get_qosprofile(prop):
if hasattr(rclpy.qos, "HistoryPolicy"):
HistoryPolicy = rclpy.qos.HistoryPolicy
else:
HistoryPolicy = rclpy.qos.QoSHistoryPolicy
if hasattr(rclpy.qos, "Duration"):
Duration = rclpy.qos.Duration
else:
Duration = rclpy.qos.QoSDuration
if hasattr(rclpy.qos, "ReliabilityPolicy"):
ReliabilityPolicy = rclpy.qos.ReliabilityPolicy
else:
ReliabilityPolicy = rclpy.qos.QoSReliabilityPolicy
if hasattr(rclpy.qos, "DurabilityPolicy"):
DurabilityPolicy = rclpy.qos.DurabilityPolicy
else:
DurabilityPolicy = rclpy.qos.QoSDurabilityPolicy
if hasattr(rclpy.qos, "LivelinessPolicy"):
LivelinessPolicy = rclpy.qos.LivelinessPolicy
else:
LivelinessPolicy = rclpy.qos.QoSLivelinessPolicy
history = HistoryPolicy.KEEP_LAST
history_type = prop.getProperty("history", "KEEP_LAST")
if history_type == "SYSTEM_DEFAULT":
history = HistoryPolicy.SYSTEM_DEFAULT
elif history_type == "KEEP_ALL":
history = HistoryPolicy.KEEP_ALL
else:
history = HistoryPolicy.KEEP_LAST
depth = 10
depth_value_str = prop.getProperty("depth", "10")
try:
depth = int(depth_value_str)
except ValueError:
pass
reliability = ReliabilityPolicy.RELIABLE
reliability_type = prop.getProperty("reliability", "RELIABLE")
if reliability_type == "SYSTEM_DEFAULT":
reliability = ReliabilityPolicy.SYSTEM_DEFAULT
elif reliability_type == "BEST_EFFORT":
reliability = ReliabilityPolicy.BEST_EFFORT
else:
reliability = ReliabilityPolicy.RELIABLE
durability = DurabilityPolicy.VOLATILE
durability_type = prop.getProperty("durability", "VOLATILE")
if durability_type == "SYSTEM_DEFAULT":
durability = DurabilityPolicy.SYSTEM_DEFAULT
elif durability_type == "TRANSIENT_LOCAL":
durability = DurabilityPolicy.TRANSIENT_LOCAL
else:
durability = DurabilityPolicy.VOLATILE
lifespan = Duration(seconds=0, nanoseconds=0)
lifespan_value_str = prop.getProperty("lifespan", "0")
try:
lifespan_value = int(lifespan_value_str)
lifespan = Duration(nanoseconds=lifespan_value)
except ValueError:
pass
deadline = Duration(seconds=0, nanoseconds=0)
deadline_value_str = prop.getProperty("deadline", "0")
try:
deadline_value = int(deadline_value_str)
deadline = Duration(nanoseconds=deadline_value)
except ValueError:
pass
liveliness = LivelinessPolicy.SYSTEM_DEFAULT
liveliness_type = prop.getProperty("liveliness", "SYSTEM_DEFAULT")
if liveliness_type == "AUTOMATIC":
liveliness = LivelinessPolicy.AUTOMATIC
elif liveliness_type == "MANUAL_BY_NODE":
liveliness = LivelinessPolicy.MANUAL_BY_NODE
elif liveliness_type == "MANUAL_BY_TOPIC":
liveliness = LivelinessPolicy.MANUAL_BY_TOPIC
else:
liveliness = LivelinessPolicy.SYSTEM_DEFAULT
liveliness_lease_duration = Duration(seconds=0, nanoseconds=0)
liveliness_lease_duration_value_str = prop.getProperty(
"liveliness_lease_duration", "0")
try:
liveliness_lease_duration_value = int(
liveliness_lease_duration_value_str)
liveliness_lease_duration = Duration(
nanoseconds=liveliness_lease_duration_value)
except ValueError:
pass
avoid_ros_namespace_conventions = False
if OpenRTM_aist.toBool(
prop.getProperty("avoid_ros_namespace_conventions"), "YES",
"NO", False):
avoid_ros_namespace_conventions = True
else:
avoid_ros_namespace_conventions = False
qos = QoSProfile(
history=history,
depth=depth,
reliability=reliability,
durability=durability,
lifespan=lifespan,
deadline=deadline,
liveliness=liveliness,
liveliness_lease_duration=liveliness_lease_duration,
avoid_ros_namespace_conventions=avoid_ros_namespace_conventions)
return qos
get_qosprofile = staticmethod(get_qosprofile)
def main():
args = sys.argv
joyname = ""
if len(args) > 1:
joyname = args[1]
else:
# Iterate over the joystick devices.
print("Available Joysticks:")
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
print('\t%s' % (fn))
sys.exit()
# We'll store the states here.
axis_states = {}
button_states = {}
axis_map = []
button_map = []
# Open the joystick device.
fn = '/dev/input/' + joyname
print('Opening %s...' % fn)
jsdev = open(fn, 'rb')
# Get the device name.
#buf = bytearray(63)
buf = array.array('b', [0] * 64)
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
js_name = buf.tostring()
print('Device name: %s' % js_name)
# Get number of axes and buttons.
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
num_axes = buf[0]
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
num_buttons = buf[0]
# Get the axis map.
buf = array.array('B', [0] * 0x40)
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
for axis in buf[:num_axes]:
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
axis_map.append(axis_name)
axis_states[axis_name] = 0.0
# Get the button map.
buf = array.array('H', [0] * 200)
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
for btn in buf[:num_buttons]:
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
button_map.append(btn_name)
button_states[btn_name] = 0
print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))
# Spin child process to read the joystick
# with blocking reads
events = Queue()
t = Process(target=readJoystick, args=(jsdev, events))
t.start()
rclpy.init()
node = Node("joystick_" + joyname)
pubjoy = node.create_publisher(Joy, joyname + '/joystick')
simTime = SimTimer(False, "veranda/timestamp", node)
# Tick time at 10 hz
dt = 0.1
def cb():
processJoystick(events, button_states, axis_states, button_map,
axis_map)
msg = formatJoystickMessage(button_states, axis_states)
pubjoy.publish(msg)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
t.terminate()
def main():
# INIT RCL ================================================================
rclpy.init()
try:
adpt.init_rclcpp()
except RuntimeError:
# Continue if it is already initialized
pass
# INIT GRAPH ==============================================================
test_graph = graph.Graph()
test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0
test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1
test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2
test_graph.add_waypoint(map_name, [-10.0, 0]) # 3
test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4
test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5
test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6
test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7
test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8
test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9
test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10
assert test_graph.get_waypoint(2).holding_point
assert test_graph.get_waypoint(9).holding_point
assert not test_graph.get_waypoint(10).holding_point
test_graph_legend = \
"""
D : Dispenser
I : Ingestor
H : Holding Point
K : Dock
Numerals : Waypoints
---- : Lanes
"""
test_graph_vis = \
test_graph_legend + \
"""
10(I,K)
|
|
8------9(H)
| |
| |
3------4------5------6------7(D,K)
| |
| |
1------2(H)
|
|
0
"""
test_graph.add_bidir_lane(0, 1) # 0 1
test_graph.add_bidir_lane(1, 2) # 2 3
test_graph.add_bidir_lane(1, 5) # 4 5
test_graph.add_bidir_lane(2, 6) # 6 7
test_graph.add_bidir_lane(3, 4) # 8 9
test_graph.add_bidir_lane(4, 5) # 10 11
test_graph.add_bidir_lane(5, 6) # 12 13
test_graph.add_dock_lane(6, 7, "A") # 14 15
test_graph.add_bidir_lane(5, 8) # 16 17
test_graph.add_bidir_lane(6, 9) # 18 19
test_graph.add_bidir_lane(8, 9) # 20 21
test_graph.add_dock_lane(8, 10, "B") # 22 23
assert test_graph.num_lanes == 24
test_graph.add_key(pickup_name, 7)
test_graph.add_key(dropoff_name, 10)
assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
and dropoff_name in test_graph.keys
# INIT FLEET ==============================================================
profile = traits.Profile(geometry.make_final_convex_circle(1.0))
robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
angular=traits.Limits(1.0, 0.45),
profile=profile)
# Manages delivery or loop requests
adapter = adpt.MockAdapter("TestDeliveryAdapter")
fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph)
# Set up task request callback function
# we will only accept delivery task here
def task_request_cb(task_profile):
from rmf_task_msgs.msg import TaskType
if (task_profile.description.task_type == TaskType.TYPE_DELIVERY):
return True
else:
return False
fleet.accept_task_requests(task_request_cb)
# Set fleet battery profile
battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8)
mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22)
motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
ambient_power_sys = battery.PowerSystem.make(20.0)
ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
ambient_power_sys)
tool_power_sys = battery.PowerSystem.make(10.0)
tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
fleet.set_recharge_threshold(0.2)
b_success = fleet.set_task_planner_params(battery_sys, motion_sink,
ambient_sink, tool_sink)
assert b_success, "set battery param failed"
cmd_node = Node("RobotCommandHandle")
# Test compute_plan_starts, which tries to place the robot on the navgraph
# Your robot MUST be near a waypoint or lane for this to work though!
starts = plan.compute_plan_starts(test_graph, map_name,
[[-10.0], [0.0], [0.0]], adapter.now())
assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]
# Alternatively, if you DO know where your robot is, place it directly!
starts = [plan.Start(adapter.now(), 0, 0.0)]
# Lambda to insert an adapter
def updater_inserter(handle_obj, updater):
updater.update_battery_soc(1.0)
handle_obj.updater = updater
# Manages and executes robot commands
robot_cmd = MockRobotCommand(cmd_node, test_graph)
fleet.add_robot(robot_cmd, "T0", profile, starts,
partial(updater_inserter, robot_cmd))
# INIT DISPENSERS =========================================================
dispenser = MockDispenser(dispenser_name)
ingestor = MockIngestor(ingestor_name)
# INIT TASK SUMMARY OBSERVER ==============================================
# Note: this is used for assertation check on TaskSummary.Msg
observer = TaskSummaryObserver()
# FINAL PREP ==============================================================
rclpy_executor = SingleThreadedExecutor()
rclpy_executor.add_node(cmd_node)
rclpy_executor.add_node(dispenser)
rclpy_executor.add_node(ingestor)
rclpy_executor.add_node(observer)
# GO! =====================================================================
adapter.start()
print("\n")
print("# SENDING SINGLE DELIVERY REQUEST ################################")
print(test_graph_vis)
dispenser.reset()
ingestor.reset()
observer.reset()
observer.add_task(test_name)
task_desc = Type.CPPTaskDescriptionMsg()
# this is the time when the robot reaches the pick up point
task_desc.start_time_sec = int(time.time()) + 50
task_desc.delivery = Type.CPPDeliveryMsg(pickup_name, dispenser_name,
dropoff_name, ingestor_name)
task_profile = Type.CPPTaskProfileMsg()
task_profile.description = task_desc
task_profile.task_id = test_name
adapter.dispatch_task(task_profile)
rclpy_executor.spin_once(1)
for i in range(1000):
if observer.all_tasks_complete():
print("Tasks Complete.")
break
rclpy_executor.spin_once(1)
# time.sleep(0.2)
results = observer.count_successful_tasks()
print("\n== DEBUG TASK REPORT ==")
print("Visited waypoints:", robot_cmd.visited_waypoints)
print(f"Sucessful Tasks: {results[0]} / {results[1]}")
assert results[0] == results[1], "Not all tasks were completed."
error_msg = "Robot did not take the expected route"
assert robot_cmd.visited_waypoints == [
0, 0, 5, 5, 6, 6, 7, 6, 5, 5, 8, 8, 10
], error_msg
# check if unstable partcipant works
# this is helpful for to update the robot when it is
print("Update a custom itinerary to fleet adapter")
traj = schedule.make_trajectory(robot_traits, adapter.now(),
[[3, 0, 0], [1, 1, 0], [2, 1, 0]])
route = schedule.Route("test_map", traj)
participant = robot_cmd.updater.get_unstable_participant()
routeid = participant.last_route_id
participant.set_itinerary([route])
new_routeid = participant.last_route_id
print(f"Previous route id: {routeid} , new route id: {new_routeid}")
assert routeid != new_routeid
# TODO(YL) There's an issue with during shutdown of the adapter, occurs
# when set_itinerary() function above is used. Similarly with a non-mock
# adpater, will need to address this in near future
print("\n~ Shutting Down everything ~")
cmd_node.destroy_node()
observer.destroy_node()
dispenser.destroy_node()
ingestor.destroy_node()
robot_cmd.stop()
adapter.stop()
rclpy_executor.shutdown()
rclpy.shutdown()
def main():
# INIT RCL ================================================================
rclpy.init()
adpt.init_rclcpp()
# INIT GRAPH ==============================================================
map_name = "test_map"
test_graph = graph.Graph()
test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0
test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1
test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2
test_graph.add_waypoint(map_name, [-10.0, 0]) # 3
test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4
test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5
test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6
test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7
test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8
test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9
test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10
assert test_graph.get_waypoint(2).holding_point
assert test_graph.get_waypoint(9).holding_point
assert not test_graph.get_waypoint(10).holding_point
"""
10(D)
|
|
8------9
| |
| |
3------4------5------6------7(D)
| |
| |
1------2
|
|
0
"""
test_graph.add_bidir_lane(0, 1) # 0 1
test_graph.add_bidir_lane(1, 2) # 2 3
test_graph.add_bidir_lane(1, 5) # 4 5
test_graph.add_bidir_lane(2, 6) # 6 7
test_graph.add_bidir_lane(3, 4) # 8 9
test_graph.add_bidir_lane(4, 5) # 10 11
test_graph.add_bidir_lane(5, 6) # 12 13
test_graph.add_dock_lane(6, 7, "A") # 14 15
test_graph.add_bidir_lane(5, 8) # 16 17
test_graph.add_bidir_lane(6, 9) # 18 19
test_graph.add_bidir_lane(8, 9) # 20 21
test_graph.add_dock_lane(8, 10, "B") # 22 23
assert test_graph.num_lanes == 24
test_graph.add_key(pickup_name, 7)
test_graph.add_key(dropoff_name, 10)
assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
and dropoff_name in test_graph.keys
# INIT FLEET ==============================================================
profile = traits.Profile(geometry.make_final_convex_circle(1.0))
robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
angular=traits.Limits(1.0, 0.45),
profile=profile)
# Manages delivery or loop requests
adapter = adpt.MockAdapter("TestDeliveryAdapter")
fleet = adapter.add_fleet("test_fleet", robot_traits, test_graph)
fleet.accept_delivery_requests(lambda x: True)
cmd_node = Node("RobotCommandHandle")
# Test compute_plan_starts, which tries to place the robot on the navgraph
# Your robot MUST be near a waypoint or lane for this to work though!
starts = plan.compute_plan_starts(test_graph, "test_map",
[[-10.0], [0.0], [0.0]], adapter.now())
assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]
# Alternatively, if you DO know where your robot is, place it directly!
starts = [plan.Start(adapter.now(), 0, 0.0)]
# Lambda to insert an adapter
def updater_inserter(handle_obj, updater):
handle_obj.updater = updater
# Manages and executes robot commands
robot_cmd = MockRobotCommand(cmd_node, test_graph)
fleet.add_robot(robot_cmd, "T0", profile, starts,
partial(updater_inserter, robot_cmd))
# INIT TASK OBSERVER ======================================================
at_least_one_incomplete = False
completed = False
def task_cb(msg):
nonlocal at_least_one_incomplete
nonlocal completed
if msg.state == TASK_STATE_COMPLETED:
completed = True
at_least_one_incomplete = False
else:
completed = False
at_least_one_incomplete = True
task_node = rclpy.create_node("task_summary_node")
task_node.create_subscription(TaskSummary, "task_summaries", task_cb, 10)
# INIT DISPENSERS =========================================================
quiet_dispenser = MockQuietDispenser(quiet_dispenser_name)
flaky_dispenser = MockFlakyDispenser(flaky_dispenser_name)
# FINAL PREP ==============================================================
rclpy_executor = SingleThreadedExecutor()
rclpy_executor.add_node(task_node)
rclpy_executor.add_node(cmd_node)
rclpy_executor.add_node(quiet_dispenser)
rclpy_executor.add_node(flaky_dispenser)
last_quiet_state = False
last_flaky_state = False
# GO! =====================================================================
adapter.start()
print("# SENDING NEW REQUEST ############################################")
request = adpt.type.CPPDeliveryMsg("test_delivery", pickup_name,
quiet_dispenser_name, dropoff_name,
flaky_dispenser_name)
quiet_dispenser.reset()
flaky_dispenser.reset()
adapter.request_delivery(request)
rclpy_executor.spin_once(1)
for i in range(1000):
if quiet_dispenser.success_flag != last_quiet_state:
last_quiet_state = quiet_dispenser.success_flag
print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
last_quiet_state)
if flaky_dispenser.success_flag != last_flaky_state:
last_flaky_state = flaky_dispenser.success_flag
print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
last_flaky_state)
if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
rclpy_executor.spin_once(1)
break
rclpy_executor.spin_once(1)
time.sleep(0.2)
print("\n== DEBUG TASK REPORT ==")
print("Visited waypoints:", robot_cmd.visited_waypoints)
print("At least one incomplete:", at_least_one_incomplete)
print("Completed:", completed)
print()
assert len(robot_cmd.visited_waypoints) == 6
assert all([x in robot_cmd.visited_waypoints for x in [0, 5, 6, 7, 8, 10]])
assert robot_cmd.visited_waypoints[0] == 2
assert robot_cmd.visited_waypoints[5] == 4
assert robot_cmd.visited_waypoints[6] == 3
assert robot_cmd.visited_waypoints[7] == 1
assert robot_cmd.visited_waypoints[8] == 2
assert robot_cmd.visited_waypoints[10] == 1
assert at_least_one_incomplete
# Uncomment this to send a second request.
# TODO(CH3):
# But note! The TaskManager has to be fixed first to allow task pre-emption
# print("# SENDING NEW REQUEST ##########################################")
# request = adpt.type.CPPDeliveryMsg("test_delivery_two",
# dropoff_name,
# flaky_dispenser_name,
# pickup_name,
# quiet_dispenser_name)
# quiet_dispenser.reset()
# flaky_dispenser.reset()
# adapter.request_delivery(request)
# rclpy_executor.spin_once(1)
#
# for i in range(1000):
# if quiet_dispenser.success_flag != last_quiet_state:
# last_quiet_state = quiet_dispenser.success_flag
# print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
# last_quiet_state)
#
# if flaky_dispenser.success_flag != last_flaky_state:
# last_flaky_state = flaky_dispenser.success_flag
# print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
# last_flaky_state)
#
# if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
# rclpy_executor.spin_once(1)
# break
#
# rclpy_executor.spin_once(1)
# time.sleep(0.2)
#
# print("\n== DEBUG TASK REPORT ==")
# print("Visited waypoints:", robot_cmd.visited_waypoints)
# print("At least one incomplete:", at_least_one_incomplete)
# print("Completed:", completed)
# print()
task_node.destroy_node()
cmd_node.destroy_node()
quiet_dispenser.destroy_node()
flaky_dispenser.destroy_node()
rclpy_executor.shutdown()
rclpy.shutdown()
