Feedback

.github/workflows/main.yaml

@@ -0,0 +1,37 @@
+name: main
+
+on:
+  pull_request:
+    branches:
+      - main
+  push:
+    branches:
+      - main
+
+jobs:
+  build-and-test:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04]
+      fail-fast: false
+    steps:
+      - name: Setup ROS 2
+        uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: foxy
+      - name: build and test
+        uses: ros-tooling/[email protected]
+        with:
+          package-name: follow_wall_k_bots
+          import-token: ${{secrets.GITHUB_TOKEN}}
+          target-ros2-distro: foxy
+          colcon-defaults: |
+            {
+              "test": {
+                "ctest-args": [
+                "-LE", "copyright"
+                ]
+              }
+            }
+

.vscode/c_cpp_properties.json

@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/opt/ros/foxy/include"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "gnu17",
+            "cppStandard": "gnu++14",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 3.5)
+project(follow_wall_k_bots)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_BUILD_TYPE DEBUG)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+list(APPEND AMENT_LINT_AUTO_EXCLUDE
+  ament_cmake_copyright
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(ament_cmake_gtest)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+ament_add_gtest(node_test test/node_test.cpp)
+target_link_libraries(node_test ${PROJECT_NAME})
+
+set(dependencies
+  rclcpp
+  rclcpp_lifecycle
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/follow_wall/follow_wall.cpp
+)
+
+add_executable(follow_wall_main  src/follow_wall_main.cpp)
+ament_target_dependencies(follow_wall_main ${dependencies})
+
+target_link_libraries(follow_wall_main ${PROJECT_NAME})
+
+install(TARGETS
+  follow_wall_main
+  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)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all
+  # source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+ament_package()

README.md

@@ -1,2 +1,48 @@
 [![Open in Visual Studio Code](https://classroom.github.com/assets/open-in-vscode-f059dc9a6f8d3a56e377f745f24479a46679e63a5d9fe6f495e02850cd0d8118.svg)](https://classroom.github.com/online_ide?assignment_repo_id=6883151&assignment_repo_type=AssignmentRepo)
-# folllow_wall
+
+[![GitHub Action
+Status](https://github.com/Docencia-fmrico/follow-wall-k-bots/workflows/main/badge.svg)](https://github.com/Docencia-fmrico/follow-wall-k-bots)
+
+
+# Follow_Wall
+## Introducción
+En esta práctica realizaremos un algoritmo basado en seguir la pared, para ello utilizamos el sensor láser y los LifeCycle nodes de ROS2.
+## Extracción valores del láser
+Para saber donde están los obstáculos, en este caso la pared, usamos el láser. Como procesado, extraemos la medida mínima en un cono de amplitud de 10º centrado en tres regiones: la frontal (0), la izquierda (pi/2) y la derecha (-pi/2). Finalmente, las tres medidas se guardan en un vector.   
+<p align="center">
+<img src="./images/laserregions.png" width="75%" height="75%">
+</p align>
+
+## Control de Movimiento
+Aprovechando las características de los LifeCycle Nodes y sus estados y transiciones, usamos la transición on_configure() para ver si el robot tiene algún obstáculo cercano a izquierda o derecha y comenzar siguiendo ese lado del robot directamente. Si no hay nada cerca, se elige un lado del robot para seguir la pared al azar.
+```
+FollowWallNode::on_configure() {
+  // Esperar por medidas del láser
+  while (laser_regions.size() == 0) {continue;}
+
+  // Elegir un lado a seguir
+  if (laser_regions[A] < distance) {side = LEFT;}
+  else if (laser_regions[C] < distance) {side = RIGHT;}
+  else {
+    if (rand() % 2 == 0) {side = LEFT;}
+    else {side = RIGHT;}
+  }
+}
+```
+Una vez se ha realizado la configuración, se espera a que el nodo sea activado para comenzar a mover el robot. Si se desease cambiar la configuración de qué lado sigue el robot, basta con volver a ejecutar la transición on_configure().
+
+[[Video de reconfiguración]](https://youtu.be/Y4X0Fe2ARrQ)
+
+Cuando se active el nodo se realizará el siguiente proceso de comprobación. Una vez hecho, en función del estado seleccionado se seleccionarán velocidades lineales y angulares correspondientes a cada estado:
+<p align="center">
+<img src="./images/casecontrol.png" width="75%" height="75%">
+</p align>
+En el caso de que se aleje al robot de la pared, este comenzará a girar hacía el lado que estaba siguiendo. Se ha contemplado que si ha entrado en dicho estado, un número concreto de iteraciones, el robot deje de hacer este movimiento y vuelva a buscar la pared avanzando hacia una dirección hasta volver a encontrarla.
+
+[[Video de alejar de pared]](https://youtu.be/vCkj9qj-gTk)
+
+## Video DEMO
+
+[[Demo]](https://youtu.be/Omu6V5bajiw)
+[[Demo Kobuki Real]](https://youtu.be/rBmrcK2q36I)
+

include/follow_wall/follow_wall.hpp

@@ -0,0 +1,91 @@
+// Copyright 2022 K-Bots
+//
+// 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 FOLLOW_WALL__FOLLOW_WALL_HPP_
+
+#define FOLLOW_WALL__FOLLOW_WALL_HPP_
+
+#include <vector>
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+class FollowWallNode : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  FollowWallNode();
+
+  using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+  CallbackReturnT on_configure(const rclcpp_lifecycle::State & state);
+  CallbackReturnT on_activate(const rclcpp_lifecycle::State & state);
+  CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state);
+  CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state);
+  CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state);
+  CallbackReturnT on_error(const rclcpp_lifecycle::State & state);
+
+  static bool comparator(float a, float b);
+
+  void LaserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
+  int angle2pos(float angle, float min, float max, int size);
+  void CheckState();
+  void FollowTheWall();
+  void do_work();
+  void LookForWall();
+
+  std::vector<float> laser_regions;
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laserSub_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr pubVelocity_;
+
+  const float LINEAL_VELOCITY = 0.6;
+  const float ANGULAR_VELOCITY = 1;
+  const float MAX_DISTANCE = 0.5;
+  const float MIN_DISTANCE = 0.4;
+  const float RESTART_VALUE = 1.5;
+  const int MAX_ITERATIONS = 50;
+  bool wall_found = false;
+
+  int counter_ = 0;
+  int state_ = GOING_FORWARD;
+  int side_ = 0;
+
+  enum robot_side
+  {
+    LEFT_SIDE = 1,
+    RIGHT_SIDE
+  };
+
+  enum movement
+  {
+    GOING_FORWARD = 1,
+    TURN_SAME_SIDE,
+    TURN_OPPOSITE_SIDE,
+    WALL_AHEAD
+  };
+
+  enum laser_side
+  {
+    LEFT = 0,
+    CENTER,
+    RIGHT
+  };
+};
+
+#endif  // FOLLOW_WALL__FOLLOW_WALL_HPP_

package.xml

@@ -0,0 +1,23 @@
+<?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>follow_wall_k_bots</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">lucia</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>