... | ... | @@ -566,7 +566,8 @@ Do not forget to add the `UndockExecutor` to the executor registry, like it was |
|
|
You should now start your `tst_executor` node, and we can use a service call to execute our first TST:
|
|
|
|
|
|
```bash
|
|
|
ros2 service call /execute_tst air_lab_interfaces/srv/ExecuteTst "tst_file: '`ros2 pkg prefix air_tsts`/share/air_tsts/tsts/undock.json'"```
|
|
|
ros2 service call /execute_tst air_lab_interfaces/srv/ExecuteTst "tst_file: '`ros2 pkg prefix air_tsts`/share/air_tsts/tsts/undock.json'"
|
|
|
```
|
|
|
|
|
|
You should see your robot undocking.
|
|
|
|
... | ... | @@ -626,12 +627,6 @@ service = create_service<...>("pause_tst", ..., rmw_qos_profile_services_default |
|
|
|
|
|
Note that the ```execute_tst``` call is not reentrant, and should not be added to the group.
|
|
|
|
|
|
## Parameters
|
|
|
|
|
|
### python
|
|
|
|
|
|
### c++
|
|
|
|
|
|
## Test cases
|
|
|
|
|
|
We provide some test cases:
|
... | ... | @@ -640,61 +635,53 @@ We provide some test cases: |
|
|
roscd air_tsts/tsts
|
|
|
```
|
|
|
|
|
|
* `dock.json`: dock
|
|
|
* `undock.json`: undock
|
|
|
* `dock.json`: dock
|
|
|
* `drive_to.json`: go to three positions in sequence
|
|
|
* `drive_to_repeat.json`: go to three positions in sequence and repeat once
|
|
|
* `drive_home.json`: go to near the home position and dock
|
|
|
* `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
|
|
|
|
|
|
## 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 `dock`, `drive_to` and `explore`.
|
|
|
|
|
|
**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`$.
|
|
|
**Dock** Inspire yourself from the `UndockExecutor` to create this executor. Essentially duplicating `UndockExecutor` and replacing Undock by Dock is *almost* all you have to do.
|
|
|
|
|
|
The basic structure of `drive_to` is:
|
|
|
**Drive to** Drive to a given location and set the maximum velocity. To drive to a given location, you should send a goal to the navigate action server, like you did in *Lab 2*. Also in this lab you will need to se the `maximum_speed` by publishing on the `/speed_limit` topic. `drive_to` has three parameters that you need to get: the destination `p`, the `heading` and the `maximum-speed`.
|
|
|
|
|
|
* 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:
|
|
|
To get parameters for a node, 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]
|
|
|
self.node().getParameter(TstML.TSTNode.ParameterType.Specific, "nameoftheparameter")
|
|
|
```
|
|
|
|
|
|
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`.
|
|
|
and in C++:
|
|
|
|
|
|
After creating your publisher/subscriber, you should sleep, so that all the connections are properly established by ROS.
|
|
|
```c++
|
|
|
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
|
|
|
```
|
|
|
|
|
|
In python, you can sleep with:
|
|
|
And to test if a parameter is available:
|
|
|
|
|
|
```python
|
|
|
import time
|
|
|
time.sleep(2.0)
|
|
|
self.node().hasParameter(TstML.TSTNode.ParameterType.Specific, "nameoftheparameter")
|
|
|
```
|
|
|
|
|
|
In C++, you can:
|
|
|
and in C++:
|
|
|
|
|
|
```c++
|
|
|
#include <QThread>
|
|
|
QThread::sleep(2);
|
|
|
QVariant p = node()->hasParameter(TstML::TSTNode::ParameterType::Specific, "nameoftheparameter");
|
|
|
```
|
|
|
|
|
|
Heading is equivalent to Yaw. But it needs to be specify as a quaternion, you can use the following formulat to compute a yaw quaternion:
|
|
|
|
|
|
$ (x=0, y=0,z=sin(yaw/2),q=cos(yaw/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
|
... | ... | |