initial development of ROS 2 release

README.md

@@ -14,28 +14,28 @@ Please refer to the FlexBE Homepage ([flexbe.github.io](http://flexbe.github.io)
 
 Execute the following commands to install FlexBE:
 
-    roscd && cd ../src
+    cd "ros2_ws"/src
     git clone https://github.com/team-vigir/flexbe_behavior_engine.git
-    
+
 Furthermore, create your own repository for behavior development (contains examples):
-    
-    rosrun flexbe_widget create_repo [your_project_name]
-    
+
+    ros2 run flexbe_widget create_repo [your_project_name]
+
 Finally, it is recommended to install the FlexBE App user interface by following [these steps](http://philserver.bplaced.net/fbe/download.php).
 
 ## Usage
 
 Use the following launch file for running the onboard engine:
 
-    roslaunch flexbe_onboard behavior_onboard.launch
-    
+    ros2 launch flexbe_onboard behavior_onboard.launch.py
+
 Use the following launch file for running the operator control station (requires the FlexBE App):
 
-    roslaunch flexbe_app flexbe_ocs.launch
+    ros2 launch flexbe_app flexbe_ocs.launch.py
 
-Use the following lunach file to run both of the above, e.g., for testing on a single computer:
+Use the following launch file to run both of the above, e.g., for testing on a single computer:
 
-    roslaunch flexbe_app flexbe_full.launch
+    ros2 launch flexbe_app flexbe_full.launch.py
 
 ## Next Steps
 

flexbe_behavior_engine/CMakeLists.txt

@@ -1,4 +1,14 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(flexbe_behavior_engine)
-find_package(catkin REQUIRED)
-catkin_metapackage()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()

flexbe_behavior_engine/package.xml

@@ -12,8 +12,6 @@
 
   <url>http://ros.org/wiki/flexbe</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
   <exec_depend>flexbe_core</exec_depend>
   <exec_depend>flexbe_input</exec_depend>
   <exec_depend>flexbe_mirror</exec_depend>
@@ -23,7 +21,10 @@
   <exec_depend>flexbe_testing</exec_depend>
   <exec_depend>flexbe_widget</exec_depend>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
   <export>
+    <build_type>ament_cmake</build_type>
     <metapackage/>
   </export>
 

flexbe_core/.flake8

@@ -0,0 +1,3 @@
+[flake8]
+exclude = .git
+max-line-length = 119

flexbe_core/src/flexbe_core/__init__.py → flexbe_core/flexbe_core/__init__.py

@@ -1,12 +1,9 @@
-#!/usr/bin/env python
 
 #
 #    Please use EventState as parent class for new states because it extends all other parent classes.
 #    For a behavior, choose OperatableStateMachine as state machine.
 #
-
-from .core import EventState  # noqa: F401
-from .core import OperatableStateMachine, ConcurrencyContainer, PriorityContainer  # noqa: F401
+from .core import EventState, OperatableStateMachine, ConcurrencyContainer, PriorityContainer  # noqa: F401
 
 from .behavior import Behavior  # noqa: F401
 
@@ -16,6 +13,16 @@
 from .state_logger import StateLogger  # noqa: F401
 
 
+def set_node(node):
+    from .proxy import initialize_proxies
+    from .core import RosState, RosStateMachine
+    Logger.initialize(node)
+    StateLogger.initialize_ros(node)
+    initialize_proxies(node)
+    RosState.initialize_ros(node)
+    RosStateMachine.initialize_ros(node)
+
+
 class Autonomy:
     """
     Provides constants for the available required Autonomy Levels.
@@ -49,3 +56,13 @@ class Autonomy:
     Use this for outcomes that always need an operator input.
     A use of this level is not recommended.
     """
+
+
+__all__ = [
+    'Behavior',
+    'BehaviorLibrary',
+    'Logger',
+    'StateLogger',
+    'set_node',
+    'Autonomy'
+]

flexbe_core/src/flexbe_core/behavior_library.py → flexbe_core/flexbe_core/behavior_library.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python
 import os
-from rospkg import RosPack
+from ament_index_python import get_packages_with_prefixes
+from ros2pkg import api as ros2pkg
+from catkin_pkg.package import parse_package
 import xml.etree.ElementTree as ET
 import zlib
 
@@ -12,20 +13,23 @@ class BehaviorLibrary(object):
     Provides access to all known behaviors.
     '''
 
-    def __init__(self):
-        self._rp = RosPack()
-        self._behavior_lib = dict()
+    def __init__(self, node):
+        # self._behavior_lib = dict()
+        self._node = node
+        Logger.initialize(node)
         self.parse_packages()
 
     def parse_packages(self):
         """
-        Parses all ROS packages to update the internal behavior library.
+        Parses all ROS2 packages to update the internal behavior library.
         """
         self._behavior_lib = dict()
-        for pkg in self._rp.list():
-            for export in self._rp._load_manifest(pkg).exports:
-                if export.tag == "flexbe_behaviors":
-                    self._add_behavior_manifests(self._rp.get_path(pkg), pkg)
+        # for pkg_name, pkg_path in ros2pkg.get_packages_with_prefixes().items():
+        for pkg_name, pkg_path in get_packages_with_prefixes().items():
+            pkg = parse_package(os.path.join(pkg_path, 'share', pkg_name))
+            for export in pkg.exports:
+                if export.tagname == "flexbe_behaviors":
+                    self._add_behavior_manifests(os.path.join(pkg_path, 'lib', pkg_name, 'manifest'), pkg_name)
 
     def _add_behavior_manifests(self, path, pkg=None):
         """
@@ -124,6 +128,7 @@ def get_sourcecode_filepath(self, be_id, add_tmp=False):
         if be_entry is None:
             # rely on get_behavior to handle/log missing package
             return None
+        #TODO Replace use of non-existing self._rp
         try:
             module_path = __import__(be_entry["package"]).__path__[-1]
         except ImportError:

flexbe_core/src/flexbe_core/core/__init__.py → flexbe_core/flexbe_core/core/__init__.py

@@ -16,3 +16,21 @@
 from .event_state import EventState  # noqa: F401
 
 from .user_data import UserData  # noqa: F401
+
+__all__ = [
+    'PreemptableState',
+    'OperatableStateMachine',
+    'LockableStateMachine',
+    'RosStateMachine',
+    'StateMachine',
+    'ConcurrencyContainer',
+    'PriorityContainer',
+    'State',
+    'RosState',
+    'ManuallyTransitionableState',
+    'LockableState',
+    'PreemptableState',
+    'OperatableState',
+    'EventState',
+    'UserData'
+]

flexbe_core/src/flexbe_core/core/concurrency_container.py → flexbe_core/flexbe_core/core/concurrency_container.py

@@ -27,7 +27,7 @@ def sleep_duration(self):
         for state in self._states:
             if state.sleep_duration > 0:
                 sleep_dur = state.sleep_duration if sleep_dur is None else min(sleep_dur, state.sleep_duration)
-        return sleep_dur or 0.
+        return self._sleep_dur or 0.
 
     def _execute_current_state(self):
         # execute all states that are done with sleeping and determine next sleep duration

flexbe_core/src/flexbe_core/core/event_state.py → flexbe_core/flexbe_core/core/event_state.py

@@ -22,6 +22,8 @@ class EventState(OperatableState):
 
     def __init__(self, *args, **kwargs):
         super(EventState, self).__init__(*args, **kwargs)
+        # StateLogger.initialize_ros(EventState._node)
+
         self.__execute = self.execute
         self.execute = self._event_execute
 
@@ -75,6 +77,7 @@ def _event_execute(self, *args, **kwargs):
         if repeat or outcome is not None and not PreemptableState.preempt:
             self._entering = True
             self.on_exit(*args, **kwargs)
+
         return outcome
 
     def _notify_skipped(self):

flexbe_core/src/flexbe_core/core/operatable_state.py → flexbe_core/flexbe_core/core/operatable_state.py

@@ -53,8 +53,8 @@ def _operatable_execute(self, *args, **kwargs):
             # autonomy level is high enough, report the executed transition
             elif outcome is not None and outcome in self.outcomes:
                 Logger.localinfo("State result: %s > %s" % (self.name, outcome))
-                self._pub.publish(self._outcome_topic, UInt8(self.outcomes.index(outcome)))
-                self._pub.publish(self._debug_topic, String("%s > %s" % (self.path, outcome)))
+                self._pub.publish(self._outcome_topic, UInt8(data=self.outcomes.index(outcome)))
+                self._pub.publish(self._debug_topic, String(data="%s > %s" % (self.path, outcome)))
                 if self._force_transition:
                     StateLogger.log('flexbe.operator', self, type='forced', forced=outcome,
                                     requested=self._last_requested_outcome)

flexbe_core/src/flexbe_core/core/operatable_state_machine.py → flexbe_core/flexbe_core/core/operatable_state_machine.py

@@ -95,6 +95,7 @@ def _execute_current_state(self):
             outcome = super(OperatableStateMachine, self)._execute_current_state()
             self._last_exception = None
         except Exception as e:
+            # Error here
             outcome = None
             self._last_exception = e
             Logger.logerr('Failed to execute state %s:\n%s' % (self.current_state_label, str(e)))

flexbe_core/flexbe_core/core/ros_state.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+from rclpy.exceptions import ParameterNotDeclaredException
+from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
+
+from flexbe_core.core.state import State
+from flexbe_core.logger import Logger
+from flexbe_core.state_logger import StateLogger
+
+
+class RosState(State):
+    """
+    A state to interface with ROS.
+    """
+    _node = None
+    _breakpoints = []
+
+    @staticmethod
+    def initialize_ros(node):
+        RosState._node = node
+        try:
+            RosState._breakpoints = node.get_parameter('breakpoints')
+        except ParameterNotDeclaredException as e:
+            node.declare_parameter('breakpoints', [])
+            RosState._breakpoints = node.get_parameter('breakpoints')
+        # RosState._breakpoints = node.declare_parameter('breakpoints', [])
+        ProxyPublisher._initialize(RosState._node)
+        ProxySubscriberCached._initialize(RosState._node)
+        StateLogger.initialize_ros(RosState._node)
+        Logger.initialize(RosState._node)
+
+    def __init__(self, *args, **kwargs):
+        super(RosState, self).__init__(*args, **kwargs)
+        self._rate = RosState._node.create_rate(10)
+        self._is_controlled = False
+
+        self._pub = ProxyPublisher()
+        self._sub = ProxySubscriberCached()
+
+    def sleep(self):
+        self._rate.sleep()
+
+    @property
+    def sleep_duration(self):
+        return self._rate._timer.timer_period_ns
+
+    def set_rate(self, rate):
+        """
+        Set the execution rate of this state,
+        i.e., the rate with which the execute method is being called.
+
+        Note: The rate is best-effort, real-time support is not yet available.
+
+        @type label: float
+        @param label: The desired rate in Hz.
+        """
+        self._rate.destroy()
+        self._rate = RosState._node.create_rate(rate)
+
+    def _enable_ros_control(self):
+        self._is_controlled = True
+
+    def _disable_ros_control(self):
+        self._is_controlled = False
+
+    @property
+    def is_breakpoint(self):
+        return self.path in RosState._breakpoints.get_parameter_value().string_array_value

flexbe_core/src/flexbe_core/core/ros_state_machine.py → flexbe_core/flexbe_core/core/ros_state_machine.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-import rospy
+import rclpy
 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
 
 from flexbe_core.core.state_machine import StateMachine
@@ -9,6 +9,13 @@ class RosStateMachine(StateMachine):
     """
     A state machine to interface with ROS.
     """
+    _node = None
+
+    @staticmethod
+    def initialize_ros(node):
+        RosStateMachine._node = node
+        ProxyPublisher._initialize(node)
+        ProxySubscriberCached._initialize(node)
 
     def __init__(self, *args, **kwargs):
         super(RosStateMachine, self).__init__(*args, **kwargs)
@@ -19,10 +26,10 @@ def __init__(self, *args, **kwargs):
 
     def wait(self, seconds=None, condition=None):
         if seconds is not None:
-            rospy.sleep(seconds)
+            RosStateMachine._node.create_rate(1 / seconds).sleep()
         if condition is not None:
-            rate = rospy.Rate(100)
-            while not rospy.is_shutdown():
+            rate = RosStateMachine._node.create_rate(100)
+            while rclpy.ok():
                 if condition():
                     break
                 rate.sleep()

flexbe_core/src/flexbe_core/core/state_machine.py → flexbe_core/flexbe_core/core/state_machine.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import time
 from flexbe_core.core.state import State
 from flexbe_core.core.user_data import UserData
 from flexbe_core.core.exceptions import StateError, StateMachineError
@@ -88,7 +89,8 @@ def _execute_current_state(self):
         with UserData(reference=self._userdata, remap=self._remappings[self._current_state.name],
                       input_keys=self._current_state.input_keys, output_keys=self._current_state.output_keys
                       ) as userdata:
-            outcome = self._current_state.execute(userdata)
+                      outcome = self._current_state.execute(userdata)
+
         if outcome is not None:
             try:
                 target = self._transitions[self._current_state.name][outcome]