Lab 03

During this lab, you should learn how to use ROS timers, action servers and ros_numpy library.

ROS has built-in types for Time and Duration and their arithmetics.

Time can be used to get the current time of your ROS system.

time_now = rospy.Time.now() # current time with int32 secs and nsecs values
seconds = time_now.to_sec() # time in floating point format

Duration can be created, e.g., equal to a certain number of seconds and then used, e.g., to wait in your code for such a duration.

d = rospy.Duration.from_sec(42.1)
rospy.sleep(d)

The result of subtracting two ROS Time instances is also an instance of ROS Duration.

time_start = rospy.Time.now()
rospy.sleep(rospy.Duration.from_sec(3.0))
d = rospy.Time.now() - time_start 

Rate is a very convenient way to enforce approximately constant frequency of, e.g., iterating inside a loop. The rate.sleep() will try to keep the given frequency independent of the duration of other operations during the loop.

rate = Rate(10) # create rate with 10 Hz frequency
while not rospy.is_shutdown():
    # do some calculation
    rate.sleep()

Timer object can be used to periodically call some function, e.g., to calculate something and publish it. In most cases, the timer can be used instead of a custom multithreading of your application.

def my_callback(event):
    # calculate something to msg
    publisher.publish(msg)

rospy.Timer(rospy.Duration(2), my_callback) # calls function (callback) my_callback every two seconds

The ROS action servers/clients provide the ability to execute and interface preemptable tasks that can take a long time to execute. It also allows you to get a regular feedback from tasks that are currently being executed. Examples of such a task can be moving a robot to a target location or following a given path with a robot. You can check out the official python tutorial action server and action client.

The action messages have three parts, the goal (similar to services' request), the results (similar to services' response), and the feedback part to enable reporting on the action run. The action server and action clients can be created using actionlib library in python.

The definition of an example action GoToPosition.action looks as follows:

#goal definition
geometry_msgs/Point position
---
#result definition
bool success
string message
---
#feedback
float32 position_error
float32 angle_error
string message

The action server part listens for a goal message and once the goal is received, it starts executing procedures to fulfill the given goal. While working on finishing the goal, the action server can periodically report the current state of goal processing through a feedback message, but also has to check if it was not stopped through preemption request. Upon finishing or failing to fulfill the goal, the action server sends its result to the client. The action server code could look like this:

def execute_cb(goal):
    success = True # just to save global success status somewhere
    rate = rospy.Rate(10) # run the action while loop with 10Hz frequency

    while not rospy.is_shutdown(): #while the node is running
        if action_server.is_preempt_requested(): #the action was canceled from outside
            action_server.set_preempted()
            success = False
            break # break the action loop
        
        .... # do something now, shush shush
        
        feedback = GoToPositionFeedback() # create a feedback message
        feedback.message = "still working on it" # fill the feedback message
        action_server.publish_feedback(feedback) # send the feedback
        
        # check finish/fail conditions
        if #finished or #failed:
            success = True/False #based on finished or failed
            break # break the action loop

        rate.sleep() # sleep for rest of rate's period

    result = GoToPositionResult() # create result message when action finishes
    result.success = success
    if success:
        result.message = "finished successfully"
        action_server.set_succeeded(result) # send the result
    else:
        result.message = "failed miserably"
        action_server.set_aborted(result) # send the result

# create action server "robocop_goto_as" with GoToPositionAction type
action_server = actionlib.SimpleActionServer("robocop_goto_as", GoToPositionAction, execute_cb=execute_cb,
                                                                                           auto_start=False)
action_server.start()

The action client can be used to interface the action server efficiently. A simple client can look like this:

# create "robocop_goto_as" client with GoToPositionAction action type
client = actionlib.SimpleActionClient('robocop_goto_as', GoToPositionAction)
client.wait_for_server() # wait for the server to be alive
goal = GoToPositionGoal() # create the goal message
goal.position = ... # fill the position
client.send_goal(goal) # send the goal message
client.wait_for_result() # wait for the server to finish

If you have the newest singularity image (or you have installed ros-noetic-actionlib-tools) then you can use GUI action client: rosrun actionlib_tools axclient.py /robocop_goto_as aro_robocop_goto_as/GoToPositionAction.

An alternative is to use classical rostopics both to publish a goal and to receive feedback or result. You can publish a goal from the terminal using: rostopic pub /robocop_goto_as/goal aro_robocop_goto_as/GoToPositionGoal ….

You can use rostopic echo for getting the feedback or the result as: rostopic echo /robocop_goto_as/feedback, rostopic echo /robocop_goto_as/result.

Library ros_numpy includes tools for a convenient conversion of ROS messages to and from numpy arrays. The library contains two main functions numpify and msgify:

arr = numpify(msg, ...) # tries to get a numpy object from a ROS message
msg = msgify(MessageType, arr, ...) # tries to convert a numpy object to a ROS message

Currently, the supported type of messages are OccupancyGrid, Vector3, Point, Quaternion, Transform, Pose from geometry_msgs package, and PointCloud2 and Image from sensor_msgs package. Notice, that you can also directly use the internal functions for particular types such as ros_numpy.geometry.point_to_numpy to get numpy vector from geometry_msgs/Point.

A good example of a complex robotic system is the Unmanned Ground Vehicle (UGV) and Unmanned Aerial Vehicle (UAV) system developed for the DARPA SubT challenge by CTU.

You can take a look at the TF tree for the CTU's Unmanned Ground Vehicles in DARPA SubT challenge. A zoomable version is here .

Similar complexity has the node graph, here of the CTU's Unmanned Aerial Vehicles. You can find full image for zooming here .

Today's lab task is to program a simple action server and client that will drive the Turtlebot robot to a given goal position in simulation environment. You can start by downloading the required packages lab03.zip . Follow the TODOs in the code, namely in the action server node robocop_goto_as.py and the action client node robocop_rviz_goto_ac.py.

The action server uses custom action type GoToPosition.action on topic “robocop_goto_as”, and receives the goal position where the robot is supposed to drive. After receiving the goal, the server calculates the distance (position_error) between the current robot's position and the goal position in each iteration. It also calculates the angular distance (angle_error) between the robot's current heading and the heading toward the goal position. Based on the errors the robot either rotates towards the goal (angle_error < -0.1 rad or angle_error > 0.1 rad) or drives straight forward to the goal. In each iteration, the robot reports the position_error and the angle_error in the feedback message. The action ends with success when the positional error is below a given threshold (position_error < 0.1 m) or ends with failure when there is an obstacle in front of the robot (self.min_dist_ahead < 0.3 m).

In the action server, you should implement the following:

1. Create the action server object,
2. Fill the calculated position_error and angle_error errors to the feedback message and send the message,
3. Set suitable values to the angular and linear speeds to be sent to the robot based on the calculated errors,
4. Once the goal is reached or the minimum obstacle distance ahead the robot is violated, fill in the result message based on the outcome of the action and send it to the client,
5. Test sending the goal to the action server using rostopic pub or the axclient.py as described above.

The action client is used to receive a navigation goal from RViz tool and thus to interface the RViz with the action server. In RViz's Tools Panel you can use the 2D Nav Goal button and then click on the desired position in the world. This will publish PoseStamped message to the /move_base_simple/goal which should be then sent to the action server using the client in robocop_rviz_goto_ac.py. The client also monitors the status of the server and it cancels the current goal if a new goal is received from the RViz.

In the action client, you should implement the following:

1. Create the action client object,
2. Create the GoToPositionGoal from the message generated in RViz's 2D Nav Goal,
3. Create a ROS Timer to periodically report on the status of the action server,
4. Test setting the goal position through RViz.