Skip to content

Commit

Permalink
[mock] fake dock and rotate actions (#10)
Browse files Browse the repository at this point in the history
  • Loading branch information
stonier authored Mar 1, 2019
1 parent dd8848f commit e063fce
Show file tree
Hide file tree
Showing 8 changed files with 516 additions and 8 deletions.
4 changes: 4 additions & 0 deletions .settings/org.eclipse.core.resources.prefs
Original file line number Diff line number Diff line change
@@ -1,10 +1,14 @@
eclipse.preferences.version=1
encoding//py_trees_ros_tutorials/mock/action_clients.py=utf-8
encoding//py_trees_ros_tutorials/mock/actions.py=utf-8
encoding//py_trees_ros_tutorials/mock/battery.py=utf-8
encoding//py_trees_ros_tutorials/mock/dashboard.py=utf-8
encoding//py_trees_ros_tutorials/mock/dock.py=utf-8
encoding//py_trees_ros_tutorials/mock/gui/configuration_group_box.py=utf-8
encoding//py_trees_ros_tutorials/mock/gui/configuration_group_box_ui.py=utf-8
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/rotate.py=utf-8
encoding//py_trees_ros_tutorials/mock/safety_sensors.py=utf-8
11 changes: 6 additions & 5 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
<license>BSD</license>

<url type="website">http://ros.org/wiki/py_trees</url>
<url type="repository">https://github.com/splintered-treality/py_trees_ros</url>
<url type="bugtracker">https://github.com/splintered-treality/py_trees_ros/issues</url>
<url type="repository">https://github.com/splintered-treality/py_trees_ros_tutorials</url>
<url type="bugtracker">https://github.com/splintered-treality/py_trees_ros_tutorials/issues</url>

<author>Daniel Stonier</author>

Expand All @@ -35,7 +35,7 @@
<doc_depend>python-rospkg</doc_depend>
<doc_depend>python3-pyqt5</doc_depend>
<doc_depend>py_trees</doc_depend>
<doc_depend>py_trees_msgs</doc_depend>
<doc_depend>py_trees_interfaces</doc_depend>
<doc_depend>rosbag</doc_depend>
<doc_depend>rospy</doc_depend>
<doc_depend>sensor_msgs</doc_depend>
Expand All @@ -55,12 +55,13 @@
<!-- core dependencies -->
<exec_depend>py_trees</exec_depend>
<exec_depend>py_trees_ros</exec_depend>
<exec_depend>py_trees_ros_interfaces</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>uuid_msgs</exec_depend>
<exec_depend>unique_identifier_msgs</exec_depend>

<!-- launch dependencies -->
<exec_depend>python3-pyqt5</exec_depend>
<exec_depend>python3-qt5-bindings</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>ros2launch</exec_depend>
Expand Down
150 changes: 150 additions & 0 deletions py_trees_ros_tutorials/mock/action_clients.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,150 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# License: BSD
# https://raw.githubusercontent.com/stonier/py_trees/devel/LICENSE
#
##############################################################################
# Documentation
##############################################################################

"""
Mocks a battery provider.
"""


##############################################################################
# Imports
##############################################################################

import uuid

import action_msgs.msg as action_msgs # GoalStatus
import action_msgs.srv as action_srvs # CancelGoal
import py_trees_ros_interfaces.action as py_trees_ros_actions # Dock, Rotate
import py_trees_ros.utilities
import rclpy
# import rclpy.action # ActionClient

##############################################################################
# Class
##############################################################################


class ActionClient(object):
"""
Generic action client that can be used to test the mock action servers.
Args:
node_name (:obj:`str`): name of the action server (e.g. move_base)
action_name (:obj:`str`): name of the action server (e.g. move_base)
action_type (:obj:`any`): type of the action server (e.g. move_base_msgs.msg.MoveBaseAction
worker (:obj:`func`): callback to be executed inside the execute loop, no args
goal_recieved_callback(:obj:`func`): callback to be executed immediately upon receiving a goal
duration (:obj:`float`): forcibly override the dyn reconf time for a goal to complete (seconds)
Use the ``dashboard`` to dynamically reconfigure the parameters.
"""
def __init__(self,
node_name,
action_name,
action_type_string,
action_server_namespace):
self.node = rclpy.create_node(node_name)
self.action_name = action_name
self.action_type = action_type_string
self.action_server_namespace = action_server_namespace
self.goal_type = getattr(
py_trees_ros_actions,
action_type_string + "_Goal"
)
self.result_type = getattr(
py_trees_ros_actions,
action_type_string + "_Result"
)
self.clients = {
"goal": self.node.create_client(
self.goal_type,
"{}/goal".format(self.action_server_namespace)
),
"result": self.node.create_client(
self.result_type,
"{}/result".format(self.action_server_namespace)
),
"cancel": self.node.create_client(
action_srvs.CancelGoal,
"{}/cancel".format(self.action_server_namespace)
)
}
# self.action_client = rclpy.action.ActionClient(self.node, action_type, action_name)

def feedback_callback(self, feedback):
self.node.get_logger().info('Received feedback: {0}'.format(feedback.sequence))

def process_result(self, future):
status = future.result().action_status
if status == action_msgs.GoalStatus.STATUS_SUCCEEDED:
self.node.get_logger().info('goal success!')
else:
self.node.get_logger().info('goal failed with status {0}'.format(status))

def send_goal(self, uuid_msg):
# send and wait for the goal response
self.node.get_logger().info('sending goal request...')
request = self.goal_type.Request(action_goal_id=uuid_msg)
self.node.get_logger().info("goal request: %s" % request)
future = self.clients["goal"].call_async(request)
rclpy.spin_until_future_complete(self.node, future)
if future.result() is not None and future.result().accepted is True:
self.node.get_logger().info("...goal accepted %s" % future.result())
else:
self.node.get_logger().info('...goal rejected [%r]' % (future.exception()))

def spin(self):
initialised = {name: False for name in self.clients.keys()}
try:
while rclpy.ok() and not all(value for value in initialised.values()):
for name, client in self.clients.items():
if client.wait_for_service(timeout_sec=0.1):
initialised[name] = True
self.node.get_logger().info("'{}' initialised".format(name))
else:
self.node.get_logger().info("service '{}' unavailable, waiting...".format(self.action_server_namespace + "/" + name))

uuid_msg = py_trees_ros.utilities.uuid4_to_msg()
self.send_goal(uuid_msg)

request = self.result_type.Request(action_goal_id=uuid_msg)
self.node.get_logger().info("result request %s" % request)
future = self.clients["result"].call_async(request)
self.node.get_logger().info(" future %s" % future)
while rclpy.ok():
rclpy.spin_once(self.node, timeout_sec=0.1)
if future.done():
self.process_result(future)
break
except KeyboardInterrupt:
pass
self.node.destroy_node()


def dock(args=None):
rclpy.init(args=args)
action_client = ActionClient(
node_name="dock_client",
action_name="dock",
action_type_string="Dock", # py_trees_ros_actions.Dock
action_server_namespace="/docking_controller"
)
action_client.spin()


def rotate(args=None):
rclpy.init(args=args)
action_client = ActionClient(
node_name="rotate_client",
action_name="rotate",
action_type_string="Rotate", # py_trees_ros_actions.Rotate
action_server_namespace="/rotation_controller"
)
action_client.spin()
Loading

0 comments on commit e063fce

Please sign in to comment.