From 63f27e008e1ea863effcf172b011a5bc51b6dfbd Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 7 Apr 2025 16:42:05 -0400 Subject: [PATCH] speed data result --- src/modelec/CMakeLists.txt | 10 ++++- ..._odo_interface.h => pcb_odo_interface.hpp} | 0 src/modelec/include/modelec/speed_result.hpp | 26 +++++++++++++ src/modelec/launch/test.modelec.launch.yml | 5 +++ src/modelec/src/pcb_odo_interface.cpp | 2 +- src/modelec/src/speed_result.cpp | 37 +++++++++++++++++++ .../include/modelec_gui/pages/test_page.hpp | 5 +++ src/modelec_gui/src/pages/test_page.cpp | 25 +++++++++++++ 8 files changed, 108 insertions(+), 2 deletions(-) rename src/modelec/include/modelec/{pcb_odo_interface.h => pcb_odo_interface.hpp} (100%) create mode 100644 src/modelec/include/modelec/speed_result.hpp create mode 100644 src/modelec/src/speed_result.cpp diff --git a/src/modelec/CMakeLists.txt b/src/modelec/CMakeLists.txt index 54b5ffe..4360ce2 100644 --- a/src/modelec/CMakeLists.txt +++ b/src/modelec/CMakeLists.txt @@ -108,6 +108,13 @@ target_include_directories(lidar_controller PUBLIC $ ) +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 + $ + $ +) + # 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} ) diff --git a/src/modelec/include/modelec/pcb_odo_interface.h b/src/modelec/include/modelec/pcb_odo_interface.hpp similarity index 100% rename from src/modelec/include/modelec/pcb_odo_interface.h rename to src/modelec/include/modelec/pcb_odo_interface.hpp diff --git a/src/modelec/include/modelec/speed_result.hpp b/src/modelec/include/modelec/speed_result.hpp new file mode 100644 index 0000000..56bc032 --- /dev/null +++ b/src/modelec/include/modelec/speed_result.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +namespace Modelec +{ + class SpeedResultNode : public rclcpp::Node { + public: + SpeedResultNode(); + ~SpeedResultNode() override; + + private: + + rclcpp::Time start_time_; + + std::string fileName_; + + std::ofstream file_; + + rclcpp::Subscription::SharedPtr sub_speed_; + + void SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg); + }; +} \ No newline at end of file diff --git a/src/modelec/launch/test.modelec.launch.yml b/src/modelec/launch/test.modelec.launch.yml index c0d5f71..634b02c 100644 --- a/src/modelec/launch/test.modelec.launch.yml +++ b/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' diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp index 342599a..2c5f5d8 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/src/modelec/src/pcb_odo_interface.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/src/modelec/src/speed_result.cpp b/src/modelec/src/speed_result.cpp new file mode 100644 index 0000000..6780a97 --- /dev/null +++ b/src/modelec/src/speed_result.cpp @@ -0,0 +1,37 @@ +#include + +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( + "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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_page.hpp index d4ff544..6e596fe 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_page.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -40,10 +41,14 @@ namespace ModelecGUI { QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_; QHBoxLayout *speedLayout_; + QPushButton *startTest_; + rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_add_waypoint_; + // client rclcpp::Client::SharedPtr client_; rclcpp::Client::SharedPtr client_start_; diff --git a/src/modelec_gui/src/pages/test_page.cpp b/src/modelec_gui/src/pages/test_page.cpp index 5a297ce..51a50af 100644 --- a/src/modelec_gui/src/pages/test_page.cpp +++ b/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(); + 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(); + 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( + "/odometry/add_waypoint", 10); + // Set up service client client_ = node_->create_client("odometry/speed");