Ros2

documentation/README.md

@@ -5,4 +5,5 @@
 - Setup.py
 - [Preemption](PREEMPTION.md)
 - Service
+- [SMACH](SMACH.md)
 - [Miscellaneous](MISC.md)

documentation/SMACH.md

@@ -0,0 +1,79 @@
+### SMACH on ROS2
+
+SMACH is not officially supported in ROS Humble. The ros2 branch of `ros/executive_smach` provides a compatible wrapper providing supported version of SMACH. 
+
+#### Key Notes
+
+##### State Machines
+- State Machines remain mainly unchanged from ROS Noetic. 
+
+##### States
+- There exists 2 usable version of states. 
+    1. `smach.State`
+        - This is the base version build upon the smach library. 
+        - It can be used for non-ROS applications as it is not dependent on it. 
+        - from `executive_ros` package:
+            ```python
+                class State(object):
+                """Base class for SMACH states.
+
+                A SMACH state interacts with SMACH containers in two ways. The first is its
+                outcome identifier, and the second is the set of userdata variables which
+                it reads from and writes to at runtime. Both of these interactions are
+                declared before the state goes active (when its C{execute()} method is
+                called) and are checked during construction.
+                """
+                def __init__(self, outcomes=[], input_keys=[], output_keys=[], io_keys=[]):
+                    ...
+            ```
+
+    2. `RosState` (**Use This for ROS2 dependent packages**)
+        - This is a subclass of `smach.State` which is designed for use in ROS-based applications. 
+             - For instance, this state has contains a parameter which allows a `node` to be passed. while `smach.State` does not.
+        - from `executive_ros` package:
+            ```python
+                class RosState(smach.State):
+                """
+                A state that can interact with a ROS node.
+                """
+                def __init__(self, node, **kwargs):
+                    ...
+            ```
+
+- States need a `rclpy.Node` to be passed as parameter during creation.
+    ```python
+        from smach import StateMachine
+        from smach_ros import RosState
+
+        class StateA(RosState):
+            def __init__(self, node, args):
+                super().__init__(node, args)
+
+            def execute(self, userdata):
+                pass
+
+        class StateMachineExample(StateMachine):
+            def __init__(self, node):
+                super().__init__(node, args)
+
+                with self:
+                    StateMachine.add('StateA', StateA(node), transition={})
+
+        def main():
+            rclpy.init()
+            node = rclpy.create_node("state_machine_example")
+            sm = StateMachineExample(node)
+
+            node.get_logger().info("Starting the state machine...")
+            outcome = sm.execute()
+
+            rclpy.shutdown()
+
+        if __name__ == '__main__':
+            main()
+
+    ```
+
+#### Reference
+- For example code, refer to `skills/src/lasr_skills/` folder 
+- Modules and Classes provided by [executive_smach](https://github.com/ros/executive_smach) can be found on thier Github page.

skills/src/lasr_skills/describe_people.py

@@ -1,6 +1,6 @@
 import rclpy
 import smach
-from ros_state import RosState
+from smach_ros import RosState
 from lasr_vision_interfaces.srv import Vqa
 from .vision import GetImage
 
@@ -35,7 +35,6 @@ def __init__(
         node,
     ):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img_raw"],

skills/src/lasr_skills/detect.py

@@ -1,7 +1,7 @@
 import cv2
 import cv2_img
 import rclpy
-from ros_state import RosState
+from smach_ros import RosState
 from sensor_msgs.msg import Image
 from lasr_vision_msgs.srv import YoloDetection
 
@@ -19,7 +19,6 @@ def __init__(
         debug_publisher: str = "/skills/detect/debug",
     ):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img_msg"],

skills/src/lasr_skills/detect_clothing.py

@@ -5,7 +5,6 @@
 from lasr_vision_interfaces.srv import TorchFaceFeatureDetectionDescription
 from lasr_skills import DescribePeople
 
-
 colour_list = ["blue", "yellow", "black", "white", "red", "orange", "gray"]
 cloth_list = ["t shirt", "shirt", "blouse", "sweater", "coat", "jacket"]
 cloth_type_map = {

skills/src/lasr_skills/detect_gesture.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 import rclpy
 import cv2
 import cv2_img
@@ -23,7 +23,6 @@ def __init__(
         debug_publisher: str = "/skills/gesture_detection/debug",
     ):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img_msg"],
@@ -133,7 +132,6 @@ def execute(self, userdata):
 class ProcessPointingDirection(RosState):
     def __init__(self, node):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["gesture_detected"],

skills/src/lasr_skills/detect_wave.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 from std_msgs.msg import Header
 from cv2_pcl import pcl_to_img_msg
 from lasr_vision_interfaces.srv import BodyPixKeypointDetection

skills/src/lasr_skills/face_person.py

@@ -1,6 +1,6 @@
 import rclpy
 import smach
-from ros_state import RosState
+from smach_ros import RosState
 from lasr_vision_interfaces.srv import BodyPixKeypointDetection
 from .vision import GetCroppedImage
 from lasr_skills.play_motion import PlayMotion
@@ -54,7 +54,6 @@ def __init__(
         init_state="u1m",
     ):
         super().__init__(
-            self,
             outcomes=["finished", "failed", "truncated"],
             input_keys=[],
             output_keys=[],
@@ -125,7 +124,6 @@ def __init__(
         node,
     ):
         super().__init__(
-            self,
             node,
             outcomes=["finished", "failed", "truncated"] + positions,
             input_keys=["img_msg"],

skills/src/lasr_skills/go_to_location.py

@@ -1,6 +1,6 @@
 from typing import Union
 import rclpy
-from ros_state import RosState
+from smach_ros import RosState
 from geometry_msgs.msg import Pose, PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 from std_msgs.msg import Header

skills/src/lasr_skills/listen.py

@@ -9,5 +9,5 @@ def __init__(
         node,
     ):
         super().__init__(
-            self, node, "transcribe_speech", TranscribeSpeech, result_slots=["sequence"]
+            node, "transcribe_speech", TranscribeSpeech, result_slots=["sequence"]
         )

skills/src/lasr_skills/look_to_point.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 import rclpy
 from rclpy.action import ActionClient
 
@@ -17,7 +17,6 @@ def __init__(
         pointstamped: Union[None, PointStamped] = None,
     ):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "aborted", "timed_out"],
             input_keys=["pointstamped"] if pointstamped is None else [],

skills/src/lasr_skills/vision/get_cropped_image.py

@@ -1,5 +1,6 @@
 import rclpy
-from ros_state import RosState
+import smach
+from smach_ros import RosState
 from lasr_vision_interfaces.msg import CDRequest
 from lasr_vision_interfaces.srv import CroppedDetection
 
@@ -21,7 +22,6 @@ def __init__(
         yolo_nms_threshold: float = 0.3,
     ):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             output_keys=["img_msg", "detection"],
@@ -67,8 +67,6 @@ def execute(self, userdata) -> str:
 
 
 if __name__ == "__main__":
-    import smach
-
     rclpy.init()
     node = rclpy.create_node("get_cropped_image")
 

skills/src/lasr_skills/vision/get_image.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 import rclpy
 from rclpy.wait_for_message import wait_for_message
 from typing import Optional
@@ -12,7 +12,6 @@ class GetImage(RosState):
 
     def __init__(self, node, topic: Optional[str] = None):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             output_keys=["img_msg"],
@@ -53,7 +52,6 @@ class GetPointCloud(RosState):
 
     def __init__(self, node, topic: Optional[str] = None):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             output_keys=["pcl_msg"],
@@ -84,7 +82,6 @@ def execute(self, userdata):
 class GetImageAndPointCloud(RosState):
     def __init__(self, node):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             output_keys=["img_msg", "pcl_msg"],

skills/src/lasr_skills/vision/image_cv2_to_msg.py

@@ -1,5 +1,5 @@
 import os
-from ros_state import RosState
+from smach_ros import RosState
 import cv2_img
 import rclpy
 
@@ -14,7 +14,6 @@ class ImageCv2ToMsg(RosState):
     def __init__(self, node):
         super().__init__(
             # self, outcomes=["succeeded", "failed"], input_keys=["img", "img_msg"], output_keys=["img_msg"]
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img"],

skills/src/lasr_skills/vision/image_msg_to_cv2.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 import cv2_img
 import cv2_pcl
 
@@ -14,7 +14,6 @@ class ImageMsgToCv2(RosState):
     def __init__(self, node):
         super().__init__(
             # self, outcomes=["succeeded", "failed"], input_keys=["img_msg", "img"], output_keys=["img"]
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img_msg"],
@@ -34,7 +33,6 @@ class PclMsgToCv2(RosState):
 
     def __init__(self, node):
         super().__init__(
-            self,
             node,
             outcomes=["succeeded", "failed"],
             input_keys=["img_msg_3d"],

skills/src/lasr_skills/wait.py

@@ -1,7 +1,7 @@
 """Generic wait state for waiting a desired number of seconds"""
 
 import rclpy
-from ros_state import RosState
+from smach_ros import RosState
 from time import sleep
 
 
@@ -11,7 +11,7 @@ def __init__(self, node, wait_time: int):
         Args:
             wait_time (int): Number of seconds to wait for and remain idle
         """
-        super().__init__(self, node, outcomes=["succeeded", "failed"])
+        super().__init__(node, outcomes=["succeeded", "failed"])
 
         if not rclpy.ok():
             rclpy.init()

skills/src/lasr_skills/wait_for_person.py

@@ -1,4 +1,4 @@
-from ros_state import RosState
+from smach_ros import RosState
 import smach
 from lasr_skills import Detect
 from lasr_skills.vision import GetImage
@@ -12,7 +12,6 @@ def __init__(
         image_topic: str = "/xtion/rgb/image_raw",
     ):
         super().__init__(
-            self,
             outcomes=["succeeded", "failed"],
             output_keys=["detections"],
         )
@@ -40,9 +39,7 @@ def __init__(
         self,
         node,
     ):
-        super().__init__(
-            self, node, outcomes=["done", "not_done"], input_keys=["detections"]
-        )
+        super().__init__(node, outcomes=["done", "not_done"], input_keys=["detections"])
 
     def execute(self, userdata):
         if len(userdata.detections.detected_objects):