... | ... | @@ -19,6 +19,7 @@ Here is some documentation: |
|
|
|
|
|
* ```seq``` allows to execute a sequence of nodes
|
|
|
* ```conc``` allows to execute a set of nodes concurrently, it has a parameter ```stop-on-first``` which controls whether the node should wait for all the children to finish or stop when the first node is done
|
|
|
* ```dock``` and ```undock``` to dock or undock the robot
|
|
|
* ```drive-to``` allows to send a ground robot to a specific position, it has the following parameters:
|
|
|
* ```p``` the destination
|
|
|
* ```heading``` the heading the robot must be facing at the end
|
... | ... | @@ -121,6 +122,358 @@ List of relevant classes for executing TST: |
|
|
|
|
|
The ```NodeExecutorRegistry``` and ```TSTNodeModelsRegistry``` are common for all TSTs and should be permanent. While ```TSTNode``` and ```Executor``` are specific to the execution of a given tree.
|
|
|
|
|
|
### Implementation
|
|
|
Implementation
|
|
|
--------------
|
|
|
|
|
|
This lab can be completed using **Python** or **C++**. Follows the instruction corresponding to your choice of programming language.
|
|
|
|
|
|
You will need to create a package for ```air_lab3```, in your ```ros2_ws/src/air_labs``` directory, with dependencies on a few packages. For python:
|
|
|
|
|
|
```bash
|
|
|
ros2 pkg create air_lab3 --build-type ament_cmake --dependencies rclpy message_generation std_msgs --node-name tst_executor
|
|
|
```
|
|
|
|
|
|
For C++:
|
|
|
|
|
|
```bash
|
|
|
ros2 pkg create air_lab3 --build-type ament_cmake --dependencies rclcpp message_generation std_msgs --node-name tst_executor
|
|
|
```
|
|
|
|
|
|
TST executor service definition
|
|
|
-------------------------------
|
|
|
|
|
|
We will need to create a new service definition. You should check and understand the [official tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html) first. We will need to create a package specially for storing our definitions:
|
|
|
|
|
|
```bash
|
|
|
ros2 pkg create --build-type ament_cmake air_lab_interfaces --dependencies std_msgs rosidl_default_generators
|
|
|
```
|
|
|
|
|
|
Then you should create a ```ExecuteTst.srv``` file in ```air_lab_interfaces/srv```, with the following content:
|
|
|
|
|
|
```
|
|
|
string tst_file # Name of the TST file to execute
|
|
|
---
|
|
|
bool success # Whether the execution was successful
|
|
|
string error_message # Error message if the execution was unsuccessful
|
|
|
```
|
|
|
|
|
|
Then we need to add this file to the build system. In ```air_lab_interfaces``` open the ```CMakeLists.txt``` file, and add the following:
|
|
|
|
|
|
```cmake
|
|
|
rosidl_generate_interfaces(${PROJECT_NAME}
|
|
|
"msg/ExecuteTst.srv"
|
|
|
)
|
|
|
```
|
|
|
|
|
|
Then you can build with ```tdde05-build```. Then you should reload the environment with ```start-tdde05```. Then you should be able to see the new message with:
|
|
|
|
|
|
```bash
|
|
|
ros2 interfaces show air_lab_interfaces/srv/ExecuteTst
|
|
|
```
|
|
|
|
|
|
TST executor service server
|
|
|
---------------------------
|
|
|
|
|
|
We will create a TST executor that is started with a service server. In that node you need to define a service called ```execute_tst``` when it is called, it loads a TST description from a file (one of the json file) and start executing it.
|
|
|
|
|
|
You should start by reading and understanding the official tutorials for [Python](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Service-And-Client.html) or [C++](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html).
|
|
|
|
|
|
You should follow an incremental approach:
|
|
|
|
|
|
* Copy/paste the service server example from the respective tutorials into your `lab3_node.py` or `lab3_node.cpp`.
|
|
|
* Make sure you understand what is happening, if you have doubts, ask questions to your lab assistant.
|
|
|
* You won't be able to run the tutorial. So you can start modifying to replace the service call with ```execute_tst``` and the ttype ```air_lab_interfaces/srv/ExecuteTst```.
|
|
|
|
|
|
|
|
|
Initialise TstML and a load a TstML file
|
|
|
----------------------------------------
|
|
|
|
|
|
First we need to load the Tst definitions in a ```TSTNodeModelsRegistry```.
|
|
|
|
|
|
### Python
|
|
|
|
|
|
You will need to create a ```TstML.TSTNodeModelsRegistry``` and use ```loadDirectory``` to load all the configuration file.
|
|
|
It is best if this ```tst_registry``` is kept as a class member of your node.
|
|
|
|
|
|
```python
|
|
|
import TstML
|
|
|
import rospkg
|
|
|
|
|
|
tst_registry = TstML.TSTNodeModelsRegistry()
|
|
|
tst_registry.loadDirectory(rospkg.RosPack().get_path("air_tst")
|
|
|
+ "/configs")
|
|
|
```
|
|
|
|
|
|
Then you can load a TST from a file:
|
|
|
|
|
|
```python
|
|
|
tst_node = TstML.TSTNode.load(filename, tst_registry)
|
|
|
```
|
|
|
|
|
|
### C++
|
|
|
|
|
|
You will need to create a ```TstML::TSTNodeModelsRegistry``` and use ```loadDirectory``` to load all the configuration file.
|
|
|
It is best if this ```tst_registry``` is kept as a class member of your node.
|
|
|
|
|
|
```C++
|
|
|
#include <TstML/TSTNodeModelsRegistry.h>
|
|
|
#include <ros/package.h>
|
|
|
|
|
|
TstML::TSTNodeModelsRegistry* tst_registry
|
|
|
= new TstML::TSTNodeModelsRegistry();
|
|
|
tst_registry->loadDirectory(
|
|
|
QString::fromStdString(
|
|
|
ros::package::getPath("air_tst") + "/configs"));
|
|
|
```
|
|
|
|
|
|
Then you can load a TST from a file:
|
|
|
|
|
|
```c+++
|
|
|
TstML::TSTNode* tst_node = TstML::TSTNode::load(
|
|
|
QUrl::fromLocalFile(QString::fromStdString(filename)),
|
|
|
tst_registry);
|
|
|
```
|
|
|
|
|
|
|
|
|
Initialise TstML::Executor and start execution
|
|
|
----------------------------------------------
|
|
|
|
|
|
### Python
|
|
|
|
|
|
First you need to create a ```NodeExecutorRegistry```, and use the ```registerNodeExecutor``` function to associate ```TSTNodeModel``` with ```AbstractNodeExecutor```.
|
|
|
We provide you with a default executor for ```sequence``` and ```concurrent```.
|
|
|
|
|
|
|
|
|
```python
|
|
|
import TstML.Executor
|
|
|
|
|
|
# Create a registry with node executors
|
|
|
tst_executor_registry = TstML.Executor.NodeExecutorRegistry()
|
|
|
|
|
|
# Setup the executors for sequence and concurrent
|
|
|
tst_executor_registry.registerNodeExecutor(
|
|
|
tst_registry.model("seq"),
|
|
|
TstML.Executor.DefaultNodeExecutor.Sequence)
|
|
|
tst_executor_registry.registerNodeExecutor(
|
|
|
tst_registry.model("conc"),
|
|
|
TstML.Executor.DefaultNodeExecutor.Concurrent)
|
|
|
```
|
|
|
|
|
|
Now you can do the following to execute nodes
|
|
|
|
|
|
```python
|
|
|
# Create an executor using the executors defined
|
|
|
# in tst_executor_registry
|
|
|
tst_executor = TstML.Executor.Executor(tst_node,
|
|
|
tst_executor_registry)
|
|
|
|
|
|
# Start execution
|
|
|
tst_executor.start()
|
|
|
|
|
|
# Block until the execution has finished
|
|
|
status = tst_executor.waitForFinished()
|
|
|
|
|
|
# Display the result of execution
|
|
|
if status.type() == TstML.Executor.ExecutionStatus.Type.Finished:
|
|
|
print("Execution successful")
|
|
|
else:
|
|
|
print("Execution failed: {}".format(status.message()))
|
|
|
|
|
|
```
|
|
|
|
|
|
Instead of displaying the results, you should set the result in the response of your service call.
|
|
|
|
|
|
\paragraph{C++}
|
|
|
|
|
|
First you need to create a ```NodeExecutorRegistry```, and use the ```registerNodeExecutor``` function to associate ```TSTNodeModel``` with ```AbstractNodeExecutor```.
|
|
|
We provide you with a default executor for ```sequence``` and ```concurrent```.
|
|
|
|
|
|
```c++
|
|
|
#include <TstML/Executor/NodeExecutorRegistry.h>
|
|
|
#include <TstML/Executor/DefaultNodeExecutor/Sequence.h>
|
|
|
#include <TstML/Executor/DefaultNodeExecutor/Concurrent.h>
|
|
|
|
|
|
// Create a registry with node executors
|
|
|
TstML::Executor::NodeExecutorRegistry* tst_executor_registry
|
|
|
= new TstML::Executor::NodeExecutorRegistry();
|
|
|
|
|
|
// Setup the executors for sequence and concurrent
|
|
|
tst_executor_registry->registerNodeExecutor
|
|
|
<TstML::Executor::DefaultNodeExecutor::Sequence>
|
|
|
(tst_registry->model("seq"));
|
|
|
tst_executor_registry->registerNodeExecutor
|
|
|
<TstML::Executor::DefaultNodeExecutor::Concurrent>
|
|
|
(tst_registry->model("conc"));
|
|
|
```
|
|
|
|
|
|
Now you can do the following to execute nodes
|
|
|
|
|
|
```c++
|
|
|
// Create an executor using the executors defined
|
|
|
// in tst_executor_registry
|
|
|
TstML::Executor::Executor* tst_executor
|
|
|
= new TstML::Executor::Executor(tst_node, tst_executor_registry)
|
|
|
|
|
|
// Start execution
|
|
|
tst_executor->start();
|
|
|
|
|
|
// Block until the execution has finished
|
|
|
TstML::Executor::ExecutionStatus status = tst_executor->waitForFinished();
|
|
|
|
|
|
// Display the result of execution
|
|
|
if(status == TstML::Executor::ExecutionStatus::Finished())
|
|
|
{
|
|
|
ROS_INFO("Execution successful");
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
std::string message = status.message().toStdString();
|
|
|
ROS_INFO("Execution successful '%s'", message.c_str());
|
|
|
}
|
|
|
|
|
|
delete tst_executor;
|
|
|
|
|
|
```
|
|
|
|
|
|
Instead of displaying the results, you should set the result in the response of your service call.
|
|
|
|
|
|
Implement a subclass of a TstML::AbstractNodeExecutor
|
|
|
-----------------------------------------------------
|
|
|
|
|
|
The main goal of the lab is to implements executors that are subclass ```AbstractNodeExecutor``` for the terminal nodes.
|
|
|
In this section, I will give you the implementation of the ```echo``` executor.
|
|
|
**It is not enough to copy the file, you need to understand what every lines do. Once you do, call the lab assistant and explain what you think every line does.**
|
|
|
|
|
|
### Python
|
|
|
|
|
|
Executor should inherit the ```TstML::AbstractNodeExecutor``` class, they need to implement a ```start```, ```pause```, ```resume```, ```stop``` and ```abort``` function.
|
|
|
They can access parameters for the node in this way:
|
|
|
|
|
|
```python
|
|
|
self.node().getParameter(TstML.TSTNode.ParameterType.Specific, "nameoftheparameter")
|
|
|
```
|
|
|
|
|
|
|
|
|
The following show an example of implementation for ```echo``` (see ```echo_executor.py``` on the website):
|
|
|
|
|
|
\inputminted[linenos,bgcolor=light-gray]{python}{echo_executor.py}
|
|
|
|
|
|
### C++
|
|
|
|
|
|
Executor should inherit the ```AbstractNodeExecutor``` class, they need to implement a ```start```, ```pause```, ```resume```, ```stop``` and ```abort``` function.
|
|
|
They can access parameters for the node in this way:
|
|
|
|
|
|
```python
|
|
|
QVariant p = node()->getParameter(TstML::TSTNode::ParameterType::Specific, "nameoftheparameter");
|
|
|
p.toString(); // convert it to a string
|
|
|
p.toInt(); // Convert it to an integer
|
|
|
p.toDouble(); // Convert it to a double
|
|
|
```
|
|
|
|
|
|
The following show an example of implementation for ```echo``` (see ```echo_executor.cpp``` on the website):
|
|
|
|
|
|
\inputminted[linenos,bgcolor=light-gray]{c++}{echo_executor.cpp}
|
|
|
|
|
|
Test execution
|
|
|
--------------
|
|
|
|
|
|
You should now start your ```tst_executor``` node, and we can use a service call to execute our first TST:
|
|
|
|
|
|
```bash
|
|
|
rosservice call /husky0/execute_tst \\
|
|
|
"tst_file: '`rospack find air_tsts`/tsts/echo.json'"
|
|
|
```
|
|
|
|
|
|
You can use the following to check was is echoed:
|
|
|
|
|
|
```bash
|
|
|
rostopic echo /husky0/test
|
|
|
```
|
|
|
|
|
|
Implement abort/stop/pause/resume
|
|
|
---------------------------------
|
|
|
|
|
|
You can use ```std_srvs/Empty``` to define a service call for ```abort```, ```stop```, ```pause``` and ```resume```.
|
|
|
Those service call need to be defined in your ```tst_executor``` node, along the ```execute_tst``` service call.
|
|
|
You can call the function ```abort```, ```stop```, ```pause``` and ```resume``` of the ```TstML.Executor.Executor```/```TstML::Executor::Executor``` class.
|
|
|
|
|
|
Test cases
|
|
|
----------
|
|
|
|
|
|
We provide some test cases:
|
|
|
|
|
|
```bash
|
|
|
roscd air_tsts/tsts
|
|
|
```
|
|
|
|
|
|
|
|
|
* ```echo.json```: publish on a topic
|
|
|
* ```drive_to.json```: go to three positions in sequence
|
|
|
* ```drive_to_repeat.json```: go to three positions in sequence and repeat once
|
|
|
* ```explore.json```: go to a start position and execute the spiral motion
|
|
|
* ```explore_record.yaml```: go to a start position and execute the spiral motion and record some sensor data in a bag file
|
|
|
|
|
|
|
|
|
|
|
|
Implementation of Executors
|
|
|
---------------------------
|
|
|
|
|
|
You need to implement the ```drive_to```, ```explore``` and ```record``` node. It involves to mostly publish and listen.
|
|
|
|
|
|
**Drive to** Need to support both using the motion planner or not. For setting the maximum speed, use ```maximum-speed``` for the linear speed and $3.0 * maximum\_speed$ for the angular one, and if none is set, default to $maximum\_speed = 0.5$.
|
|
|
|
|
|
The basic structure of ```drive_to``` is:
|
|
|
|
|
|
|
|
|
* Set the state machine controller in either position or waypoint control mode.
|
|
|
* Send the position or waypoints to the state machine controller or motion planner.
|
|
|
* Listen on position reached or waypoints finished topics to detect when the robot has completed the task
|
|
|
* To communicate with the state machine, you should use ROS topics, if you forgot about them, you can check ```lab1``` or use ```rostopic list``` and ```rostopic info``` to get information about available topics.
|
|
|
|
|
|
|
|
|
The following contains information to help you with the implementation of ```drive_to```.
|
|
|
|
|
|
Heading is equivalent to Yaw, for C++ you can look at lab 2 for conversion between Yaw and Quaternion.
|
|
|
In Python:
|
|
|
|
|
|
```python
|
|
|
import tf
|
|
|
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
|
|
# quat is an array with the coordinates as [x,y,z,w]
|
|
|
```
|
|
|
|
|
|
You might need to modify your state machine to send an event when the position has been reached and when the waypoint controller has finished:
|
|
|
|
|
|
|
|
|
* add two output events, called ```position_reached``` and ```waypoints_finished```
|
|
|
* connect the ```point_reached``` event (it is the third event, at the bottom of the state) of the position controller to ```position_reached```
|
|
|
* connect the ```waypoints_finished``` event (it is the third event) of the waypoints controller to ```waypoints_finished```
|
|
|
|
|
|
Then ```waypoints_finished``` and ```position_reached``` are available as topics with type ```std_msgs/Empty```.
|
|
|
|
|
|
After creating your publisher/subscriber, you should sleep, so that all the connections are properly established by ROS.
|
|
|
|
|
|
In python, you can sleep with:
|
|
|
|
|
|
```python
|
|
|
import time
|
|
|
time.sleep(2.0)
|
|
|
```
|
|
|
|
|
|
In C++, you can:
|
|
|
|
|
|
```c++
|
|
|
#include <QThread>
|
|
|
QThread::sleep(2);
|
|
|
```
|
|
|
|
|
|
**Explore** sample the Archimedean spiral. You can increment $\theta$ by $\pi/4$ to generate the waypoints until $r$ is superior to the $radius$ given as argument.
|
|
|
|
|
|
Demonstration
|
|
|
-------------
|
|
|
|
|
|
* Show the execution of ```drive_to_repeat.json```, ```explore.json``` and ```drive_home.json```
|
|
|
|
|
|
|
|
|
|
|
|
|