Mock Battery & LED Strip

.settings/org.eclipse.core.resources.prefs

@@ -1,2 +1,3 @@
 eclipse.preferences.version=1
 encoding//doc/conf.py=utf-8
+encoding//py_trees_ros/blackboard.py=utf-8

package.xml

@@ -46,27 +46,27 @@
   -->
 
   <!-- mocking & simulation dependencies -->
+  <exec_depend>python-termcolor</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <!--
   <exec_depend>dynamic_reconfigure</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>move_base_msgs</exec_depend>
-  <exec_depend>python-termcolor</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
   -->
 
   <!-- core dependencies -->
   <exec_depend>py_trees</exec_depend>
   <exec_depend>py_trees_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>uuid_msgs</exec_depend>
+  <exec_depend>unique_id</exec_depend>
   <!--
   <exec_depend>actionlib</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>python-rospkg</exec_depend>
   <exec_depend>rosbag</exec_depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>unique_id</exec_depend>
-  <exec_depend>uuid_msgs</exec_depend>
   -->
 
   <!--

py_trees_ros/blackboard.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+# -*- coding: utf-8 -*-
 #
 # License: BSD
 #   https://raw.github.com/stonier/py_trees_ros/license/LICENSE

py_trees_ros/demos/exchange.py

@@ -110,11 +110,11 @@ def main():
     rclpy.init(args=None)
     node = rclpy.node.Node('exchange')
     exchange.setup(node=node, timeout=15)
