c++ support for laser mapping

Author: aquintan4

exercises/laser_mapping/cpp_lib/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.10)
+project(VacuumCleanerLibs 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
+    gazebo_msgs
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+    tf2
+    tf2_geometry_msgs
+    ros_gz_interfaces
+)
+
+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 nav_msgs gazebo_msgs tf2 tf2_geometry_msgs sensor_msgs)
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json ${OpenCV_LIBS})
+set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs std_msgs ros_gz_interfaces nav_msgs)
+
+# --- 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 ${OpenCV_INCLUDE_DIRS})
+
+ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS})
+target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS})
+
+# --- libHAL.so ---
+add_library(HAL SHARED src/HAL.cpp)
+target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE})
+
+ament_target_dependencies(HAL ${HAL_ROS_DEPS})
+target_link_libraries(HAL ${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,31 @@
+#ifndef INCLUDE_HAL_HPP_
+#define INCLUDE_HAL_HPP_
+
+#include <vector>
+#include <memory>
+#include <thread>
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
+#include "common_interfaces_cpp/hal/motors.hpp"
+#include "common_interfaces_cpp/hal/laser.hpp"
+#include "common_interfaces_cpp/hal/odometry.hpp"
+
+class HAL : public rclcpp::Node
+{
+public:
+  HAL();
+  static void setV(const float speed);
+  static void setW(const float speed);
+  static const LaserData *getLaserData();
+  static const Pose3d *getPose3d();
+  static const Pose3d *getOdom();
+
+private:
+  static std::shared_ptr<MotorsNode> motors_node_;
+  static std::shared_ptr<LaserNode> laser_node_;
+  static std::shared_ptr<OdometryNode> odometry_node_;
+  static std::shared_ptr<OdometryNode> noisy_odometry_node_;
+  std::thread spin_thread_;
+};
+
+#endif

exercises/laser_mapping/cpp_lib/include/Map.hpp

@@ -0,0 +1,33 @@
+#ifndef MAP_HPP_
+#define MAP_HPP_
+
+#include <functional>
+#include <vector>
+#include <cmath>
+#include <opencv2/opencv.hpp>
+
+struct Pose3D {
+    double x = 0.0;
+    double y = 0.0;
+    double yaw = 0.0;
+};
+
+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 RTVacuum();
+
+    std::vector<double> getRobotCoordinates();
+    std::vector<double> getRobotCoordinatesWithNoise();
+    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,64 @@
+#ifndef INCLUDE_WEBGUI_HPP_
+#define INCLUDE_WEBGUI_HPP_
+
+#include <string>
+#include <vector>
+#include <mutex>
+#include <thread>
+#include <opencv2/opencv.hpp>
+#include <boost/beast/core.hpp>
+#include <boost/beast/websocket.hpp>
+#include <boost/asio/connect.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/strand.hpp>
+#include <nlohmann/json.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+namespace beast = boost::beast;
+namespace websocket = beast::websocket;
+namespace net = boost::asio;
+using tcp = net::ip::tcp;
+using json = nlohmann::json;
+
+struct Pose3D {
+    double x = 0.0;
+    double y = 0.0;
+    double yaw = 0.0;
+};
+
+class WebGUINode : public rclcpp::Node
+{
+public:
+    WebGUINode();
+    static Pose3D get_pose();
+    static Pose3D get_noisy_pose();
+    static void set_user_map(const cv::Mat& image);
+    static cv::Mat get_user_map();
+
+private:
+    void user_map_callback(sensor_msgs::msg::Image::SharedPtr msg);
+    void odom_callback(nav_msgs::msg::Odometry::SharedPtr msg);
+    void noisy_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg);
+
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr user_map_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr noisy_odom_sub_;
+
+    static Pose3D last_pose_;
+    static Pose3D last_noisy_pose_;
+    static cv::Mat user_map_;
+    static std::mutex map_mutex_;
+};
+
+class WebGUI
+{
+public:
+    WebGUI();
+    static void setUserMap(const cv::Mat& image);
+    static std::vector<double> poseToMap(double x_prime, double y_prime, double yaw_prime);
+    static std::string base64_encode(unsigned char const* bytes_to_encode, unsigned int in_len);
+};
+
+#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,74 @@
+#include "HAL.hpp"
+
+using namespace std::chrono_literals;
+
+std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
+std::shared_ptr<LaserNode> HAL::laser_node_ = nullptr;
+std::shared_ptr<OdometryNode> HAL::odometry_node_ = nullptr;
+std::shared_ptr<OdometryNode> HAL::noisy_odometry_node_ = nullptr;
+
+HAL::HAL() : Node("hal_node")
+{
+  motors_node_ = std::make_shared<MotorsNode>("/turtlebot3/cmd_vel", 4.0, 0.3);
+  laser_node_ = std::make_shared<LaserNode>("/turtlebot3/laser/scan");
+  odometry_node_ = std::make_shared<OdometryNode>("/turtlebot3/odom");
+  noisy_odometry_node_ = std::make_shared<OdometryNode>("/turtlebot3/odom_noisy", "noisy_odometry_node");
+
+  spin_thread_ = std::thread([]() {
+      rclcpp::executors::MultiThreadedExecutor executor;
+      executor.add_node(HAL::motors_node_);
+      executor.add_node(HAL::laser_node_);
+      executor.add_node(HAL::odometry_node_);
+      executor.add_node(HAL::noisy_odometry_node_);
+      
+      while (rclcpp::ok()) {
+          executor.spin_some();
+          std::this_thread::sleep_for(std::chrono::milliseconds(11));
+      }
+  });
+  spin_thread_.detach();
+}
+
+void HAL::setV(const float speed)
+{
+  if (motors_node_) {
+      motors_node_->sendV(static_cast<double>(speed));
+  }
+}
+
+void HAL::setW(const float speed)
+{
+  if (motors_node_) {
+      motors_node_->sendW(static_cast<double>(speed));
+  }
+}
+
+const LaserData *HAL::getLaserData()
+{
+  if (!laser_node_) {
+      return nullptr;
+  }
+  
+  LaserData data = laser_node_->getLaserData();
+  while (data.values.empty() && rclcpp::ok()) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(11));
+      data = laser_node_->getLaserData();
+  }
+  return new LaserData(data);
+}
+
+const Pose3d *HAL::getPose3d()
+{
+  if (!odometry_node_) {
+      return nullptr;
+  }
+  return new Pose3d(odometry_node_->getPose3d());
+}
+
+const Pose3d *HAL::getOdom()
+{
+  if (!noisy_odometry_node_) {
+      return nullptr;
+  }
+  return new Pose3d(noisy_odometry_node_->getPose3d());
+}

