Finish cpp webgui

exercises/static/exercises/follow_line/cpp_template/ros2_humble/CMakeLists.txt

@@ -9,19 +9,23 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(gazebo_msgs REQUIRED)
 
 set(dependencies
   rclcpp
   cv_bridge
   sensor_msgs
+  nav_msgs
   OpenCV
   geometry_msgs
   std_msgs
+  gazebo_msgs
   image_transport
   message_filters
 )

exercises/static/exercises/follow_line/cpp_template/ros2_humble/Frequency.hpp

@@ -4,6 +4,7 @@
 #include <chrono>
 #include <iostream>
 #include <thread>
+#include <bits/stdc++.h>
 
 using namespace std;
 
@@ -17,6 +18,7 @@ class Frequency
   Frequency(/* args */);
   ~Frequency();
 
+  static int rate;
   void tick(int ideal_cycle);
 };
 
@@ -45,11 +47,18 @@ void Frequency::tick(int ideal_cycle = 50)
 
   if (iter_ms < ideal_ms)
   {
+    rate = round(1000 / ideal_ms.count());
     this_thread::sleep_for(chrono::milliseconds(ideal_ms - iter_ms));
   }
+  else
+  {
+    rate = round(1000 / iter_ms.count());
+  }
 
   last_time = now;
   return;
 }
 
+int Frequency::rate = 0;
+
 #endif

exercises/static/exercises/follow_line/cpp_template/ros2_humble/HAL.hpp

@@ -2,7 +2,9 @@
 #define INCLUDE_HAL_HPP_
 
 #include "geometry_msgs/msg/twist.hpp"
+#include "gazebo_msgs/msg/performance_metrics.hpp"
 #include "sensor_msgs/msg/image.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "cv_bridge/cv_bridge.h"
 #include "opencv2/opencv.hpp"
@@ -18,8 +20,14 @@ class HAL : public rclcpp::Node
   {
     vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
     cam_sub_ = create_subscription<sensor_msgs::msg::Image>(
-        "/cam_f1_left/image_raw", 1,
+        "/cam_f1_left/image_raw", 10,
         std::bind(&HAL::topic_callback_info, this, _1));
+    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        "/odom", 10,
+        std::bind(&HAL::pose_callback, this, _1));
+    perf_sub_ = create_subscription<gazebo_msgs::msg::PerformanceMetrics>(
+        "/performance_metrics", 10,
+        std::bind(&HAL::performance_callback, this, _1));
   };
 
   static void set_v(const float speed)
@@ -39,10 +47,20 @@ class HAL : public rclcpp::Node
     return image_rgb;
   };
 
+  static vector<double> get_pose()
+  {
+    vector<double> v = {last_odom.pose.pose.position.x, last_odom.pose.pose.position.y, last_odom.pose.pose.position.z};
+    return v;
+  };
+
+  static double get_performance()
+  {
+    return last_perf.real_time_factor;
+  };
+
 private:
   void topic_callback_info(sensor_msgs::msg::Image::UniquePtr msg)
   {
-    cout << "Image received" << endl;
     // Convert ROS Image to OpenCV Image | sensor_msgs::msg::Image -> cv::Mat
     cv_bridge::CvImagePtr image_rgb_ptr;
     try
@@ -58,17 +76,33 @@ class HAL : public rclcpp::Node
     image_rgb = image_rgb_raw;
   };
 
+  void pose_callback(nav_msgs::msg::Odometry::UniquePtr msg)
+  {
+    last_odom = *msg;
+  };
+
+  void performance_callback(gazebo_msgs::msg::PerformanceMetrics::UniquePtr msg)
+  {
+    last_perf = *msg;
+  };
+
   // Publisher
   static rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<gazebo_msgs::msg::PerformanceMetrics>::SharedPtr perf_sub_;
 
   // Message
   static geometry_msgs::msg::Twist last_twist;
   static cv::Mat image_rgb;
+  static nav_msgs::msg::Odometry last_odom;
+  static gazebo_msgs::msg::PerformanceMetrics last_perf;
 };
 
 rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr HAL::vel_pub_ = nullptr;
 geometry_msgs::msg::Twist HAL::last_twist = geometry_msgs::msg::Twist();
+nav_msgs::msg::Odometry HAL::last_odom = nav_msgs::msg::Odometry();
+gazebo_msgs::msg::PerformanceMetrics HAL::last_perf = gazebo_msgs::msg::PerformanceMetrics();
 cv::Mat HAL::image_rgb = cv::Mat();
 
 #endif

exercises/static/exercises/follow_line/cpp_template/ros2_humble/Lap.hpp

@@ -0,0 +1,77 @@
+#ifndef INCLUDE_LAP_HPP_
+#define INCLUDE_LAP_HPP_
+
+#include <chrono>
+#include <iostream>
+#include <thread>
+#include <string>
+#include "HAL.hpp"
+#include <stdio.h>
+
+using namespace std;
+
+class Lap
+{
+private:
+  chrono::milliseconds lap_time;
+  chrono::_V2::system_clock::time_point start_time;
+
+  bool paused = false;
+
+public:
+  Lap();
+  ~Lap();
+
+  void pause();
+  void start();
+  void reset();
+  std::string getLapTime();
+};
+
+Lap::Lap()
+{
+  this->reset();
+}
+
+Lap::~Lap()
+{
+}
+
+std::string Lap::getLapTime()
+{
+  if (!paused)
+  {
+    lap_time += chrono::duration_cast<chrono::milliseconds>(chrono::high_resolution_clock::now() - start_time);
+    start_time = chrono::high_resolution_clock::now();
+  }
+
+  char buff[70];
+  const int sec = lap_time.count() / 1000;
+  const int min = sec / 60;
+  const int h = min / 60;
+  sprintf(buff, "%d:%d:%d.%ld",h, min, sec, lap_time.count() % 1000);
+  return std::string(buff);
+}
+
+void Lap::reset()
+{
+  start_time = chrono::high_resolution_clock::now();
+  lap_time = chrono::milliseconds(0);
+  paused = false;
+  return;
+}
+
+void Lap::pause()
+{
+  paused = true;
+  return;
+}
+
+void Lap::start()
+{
+  start_time = chrono::high_resolution_clock::now();
+  paused = false;
+  return;
+}
+
+#endif