Merge pull request #3795 from JdeRobot/cpp-laser-mapping-update

Cpp laser mapping update

Author: javizqh

database/exercises/db.sql

@@ -124,7 +124,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
 11	montecarlo_laser_loc	Montecarlo Laser Loc	Calculate the position of the robot based on the	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/montecarlo_laser_loc
 12	montecarlo_visual_loc	Montecarlo Visual Loc	Calculate the position of the robot based on the	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/montecarlo_visual_loc
 13	marker_visual_loc	Marker Visual Loc	Calculate the position of the robot based on the	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/marker_visual_loc
-14	laser_mapping	Laser Mapping	Build a map based on sensor readings	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/laser_mapping
+14	laser_mapping	Laser Mapping	Build a map based on sensor readings	["ROS2", "MULTILANGUAGE"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/laser_mapping
 15	basic_computer_vision	Basic Computer Vision	Basic Computer Vision exercise using React and RAM	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/basic_computer_vision
 16	follow_road	Drone Follow Road	Drone Follow Road exercise	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/follow_road
 17	pick_place	Pick and Place	Pick and Place exercise	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/pick_place

exercises/laser_mapping/cpp_lib/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.10)
+project(ObstacleAvoidanceLibs CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+find_package(ament_cmake REQUIRED)
+
+set(ROS2_PACKAGES
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+    cv_bridge
+)
+
+foreach(pkg IN LISTS ROS2_PACKAGES)
+    find_package(${pkg} REQUIRED)
+endforeach()
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(nlohmann_json REQUIRED)
+find_package(OpenCV REQUIRED)
+
+set(COMMON_INTERFACES_INCLUDE "/home/ws/install/common_interfaces_cpp/include")
+set(COMMON_INTERFACES_LIB "/home/ws/install/common_interfaces_cpp/lib/libcommon_interfaces_cpp.so")
+
+set(WEBGUI_ROS_DEPS  rclcpp geometry_msgs std_msgs nav_msgs sensor_msgs cv_bridge)
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json ${OpenCV_LIBS})
+set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs nav_msgs cv_bridge)
+set(HAL_SYS_DEPS     ${OpenCV_LIBS})
+
+# --- libFrequency.so ---
+add_library(Frequency SHARED src/Frequency.cpp)
+target_include_directories(Frequency PUBLIC include)
+
+# --- libWebGUI.so ---
+add_library(WebGUI SHARED 
+    src/WebGUI.cpp 
+    src/Map.cpp 
+)
+target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
+ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS})
+target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS} ${COMMON_INTERFACES_LIB})
+
+# --- libHAL.so ---
+add_library(HAL SHARED src/HAL.cpp)
+target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
+ament_target_dependencies(HAL ${HAL_ROS_DEPS})
+target_link_libraries(HAL ${HAL_SYS_DEPS} ${COMMON_INTERFACES_LIB})

exercises/laser_mapping/cpp_lib/include/Frequency.hpp

@@ -0,0 +1,22 @@
+#ifndef INCLUDE_FREQUENCY_HPP_
+#define INCLUDE_FREQUENCY_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+class Frequency
+{
+private:
+  std::chrono::high_resolution_clock::time_point last_time;
+  int is_first_iteration;
+
+public:
+  Frequency();
+  ~Frequency();
+
+  static int rate;
+  void tick(int ideal_cycle = 50);
+};
+
+#endif

exercises/laser_mapping/cpp_lib/include/HAL.hpp

