MTC Action Server (Rclpy)

[matthias88]

Hello,

I would like to implement MTC in an rclpy action server e.g. Minimal Action Server. The idea would be, that the client sends a task request e.g. pose of an object and the server calculates the task, sends feedback during planning and eventually cancelling or executing the task. My naive approach was, that I create a MTC Node with different task methods and call them in the execution callback. Thereby the action server and task constructor are not blocking each other, I wanted to use a multithreaded executor. Due to the pybinding_tools, see #520 and #563, I am not able to inherit/initialize as usual in rclpy and I am also not able to extend the MTC Node with e.g. pub/sub/timer or use it within multithreaded executor. Can anybody help? Attached you can find some pseudocode:

class TaskConstructor(Node): # How to inherit and initialize properly?

    def __init__(self):
        rclcpp.init()
        rclcpp.Node("task_constructor") # super().__init__('task_constructor')
        self.logger = get_logger("task_constructor")
        self.pub_status_task = self.create_publisher(String, 'status_task', 10)
        self.sub_start_task = self.create_subscription(Bool, 'start_task', self.start_callback, 10)

    def test_task(self):
            
        msg = String()
        msg.data = "Starting mp_test_task"
        self.get_logger().info('Publishing: "{0}"'.format(msg.data))
        self.pub_status_mp.publish(msg)

        # Cartesian and joint-space interpolation planners
        cartesian = core.CartesianPath()
        cartesian.max_acceleration_scaling_factor = settings.SCALING_FACTOR_SPEED
        cartesian.max_velocity_scaling_factor = settings.SCALING_FACTOR_SPEED
        cartesian.step_size = 0.001
        cartesian.precision.translational = 0.001
        # cartesian.jump_threshold = 0.0  # effectively disable jump threshold.
        jointspace = core.JointInterpolationPlanner()
        jointspace.max_acceleration_scaling_factor = settings.SCALING_FACTOR_SPEED
        jointspace.max_velocity_scaling_factor = settings.SCALING_FACTOR_SPEED
        jointspace.max_step = 0.1
            
        # Define task
        task = core.Task()
        task.name = "Test pickplace"
        task.loadRobotModel(self.node)

        # start from current robot state
        task.add(stages.CurrentState("current state"))

        # moveTo named posture, using joint-space interplation
        move = stages.MoveTo("moveTo Prepos", jointspace)
        move.group = settings.GROUP_NAME_ARM
        move.timeout = 0.1
        move.setGoal("Pre_pos")
        task.add(move)

        if task.plan():
            task.publish(task.solutions[0])


class TaskActionServer(Node):

    def __init__(self):
        super().__init__('task_action_server')

        self._action_server = ActionServer(
            self,
            Task,
            'task',
            execute_callback=self.execute_callback,
            callback_group=ReentrantCallbackGroup(),
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback)
        
        mp_callback_group = MutuallyExclusiveCallbackGroup()
        self.pub_start_mp = self.create_publisher(String, 'start_task', 10, callback_group=mp_callback_group)
        self.pub_cancel_mp = self.create_publisher(Bool, 'cancel_task', 10, callback_group=mp_callback_group)
        self.sub = self.create_subscription(String, 'status_task', self.mp_callback, 10, callback_group=mp_callback_group)

        self.mp_finished = False
        self.mp_status = ""
        self.mp_cancel = False

    def destroy(self):
        self._action_server.destroy()
        super().destroy_node()
    
    def mp_callback(self, msg):
        # self.get_logger().info('Status MotionPlanning: "%s"' % msg.data)
        self.mp_status = str(msg.data)
        if msg.data == "Finished":
            self.mp_finished = True

    def goal_callback(self, goal_request):
        """Accept or reject a client request to begin an action."""
        # This server allows multiple goals in parallel
        self.get_logger().info('Received goal request')
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        """Accept or reject a client request to cancel an action."""
        self.get_logger().info('Received cancel request')

        self.mp_cancel = True
        
        msg = Bool()
        msg.data = self.mp_cancel

        # Publish start signal for motion planning test cycle node
        self.pub_cancel_mp.publish(msg)
        return CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        self.mp_cancel = False

        """Execute a goal."""
        self.get_logger().info('Executing goal...')

        # Append status
        feedback_msg = Task.Feedback()
        feedback_msg.str_status = "Calling task"

        self.mp_finished = False

        msg = String()
        msg.data = feedback_msg.str_status
        self.get_logger().info('Publishing Feedback: {0}'.format(feedback_msg.str_status))

        # Publish the feedback
        goal_handle.publish_feedback(feedback_msg)

        # Publish start signal for motion planning test cycle node
        self.pub_start_mp.publish(msg)

        # Start executing the action
        timeout = 20   # [seconds]

        timeout_start = time.time()

        while time.time() < timeout_start + timeout:
        # for i in range(1,10):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled')
                return Task.Result()

            # Update status
            if not self.mp_finished:
                if self.mp_status != feedback_msg.str_status and self.mp_status != "" and self.mp_status != "Finishing task":
                    feedback_msg.str_status = self.mp_status
                    self.get_logger().info('Publishing Feedback: {0}'.format(feedback_msg.str_status))

                    # Publish the feedback
                    goal_handle.publish_feedback(feedback_msg)

            else:
                feedback_msg.str_status = self.mp_status
                self.get_logger().info('Publishing Feedback: {0}'.format(feedback_msg.str_status))

                # Publish the feedback
                goal_handle.publish_feedback(feedback_msg)

                break

        goal_handle.succeed()

        # Populate result message
        result = Task.Result()
        b_done = True
        result.b_done = b_done

        self.get_logger().info('Task finished with result: {0}'.format(result.b_done))

        return result


def main(args=None):

    rclpy.init(args=args)
    try:
        task_action_server = TaskActionServer()
        task_constructor = TaskConstructor()
 
        
        # Use a MultiThreadedExecutor to enable processing goals concurrently
        executor = MultiThreadedExecutor()

        executor.add_node(task_action_server)
        executor.add_node(task_constructor)
        executor.spin()
    
    except KeyboardInterrupt:
        pass
    except ExternalShutdownException:
        sys.exit(1)


if __name__ == '__main__':
    main()

[rhaschke]

I'm not quite sure about the actual problem you are facing. Indeed, you cannot pass a rclcpp.Node to rclpy methods expecting an rclpy.Node. These are two different classes. However, you can write your server using rclpy.Node classes as usual.

[matthias88]

Thank you a lot for your quick reply. But how can I "communicate" between the server and the MTC Node? At least I was not able or don`t know how to "extend" the MTC Node with e.g. Pub/Sub to start task planning method for example in the start_callback (yes, because it is a rclcpp.Node, but what are my alternatives/possibilities?). Is there a possibility to wrap it somehow into a rclpy.Node? Thank you in advance for your patience :-)

[rhaschke]

Why do you want to communicate between those nodes? Simply create your task in the server's execute_callback.

[matthias88]

If I want to cancel the task, set a timeout etc. I wasn't able to realize it directly in the execute_callback - I realized a similiar working example with moveit_py. Therefore I wanted to communicate between those nodes, but I am gonna test your recommendation.

[rhaschke]

If I want to cancel the task, set a timeout etc. I wasn't able to realize it directly in the execute_callback

As you want to have parallel planning, just assign an MTC task with each goal handle. Then you can easily cancel etc. the task.

rclcpp_init only needs to be called once in main().

[matthias88]

Thank you so much, I am going to test it