Source code for py_trees_ros_tutorials.mock.move_base

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# License: BSD
#   https://github.com/splintered-reality/py_trees_ros_tutorials/raw/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 py_trees_ros.mock.actions
import py_trees_ros_interfaces.action as py_trees_actions
import rclpy
import sys

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


[docs]class MoveBase(py_trees_ros.mock.actions.GenericServer): """ Simulates a move base style interface. Node Name: * **move_base_controller** Action Servers: * **/move_base** (:class:`py_trees_ros_interfaces.action.MoveBase`) * point to point move base action Args: duration: mocked duration of a successful action """ def __init__(self, duration=None): super().__init__( node_name="move_base_controller", action_name="move_base", action_type=py_trees_actions.MoveBase, generate_feedback_message=self.generate_feedback_message, duration=duration ) self.pose = geometry_msgs.PoseStamped() self.pose.pose.position = geometry_msgs.Point(x=0.0, y=0.0, z=0.0)
[docs] def generate_feedback_message(self) -> py_trees_actions.MoveBase.Feedback: """ Do a fake pose incremenet and populate the feedback message. Returns: :class:`py_trees_actions.MoveBase.Feedback`: the populated feedback message """ # 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.position.x += 0.01 msg = py_trees_actions.MoveBase.Feedback() # .Feedback() is more proper, but indexing can't find it msg.base_position = self.pose return msg
[docs]def main(): """ Entry point for the mock move base 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 = MoveBase() executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) executor.add_node(move_base.node) try: executor.spin() except KeyboardInterrupt: move_base.abort() # caveat: often broken, with multiple spin_once or shutdown, error is the # mysterious: # The following exception was never retrieved: PyCapsule_GetPointer # called with invalid PyCapsule object executor.shutdown() # finishes all remaining work and exits move_base.shutdown() rclpy.shutdown()