Week 5 Preemptive Action Client

Week 5 ROS Actions

Preemptive Client Node

The Code

Copy all of the code below into your preemptive_action_client.py file. Then, review the explainer below to understand how this all works.

#!/usr/bin/env python3

import rospy
import actionlib

from com2009_msgs.msg import CameraSweepAction, CameraSweepGoal, CameraSweepFeedback

class preemptiveActionClient():
   
    def feedback_callback(self, feedback_data: CameraSweepFeedback):
        self.captured_images = feedback_data.current_image
        print(f"FEEDBACK: Current yaw: {feedback_data.current_angle:.1f} degrees. "
            f"Image(s) captured so far: {self.captured_images}...")

    def __init__(self):
        self.captured_images = 0
        self.action_complete = False

        node_name = "preemptive_camera_sweep_action_client"
        action_server_name = "/camera_sweep_action_server"
        
        rospy.init_node(node_name)

        self.rate = rospy.Rate(1)

        self.goal = CameraSweepGoal()

        self.client = actionlib.SimpleActionClient(action_server_name, 
                    CameraSweepAction)
        self.client.wait_for_server()

        rospy.on_shutdown(self.shutdown_ops)

    def shutdown_ops(self):
        if not self.action_complete:
            rospy.logwarn("Received a shutdown request. Cancelling Goal...")
            self.client.cancel_goal()
            rospy.logwarn("Goal Cancelled")
            print(f"RESULT: {self.captured_images} image(s) saved.")

    def send_goal(self, images, angle):
        self.goal.sweep_angle = angle
        self.goal.image_count = images
        
        # send the goal to the action server:
        self.client.send_goal(self.goal, feedback_cb=self.feedback_callback)

    def main(self):
        self.send_goal(images = 10, angle = 90)
        i = 1
        print("While we're waiting, let's do our seven-times tables...")
        while self.client.get_state() < 2:
            print(f"STATE: Current state code is {self.client.get_state()}")
            print(f"TIMES TABLES: {i} times 7 is {i*7}")
            i += 1
            self.rate.sleep()
        self.action_complete = True
        print(f"RESULT: Action State = {self.client.get_state()}")
        print(f"RESULT: {self.captured_images} images saved to {self.client.get_result()}")

if __name__ == '__main__':
    {BLANK}

FILL IN THE {BLANK}! We have contained all our code inside a nice Python Class now, but how do we actually instantiate it and invoke the Action Call? (We have been doing this from the very beginning, and the process is very much the same here!)

The Code Explained:

Our imports are all exactly the same as last time here. The main difference now though is that everything else is now contained within a Python Class:

class preemptiveActionClient():

The feedback callback function is exactly the same, except we are now making the captured_images variable available across the whole class using the self prefix (previously this was achieved using the global statement).

def feedback_callback(self, feedback_data: CameraSweepFeedback):
    self.captured_images = feedback_data.current_image
    print(f"FEEDBACK: Current yaw: {feedback_data.current_angle:.1f} degrees. "
        f"Image(s) captured so far: {self.captured_images}...")

We now have an __init__() method inside the class, which will be executed as soon as the class is instantiated. Here, we do all our initialisations:

• Initialise some variables (captured_images, action_complete) and make them available throughout the class by assigning them to self.
• Initialise the node (with a name).
• Create the action goal object, which will be populated later.
• Create a connection to the action server.
• Specify a function to be executed when the node is stopped (shutdown_ops()).

def __init__(self):
    self.captured_images = 0
    self.action_complete = False

    node_name = "preemptive_camera_sweep_action_client"
    action_server_name = "/camera_sweep_action_server"
    
    rospy.init_node(node_name)

    self.rate = rospy.Rate(1)

    self.goal = CameraSweepGoal()

    self.client = actionlib.SimpleActionClient(action_server_name, 
                CameraSweepAction)
    self.client.wait_for_server()

    rospy.on_shutdown(self.shutdown_ops)

The actual shutdown operations are then defined within the shutdown_ops() function. This is how we make sure that the current goal is cancelled (using cancel_goal()), so that it doesn't just keep on running when this node is stopped prematurely (before the action has completed). This function will also execute when the action server completes successfully, so we use an action_complete flag to check whether this is the case (i.e. don't attempt to cancel the goal if it has already finished!):

def shutdown_ops(self):
    if not self.action_complete:
        rospy.logwarn("Received a shutdown request. Cancelling Goal...")
        self.client.cancel_goal()
        rospy.logwarn("Goal Cancelled")
        print(f"RESULT: {self.captured_images} image(s) saved.")

The way the goal is defined and issued to the server is exactly the same as before, except this time it's done within a class method, so that it can be called from main():

def send_goal(self, images, angle):
    self.goal.sweep_angle = angle
    self.goal.image_count = images
    
    # send the goal to the action server:
    self.client.send_goal(self.goal, feedback_cb=self.feedback_callback)

Inside the main() method, we then call the goal and then do exactly the same as we did before: monitor the state of the action with a while loop, do some concurrent operations (seven-times tables again!), and print the action result on completion (assuming the node hasn't been shutdown before the action completes).

def main(self):
    self.send_goal(images = 10, angle = 90)
    i = 1
    print("While we're waiting, let's do our seven-times tables...")
    while self.client.get_state() < 2:
        print(f"STATE: Current state code is {self.client.get_state()}")
        print(f"TIMES TABLES: {i} times 7 is {i*7}")
        i += 1
        self.rate.sleep()
    self.action_complete = True
    print(f"RESULT: Action State = {self.client.get_state()}")
    print(f"RESULT: {self.captured_images} images saved to {self.client.get_result()}")

The only difference is that we set the action_complete flag to True if the action manages to complete successfully.

Finally, at the very bottom we need to actually launch the action server by instantiating the class and running the right class method from within it (taking care to do some error checking at the same time...

if __name__ == '__main__':
    ...