Feedback

README.md

@@ -1,2 +1,94 @@
-[![Open in Visual Studio Code](https://classroom.github.com/assets/open-in-vscode-c66648af7eb3fe8bc4f294546bfd86ef473780cde1dea487d3c4ff354943c9ae.svg)](https://classroom.github.com/online_ide?assignment_repo_id=7776361&assignment_repo_type=AssignmentRepo)
-# attention
+# Attention objects with graphs.
+
+In this practice we were asked to implement an object vision system in ROS2 using the graph tool and subscribing to the /get_model_states topic of gazbo to obtain the positions of all the models in the world and attend only those that were less than 5 meters away.
+
+### Navigation.
+The first thing we had to do was to make the robot move around the map, to detect the different objects in the area, to implement this functionality what we have done was to make use of previous practices, in this case the planning.
+Our navigation is based on an "infinite" plan consisting of several goals of specific positions. Once it reaches the last position it goes back to the first plan.
+```
+(:durative-action move_location
+    :parameters (?robot - robot ?prev_room - location ?next_room - location)
+    :duration (= ?duration 5)
+    :condition 
+      (and
+        (at start(robotat ?robot ?prev_room))
+      )
+    :effect 
+      (and  
+        (at end(robotat ?robot ?next_room))
+        (at start(not(robotat ?robot ?prev_room)))
+      )
+)
+```
+In addition we had to modify certain parameters of the navigation so that the robot moved faster and the goal tolerance was much less restrictive.
+
+## Attention.
+This has been the most difficult part, because to start with we had to subscribe to the "/get_model_states" topic of the gazebo itself. To do this we needed to install a plugin within the simulator environment to get the topic, because it did not exist. This topic what it does is to tell you the position of all the objects that are inside our simulation. 
+
+![WhatsApp Image 2022-05-09 at 11 36 12 PM](https://user-images.githubusercontent.com/73531592/167507706-a49950b0-d766-407e-8571-20861923c202.jpeg)
+
+Once the topic was installed we had to save all these positions of the tfs of these objects in a graph. In this graph we save the positions of the tfs that are within the range of 5 meters or in case of being at a greater distance, an empty tf.
+
+![WhatsApp Image 2022-05-09 at 11 36 10 PM](https://user-images.githubusercontent.com/73531592/167507362-60f453ad-22e2-462a-b6a9-70ac85cc4719.jpeg)
+
+![WhatsApp Image 2022-05-09 at 11 37 26 PM](https://user-images.githubusercontent.com/73531592/167507397-15ec6c63-e596-4e45-a8cf-d1db78f032b5.jpeg)
+
+Once the graph was obtained, we simply had to extract the information from the graph and with this information move the joint of the robot's head to observe the object in question.
+To move the head of the robot as all the tfs had height 0, the only thing we had to do was to move sideways. To obtain the rotation of the head with respect to the desired point (the object to be observed), we have obtained the coordinates of the model and the robot thanks to the tfs and we have made an atan2, to obtain the desired angle.
+With this the robot could now turn its head to the desired model and keep the view for about 5 seconds.
+
+![WhatsApp Image 2022-05-09 at 11 36 11 PM](https://user-images.githubusercontent.com/73531592/167507720-30ad07df-fdd2-4779-ae5d-bfd6db0e3937.jpeg)
+
+
+
+###                                                                     ---- Español ----
+
+# Atención objectos con grafos.
+
+### Resumen de la práctica.
+En esta práctica se nos pedía implementar un sistema de visión de objetos en ROS2 utilizando la herramienta de grafos y suscribiéndonos al topic de /get_model_states de gazbo para obtener las posiciones de todos los modelos dentro del mundo y atender sólo las que estuvieran a menos de 5 metros de distancia.
+
+
+## Navegación.
+Lo primero que teníamos que hacer era que el robot se moviera por todo el mapa, para ir detectando los distintos objetos de la zona, para implementar esta funcionalidad lo que hemos hecho ha sido hacer uso de prácticas anteriores, en este caso la de planificación.
+Nuestra navegación se basa en un plan “infinito” que consiste en varios goals de posiciones concretas. Una vez llega a la última posición vuelve a pasar al primer plan.
+```
+(:durative-action move_location
+    :parameters (?robot - robot ?prev_room - location ?next_room - location)
+    :duration (= ?duration 5)
+    :condition 
+      (and
+        (at start(robotat ?robot ?prev_room))
+      )
+    :effect 
+      (and  
+        (at end(robotat ?robot ?next_room))
+        (at start(not(robotat ?robot ?prev_room)))
+      )
+)
+```
+Además hemos tenido que modificar ciertos parametros de la navegación para que el robot se moviera más rapido y la toleracia del goal fuera mucho menos restrictiva.
+
+
+## Atención.
+Esta ha sido la parte mas difícil, pues para empezar teníamos que suscribirnos al topic de “/get_model_states” del propio gazebo. Para hacer esto hemos necesitado instalar un plugin dentro del entorno del simulador para poder obtener el topic, pues de por si no existía. Este topic lo que hace es decirte la posición de todos los objetos que están dentro de nuestra simulación.
+
+![WhatsApp Image 2022-05-09 at 11 36 12 PM](https://user-images.githubusercontent.com/73531592/167507706-a49950b0-d766-407e-8571-20861923c202.jpeg)
+
+Una vez instalado el topic debíamos guardar todas estas posiciones de las tfs de estos objetos dentro de un grafo. En este grafo guardamos las posiciones de las tfs que están dentro del rango de 5 metros o en caso de estar a mayor distancia, una tf vacia.
+
+![WhatsApp Image 2022-05-09 at 11 36 10 PM](https://user-images.githubusercontent.com/73531592/167507362-60f453ad-22e2-462a-b6a9-70ac85cc4719.jpeg)
+
+![WhatsApp Image 2022-05-09 at 11 37 26 PM](https://user-images.githubusercontent.com/73531592/167507397-15ec6c63-e596-4e45-a8cf-d1db78f032b5.jpeg)
+
+Una vez obtenido el grafo simplemente nos quedaba, sacar la información del grafo y con esta información mover el joint de la cabeza del robot para que observara al objeto en cuestión.
+Para mover la cabeza del robot como todas las tfs tenían altura 0, lo único que hemos tenido que hacer ha sido movernos hacia los lados. Para obtener la rotación de la cabeza con respecto al punto deseado ( el objeto al que atender), hemos conseguido las coordenadas del modelo y del robot gracias a las tfs y hemos realizado un atan2, para conseguir el ángulo deseado.
+Con esto el robot ya podría girar la cabeza hasta el modelo deseado y mantener la vista unos 5 segundos.
+
+![WhatsApp Image 2022-05-09 at 11 36 11 PM](https://user-images.githubusercontent.com/73531592/167507720-30ad07df-fdd2-4779-ae5d-bfd6db0e3937.jpeg)
+
+
+- [Guillermo Bernal Ruiz](https://github.com/gbernalr)
+- [Juan Camilo Carmona Sanchez](https://github.com/camilo-0715)
+- [Luis Miguel López Martín](https://github.com/chuismiguel)
+- [Jose Manuel Tostado Felipe](https://github.com/Josetost)

attention/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 3.5)
+project(attention)
+
+set(CMAKE_BUILD_TYPE Debug)
+
+set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH}  "${CMAKE_CURRENT_LIST_DIR}/cmake")
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}")
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(gazebo_msgs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(ros2_knowledge_graph REQUIRED)
+
+find_package(trajectory_msgs REQUIRED)
+
+find_package(ros2_knowledge_graph_msgs REQUIRED)
+
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+find_package(ZMQ)
+if(ZMQ_FOUND)
+    message(STATUS "ZeroMQ found.")
+    add_definitions(-DZMQ_FOUND)
+else()  
+  message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.")
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+
+set(dependencies
+    rclcpp
+    rclcpp_lifecycle
+    rclcpp_action
+    geometry_msgs
+    gazebo_msgs
+    ament_index_cpp
+    ros2_knowledge_graph
+    trajectory_msgs
+    ros2_knowledge_graph_msgs
+
+    tf2
+    tf2_ros
+    tf2_geometry_msgs
+)
+
+include_directories(include ${ZMQ_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED
+  src/attention/GetModels.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+add_executable(get_models_node src/get_models_node.cpp)
+ament_target_dependencies(get_models_node ${dependencies})
+target_link_libraries(get_models_node ${PROJECT_NAME})
+
+install(TARGETS
+  ${plugin_libs}
+  get_models_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+
+add_executable(head_node src/set_headjoint.cpp)
+ament_target_dependencies(head_node ${dependencies})
+target_link_libraries(head_node ${ZMQ_LIBRARIES})
+
+install(TARGETS
+  ${plugin_libs}
+  head_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+
+ament_package()

attention/include/attention/GetModels.hpp

@@ -0,0 +1,85 @@
+// Copyright 2022 Grupo Día Libre
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ATTENTION__GETMODELS_HPP_
+#define ATTENTION__GETMODELS_HPP_
+
+#include <memory>
+#include <cmath>
+
+
+#include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "gazebo_msgs/msg/model_states.hpp"
+
+#include "tf2/transform_datatypes.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/convert.h"
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "ros2_knowledge_graph/graph_utils.hpp"
+#include "ros2_knowledge_graph/GraphNode.hpp"
+
+namespace attention
+{
+
+class GetModels : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  GetModels();
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 
+  on_activate(const rclcpp_lifecycle::State & state);
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 
+  on_deactivate(const rclcpp_lifecycle::State & state);
+
+  void do_work();
+
+private:
+  rclcpp::Subscription<gazebo_msgs::msg::ModelStates>::SharedPtr sub_;
+  std::shared_ptr<ros2_knowledge_graph::GraphNode> graph_;
+
+  std::vector<std::string> model_names;
+
+  std::vector<std::string> tfs_name;
+  std::vector<geometry_msgs::msg::TransformStamped> tfs_to_graph;
+
+  std::vector<std::string> nodes_in_graph;
+
+  void model_state_cb(const gazebo_msgs::msg::ModelStates::SharedPtr msg);
+  void add_nodes_to_graph();
+  void place_tfs(const gazebo_msgs::msg::ModelStates::SharedPtr msg);
+
+  bool tfs_placed;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
+
+  //No va el grafo no se por que
+  //rclcpp_lifecycle::LifecyclePublisher<std::vector<std::string>>::SharedPtr pub_;
+  //std::vector<std::string>> names_;
+
+};
+
+}  // namespace attention
+
+#endif  // ATTENTION__GET MODELS_HPP_

attention/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>attention</name>
+  <version>0.1.0</version>
+  <description>Mastering Behavior Trees in ROS2</description>
+  <maintainer email="[email protected]">fmrico</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp_action</depend>
+  <depend>behaviortree_cpp_v3</depend>
+  <depend>action_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>libzmq3-dev</depend>
+  <depend>kobuki_ros_interfaces</depend>
+  <depend>ament_index_cpp</depend>
+  <depend>ros2_knowledge_graph</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>ros2_knowledge_graph_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>