[infra] upgrade to dashing (#92)

* [actions] *_Feedback/Goal -> *.Feedback/Goal * [comms] topic/service lookup need */msg/* or */srv/* now * [watcher] bugfix serialisation of tip * [actions] parameter initialisation with declarations * [blackboard] bugfix lookup names and set qos profiles * [watcher] cleanup subscription * [exchange] dynamic pubsub working in dashing * [actions] server cancel priority and bugfix preempt code * [behaviours] qos profiles * 1.1.0
splintered-reality · Jun 19, 2019 · 3680dca · 3680dca
1 parent d0ab0e5
commit 3680dca
Show file tree

Hide file tree

Showing 14 changed files with 70 additions and 53 deletions.
diff --git a/.pydevproject b/.pydevproject
@@ -1,41 +1,41 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no"?>
 <?eclipse-pydev version="1.0"?><pydev_project>
-
+
+
 
 
 
     <pydev_pathproperty name="org.python.pydev.PROJECT_SOURCE_PATH">
-
+
+
 
 
 
         <path>/${PROJECT_DIR_NAME}</path>
-
+
+
 
 
 
     </pydev_pathproperty>
-
+
+
 
 
 
     <pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 3.6</pydev_property>
-
+
+
 
 
 
     <pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">python3</pydev_property>
-
+                    
 
 
 
-    <pydev_pathproperty name="org.python.pydev.PROJECT_EXTERNAL_SOURCE_PATH">
-
-
-        <path>/opt/ros/crystal/lib/python3.6/site-packages</path>
-
 
-    </pydev_pathproperty>
 
 
+
 </pydev_project>
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -4,6 +4,11 @@ Changelog
 
 Forthcoming
 -----------
+* ...
+
+1.1.0 (2019-06-19)
+------------------
+
 * [actions] bugfix action client, don't cancel if not RUNNING
 * [conversions] bugfix msg_to_behaviour for decorators
 * [watchers] bugfix tree-watchers dot-graph to string functionality

diff --git a/package.xml b/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>py_trees_ros</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>
     ROS2 extensions and behaviours for py_trees.
   </description>

diff --git a/py_trees_ros/actions.py b/py_trees_ros/actions.py
@@ -146,13 +146,14 @@ def update(self):
             self.node.get_logger().warn("got result, but future not yet done [{}]".format(self.qualified_name))
             return py_trees.common.Status.RUNNING
         else:
-            self.feedback_message = "successfully completed"
             self.node.get_logger().info("goal result [{}]".format(self.qualified_name))
             self.node.get_logger().info("  status: {}".format(self.result_status_string))
             self.node.get_logger().info("  message: {}".format(self.result_message))
             if self.result_status == action_msgs.GoalStatus.STATUS_SUCCEEDED:  # noqa
+                self.feedback_message = "successfully completed"
                 return py_trees.common.Status.SUCCESS
             else:
+                self.feedback_message = "failed"
                 return py_trees.common.Status.FAILURE
 
     def terminate(self, new_status: py_trees.common.Status):
@@ -283,5 +284,5 @@ def get_result_callback(self, future: rclpy.task.Future):
             future: incoming goal result delivered from the action server
         """
         self.result_message = future.result()
-        self.result_status = future.result().action_status
+        self.result_status = future.result().status
         self.result_status_string = self.status_strings[self.result_status]
diff --git a/py_trees_ros/blackboard.py b/py_trees_ros/blackboard.py
@@ -70,7 +70,6 @@ def shutdown(self):
         """
         Shutdown the temporarily created publisher.
         """
-        # print("      DJS: view.shutdown [%s]" % self.publisher.topic)
         self.node.destroy_publisher(self.publisher)
 
     def _update_sub_blackboard(self):
@@ -214,15 +213,20 @@ def setup(self, timeout):
         .. seealso:: This class is used as illustrated above in :class:`~py_trees_ros.trees.BehaviourTree`.
         """
         self.node = node
-        self.publisher = self.node.create_publisher(std_msgs.String, '~/exchange/blackboard')
+        self.publisher = self.node.create_publisher(
+            msg_type=std_msgs.String,
+            topic='~/exchange/blackboard',
+            qos_profile=rclpy.qos.qos_profile_system_default
+        )
         for name in ["get_blackboard_variables",
                      "open_blackboard_watcher",
                      "close_blackboard_watcher"]:
             camel_case_name = ''.join(x.capitalize() for x in name.split('_'))
             self.services[name] = self.node.create_service(
                 srv_type=getattr(py_trees_srvs, camel_case_name),
                 srv_name='~/exchange/' + name,
-                callback=getattr(self, "_{}_service".format(name))
+                callback=getattr(self, "_{}_service".format(name)),
+                qos_profile=rclpy.qos.qos_profile_services_default
             )
 
     def _get_nested_keys(self):
@@ -298,7 +302,7 @@ def _close_blackboard_watcher_service(self, request, response):
         for view in self.views:
             # print("   DJS: close watcher? [%s][%s]" % (view.topic_name, request.topic_name))
             if view.topic_name == request.topic_name:
-                # view.shutdown()  # that node.destroy_publisher call makes havoc
+                view.shutdown()  # that node.destroy_publisher call makes havoc
                 response.result = True
                 break
         self.views[:] = [view for view in self.views if view.topic_name != request.topic_name]
@@ -347,9 +351,9 @@ def __init__(
             'close': None
         }
         self.service_type_strings = {
-            'list': 'py_trees_ros_interfaces/GetBlackboardVariables',
-            'open': 'py_trees_ros_interfaces/OpenBlackboardWatcher',
-            'close': 'py_trees_ros_interfaces/CloseBlackboardWatcher'
+            'list': 'py_trees_ros_interfaces/srv/GetBlackboardVariables',
+            'open': 'py_trees_ros_interfaces/srv/OpenBlackboardWatcher',
+            'close': 'py_trees_ros_interfaces/srv/CloseBlackboardWatcher'
         }
         self.service_types = {
             'list': py_trees_srvs.GetBlackboardVariables,
@@ -399,7 +403,8 @@ def create_service_client(self, key: str):
             )
         client = self.node.create_client(
             srv_type=self.service_types[key],
-            srv_name=self.service_names[key]
+            srv_name=self.service_names[key],
+            qos_profile=rclpy.qos.qos_profile_services_default
             )
         # hardcoding timeouts will get us into trouble
         if not client.wait_for_service(timeout_sec=3.0):

diff --git a/py_trees_ros/mock/actions.py b/py_trees_ros/mock/actions.py
@@ -74,13 +74,14 @@ def __init__(self,
                  duration=None):
         self.node = rclpy.create_node(
             node_name,
-            initial_parameters=[
+            parameter_overrides=[
                 rclpy.parameter.Parameter(
                     'duration',
                     rclpy.parameter.Parameter.Type.DOUBLE,
                     5.0  # seconds
                 ),
-            ]
+            ],
+            automatically_declare_parameters_from_overrides=True
         )
         # override
         if duration is not None:
@@ -109,7 +110,6 @@ def __init__(self,
             self.generate_feedback_message = generate_feedback_message
         self.goal_received_callback = goal_received_callback
         self.goal_handle = None
-        self.preempt_goal_ids = []  # list of unique_identifier_msgs.UUID (goal_id's)
 
         self.action_server = rclpy.action.ActionServer(
             node=self.node,
@@ -171,28 +171,27 @@ def execute_goal_callback(
             self.percent_completed += increment
             with self.goal_lock:
                 if goal_handle.is_active:
-                    if goal_handle.goal_id in self.preempt_goal_ids:
-                        self.preempt_goal_ids.remove(goal_handle.goal_id)
+                    if goal_handle.is_cancel_requested:
                         result = self.action_type.Result()
-                        result.message = "goal pre-empted at {percentage:.2f}%%".format(
+                        result.message = "goal cancelled at {percentage:.2f}%%".format(
                             percentage=self.percent_completed)
                         self.node.get_logger().info(result.message)
-                        goal_handle.set_aborted()
+                        goal_handle.canceled()
                         return result
-                    elif goal_handle.is_cancel_requested:
+                    elif goal_handle.goal_id != self.goal_handle.goal_id:
                         result = self.action_type.Result()
-                        result.message = "goal cancelled at {percentage:.2f}%%".format(
+                        result.message = "goal pre-empted at {percentage:.2f}%%".format(
                             percentage=self.percent_completed)
                         self.node.get_logger().info(result.message)
-                        goal_handle.set_canceled()
+                        goal_handle.abort()
                         return result
                     elif self.percent_completed >= 100.0:
                         self.percent_completed = 100.0
                         self.node.get_logger().info("sending feedback 100%%")
                         result = self.action_type.Result()
                         result.message = "goal executed with success"
                         self.node.get_logger().info(result.message)
-                        goal_handle.set_succeeded()
+                        goal_handle.succeed()
                         return result
                     else:
                         self.node.get_logger().info("sending feedback {percentage:.2f}%%".format(
@@ -208,9 +207,6 @@ def execute_goal_callback(
     def handle_accepted_callback(self, goal_handle):
         self.node.get_logger().info("handle accepted")
         with self.goal_lock:
-            if self.goal_handle is not None:
-                self.node.get_logger().info("pre-empting")
-                self.preempt_goal_ids.append(self.goal_handle.goal_id)
             self.goal_handle = goal_handle
             goal_handle.execute()
 
@@ -222,7 +218,7 @@ def abort(self):
         with self.goal_lock:
             if self.goal_handle and self.goal_handle.is_active:
                 self.node.get_logger().info("aborting...")
-                self.goal_handle.set_aborted()
+                self.goal_handle.abort()
 
     def shutdown(self):
         """

diff --git a/py_trees_ros/mock/dock.py b/py_trees_ros/mock/dock.py
@@ -51,7 +51,7 @@ def generate_feedback_message(self):
         Create some appropriate feedback.
         """
         # TODO: send some feedback message
-        msg = py_trees_actions.Dock_Feedback(
+        msg = py_trees_actions.Dock.Feedback(  # noqa
             percentage_completed=self.percent_completed
         )
         return msg
diff --git a/py_trees_ros/programs/blackboard_watcher.py b/py_trees_ros/programs/blackboard_watcher.py
@@ -29,7 +29,6 @@
 import rclpy
 import std_msgs.msg as std_msgs
 import sys
-import time
 
 ##############################################################################
 # Classes
@@ -118,6 +117,7 @@ def main(command_line_args=sys.argv[1:]):
     blackboard_watcher = py_trees_ros.blackboard.BlackboardWatcher(
         namespace_hint=args.namespace
     )
+    subscription = None
     ####################
     # Setup
     ####################
@@ -163,10 +163,11 @@ def main(command_line_args=sys.argv[1:]):
             blackboard_watcher.node.get_logger().info(
                 "creating subscription [{}]".format(watcher_topic_name)
             )
-            blackboard_watcher.node.create_subscription(
+            subscription = blackboard_watcher.node.create_subscription(
                 msg_type=std_msgs.String,
                 topic=watcher_topic_name,
-                callback=blackboard_watcher.echo_blackboard_contents
+                callback=blackboard_watcher.echo_blackboard_contents,
+                qos_profile=rclpy.qos.qos_profile_system_default
             )
             # stream
             try:
@@ -188,6 +189,8 @@ def main(command_line_args=sys.argv[1:]):
             py_trees_ros.exceptions.TimedOutError) as e:
         print(console.red + "\nERROR: {}".format(str(e)) + console.reset)
         result = 1
+    if subscription is not None:
+        blackboard_watcher.node.destroy_subscription(subscription)
     blackboard_watcher.shutdown()
     rclpy.shutdown()
     sys.exit(result)
diff --git a/py_trees_ros/subscribers.py b/py_trees_ros/subscribers.py
@@ -83,7 +83,7 @@ def __init__(self,
                  name="Subscriber Handler",
                  topic_name="/foo",
                  topic_type=None,
-                 qos_profile=rclpy.qos.qos_profile_default,
+                 qos_profile=rclpy.qos.qos_profile_system_default,
                  clearing_policy=py_trees.common.ClearingPolicy.ON_INITIALISE
                  ):
         super(Handler, self).__init__(name)
@@ -116,7 +116,7 @@ def setup(self, **kwargs):
             msg_type=self.topic_type,
             topic=self.topic_name,
             callback=self._callback,
-            # qos_profile=self.qos_profile
+            qos_profile=self.qos_profile
         )
 
     def initialise(self):

diff --git a/py_trees_ros/trees.py b/py_trees_ros/trees.py
@@ -208,8 +208,13 @@ def shutdown(self):
         if self.timer is not None:
             self.timer.cancel()
             self.node.destroy_timer(self.timer)
-        self.node.destroy_node()
+        # call shutdown on each node first, in case it has
+        # some esoteric shutdown steps that needs the node
         super().shutdown()
+        # shutdown the node - this *should* automagically clean
+        # up any non-estoeric shutdown of ros communications
+        # inside behaviours
+        self.node.destroy_node()
 
     def _tick_tock_timer_callback(
             self,
@@ -404,7 +409,7 @@ def setup(self):
         # type in the namespace to do auto-discovery of names
         topic_names = {}
         for key, topic_type_string in [
-            ('snapshots', 'py_trees_ros_interfaces/BehaviourTree')
+            ('snapshots', 'py_trees_ros_interfaces/msg/BehaviourTree')
         ]:
             topic_names[key] = utilities.find_topic(
                 self.node,

diff --git a/py_trees_ros/utilities.py b/py_trees_ros/utilities.py
@@ -242,7 +242,8 @@ def __init__(self, node, publisher_details, introspection_topic_name="publishers
             else:
                 self.__dict__[name] = node.create_publisher(
                     msg_type=publisher_type,
-                    topic=topic_name
+                    topic=topic_name,
+                    qos_profile=rclpy.qos.qos_profile_system_default
                 )
             resolved_name = resolve_name(node, topic_name)
             message_type = publisher_type.__class__.__module__.split('.')[0] + "/" + publisher_type.__class__.__name__
@@ -302,7 +303,8 @@ def __init__(self, node, subscriber_details, introspection_topic_name="subscribe
                 self.__dict__[name] = node.create_subscription(
                     msg_type=subscriber_type,
                     topic=topic_name,
-                    callback=callback
+                    callback=callback,
+                    qos_profile=rclpy.qos.qos_profile_system_default
                 )
 
             resolved_name = resolve_name(node, topic_name)

diff --git a/py_trees_ros/version.py b/py_trees_ros/version.py
@@ -15,4 +15,4 @@
 # Version
 ##############################################################################
 
-__version__ = '1.0.0'
+__version__ = '1.1.0'
diff --git a/setup.py b/setup.py
@@ -7,7 +7,7 @@
 
 setup(
     name=package_name,
-    version='1.0.0',
+    version='1.1.0',
     packages=find_packages(
         exclude=['doc*', 'tests*', 'graveyard*', 'scripts*']
     ),

diff --git a/tests/test_action_client.py b/tests/test_action_client.py
@@ -40,8 +40,8 @@ def create_action_client():
         name="dock",
         action_type=py_trees_actions.Dock,
         action_name="dock",
-        action_goal=py_trees_actions.Dock.Goal(dock=True),
-        generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.percentage_completed)
+        action_goal=py_trees_actions.Dock.Goal(dock=True),  # noqa
+        generate_feedback_message=lambda msg: "{:.2f}%%".format(msg.feedback.percentage_completed)
     )
     return behaviour
 
@@ -84,7 +84,7 @@ def __init__(self):
 
     def execute_goal_callback(self, goal_handle):
         result = self.action_type.Result()
-        goal_handle.set_aborted()
+        goal_handle.abort()
         return result
 
 ##############################################################################