@@ -0,0 +1,37 @@
+#ifndef INCLUDE_HAL_HPP_
+#define INCLUDE_HAL_HPP_
+
+#include <memory>
+#include <thread>
+#include <vector>
+#include <array>
+
+class MotorsNode;
+class OdometryNode;
+class LaserNode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
+
+class HAL
+{
+public:
+    HAL() = delete;
+
+    static void set_v(const float velocity);
+    static void set_w(const float velocity);
+    static std::array<double, 3> get_pose3d();
+    static std::array<double, 3> get_odom();
+    static std::vector<float> get_laser_data();
+
+private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    static std::shared_ptr<MotorsNode> motors_node_;
+    static std::shared_ptr<OdometryNode> odometry_node_;
+    static std::shared_ptr<OdometryNode> noisy_odometry_node_;
+    static std::shared_ptr<LaserNode> laser_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/laser_mapping/cpp_lib/include/Map.hpp

@@ -0,0 +1,29 @@
+#ifndef INCLUDE_MAP_HPP_
+#define INCLUDE_MAP_HPP_
+
+#include <functional>
+#include <tuple>
+#include <opencv2/opencv.hpp>
+#include "common_interfaces_cpp/hal/odometry.hpp"
+
+class Map
+{
+public:
+    Map(std::function<Pose3d()> pose_getter, std::function<Pose3d()> noisy_pose_getter);
+
+    cv::Mat rtx(double angle, double tx, double ty, double tz);
+    cv::Mat rty(double angle, double tx, double ty, double tz);
+    cv::Mat rtz(double angle, double tx, double ty, double tz);
+    cv::Mat rt_vacuum();
+
+    std::tuple<double, double, double> get_robot_coordinates();
+    std::tuple<double, double, double> get_robot_coordinates_with_noise();
+
+    void reset();
+
+private:
+    std::function<Pose3d()> pose_getter_;
+    std::function<Pose3d()> noisy_pose_getter_;
+};
+
+#endif

exercises/laser_mapping/cpp_lib/include/WebGUI.hpp

@@ -0,0 +1,29 @@
+#ifndef INCLUDE_WEBGUI_HPP_
+#define INCLUDE_WEBGUI_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <memory>
+#include <thread>
+
+class WebGUINode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
+
+class WebGUI
+{
+public:
+    WebGUI() = delete;
+
+    static void set_user_map(const cv::Mat& image);
+    static std::vector<double> pose_to_map(double x_prime, double y_prime, double yaw_prime);
+
+private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    static std::shared_ptr<WebGUINode> gui_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/laser_mapping/cpp_lib/src/Frequency.cpp

@@ -0,0 +1,34 @@
+#include "Frequency.hpp"
+
+int Frequency::rate = 0;
+
+Frequency::Frequency() : is_first_iteration(1) {}
+
+Frequency::~Frequency() {}
+
+void Frequency::tick(int ideal_cycle)
+{
+  const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
+  const auto now = std::chrono::high_resolution_clock::now();
+
+  if (is_first_iteration == 1)
+  {
+    last_time = now;
+    is_first_iteration = 0;
+    return;
+  }
+
+  const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
+
+  if (iter_ms < ideal_ms)
+  {
+    rate = std::round(1000.0 / ideal_ms.count());
+    std::this_thread::sleep_for(ideal_ms - iter_ms);
+  }
+  else
+  {
+    rate = std::round(1000.0 / iter_ms.count());
+  }
+
+  last_time = now;
+}

exercises/laser_mapping/cpp_lib/src/HAL.cpp

@@ -0,0 +1,75 @@
+#include "HAL.hpp"
+#include "common_interfaces_cpp/hal/motors.hpp"
+#include "common_interfaces_cpp/hal/odometry.hpp"
+#include "common_interfaces_cpp/hal/laser.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
+std::shared_ptr<OdometryNode> HAL::odometry_node_ = nullptr;
+std::shared_ptr<OdometryNode> HAL::noisy_odometry_node_ = nullptr;
+std::shared_ptr<LaserNode> HAL::laser_node_ = nullptr;
+std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> HAL::executor_ = nullptr;
+std::thread HAL::spin_thread_;
+
+void HAL::init()
+{
+    if (!motors_node_) {
+        odometry_node_ = std::make_shared<OdometryNode>("/turtlebot3/odom", "hal_odometry");
+        noisy_odometry_node_ = std::make_shared<OdometryNode>("/turtlebot3/odom_noisy", "hal_noisy_odometry_node");
+        motors_node_ = std::make_shared<MotorsNode>("/turtlebot3/cmd_vel", 4.0, 0.3, "hal_motors");
+        laser_node_ = std::make_shared<LaserNode>("/turtlebot3/laser/scan", "hal_laser");
+
+        executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+        executor_->add_node(odometry_node_);
+        executor_->add_node(noisy_odometry_node_);
+        executor_->add_node(motors_node_);
+        executor_->add_node(laser_node_);
+
+        spin_thread_ = std::thread([]() {
+            executor_->spin();
+        });
+        spin_thread_.detach();
+    }
+}
+
+void HAL::set_v(const float velocity)
+{
+    if (motors_node_) motors_node_->sendV(static_cast<double>(velocity));
+}
+
+void HAL::set_w(const float velocity)
+{
+    if (motors_node_) motors_node_->sendW(static_cast<double>(velocity));
+}
+
+std::array<double, 3> HAL::get_pose3d()
+{
+    if (!odometry_node_) return {0.0, 0.0, 0.0};
+    
+    auto raw_pose = odometry_node_->getPose3d();
+    return {raw_pose.x, raw_pose.y, raw_pose.yaw};
+}
+
+std::array<double, 3> HAL::get_odom()
+{
+    if (!noisy_odometry_node_) return {0.0, 0.0, 0.0};
+    
+    auto raw_pose = noisy_odometry_node_->getPose3d();
+    return {raw_pose.x, raw_pose.y, raw_pose.yaw};
+}
+
+std::vector<float> HAL::get_laser_data()
+{
+    if (!laser_node_) return {};
+    
+    auto raw_laser = laser_node_->getLaserData();
+    while (raw_laser.values.empty() && rclcpp::ok()) {
+        std::this_thread::sleep_for(5ms);
+        raw_laser = laser_node_->getLaserData();
+    }
+    
+    return raw_laser.values;
+}

exercises/laser_mapping/cpp_lib/src/Map.cpp

@@ -0,0 +1,63 @@
+#include "Map.hpp"
+#include <cmath>
+
+Map::Map(std::function<Pose3d()> pose_getter, std::function<Pose3d()> noisy_pose_getter)
+    : pose_getter_(pose_getter), noisy_pose_getter_(noisy_pose_getter)
+{
+}
+
+cv::Mat Map::rtx(double angle, double tx, double ty, double tz)
+{
+    return (cv::Mat_<double>(4, 4) << 
+        1, 0, 0, tx,
+        0, std::cos(angle), -std::sin(angle), ty,
+        0, std::sin(angle), std::cos(angle), tz,
+        0, 0, 0, 1);
+}
+
+cv::Mat Map::rty(double angle, double tx, double ty, double tz)
+{
+    return (cv::Mat_<double>(4, 4) << 
+        std::cos(angle), 0, std::sin(angle), tx,
+        0, 1, 0, ty,
+        -std::sin(angle), 0, std::cos(angle), tz,
+        0, 0, 0, 1);
+}
+
+cv::Mat Map::rtz(double angle, double tx, double ty, double tz)
+{
+    return (cv::Mat_<double>(4, 4) << 
+        std::cos(angle), -std::sin(angle), 0, tx,
+        std::sin(angle), std::cos(angle), 0, ty,
+        0, 0, 1, tz,
+        0, 0, 0, 1);
+}
+
+cv::Mat Map::rt_vacuum()
+{
+    return rtz(CV_PI / 2.0, 50, 70, 0);
+}
+
+std::tuple<double, double, double> Map::get_robot_coordinates()
+{
+    Pose3d pose = pose_getter_();
+    
+    double y = -23.53 * (-31.95 - pose.y);
+    double x = -23.58 * (-20.36 - pose.x);
+
+    return {x, y, pose.yaw};
+}
+
+std::tuple<double, double, double> Map::get_robot_coordinates_with_noise()
+{
+    Pose3d pose = noisy_pose_getter_();
+    
+    double y = -23.53 * (-31.95 - pose.y);
+    double x = -23.58 * (-20.36 - pose.x);
+
+    return {x, y, pose.yaw};
+}
+
+void Map::reset()
+{
+}