[mock] move base action server/client

.project

@@ -3,6 +3,9 @@
 	<name>py_trees_ros_tutorials</name>
 	<comment></comment>
 	<projects>
+		<project>py_trees</project>
+		<project>py_trees_ros</project>
+		<project>py_trees_ros_interfaces</project>
 	</projects>
 	<buildSpec>
 		<buildCommand>

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

@@ -10,5 +10,6 @@ encoding//py_trees_ros_tutorials/mock/gui/dashboard_group_box.py=utf-8
 encoding//py_trees_ros_tutorials/mock/gui/main_window.py=utf-8
 encoding//py_trees_ros_tutorials/mock/launch.py=utf-8
 encoding//py_trees_ros_tutorials/mock/led_strip.py=utf-8
+encoding//py_trees_ros_tutorials/mock/move_base.py=utf-8
 encoding//py_trees_ros_tutorials/mock/rotate.py=utf-8
 encoding//py_trees_ros_tutorials/mock/safety_sensors.py=utf-8

package.xml

@@ -56,6 +56,7 @@
   <exec_depend>py_trees</exec_depend>
   <exec_depend>py_trees_ros</exec_depend>
   <exec_depend>py_trees_ros_interfaces</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>unique_identifier_msgs</exec_depend>

py_trees_ros_tutorials/mock/action_clients.py

@@ -148,3 +148,14 @@ def rotate(args=None):
         action_server_namespace="/rotation_controller"
         )
     action_client.spin()
+
+
+def move_base(args=None):
+    rclpy.init(args=args)
+    action_client = ActionClient(
+        node_name="move_base_client",
+        action_name="move_base",
+        action_type_string="MoveBase",  # py_trees_ros_actions.MoveBase
+        action_server_namespace="/move_base"
+        )
+    action_client.spin()

py_trees_ros_tutorials/mock/actions.py

