|
|
Lab 3: Task Execution
|
|
|
=====================
|
|
|
# Lab 3: Task Execution
|
|
|
|
|
|
**WARNING INSTRUCTIONS ARE NOT FINISHED**
|
|
|
|
|
|
The goal of this lab is to create an executor for Task-Specification-Trees. The TSTs are defined using the JSON format.
|
|
|
|
|
|
TST
|
|
|
---
|
|
|
## TST
|
|
|
|
|
|
A Task-Specification-Tree (TST) is a hierachical tree structure which represents a complex mission. For the purpose of this lab, six types of nodes are used:
|
|
|
|
|
|
You can find the specification of the TST nodes in ```air_tst/configs```, the directory on the lab computers is ```/courses/TDDE05/software/ros2_ws/src/air_tst/configs```, you can quickly access the directory with:
|
|
|
|
|
|
You can find the specification of the TST nodes in `air_tst/configs`, the directory on the lab computers is `/courses/TDDE05/software/ros2_ws/src/air_tst/configs`, you can quickly access the directory with:
|
|
|
|
|
|
```bash
|
|
|
tdde05-cd air_tst/configs
|
... | ... | @@ -19,33 +16,32 @@ tdde05-cd air_tst/configs |
|
|
|
|
|
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
|
|
|
* ```maximum-speed``` the maximum speed that the robot need to drive
|
|
|
* `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
|
|
|
* `maximum-speed` the maximum speed that the robot need to drive
|
|
|
* `explore` drives the robot in a spiral around the current location of the robot
|
|
|
* `radius` the radius of the spiral
|
|
|
* `a` and `b` representing the parameters of the spiral:
|
|
|
|
|
|
* ```explore``` drives the robot in a spiral around the current location of the robot
|
|
|
* ```radius``` the radius of the spiral
|
|
|
* ```a``` and ```b``` representing the parameters of the spiral:
|
|
|
You should use the Archimedean spiral:
|
|
|
|
|
|
You should use the Archimedean spiral:
|
|
|
```math
|
|
|
r = a + b\theta
|
|
|
```
|
|
|
```math
|
|
|
r = a + b\theta
|
|
|
```
|
|
|
|
|
|
TST Editor
|
|
|
----------
|
|
|
## TST Editor
|
|
|
|
|
|
An editor is available in the \verb|air_tst| module, and can be started with:
|
|
|
An editor is available in the \\verb|air_tst| module, and can be started with:
|
|
|
|
|
|
```bash
|
|
|
ros2 run air_tst tst_editor
|
|
|
```
|
|
|
|
|
|
You can open a specific file by giving it as an argument, for instance, the following will allow you to open the ```explore_record.json``` tst:
|
|
|
You can open a specific file by giving it as an argument, for instance, the following will allow you to open the `explore_record.json` tst:
|
|
|
|
|
|
```
|
|
|
tdde05-cd air_tsts/tsts
|
... | ... | @@ -54,13 +50,11 @@ rosrun air_tst tst_editor explore_record.json |
|
|
|
|
|
![tst_editor](uploads/9b25264312b9d2dc8f983ce717b97895/tst_editor.png)
|
|
|
|
|
|
|
|
|
TST File format
|
|
|
---------------
|
|
|
## TST File format
|
|
|
|
|
|
TST are defined using the JSON file format.
|
|
|
|
|
|
Bellow is an example, for an exploration mission, in which the robot first move to the location $(10,2)$ and then explore in a spiral of radius $10$ while recording some data:
|
|
|
Bellow is an example, for an exploration mission, in which the robot first move to the location $`(10,2)`$ and then explore in a spiral of radius $`10`$ while recording some data:
|
|
|
|
|
|
```json
|
|
|
{
|
... | ... | @@ -99,34 +93,28 @@ Bellow is an example, for an exploration mission, in which the robot first move |
|
|
}
|
|
|
```
|
|
|
|
|
|
The ```type``` indicates the type of TST node, ```children``` are used by the container nodes and contains other node definition. ```params``` contains the parameters of each nodes.
|
|
|
The `type` indicates the type of TST node, `children` are used by the container nodes and contains other node definition. `params` contains the parameters of each nodes.
|
|
|
|
|
|
TstML Library
|
|
|
-------------
|
|
|
## TstML Library
|
|
|
|
|
|
The TstML library is a library that allows to parse the specification files, the TST files, provides some basic functionnality for manipulating TSTs and executing them.
|
|
|
The generated API documentation is available in PDF on the course website.
|
|
|
The document presents the C++ API, but the python API is similar.
|
|
|
Examples of use of the TstML library are presented in the lab instruction, you only need to refer to the API if you have questions about the behavior of a specific function.
|
|
|
The TstML library is a library that allows to parse the specification files, the TST files, provides some basic functionnality for manipulating TSTs and executing them. The generated API documentation is available in PDF on the course website. The document presents the C++ API, but the python API is similar. Examples of use of the TstML library are presented in the lab instruction, you only need to refer to the API if you have questions about the behavior of a specific function.
|
|
|
|
|
|
List of relevant classes for defining TST:
|
|
|
|
|
|
* ```TSTNode```: represents an instance of a node in a TST.
|
|
|
* ```TSTNodeModel```: represents the model (or specification) of a type of TST Node.
|
|
|
* ```TSTNodeModelsRegistry``` registry of ```TSTNodeModelsRegistry``` used to define TSTs.
|
|
|
* `TSTNode`: represents an instance of a node in a TST.
|
|
|
* `TSTNodeModel`: represents the model (or specification) of a type of TST Node.
|
|
|
* `TSTNodeModelsRegistry` registry of `TSTNodeModelsRegistry` used to define TSTs.
|
|
|
|
|
|
List of relevant classes for executing TST:
|
|
|
|
|
|
* ```AbstractNodeExecutor```: base class to define the execution of a ```TSTNode```.
|
|
|
* ```ExecutionStatus```: holds the execution status
|
|
|
* ```Executor```: handles the execution of a tree.
|
|
|
* ```NodeExecutorRegistry```: contains an association between the ```TSTNodeModel``` and ```AbstractNodeExecutor```. It is used during the execution of a TST to instantiate ```AbstractNodeExecutor```.
|
|
|
|
|
|
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.
|
|
|
* `AbstractNodeExecutor`: base class to define the execution of a `TSTNode`.
|
|
|
* `ExecutionStatus`: holds the execution status
|
|
|
* `Executor`: handles the execution of a tree.
|
|
|
* `NodeExecutorRegistry`: contains an association between the `TSTNodeModel` and `AbstractNodeExecutor`. It is used during the execution of a TST to instantiate `AbstractNodeExecutor`.
|
|
|
|
|
|
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.
|
|
|
|
|
|
TST executor service definition
|
|
|
-------------------------------
|
|
|
## 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. It is good practice to create a standalone package for defining messages, so that it is independent of Python and C++. We will need to create a package specially for storing our definitions:
|
|
|
|
... | ... | @@ -134,16 +122,16 @@ We will need to create a new service definition. You should check and understand |
|
|
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:
|
|
|
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
|
|
|
string tst_file # Name of the TST file to execute, it should be a full path
|
|
|
---
|
|
|
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:
|
|
|
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}
|
... | ... | @@ -151,18 +139,23 @@ rosidl_generate_interfaces(${PROJECT_NAME} |
|
|
)
|
|
|
```
|
|
|
|
|
|
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:
|
|
|
Then we need to add the following to `package.xml` file:
|
|
|
|
|
|
```xml
|
|
|
<member_of_group>rosidl_interface_packages</member_of_group>
|
|
|
```
|
|
|
|
|
|
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
|
|
|
ros2 interface show air_lab_interfaces/srv/ExecuteTst
|
|
|
```
|
|
|
|
|
|
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:
|
|
|
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_python --dependencies rclpy air_lab_interfaces --node-name tst_executor
|
... | ... | @@ -174,10 +167,9 @@ For C++: |
|
|
ros2 pkg create air_lab3 --build-type ament_cmake --dependencies rclcpp rclcpp_action air_lab_interfaces --node-name tst_executor
|
|
|
```
|
|
|
|
|
|
TST executor service server
|
|
|
---------------------------
|
|
|
## 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.
|
|
|
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).
|
|
|
|
... | ... | @@ -185,18 +177,21 @@ 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```.
|
|
|
* You won't be able to run the tutorial. So you can start modifying to replace the service call with `execute_tst` and the type `air_lab_interfaces/srv/ExecuteTst`.
|
|
|
* Once you have modified it, you can test your calling your service with:
|
|
|
|
|
|
```bash
|
|
|
ros2 service call /execute_tst air_lab_interfaces/srv/ExecuteTst "tst_file: '`ros2 pkg prefix air_tsts`/share/air_tsts/tsts/undock.json'"
|
|
|
```
|
|
|
Note that we use *\`ros2 pkg prefix air_tsts\`* which will be replaced by the full path to the directory where the tsts are installed on the system.
|
|
|
|
|
|
Initialise TstML and a load a TstML file
|
|
|
----------------------------------------
|
|
|
## Initialise TstML and a load a TstML file
|
|
|
|
|
|
First we need to load the Tst definitions in a ```TSTNodeModelsRegistry```.
|
|
|
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.
|
|
|
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
|
... | ... | @@ -215,8 +210,7 @@ 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.
|
|
|
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>
|
... | ... | @@ -237,15 +231,11 @@ TstML::TSTNode* tst_node = TstML::TSTNode::load( |
|
|
tst_registry);
|
|
|
```
|
|
|
|
|
|
|
|
|
Initialise TstML::Executor and start execution
|
|
|
----------------------------------------------
|
|
|
## 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```.
|
|
|
|
|
|
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
|
... | ... | @@ -281,15 +271,13 @@ 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++}
|
|
|
\\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```.
|
|
|
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>
|
... | ... | @@ -335,36 +323,29 @@ else |
|
|
}
|
|
|
|
|
|
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
|
|
|
-----------------------------------------------------
|
|
|
## 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.**
|
|
|
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 `undock` 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:
|
|
|
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 `undock`:
|
|
|
|
|
|
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}
|
|
|
\\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:
|
|
|
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");
|
... | ... | @@ -373,35 +354,24 @@ 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):
|
|
|
The following show an example of implementation for `undock`:
|
|
|
|
|
|
\inputminted[linenos,bgcolor=light-gray]{c++}{echo_executor.cpp}
|
|
|
\\inputminted\[linenos,bgcolor=light-gray\]{c++}{echo_executor.cpp}
|
|
|
|
|
|
Test execution
|
|
|
--------------
|
|
|
## Test execution
|
|
|
|
|
|
You should now start your ```tst_executor``` node, and we can use a service call to execute our first TST:
|
|
|
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:
|
|
|
ros2 service call /execute_tst air_lab_interfaces/srv/ExecuteTst "tst_file: '`ros2 pkg prefix air_tsts`/share/air_tsts/tsts/undock.json'"```
|
|
|
|
|
|
```bash
|
|
|
rostopic echo /husky0/test
|
|
|
```
|
|
|
You should see your robot undocking.
|
|
|
|
|
|
Implement abort/stop/pause/resume
|
|
|
---------------------------------
|
|
|
## 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.
|
|
|
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
|
|
|
----------
|
|
|
## Test cases
|
|
|
|
|
|
We provide some test cases:
|
|
|
|
... | ... | @@ -409,35 +379,30 @@ We provide some test cases: |
|
|
roscd air_tsts/tsts
|
|
|
```
|
|
|
|
|
|
* `dock.json`: dock
|
|
|
* `undock.json`: undock
|
|
|
* `drive_to.json`: go to three positions in sequence
|
|
|
* `drive_to_repeat.json`: go to three positions in sequence and repeat once
|
|
|
* `explore.json`: execute the spiral motion
|
|
|
* `drive_to_explore.json`: go to a start position and execute the spiral motion
|
|
|
* `drive_home.json`: go to near the home position and dock
|
|
|
|
|
|
* ```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
|
|
|
---------------------------
|
|
|
## Implementation of Executors
|
|
|
|
|
|
You need to implement the ```drive_to```, ```explore``` and ```record``` node. It involves to mostly publish and listen.
|
|
|
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:
|
|
|
**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.
|
|
|
|
|
|
* 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```.
|
|
|
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:
|
|
|
Heading is equivalent to Yaw, for C++ you can look at lab 2 for conversion between Yaw and Quaternion. In Python:
|
|
|
|
|
|
```python
|
|
|
import tf
|
... | ... | @@ -447,12 +412,11 @@ quat = tf.transformations.quaternion_from_euler(0, 0, yaw) |
|
|
|
|
|
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`
|
|
|
|
|
|
* 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```.
|
|
|
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.
|
|
|
|
... | ... | @@ -470,13 +434,8 @@ In C++, you can: |
|
|
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```
|
|
|
|
|
|
|
|
|
**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` |
|
|
\ No newline at end of file |