speed data result

src/modelec/CMakeLists.txt

@@ -108,6 +108,13 @@ target_include_directories(lidar_controller PUBLIC
   $<INSTALL_INTERFACE:include>
 )
 
+add_executable(speed_result src/speed_result.cpp)
+ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interface)
+target_include_directories(speed_result PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 # Enable testing and linting
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -119,6 +126,7 @@ endif()
 # Install targets
 install(TARGETS
   serial_listener
+  pcb_alim_interface
   pcb_odo_interface
   pca9685_controller
   gamecontroller_listener
@@ -127,7 +135,7 @@ install(TARGETS
   solenoid_controller
   arm_controller
   button_gpio_controller
-  pcb_alim_interface
+  speed_result
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/modelec/include/modelec/speed_result.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <modelec_interface/msg/odometry_speed.hpp>
+#include <fstream>
+
+namespace Modelec
+{
+    class SpeedResultNode : public rclcpp::Node {
+    public:
+        SpeedResultNode();
+        ~SpeedResultNode() override;
+
+    private:
+
+        rclcpp::Time start_time_;
+
+        std::string fileName_;
+
+        std::ofstream file_;
+
+        rclcpp::Subscription<modelec_interface::msg::OdometrySpeed>::SharedPtr sub_speed_;
+
+        void SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg);
+    };
+}

src/modelec/launch/test.modelec.launch.yml

@@ -14,3 +14,8 @@ launch:
       pkg: 'modelec_gui'
       exec: "modelec_gui"
       name: "modelec_gui"
+
+- node:
+      pkg: 'modelec'
+      exec: 'speed_result'
+      name: 'speed_result'

src/modelec/src/pcb_odo_interface.cpp

@@ -1,4 +1,4 @@
-#include <modelec/pcb_odo_interface.h>
+#include <modelec/pcb_odo_interface.hpp>
 #include <modelec/utils.hpp>
 #include <modelec_interface/srv/add_serial_listener.hpp>
 

src/modelec/src/speed_result.cpp

@@ -0,0 +1,37 @@
+#include <modelec/speed_result.hpp>
+
+namespace Modelec {
+  SpeedResultNode::SpeedResultNode() : Node("SpeedResult"), fileName_("speed_data.csv"), file_(fileName_)
+  {
+    if (!file_.is_open()) {
+      std::cerr << "Error opening file: " << fileName_ << std::endl;
+    }
+
+    sub_speed_ = this->create_subscription<modelec_interface::msg::OdometrySpeed>(
+      "odometry/speed", 10,
+      std::bind(&SpeedResultNode::SpeedCallback, this, std::placeholders::_1));
+
+    start_time_ = this->now();
+    file_ << "time,x,y,theta" << std::endl;  // Write header to the file
+  }
+
+  SpeedResultNode::~SpeedResultNode()
+  {
+    file_.close();
+  }
+
+  void SpeedResultNode::SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg)
+  {
+    std::string time = std::to_string(this->now().nanoseconds() - start_time_.nanoseconds());
+    file_ << time << ","
+          << msg->x << "," << msg->y << "," << msg->theta << std::endl;
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Modelec::SpeedResultNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/modelec_gui/include/modelec_gui/pages/test_page.hpp

@@ -8,6 +8,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <modelec_interface/msg/odometry_pos.hpp>
+#include <modelec_interface/msg/odometry_add_waypoint.hpp>
 #include <modelec_interface/srv/odometry_speed.hpp>
 #include <modelec_interface/srv/odometry_start.hpp>
 #include <modelec_interface/srv/odometry_get_pid.hpp>
@@ -40,10 +41,14 @@ namespace ModelecGUI {
         QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
         QHBoxLayout *speedLayout_;
 
+        QPushButton *startTest_;
+
         rclcpp::Node::SharedPtr node_;
 
         rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr sub_;
 
+        rclcpp::Publisher<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr pub_add_waypoint_;
+
         // client
         rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedPtr client_;
         rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedPtr client_start_;

src/modelec_gui/src/pages/test_page.cpp

@@ -118,6 +118,27 @@ namespace ModelecGUI
         speedLayout_->addWidget(ySpeedBox_);
         speedLayout_->addWidget(thetaSpeedBox_);
 
+        startTest_ = new QPushButton("Start Test");
+        connect(startTest_, &QPushButton::clicked, this, [this]() {
+            auto firstRequest = std::make_shared<modelec_interface::msg::OdometryAddWaypoint>();
+            firstRequest->id = 0;
+            firstRequest->is_end = false;
+            firstRequest->x = 100.0;
+            firstRequest->y = 0.0;
+            firstRequest->theta = 0.0;
+
+            pub_add_waypoint_->publish(*firstRequest);
+
+            auto secondRequest = std::make_shared<modelec_interface::msg::OdometryAddWaypoint>();
+            secondRequest->id = 1;
+            secondRequest->is_end = true;
+            secondRequest->x = 0.0;
+            secondRequest->y = 0.0;
+            secondRequest->theta = 0.0;
+
+            pub_add_waypoint_->publish(*secondRequest);
+        });
+
         mainLayout_ = new QVBoxLayout(this);
         mainLayout_->addWidget(startButton_);
         mainLayout_->addLayout(posLayout_);
@@ -126,6 +147,7 @@ namespace ModelecGUI
         mainLayout_->addWidget(setPID_);
         mainLayout_->addWidget(askSpeed_);
         mainLayout_->addLayout(speedLayout_);
+        mainLayout_->addWidget(startTest_);
         setLayout(mainLayout_);
 
         // Set up subscription
@@ -133,6 +155,9 @@ namespace ModelecGUI
             "/odometry/get_position", 10,
             std::bind(&TestPage::PositionCallback, this, std::placeholders::_1));
 
+        pub_add_waypoint_ = node_->create_publisher<modelec_interface::msg::OdometryAddWaypoint>(
+            "/odometry/add_waypoint", 10);
+
         // Set up service client
         client_ = node_->create_client<modelec_interface::srv::OdometrySpeed>("odometry/speed");