• +4917626969472
  • info@ur-techpartner.de

ROS Basics (15/17) – Improved Action Server

Robot Operating System(ROS1/ROS2) ROS Programming

ROS Basics – Implement an improved Action Server

In previous blogs, we have implemented a simple action server. In this blog we will learn how to add more features including preempting a goal request (cancel) and providing feedback while performing a goal task

Creating an Improved Action Server File

We will implement a more sophisticated action server for the action “Timer.action”, that we are already using.

We will create new action server file in the package “ros_basics”.

Go to the package

Run the command

roscd ros_basics/src

Also run

ls

(To verify that the improved_action_server file is not already created.

Now create the file “improved_action_server.py”

For this run the command

touch improved_action_server.py

Run the command to give execute permission

chmod +x improved_action_server.py

Open the file using “gedit” command

Run the command

gedit improved_action_server.py

Copy and paste following code in it.

#! /usr/bin/env python
import rospy
import time
import actionlib
from ros_basics.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback

def do_timer(goal):
        start_time = time.time()
        update_count = 0

        if goal.time_to_wait.to_sec() > 60.0:
                result = TimerResult()
                result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
                result.updates_sent = update_count
                server.set_aborted(result, "Timer aborted due to too-long wait")
                return

        while (time.time() - start_time) < goal.time_to_wait.to_sec():
                if server.is_preempt_requested():
                        result = TimerResult()
                        result.time_elapsed = \
                        rospy.Duration.from_sec(time.time() - start_time)
                        result.updates_sent = update_count
                        server.set_preempted(result, "Timer preempted")
                        return

                feedback = TimerFeedback()
                feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
                feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed
                server.publish_feedback(feedback)
                update_count += 1
                time.sleep(1.0)        

        result = TimerResult()
        result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
        result.updates_sent = update_count
        server.set_succeeded(result, "Timer completed successfully")


rospy.init_node('timer_action_server')
server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False)
server.start()
rospy.spin()

 

All above executed commands are shown in figure below

The file “action_client.py” will look like in figure below

Save and close the file

Explaining the Code in Python file “improved_action_server.py”

We will go through the key key parts of the code. We may skip the lines that we have explained previously.

from ros_basics.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback
update_count = 0

(Since in this action server, we will provide feedback, therefore we have added “TimerFeedback” in import list. In next line we have added a variable to keep record of number of feedbacks we sent).

def do_timer(goal):

(In this line we are defining a callback method named “do_timer()” which takes the argument “goal”)

if goal.time_to_wait.to_sec() > 60.0:
result = TimerResult()
result.time_elapsed = rospy.Duration.from_sec(time.time() – start_time)
result.updates_sent = update_count
server.set_aborted(result, “Timer aborted due to too-long wait”)
return

(In above lines, we are checking if the timer has to wait more than 60 seconds. Since we do not want to wait for too long. Therefore, in this case we will abort the process. Here we call the method “set_aborted()” instead of “set_succeeded()”. We pass the result and also a string explaining the reason why the process is aborted. In the end we return from the callback function.

while (time.time() – start_time) < goal.time_to_wait.to_sec():

(In above line we have started a loop, that will run until requested wait time is completed. The purpose of this loop is that the program should sleep in increments instead of sleeping for the whole duration of waiting time. In this case, we may perform other tasks while waiting for the time to complete).

if server.is_preempt_requested():
result = TimerResult()
result.time_elapsed = \
rospy.Duration.from_sec(time.time() – start_time)
result.updates_sent = update_count
server.set_preempted(result, “Timer preempted”)
return

(Within while loop, we will first check it the request is preempted at any stage. This condition will be true if the client send cancel_goal to the server. In this case, the program will be preempted. The result and a status message that the “Timer preempted” will be passed to the client. Then we will return from callback method).

feedback = TimerFeedback()
feedback.time_elapsed = rospy.Duration.from_sec(time.time() – start_time)
feedback.time_remaining = goal.time_to_wait – feedback.time_elapsed
server.publish_feedback(feedback)
update_count += 1

(In above lines, we created an object of TimerFeedback(). This corresponds to the feedback part of Timer.action. You can open the file “Timer.action” and see that this part has two variables which are time_elapsed, time_remaining. In these lines, we fill these two fields (or variables). Then the server calls the function publish_feedback() to send the feedback to the client. In last line, we increment the update_count variable to count the number of feedbacks sent).

time.sleep(1.0)

(Now the program will sleep for 1 second. Its not mandatory to sleep here. However, for keeping things simple and not sending feedback very quickly we sleep for 1 second. Also, you should be careful for not sleeping a long time. Because you may end up for sleeping longer than the requested waiting time).

result = TimerResult()
result.time_elapsed = rospy.Duration.from_sec(time.time() – start_time)
result.updates_sent = update_count
server.set_succeeded(result, “Timer completed successfully”)

(The above lines will be executed when we will get out of the while loop. It means that we have successfully completed the requested waiting duration. So after exiting the loop, we will notify the client that the goal is achieved. The above lines are similar to the simple action server. The callback method do_timer() ends here).

rospy.init_node(‘timer_action_server’)
server = actionlib.SimpleActionServer(‘timer’, TimerAction, do_timer, False)
server.start()
rospy.spin()

(The above lines are same as in simple action server).

Executing “improved_action_server.py” file

Now we will execute the file “improved_action_server.py” and will verify if everything is working fine.

First open the terminal and run the action server

Run the command

rosrun ros_basics improved_action_server.py

The terminal will look like in figure below

Now open another terminal and run the command

rostopic list

You will see a list of five topics along with /timer as shown in figure below

It means that the program is executed successfully, and the action server is running.
Next we will implement an improved action client in accordance with the features added in improved action server to use the action.

No comments