====== Lab 02 ======

During this lab, you should learn how to start some additional features of ROS work such as timers and services.

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

Slides used in this lab are available here: https://docs.google.com/presentation/d/1CHjWHuDJ3mwQ-lRneVF0dozPUXE1AtJmxgp8sSfzqYA/edit?usp=sharing . 

===== Robot Operating System (ROS) new buzzwords =====

There are several new key aspects/buzzwords to know from today's lab:
  * **Service** - Similar to topics, but used for synchronous communication with request + response model.
  * **Gazebo** - Program for simulating robot physics.
  * **ROS Parameters** - a way of using global parameters among multiple nodes, i.e., using parameter server.
  * **Config Files** - yaml files with parameters for ROS nodes, they are loaded in launch files.
  * **tf (transform system)** - tf library is for working with transformations between frames of reference (e.g., transformation of camera measurements to GPS coordinates).


===== Robot Operating System (ROS) continuation =====

==== 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 = self.get_clock().now() # current time with int64 nanoseconds
seconds = time_now.nanoseconds * 1e-9 # 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 = rclpy.duration.Duration(seconds=42.1)
self.get_clock().sleep(d)
</code>

The result of subtracting two ROS **Time** instances is also an instance of ROS **Duration**. 
<code>
time_start = self.get_clock().now()
d = rclpy.duration.Duration(seconds=3)
self.get_clock().sleep_for(d)
diff = self.get_clock().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 = self.create_rate(10) # create rate with 10 Hz frequency
while rclpy.ok():
    # 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)

self.timer = self.create_timer(2.0, my_callback) # calls function (callback) my_callback every two seconds
</code>


==== Services ====

Services are similar to topics, but they are used for synchronous communication with request + response model.
They are useful for not so frequent messaging and in systems without communication dropouts.

In terminal you can use commands:
  * ''ros2 interface list'' - shows all available service types (and messages, too)
  * ''ros2 interface show'' - displays the definition of a given service type
  * ''ros2 interface package'' - lists services of a certain package,
  * ''ros2 service list'' - shows all running services
  * ''ros2 service info <service_name>'' - gives you information about a specified service
  * ''ros2 service call <service_name> <service_type> <request data>'' - calls the specified service with specified request.

In python code you can [[https://docs.ros.org/en/kilted/Tutorials/Beginner-Client-Libraries/Single-Package-Define-And-Use-Interface.html | create your own service definitions ]] or you can use existing ones.
The services, similarly to topics, have //Service server// part (similar to topic subscriber) and //Service client// part (similar to topic publisher).
The service client sends service requests (e.g., for ''SetBool'' service it is ''SetBool_Request'') and the service server accepts the requests, process them using some callback and generates a response (e.g., ''SetBool_Response''), which is sent back to service client.

The service server part of code would look like:
<code>
def activate_cb(request, response):
    # react to the service request of type SetBool
    return response # response of type SetBool

s = self.create_service('/activate', SetBool, activate_cb)
</code>

The service client code can look like:
<code>
service_client = self.create_client('/activate', SetBool) # creates the service client
while not service_client.wait_for_service(timeout_sec=1.0): # waits for the service to be alive
  pass
req = std_srvs.srv.SetBool_Request() # creates the service request
req.data = True # fill the request
future = service_client.call_async(req) # call the service and get the response
rclpy.spin_until_future_complete(self, future)
response = future.result()
</code>

==== Launch files ====

XML files that automatize the start-up of nodes. 
The launch files enable functionalities such as:
  * Launching of multiple nodes,
  * Remapping of topics,
  * Grouping of nodes to namespaces,
  * Better handling of parameters/arguments as shown below.
The xml file can include tag elements:
  * ''<launch>'' - root element
  * ''<node>'' - element that starts a new node with parameters such as: //name// - custom unique name of node, //pkg// - package of the node, //exec// - name of the executable of the node, //output// - either output to screen or log file , //respawn// - restart the node if terminated, //on_exit="shutdown"// - terminates other nodes in launchfile if this node is terminated.
  * ''<arg>'' - argument of a launch file,
  * ''<include>'' - element for including (starting) other launch files,
  * ''<param>'' - to set parameters (described in following section),
  * ''<set_param>'' - parameters for more nodes
  * ''<group>'' - to group individual nodes together into one namespace (e.g., /camera/...).

==== ROS Parameter ====

You can pass parameters to ROS nodes (and thus change the node behaviour) in different ways:
  * You can provide parameters to nodes using command line: ''ros2 run <package> <node> --ros-args -p param1:=value1 -p param2:=value2''
  * A better way is to set the parameters in the launch file ''<param name="arg1" value="value1"/>''. You can specify either private parameters inside ''<node ...> </node>'' tags or global parameters outside the node.
  * The ultimate option is to use //yaml// file which is loaded in the launch file using ''<param file="$(find-pkg-share my_package)/config/my_params.yaml" />'', again either inside node or outside.
Once you have your python node you can load your private/global params using:
<code>
self.declare_parameter("is_cool", False)
is_cool = self.get_parameter("is_cool").get_parameter_value().bool_value

</code>
==== TF (transform system) ====

TF is a ROS library for working with transformations between frames of reference.
TF allows you to get, e.g., a position of a robot from any time by linear interpolation between measurements of the position. 
Moreover, it allows to create even complicated chains of transformations and expressing the position in any of the reference frames in the chain.
For example, it allows you to get position in GPS coordinate frame of some object detected in camera measurement.

Once you run the turtlebot simulation try to:
  * run ''ros2 topic echo /tf'' in terminal to see the published transformations,
  * visualize the tf tree in ''rqt'' in top menu Plugins->Visualization->TF Tree or by command ''ros2 run rqt_tf_tree rqt_tf_tree''. 
  * print a particular TF in terminal using ''ros2 run tf2_ros tf_echo base_link base_footprint''
  * monitor all TFs in terminal using ''ros2 run tf2_ros tf_monitor''


===== 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//.

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

Implement a simple service client node and service server node in Python [[https://docs.ros.org/en/kilted/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Service-And-Client.html | ROS tutorial]].
Both can be placed inside the aro_reactive_control/scripts of the student-packages.
  - Implement a node with a service server inside ''server.py'' that creates service '/activate' with SetBool type. The node will print every one second the last value received through the service.
  - Launch the node using rosrun.
  - Implement service client inside ''client.py'' that calls service '/activate' with SetBool type every ten seconds and inverts the sent True/False value.
  - Launch the publisher using rosrun.

===== Homework 2 assignment =====

Follow the assignment of the homework [[courses:aro:tutorials:homework02|HW2]].