====== Lab 03 ======

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

<WRAP center round help 60%>
Do you have any questions regarding the last week's lab or the homework?
</WRAP>

===== ROS time, duration, rate, and timer =====

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.
<code>
time_now = rospy.Time.now() # current time with int32 secs and nsecs values
seconds = time_now.to_sec() # time in floating point format
</code>

**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.
<code>
d = rospy.Duration.from_sec(42.1)
rospy.sleep(d)
</code>

The result of subtracting two ROS **Time** instances is also an instance of ROS **Duration**. 
<code>
time_start = rospy.Time.now()
rospy.sleep(rospy.Duration.from_sec(3.0))
d = rospy.Time.now() - time_start 
</code>

**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.
<code>
rate = Rate(10) # create rate with 10 Hz frequency
while not rospy.is_shutdown():
    # do some calculation
    rate.sleep()
</code>

**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.
<code>
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
</code>

===== ROS action server and client =====

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  [[http://wiki.ros.org/actionlib_tutorials/Tutorials/Writing%20a%20Simple%20Action%20Server%20using%20the%20Execute%20Callback%20%28Python%29 | action server]] and [[http://wiki.ros.org/actionlib_tutorials/Tutorials/Writing%20a%20Simple%20Action%20Client%20%28Python%29 | action client]].

==== Action messages ====

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:
<code>
#goal definition
geometry_msgs/Point position
---
#result definition
bool success
string message
---
#feedback
float32 position_error
float32 angle_error
string message
</code>

==== Action server ====

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:
<code>
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()
</code>

==== Action client ====

The action client can be used to interface the action server efficiently.
A simple client can look like this:
<code>
# 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
</code>

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''.

<WRAP center round important 60%>
To run the ''axclient.py'' you need to run ''./scripts/download_singularity_aro'' in deploy folder to update the image to the newest version.
</WRAP>


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''.

===== ROS numpy =====

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'':
<code>
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
</code>

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 Complex ROS-based system example =====

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.
{{youtube>zUFofmnJ7pc?medium}}

You can take a look at the TF tree for the CTU's Unmanned Ground Vehicles in DARPA SubT challenge. A zoomable version is [[ https://cw.fel.cvut.cz/wiki/_media/courses/aro/tutorials/frames_darpa_uav.png | here ]].
{{ :courses:aro:tutorials:frames_darpa_uav.png?750 |}}


Similar complexity has the node graph, here of the CTU's Unmanned Aerial Vehicles. You can find full image for zooming [[ https://cw.fel.cvut.cz/wiki/_media/courses/aro/tutorials/node_graph_darpa.svg | here ]].
{{ :courses:aro:tutorials:node_graph_darpa.jpg?600 |}}


<WRAP center round info 100%>
**If you are interested more in Aerial robotics or Vision for robotics consider [[ :courses:mrs:start | Multi-robot Aerial Systems ]] and/or [[ :courses:b3b33vir:start | Vision for Robotics ]] courses in next semester.**
</WRAP> 
^ [[ :courses:mrs:start | B(E)3M33MRS - Multi-robot Aerial Systems ]] ^  [[ :courses:b3b33vir:start | B3B33VIR - Vision for Robotics ]] ^
| {{youtube>DEUZ77Vk2zE?medium}} | {{youtube>h6Srq8kpREA?medium}} |


===== Lab task =====

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 {{ :courses:aro:tutorials:lab03.zip | 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:
  - Create the action server object,
  - Fill the calculated position_error and angle_error errors to the feedback message and send the message,
  - Set suitable values to the angular and linear speeds to be sent to the robot based on the calculated errors,
  - 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,
  - 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:
  - Create the action client object,
  - Create the GoToPositionGoal from the message generated in RViz's 2D Nav Goal,
  - Create a ROS Timer to periodically report on the status of the action server,
  - Test setting the goal position through RViz.

<WRAP center round info 60%>
Notice the wrong behavior of the goto action when using the RViz 2D Nav Goal.
Could you explain why is it behaving weirdly?
</WRAP>