exercises/laser_mapping/cpp_lib/src/Map.cpp

@@ -0,0 +1,70 @@
+#include "Map.hpp"
+
+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) {
+    cv::Mat RT = (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);
+    return RT;
+}
+
+cv::Mat Map::RTy(double angle, double tx, double ty, double tz) {
+    cv::Mat RT = (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);
+    return RT;
+}
+
+cv::Mat Map::RTz(double angle, double tx, double ty, double tz) {
+    cv::Mat RT = (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);
+    return RT;
+}
+
+cv::Mat Map::RTVacuum() {
+    return RTz(M_PI / 2.0, 50, 70, 0);
+}
+
+std::vector<double> Map::getRobotCoordinates() {
+    Pose3D pose = pose_getter_();
+    double x = pose.x;
+    double y = pose.y;
+
+    double scale_y = -23.53;
+    double offset_y = -31.95;
+    y = scale_y * (offset_y - y);
+
+    double scale_x = -23.58;
+    double offset_x = -20.36;
+    x = scale_x * (offset_x - x);
+
+    return {x, y, pose.yaw};
+}
+
+std::vector<double> Map::getRobotCoordinatesWithNoise() {
+    Pose3D pose = noisy_pose_getter_();
+    double x = pose.x;
+    double y = pose.y;
+
+    double scale_y = -23.53;
+    double offset_y = -31.95;
+    y = scale_y * (offset_y - y);
+
+    double scale_x = -23.58;
+    double offset_x = -20.36;
+    x = scale_x * (offset_x - x);
+
+    return {x, y, pose.yaw};
+}
+
+void Map::reset() {
+}