@@ -62,7 +62,7 @@ class GenericServer(object):
     """
     def __init__(self,
                  action_name,
-                 action_type_string,  # "Dock" or "Rotate"
+                 action_type,  # e.g. (py_trees_ros_interfaces.action, "Dock")
                  custom_execute_callback=lambda: None,
                  goal_received_callback=lambda request: None,
                  duration=None):
@@ -88,14 +88,14 @@ def __init__(self,
 
         self.service_callbacks = {}
         self.service_callbacks["goal"] = {
-            "module": py_trees_actions,
+            "module": action_type[0],
             "callback": self.goal_service_callback,
-            "class": action_type_string + "_Goal"
+            "class": action_type[1] + "_Goal"
         }
         self.service_callbacks["result"] = {
-            "module": py_trees_actions,
+            "module": action_type[0],
             "callback": self.result_service_callback,
-            "class": action_type_string + "_Result"
+            "class": action_type[1] + "_Result"
         }
         self.service_callbacks["cancel"] = {
             "module": action_srvs,
@@ -122,7 +122,7 @@ def __init__(self,
 
         self.feedback_message_type = getattr(
             py_trees_actions,
-            action_type_string + "_Feedback"
+            action_type[1] + "_Feedback"
         )
         self.feedback_publisher = self.node.create_publisher(
             msg_type=self.feedback_message_type,

py_trees_ros_tutorials/mock/dock.py

@@ -36,7 +36,7 @@ class Dock(actions.GenericServer):
     def __init__(self):
         super().__init__(
             action_name="docking_controller",
-            action_type_string="Dock",
+            action_type=(py_trees_actions, "Dock"),
             custom_execute_callback=self.custom_execute_callback,
             goal_received_callback=self.goal_received_callback,
             duration=2.0

py_trees_ros_tutorials/mock/launch.py

@@ -35,7 +35,8 @@ def generate_launch_description():
     # Mock Robot Nodes
     ##########################################################################
     for node_name in ['battery', 'dashboard', 'docking_controller',
-                      'led_strip', 'rotation_controller', 'safety_sensors']:
+                      'led_strip', 'move_base', 'rotation_controller',
+                      'safety_sensors']:
         node_executable = "mock-{}".format(node_name.replace('_', '-'))
         launch_description.add_action(
             launch_ros.actions.Node(

py_trees_ros_tutorials/mock/move_base.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# License: BSD
+#   https://raw.githubusercontent.com/stonier/py_trees/devel/LICENSE
+#
+##############################################################################
+# Documentation
+##############################################################################
+
+"""
+Mocks a simple action server that rotates the robot 360 degrees.
+"""
+
+
+##############################################################################
+# Imports
+##############################################################################
+
+import argparse
+import geometry_msgs.msg as geometry_msgs
+import math
+import py_trees_ros.utilities
+import py_trees_ros_interfaces.action as py_trees_actions
+import rclpy
+import sys
+
+from . import actions
+
+##############################################################################
+# Class
+##############################################################################
+
+
+class MoveBase(actions.GenericServer):
+    """
+    Simulates:
+
+    * move base interface
+    * publishing on /odom (nav_msgs.msg.Odometry)
+    * publishing on /pose (geometry_msgs.msg.PoseWithCovarianceStamped)
+
+    Args:
+        odometry_topic (:obj:`str`): name of the odometry topic
+        pose_topic (:obj:`str`): name of the pose (with covariance stamped) topic
+        duration (:obj:`int`): time for a goal to complete (seconds)
+    """
+    def __init__(self, odometry_topic='/odom', pose_topic='/pose', duration=None):
+        super().__init__(action_name="move_base",
+                         action_type=(py_trees_actions, "MoveBase"),
+                         custom_execute_callback=self.custom_execute_callback,
+                         duration=duration
+                         )
+        # self.odometry = nav_msgs.Odometry()
+        # self.odometry.pose.pose.position = geometry_msgs.Point(0, 0, 0)
+        self.pose = geometry_msgs.PoseWithCovarianceStamped()
+        self.pose.pose.pose.position = geometry_msgs.Point(x=0.0, y=0.0, z=0.0)
+
+        # publishers
+        not_latched = False  # latched = True
+        self.publishers = py_trees_ros.utilities.Publishers(
+            self.node,
+            [
+                ('pose', pose_topic, geometry_msgs.PoseWithCovarianceStamped, not_latched),
+                # ('odometry', odometry_topic, nav_msgs.Odometry, not_latched)
+            ]
+        )
+        self.publishers.pose.publish(self.pose)
+        # self.publishers.odometry.publish(self.odometry)
+
+        self.publish_timer = self.node.create_timer(
+            timer_period_sec=0.5,
+            callback=self.publish
+        )
+
+    def custom_execute_callback(self):
+        """
+        Increment the odometry and pose towards the goal.
+        """
+        # actually doesn't go to the goal right now...
+        # but we could take the feedback from the action
+        # and increment this to that proportion
+        # self.odometry.pose.pose.position.x += 0.01
+        self.pose.pose.pose.position.x += 0.01
+
+    def publish(self, unused_event):
+        """
+        Most things expect a continous stream of odometry/pose messages, so we
+        run this from a timer.
+        """
+        # self.publishers.odometry.publish(self.odometry)
+        self.publishers.pose.publish(self.pose)
+
+
+def main():
+    """
+    Entry point for the mock batttery node.
+    """
+    parser = argparse.ArgumentParser(description='Mock a docking controller')
+    command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
+    parser.parse_args(command_line_args)
+    rclpy.init()  # picks up sys.argv automagically internally
+    move_base_controller = MoveBase()
+    move_base_controller.spin()
+    rclpy.shutdown()

py_trees_ros_tutorials/mock/rotate.py

@@ -39,7 +39,7 @@ class Rotate(actions.GenericServer):
     """
     def __init__(self, rotation_rate=1.57):
         super().__init__(action_name="rotation_controller",
-                         action_type_string="Rotate",
+                         action_type=(py_trees_actions, "Rotate"),
                          custom_execute_callback=self.custom_execute_callback,
                          duration=2.0 * math.pi / rotation_rate
                          )

setup.py

@@ -82,11 +82,13 @@
              'mock-battery = py_trees_ros_tutorials.mock.battery:main',
              'mock-dashboard = py_trees_ros_tutorials.mock.dashboard:main',
              'mock-docking-controller = py_trees_ros_tutorials.mock.dock:main',
-             'mock-rotation-controller = py_trees_ros_tutorials.mock.rotate:main',
              'mock-led-strip = py_trees_ros_tutorials.mock.led_strip:main',
+             'mock-move-base = py_trees_ros_tutorials.mock.move_base:main',
+             'mock-rotation-controller = py_trees_ros_tutorials.mock.rotate:main',
              'mock-safety-sensors = py_trees_ros_tutorials.mock.safety_sensors:main',
              # Mock Tests
              'mock-dock-client = py_trees_ros_tutorials.mock.action_clients:dock',
+             'mock-move-base-client = py_trees_ros_tutorials.mock.action_clients:move_base',
              'mock-rotate-client = py_trees_ros_tutorials.mock.action_clients:rotate',
              # Launchers (directly runnable)
              'launch-mock-robot = py_trees_ros_tutorials.mock.launch:main',