-    unused_publisher_timer = node.create_timer(
+    publisher_timer = node.create_timer(
         timer_period_sec=1.0,
         callback=functools.partial(periodically_publish, exchange=exchange)
     )
-    unused_increment_timer = node.create_timer(
+    increment_timer = node.create_timer(
         timer_period_sec=2.0,
         callback=functools.partial(periodically_increment, exchange=exchange)
     )
@@ -127,6 +127,10 @@ def main():
         rclpy.spin(node)
     except KeyboardInterrupt:
         pass
+    publisher_timer.cancel()
+    increment_timer.cancel()
+    node.destroy_timer(publisher_timer)
+    node.destroy_timer(increment_timer)
     node.destroy_node()
 
 

py_trees_ros/mock/__init__.py

@@ -13,9 +13,9 @@
 # Imports
 ##############################################################################
 
-from . import action_server
+# from . import action_server
 from . import battery
 from . import led_strip
-from . import move_base
-from . import rotate
-from . import safety_sensors
+# from . import move_base
+# from . import rotate
+# from . import safety_sensors

py_trees_ros/mock/battery.py

@@ -16,16 +16,21 @@
 # Imports
 ##############################################################################
 
-import dynamic_reconfigure.server
-import rospy
+import argparse
+# import dynamic_reconfigure.server
+import py_trees_ros
+import rclpy
 import sensor_msgs.msg as sensor_msgs
+import sys
 
-from py_trees_msgs.cfg import BatteryConfig
+# from py_trees_msgs.cfg import BatteryConfig
 
 ##############################################################################
 # Class
 ##############################################################################
 
+def foo():
+    print("Foo")
 
 class Battery:
     """
@@ -52,17 +57,22 @@ class Battery:
     """
     def __init__(self):
         # ros communications
-        self.battery_publisher = rospy.Publisher('~state', sensor_msgs.BatteryState, queue_size=1, latch=True)
+        self.node = rclpy.create_node("battery")
+        self.battery_publisher = self.node.create_publisher(
+            msg_type=sensor_msgs.BatteryState,
+            topic="~/state",
+            qos_profile = py_trees_ros.utilities.qos_profile_latched_topic()
+        )
 
         # initialisations
         self.battery = sensor_msgs.BatteryState()
-        self.battery.header.stamp = rospy.Time.now()
+        self.battery.header.stamp = rclpy.clock.Clock().now().to_msg()
         self.battery.voltage = float('nan')
         self.battery.current = float('nan')
         self.battery.charge = float('nan')
         self.battery.capacity = float('nan')
         self.battery.design_capacity = float('nan')
-        self.battery.percentage = 100
+        self.battery.percentage = 100.0
         self.battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
         self.battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
         self.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_FULL
@@ -74,11 +84,11 @@ def __init__(self):
         self.charging_increment = 0.01
 
         # dynamic_reconfigure
-        self.parameters = None
-        self.dynamic_reconfigure_server = dynamic_reconfigure.server.Server(
-            BatteryConfig,
-            self.dynamic_reconfigure_callback
-        )
+#         self.parameters = None
+#         self.dynamic_reconfigure_server = dynamic_reconfigure.server.Server(
+#             BatteryConfig,
+#             self.dynamic_reconfigure_callback
+#         )
 
     def dynamic_reconfigure_callback(self, config, level):
         """
@@ -107,12 +117,37 @@ def spin(self):
         """
         Spin around, updating battery state and publishing the result.
         """
-        rate = rospy.Rate(5)  # hz
-        while not rospy.is_shutdown():
-            if self.parameters.charging:
-                self.dynamic_reconfigure_server.update_configuration({"charging_percentage": min(100, self.battery.percentage + self.charging_increment)})
-            else:
-                self.dynamic_reconfigure_server.update_configuration({"charging_percentage": max(0, self.battery.percentage - self.charging_increment)})
-            self.battery.header.stamp = rospy.get_rostime()  # get_rostime() returns the time in rospy.Time structure
-            self.battery_publisher.publish(self.battery)
-            rate.sleep()
+        # TODO: with rate and spin_once, once rate is implemented in rclpy
+        unused_timer = self.node.create_timer(
+            timer_period_sec=0.2,
+            callback=self.publish
+        )
+        try:
+            rclpy.spin(self.node)
+        except KeyboardInterrupt:
+            pass
+        self.node.destroy_node()
+
+
+    def publish(self):
+        """
+        Update and publish.
+        """
+#        if self.parameters.charging:
+#            self.dynamic_reconfigure_server.update_configuration({"charging_percentage": min(100, self.battery.percentage + self.charging_increment)})
+#        else:
+#            self.dynamic_reconfigure_server.update_configuration({"charging_percentage": max(0, self.battery.percentage - self.charging_increment)})
+        self.battery.header.stamp = rclpy.clock.Clock().now().to_msg()
+        self.battery_publisher.publish(msg=self.battery)
+
+def main():
+    """
+    Entry point for the mock batttery node.
+    """
+    parser = argparse.ArgumentParser(description='Mock a battery/charging source')
+    command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
+    parser.parse_args(command_line_args)
+    rclpy.init(args=None)
+    battery = Battery()
+    battery.spin()
+    rclpy.shutdown()

py_trees_ros/mock/led_strip.py

@@ -16,11 +16,16 @@
 # Imports
 ##############################################################################
 
-import rospy
+import argparse
+import functools
 import math
+import py_trees_ros
+import rclpy
 import std_msgs.msg as std_msgs
-import termcolor
+import sys
+import termcolor  # TODO: replace this with py_trees.console
 import threading
+import uuid
 
 ##############################################################################
 # Class
@@ -48,10 +53,20 @@ class LEDStrip(object):
     _valid_colours = {'red': 'red', 'green': 'green', 'yellow': 'yellow', 'blue': 'blue', 'purple': 'magenta', 'white': 'white'}
 
     def __init__(self):
-        self.command_subscriber = rospy.Subscriber('~command', std_msgs.String, self.command_callback)
-        self.display_publisher = rospy.Publisher('~display', std_msgs.String, queue_size=1, latch=True)
-        self.duration = rospy.Duration(3.0)
+        self.node = rclpy.create_node("led_strip")
+        self.command_subscriber = self.node.create_subscription(
+            msg_type=std_msgs.String,
+            topic='~/command',
+            callback=self.command_callback,
+            )
+        self.display_publisher = self.node.create_publisher(
+            msg_type=std_msgs.String,
+            topic="~/display",
+            qos_profile = py_trees_ros.utilities.qos_profile_latched_topic()
+        )
+        self.duration_sec = 3.0
         self.last_text = ''
+        self.last_uuid = None
         self.lock = threading.Lock()
         self.flashing_timer = None
 
@@ -65,12 +80,12 @@ def get_display_string(self, width, label="Foo"):
         """
         # top and bottom of print repeats the pattern as many times as possible
         # in the space specified
-        top_bottom = LEDStrip._pattern * (width / len(LEDStrip._pattern))
+        top_bottom = LEDStrip._pattern * int(width / len(LEDStrip._pattern))
         # space for two halves of the pattern on either side of the pattern name
         mid_pattern_space = (width - len(label) - self._pattern_name_spacing * 2) / 2
 
         # pattern for the mid line
-        mid = LEDStrip._pattern * (mid_pattern_space / len(LEDStrip._pattern))
+        mid = LEDStrip._pattern * int(mid_pattern_space / len(LEDStrip._pattern))
 
         # total length of the middle line with pattern, spacing and name
         mid_len = len(mid) * 2 + self._pattern_name_spacing * 2 + len(label)
@@ -103,17 +118,47 @@ def command_callback(self, msg):
             text = self.generate_led_text(msg.data)
             # don't bother publishing if nothing changed.
             if self.last_text != text:
-                self.display_publisher.publish(std_msgs.String(text))
+                print("{}".format(text))
+                self.last_text = text
+                self.last_uuid = uuid.uuid4()
+                self.display_publisher.publish(std_msgs.String(data=text))
             if self.flashing_timer is not None:
-                self.flashing_timer.shutdown()
-            self.flashing_timer = rospy.Timer(period=self.duration, callback=self.cancel_flashing, oneshot=True)
-            self.last_text = text
-
-    def cancel_flashing(self, unused_event):
+                self.flashing_timer.cancel()
+                self.node.destroy_timer(self.flashing_timer)
+            # TODO: convert this to a one-shot once rclpy has the capability
+            # Without oneshot, it will keep triggering, but do nothing while
+            # it has the uuid check
+            self.flashing_timer = self.node.create_timer(
+                timer_period_sec=self.duration_sec,
+                callback=functools.partial(
+                    self.cancel_flashing,
+                    last_uuid=self.last_uuid
+                )
+            )
+
+    def cancel_flashing(self, last_uuid):
         with self.lock:
-            self.flashing_timer = None
-            self.display_publisher.publish(std_msgs.String(""))
-            self.last_text = ""
+            if self.last_uuid == last_uuid:
+                # We're still relevant, publish and make us irrelevant
+                self.display_publisher.publish(std_msgs.String(data=""))
+                self.last_text = ""
+                self.last_uuid = uuid.uuid4()
 
     def spin(self):
-        rospy.spin()
+        try:
+            rclpy.spin(self.node)
+        except KeyboardInterrupt:
+            pass
+        self.node.destroy_node()
+
+def main():
+    """
+    Entry point for the mock led strip.
+    """
+    parser = argparse.ArgumentParser(description='Mock an led strip')
+    command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
+    parser.parse_args(command_line_args)
+    rclpy.init(args=None)
+    led_strip = LEDStrip()
+    led_strip.spin()
+    rclpy.shutdown()

py_trees_ros/programs/__init__.py

@@ -0,0 +1,18 @@
+#
+# License: BSD
+#   https://raw.github.com/stonier/py_trees_ros/license/LICENSE
+#
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Entry points to the programs (tools and utilities).
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+from . import blackboard_watcher
+from . import tree_watcher

py_trees_ros/programs/tree_watcher.py

@@ -160,12 +160,12 @@ def main():
     # Arg Handling
     ####################
     print("Args: {}".format(args))
+    if args.ascii_snapshot:
+        tree_watcher.connect_to_ascii_snapshot()
     if args.ascii_tree:
         tree_watcher.connect_to_ascii_tree()
     if args.dot_tree:
         tree_watcher.connect_to_dot_tree()
-    if args.ascii_snapshot:
-        tree_watcher.connect_to_ascii_snapshot()
 
     ####################
     # Execute

py_trees_ros/trees.py

@@ -149,11 +149,11 @@ def _setup_publishers(self):
         latched = True
         self.publishers = utilities.Publishers(self.node,
             [
-                ("ascii_tree", "~/ascii/tree", std_msgs.String, latched, 2),
-                ("ascii_snapshot", "~/ascii/snapshot", std_msgs.String, latched, 2),
-                ("dot_tree", "~/dot/tree", std_msgs.String, latched, 2),
-                ("log_tree", "~/log/tree", py_trees_msgs.BehaviourTree, latched, 2),
-                ("tip", "~/tip", py_trees_msgs.Behaviour, latched, 2)
+                ("ascii_tree", "~/ascii/tree", std_msgs.String, latched),
+                ("ascii_snapshot", "~/ascii/snapshot", std_msgs.String, latched),
+                ("dot_tree", "~/dot/tree", std_msgs.String, latched),
+                ("log_tree", "~/log/tree", py_trees_msgs.BehaviourTree, latched),
+                ("tip", "~/tip", py_trees_msgs.Behaviour, latched)
             ]
         )
 
@@ -173,7 +173,6 @@ def _publish_tree_modifications(self, root):
         if self.publishers is None:
             self.node.get_logger().error("call setup() on this tree to initialise the ros components")
             return
-        print("[DJS] publishing the ascii_tree: ")
         self.publishers.ascii_tree.publish(
             std_msgs.String(
                 data=py_trees.display.ascii_tree(root)
@@ -286,11 +285,12 @@ def setup(self, timeout):
             self.topic_type_strings['log/tree'],
             self.namespace_hint
         )
+
         # fineprint: assumption that the others are set relative to that and not remapped!
         root = "/" + "".join(self.topic_names['log/tree'].split('/')[:-2])
         self.topic_names['ascii/snapshot'] = root + "/ascii/snapshot"
         self.topic_names['ascii/tree']     = root + "/ascii/tree"
-        self.topic_names['dot/tree']       = root + "dot/tree"
+        self.topic_names['dot/tree']       = root + "/dot/tree"
         self.topic_names['tip']            = root + "/tip"
 
     def connect_to_ascii_tree(self):
@@ -327,5 +327,4 @@ def callback_string_to_stdout(self, msg):
         Args
             timeout (:class:`std_msgs.msg.String`): string message
         """
-        print("DJS: Dude")
         print("{}".format(msg.data))