| ... | ... | @@ -104,4 +104,193 @@ SELECT ?x ?y FROM 'salientpoints' WHERE { <someid> a <someklass> ; |
|
|
|
- In the call to `/kDBServerNodelet/sparql_query`, the fields `bindings` and `service` are unused for this lab and can be set to an empty string.
|
|
|
|
* If the object has not been observed yet in the database. Add new observation using the insert triples service call (replace 'salientpoints' with the TST parameter for `graphname`).
|
|
|
|
* In C++, add `air_simple_sim_msgs` as a dependency in `CMakeLists.txt`, using `find_package` and setting the correct dependencies for your executable.
|
|
|
|
* In the following section, we cover hints on how to interpret the results of a queries, including parsing of JSON.
|
|
|
|
|
|
|
|
You can test your TST executor with:
|
|
|
|
|
|
|
|
```bash
|
|
|
|
rosservice call /husky0/execute_tst \\
|
|
|
|
"tst_file: '`rospack find air_tsts`/tsts/explore_record_semantic.json'"
|
|
|
|
```
|
|
|
|
|
|
|
|
## Python
|
|
|
|
|
|
|
|
**Parsing results** The results of the queries are in the JSON format. You can parse JSON this way in Python:
|
|
|
|
|
|
|
|
```python
|
|
|
|
import json
|
|
|
|
|
|
|
|
data = json.loads(x)
|
|
|
|
for row in data["results"]["bindings"]:
|
|
|
|
print(row)
|
|
|
|
```
|
|
|
|
|
|
|
|
**Transformation** If you need to transform points from a frame to an other, you can use `tf.transform_listener`, look at `move_to_point.py` script from lab 2 for an example.
|
|
|
|
You can also use the `transformPoint(dst_frame, point)` function to transform the points from the sensor frame to the global frame.
|
|
|
|
|
|
|
|
|
|
|
|
## C++
|
|
|
|
|
|
|
|
**Parsing results** The results of the queries are in the JSON format. You can parse JSON this way in C++:
|
|
|
|
|
|
|
|
```c++
|
|
|
|
#include <QJsonDocument>
|
|
|
|
|
|
|
|
QJsonDocument doc = QJsonDocument::fromJson(QByteArray::fromStdString(result));
|
|
|
|
```
|
|
|
|
|
|
|
|
You can check the API online for [QJsonDocument](https://doc.qt.io/qt-5/qjsondocument.html)
|
|
|
|
|
|
|
|
**Transformation** If you need to transform points from a frame to an other, you can use `tf::transform_listener`, look at `ls_to_occ` program from lab 3 for an example.
|
|
|
|
You can also use the `transformPoint(dst_frame, point)` function to transform the points from the sensor frame to the global frame.
|
|
|
|
|
|
|
|
# Visualisation
|
|
|
|
|
|
|
|
In this part you should query the database (using a service call) for all the objects.
|
|
|
|
And then display the resulting object in rviz.
|
|
|
|
To do that we can publish `visualization_msgs/MarkerArray` message on a topic. Use `ros2 interface show` to display the structure of the message.
|
|
|
|
|
|
|
|
## Python
|
|
|
|
|
|
|
|
The following show an example of how to publish a marker to show two cubes in Python, you can add this to a new node (don't forget to update your `setup.py`):
|
|
|
|
|
|
|
|
```python
|
|
|
|
import rclpy
|
|
|
|
import visualization_msgs.msg
|
|
|
|
|
|
|
|
def create_point(x, y, z):
|
|
|
|
msg = geometry_msgs.msg.Point()
|
|
|
|
msg.x = x
|
|
|
|
msg.y = y
|
|
|
|
msg.z = z
|
|
|
|
return msg
|
|
|
|
|
|
|
|
def create_color(r, g, b, a):
|
|
|
|
msg = std_msgs.msg.ColorRGBA()
|
|
|
|
msg.r = r
|
|
|
|
msg.g = g
|
|
|
|
msg.b = b
|
|
|
|
msg.a = a
|
|
|
|
return msg
|
|
|
|
|
|
|
|
def timer_callback():
|
|
|
|
marker = visualization_msgs.msg.Marker()
|
|
|
|
marker.id = 1242 # identifier the marker, should be unique
|
|
|
|
marker.header.frame_id = "odom"
|
|
|
|
marker.type = visualization_msgs.msg.Marker.CUBE_LIST
|
|
|
|
marker.action = 0
|
|
|
|
marker.scale.x = 0.5
|
|
|
|
marker.scale.y = 0.5
|
|
|
|
marker.scale.z = 0.5
|
|
|
|
marker.pose.orientation.w = 1.0
|
|
|
|
marker.color.a = 1.0
|
|
|
|
|
|
|
|
marker.points.append(create_point(2, 3, 0))
|
|
|
|
marker.points.append(create_point(5, 2, 0))
|
|
|
|
|
|
|
|
marker.colors.append(create_color(1.0, 0.5, 0.5, 1.0))
|
|
|
|
marker.colors.append(create_color(0.5, 1.0, 0.5, 1.0))
|
|
|
|
|
|
|
|
marker_array = visualization_msgs.msg.MarkerArray
|
|
|
|
marker_array.markers.append(marker)
|
|
|
|
|
|
|
|
display_marker_pub.publish(marker_array)
|
|
|
|
|
|
|
|
|
|
|
|
rclpy.init()
|
|
|
|
node = rclpy.Node('visualise_semantic_objects')
|
|
|
|
|
|
|
|
display_marker_pub = node.create_publisher(visualization_msgs.msg.MarkerArray, 'semantic_sensor_visualisation', 10)
|
|
|
|
|
|
|
|
timer = self.create_timer(0.5, timer_callback)
|
|
|
|
|
|
|
|
rclpy.spin(node)
|
|
|
|
```
|
|
|
|
|
|
|
|
Check bellow to see how to see the results in RViz.
|
|
|
|
|
|
|
|
## C++
|
|
|
|
|
|
|
|
The following show an example of how to publish a marker to show two cubes in C++, you can add this to a new node (don't forget to update your `CMakeLists.txt`):
|
|
|
|
|
|
|
|
```c++
|
|
|
|
#include <chrono>
|
|
|
|
|
|
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include <visualization_msgs/MarkerArray.h>
|
|
|
|
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
|
|
{
|
|
|
|
rclcpp::init(argc, argv);
|
|
|
|
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("visualise_semantic_objects");
|
|
|
|
|
|
|
|
auto display_marker_pub = node->create_publisher<visualization_msgs::MarkerArray>("semantic_sensor_visualisation", 10, true);
|
|
|
|
|
|
|
|
auto timer_ = node->create_wall_timer(500ms, [display_marker_pub]()
|
|
|
|
{
|
|
|
|
visualization_msgs::msg::Marker marker;
|
|
|
|
marker.id = 1242; // identifier the marker, should be unique
|
|
|
|
marker.header.frame_id = "odom";
|
|
|
|
marker.type = visualization_msgs::Marker::CUBE_LIST;
|
|
|
|
marker.action = 0;
|
|
|
|
marker.scale.x = 0.5;
|
|
|
|
marker.scale.y = 0.5;
|
|
|
|
marker.scale.z = 0.5;
|
|
|
|
marker.pose.orientation.w = 1.0;
|
|
|
|
marker.color.a = 1.0;
|
|
|
|
|
|
|
|
geometry_msgs::msg::Point pt1;
|
|
|
|
pt1.x = 2;
|
|
|
|
pt1.y = 3;
|
|
|
|
marker.points.push_back(pt1);
|
|
|
|
geometry_msgs::msg::Point pt2;
|
|
|
|
pt2.x = 5;
|
|
|
|
pt2.y = 2;
|
|
|
|
marker.points.push_back(pt2);
|
|
|
|
|
|
|
|
std_msgs::ColorRGBA colorA;
|
|
|
|
colorA.r = 1.0;
|
|
|
|
colorA.g = 0.5;
|
|
|
|
colorA.b = 0.5;
|
|
|
|
colorA.a = 1.0;
|
|
|
|
|
|
|
|
marker.colors.push_back(colorA);
|
|
|
|
std_msgs::ColorRGBA colorB;
|
|
|
|
colorB.r = 0.5;
|
|
|
|
colorB.g = 1.0;
|
|
|
|
colorB.b = 0.5;
|
|
|
|
colorB.a = 1.0;
|
|
|
|
marker.colors.push_back(colorB);
|
|
|
|
|
|
|
|
visualization_msgs::MarkerArray array;
|
|
|
|
array.markers.push_back(marker);
|
|
|
|
display_marker_pub->publish(array);
|
|
|
|
}
|
|
|
|
|
|
|
|
rclcpp::spin(node);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
```
|
|
|
|
|
|
|
|
Check bellow to see how to see the results in RViz.
|
|
|
|
|
|
|
|
## RViz
|
|
|
|
|
|
|
|
In RViz you can select to display a marker array and you should get something similar to:
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
## TST
|
|
|
|
|
|
|
|
The final step is to write a simple program that query the database for all the humans (all the objects of class `human`), generate a TST for driving to all the location of the humans (as a sequence of `drive_to`) and execute that TST. For simplicity, you can make your program output the TST to a file and then execute the TST with the service call of lab 3.
|
|
|
|
|
|
|
|
# Demonstration
|
|
|
|
|
|
|
|
Your demonstration should:
|
|
|
|
|
|
|
|
* Show the exploration with recording of data in the database
|
|
|
|
* Run the query script to visualise the result
|
|
|
|
* Generate the TST and execute it
|
|
|
|
|
