Update gitignore

Author: javizqh

.gitignore

@@ -118,7 +118,7 @@ webpack-stats.json
 *.DS_Store
 react_frontend/static
 developer_scripts
-src
+# src
 !/react_frontend/src
 
 # Intermediate file created in root during development

exercises/vacuum_cleaner/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/vacuum_cleaner/cpp_lib/src/HAL.cpp

@@ -0,0 +1,61 @@
+#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<BumperNode> HAL::bumper_node_ = nullptr;
+
+HAL::HAL() : Node("hal_node")
+{
+  std::vector<std::string> bumper_topics = {
+      "/roombaROS/events/right_bumper",
+      "/roombaROS/events/center_bumper",
+      "/roombaROS/events/left_bumper"
+  };
+
+  motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 1.0, 1.0);
+  laser_node_ = std::make_shared<LaserNode>("/roombaROS/laser/scan");
+  bumper_node_ = std::make_shared<BumperNode>(bumper_topics);
+
+  spin_thread_ = std::thread([]() {
+      rclcpp::executors::SingleThreadedExecutor executor;
+      executor.add_node(HAL::motors_node_);
+      executor.add_node(HAL::laser_node_);
+      executor.add_node(HAL::bumper_node_);
+      
+      while (rclcpp::ok()) {
+          executor.spin_some();
+          std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      }
+  });
+  spin_thread_.detach();
+}
+
+void HAL::set_v(const float speed)
+{
+  if (motors_node_) motors_node_->sendV(static_cast<double>(speed));
+}
+
+void HAL::set_w(const float speed)
+{
+  if (motors_node_) motors_node_->sendW(static_cast<double>(speed));
+}
+
+const LaserData *HAL::get_laser_data()
+{
+  if (!laser_node_) return nullptr;
+  return new LaserData(laser_node_->getLaserData());
+}
+
+std::vector<bool> HAL::get_bumper_data()
+{
+  std::vector<bool> v = {false, false, false};
+  if (bumper_node_) {
+      BumperData b_data = bumper_node_->getBumperData();
+      if (b_data.state == 1 && b_data.bumper >= 0 && b_data.bumper < 3) {
+          v[b_data.bumper] = true;
+      }
+  }
+  return v;
+}

exercises/vacuum_cleaner/cpp_lib/src/WebGUI.cpp

@@ -0,0 +1,107 @@
+#include "WebGUI.hpp"
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <iomanip>
+#include <sstream>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+// Statics
+std::string WebGUI::img_payload = "";
+nav_msgs::msg::Odometry WebGUINode::last_odom = nav_msgs::msg::Odometry();
+gazebo_msgs::msg::PerformanceMetrics WebGUINode::last_perf = gazebo_msgs::msg::PerformanceMetrics();
+
+// WebGUINode Implementation
+WebGUINode::WebGUINode() : Node("webgui_node")
+{
+    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        "/odom", 10, std::bind(&WebGUINode::pose_callback, this, _1));
+    perf_sub_ = create_subscription<gazebo_msgs::msg::PerformanceMetrics>(
+        "/performance_metrics", 10, std::bind(&WebGUINode::performance_callback, this, _1));
+}
+
+std::vector<double> WebGUINode::get_pose()
+{
+    tf2::Quaternion tf_quat;
+    tf2::fromMsg(last_odom.pose.pose.orientation, tf_quat);
+    tf2::Matrix3x3 m(tf_quat);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+
+    const double x = -30.0 * last_odom.pose.pose.position.x + 171.0;
+    const double y = 15.0 * last_odom.pose.pose.position.y + 63.0;
+    return {x, y, yaw};
+}
+
+double WebGUINode::get_performance() { return last_perf.real_time_factor; }
+
+void WebGUINode::pose_callback(nav_msgs::msg::Odometry::UniquePtr msg) { last_odom = *msg; }
+void WebGUINode::performance_callback(gazebo_msgs::msg::PerformanceMetrics::UniquePtr msg) { last_perf = *msg; }
+
+// WebSocket Session Helper
+class session : public std::enable_shared_from_this<session>
+{
+    tcp::resolver resolver_;
+    websocket::stream<beast::tcp_stream> ws_;
+    beast::flat_buffer buffer_;
+    std::string host_, text_;
+
+public:
+    explicit session(net::io_context &ioc) : resolver_(net::make_strand(ioc)), ws_(net::make_strand(ioc)) {}
+
+    void run(char const *host, char const *port, char const *text) {
+        host_ = host; text_ = text;
+        buffer_.max_size(1024 * 1024);
+        resolver_.async_resolve(host, port, beast::bind_front_handler(&session::on_resolve, shared_from_this()));
+    }
+
+    void on_resolve(beast::error_code ec, tcp::resolver::results_type results) {
+        if (ec) return;
+        beast::get_lowest_layer(ws_).expires_after(std::chrono::seconds(30));
+        beast::get_lowest_layer(ws_).async_connect(results, beast::bind_front_handler(&session::on_connect, shared_from_this()));
+    }
+
+    void on_connect(beast::error_code ec, tcp::resolver::results_type::endpoint_type ep) {
+        if (ec) return;
+        beast::get_lowest_layer(ws_).expires_never();
+        ws_.set_option(websocket::stream_base::timeout::suggested(beast::role_type::client));
+        ws_.read_message_max(1024 * 1024);
+        ws_.auto_fragment(false);
+        host_ += ':' + std::to_string(ep.port());
+        ws_.async_handshake(host_, "", beast::bind_front_handler(&session::on_handshake, shared_from_this()));
+    }
+
+    void on_handshake(beast::error_code ec) {
+        if (ec) return;
+        ws_.async_write(net::buffer(text_), beast::bind_front_handler(&session::on_write, shared_from_this()));
+    }
+
+    void on_write(beast::error_code ec, std::size_t) {
+        if (ec) return;
+        ws_.async_read(buffer_, beast::bind_front_handler(&session::on_read, shared_from_this()));
+    }
+
+    void on_read(beast::error_code ec, std::size_t) {
+        if (ec) return;
+        unsigned char *cp = (unsigned char *)buffer_.data().data();
+        std::string msg(reinterpret_cast<char const *>(cp));
+        buffer_.consume(buffer_.size());
+
+        auto pose = WebGUINode::get_pose();
+        json map_j = {pose.at(0), pose.at(1), pose.at(2)};
+        std::stringstream ss;
+        ss << std::fixed << std::setprecision(2) << WebGUINode::get_performance();
+        
+        json j = {{"map", map_j.dump()}, {"brain", Frequency::rate}, {"gui", 20}, {"rtf", ss.str()}, {"fps", -1}, {"lat", -1}};
+        std::string out = j.dump();
+        ws_.async_write(net::buffer(out), beast::bind_front_handler(&session::on_write, shared_from_this()));
+    }
+};
+
+WebGUI::WebGUI()
+{
+    net::io_context ioc;
+    std::make_shared<session>(ioc)->run("127.0.0.1", "2303", "{\"map\":\"(201,85.5,0)\"}");
+    ioc.run();
+}