Skip to content

Changes

Page history
Update lab4 authored by Cyrille Berger's avatar Cyrille Berger
Hide whitespace changes
Inline Side-by-side
lab4.md
View page @ 30f8d7e3
# Lab 4: Exploration
**This is a draft, wait for the lab instructions to be finished before starting**
The goal of this lab is to fill a knowledge database with information perceived in the environment. You will extend the TST executor of lab3 to listen to a semantic sensor, fill new observation in the database and generate a TST for reaching all the location of interests.
In this lab you will:
......@@ -35,7 +33,7 @@ rm -rf $HOME/.lrs_kdb/stores/
The following service call will insert two objects in the graph named **salientpoints**, one human (with identifier 1) at coordinates `(8,10)` and one table (with identifier 2) at coordinates `(10,12)`:
```bash
ros2 service call /husky0/kDBServerNodelet/insert_triples "
ros2 service call /kdb_server/insert_triples ros2_kdb_msgs/srv/InsertTriples "
graphname: 'salientpoints'
format: 'ttl'
content: '@prefix gis: <http://www.ida.liu.se/~TDDE05/gis>
......@@ -53,7 +51,7 @@ This will insert two objects, the first has uuid `id1`, coordinate `(8,10)` and
The following query can be used to retrieve all the elements in the database:
```bash
ros2 service call /husky0/kDBServerNodelet/sparql_query "graphnames: ['salientpoints']
ros2 service call /kdb_server/sparql_query ros2_kdb_msgs/srv/QueryDatabase "graphname: 'salientpoints'
format: 'json'
query: 'SELECT ?x ?y ?z WHERE { ?x ?y ?z }'"
```
......@@ -61,7 +59,7 @@ query: 'SELECT ?x ?y ?z WHERE { ?x ?y ?z }'"
The following query can be used to retrieve all the object with their class that are near the point of coordinate `(10, 12)`:
```bash
ros2 service call /husky0/kDBServerNodelet/sparql_query "graphnames: ['salientpoints']
ros2 service call /kdb_server/sparql_query ros2_kdb_msgs/srv/QueryDatabase "graphname: 'salientpoints'
format: 'json'
query: '
PREFIX gis: <http://www.ida.liu.se/~TDDE05/gis>
......@@ -109,7 +107,7 @@ SELECT ?x ?y FROM 'salientpoints' WHERE { <someid> a <someklass> ;
You can test your TST executor with:
```bash
rosservice call /husky0/execute_tst \\
rosservice call /execute_tst \\
"tst_file: '`rospack find air_tsts`/tsts/explore_record_semantic.json'"
```
......@@ -156,7 +154,10 @@ The following show an example of how to publish a marker to show two cubes in Py
```python
import rclpy
import rclpy.node
import visualization_msgs.msg
import geometry_msgs.msg
import std_msgs.msg
def create_point(x, y, z):
msg = geometry_msgs.msg.Point()
......@@ -185,26 +186,29 @@ def timer_callback():
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.points.append(create_point(2.0, 3.0, 0.0))
marker.points.append(create_point(5.0, 2.0, 0.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)
marker_array = visualization_msgs.msg.MarkerArray()
marker_array.markers = [marker]
display_marker_pub.publish(marker_array)
def main():
global display_marker_pub
rclpy.init()
node = rclpy.node.Node('visualise_semantic_objects')
rclpy.init()
node = rclpy.Node('visualise_semantic_objects')
display_marker_pub = node.create_publisher(visualization_msgs.msg.MarkerArray, 'semantic_sensor_visualisation', 10)
display_marker_pub = node.create_publisher(visualization_msgs.msg.MarkerArray, 'semantic_sensor_visualisation', 10)
timer = node.create_timer(0.5, timer_callback)
timer = self.create_timer(0.5, timer_callback)
rclpy.spin(node)
rclpy.spin(node)
if __name__ == '__main__':
main()
```
In RViz you can select to display a marker array, and you should see two cubes. You should then modify the program to query the database and show the humans and tables. Once you have completed the lab you should see something similar to this (the exact numbers of cubes and their positions may be different for you):
......@@ -219,7 +223,7 @@ The following show an example of how to publish a marker to show two cubes in C+
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/msg/marker_array.hpp>
using namespace std::chrono_literals;
......@@ -228,14 +232,14 @@ 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 display_marker_pub = node->create_publisher<visualization_msgs::msg::MarkerArray>("semantic_sensor_visualisation", 10);
auto timer_ = node->create_wall_timer(500ms, [display_marker_pub]()
{
visualization_msgs::msg::Marker marker;
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.type = visualization_msgs::msg::Marker::CUBE_LIST;
marker.action = 0;
marker.scale.x = 0.5;
marker.scale.y = 0.5;
......@@ -252,24 +256,24 @@ int main(int argc, char** argv)
pt2.y = 2;
marker.points.push_back(pt2);
std_msgs::ColorRGBA colorA;
std_msgs::msg::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;
std_msgs::msg::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;
visualization_msgs::msg::MarkerArray array;
array.markers.push_back(marker);
display_marker_pub->publish(array);
}
});
rclcpp::spin(node);
return 0;
......