From e55ff8cf757530978e8fc09548b48e08228be2b7 Mon Sep 17 00:00:00 2001 From: Vincent Belpois Date: Fri, 19 Jan 2024 09:26:40 +0100 Subject: [PATCH] Added homemade controller to not user ros2control --- src/homemade_controller/CMakeLists.txt | 60 +++++++++ .../forward_kinematics.hpp | 12 ++ .../homemade_controller/rppico_comms.hpp | 121 ++++++++++++++++++ src/homemade_controller/package.xml | 20 +++ .../src/homemade_controller.cpp | 84 ++++++++++++ 5 files changed, 297 insertions(+) create mode 100644 src/homemade_controller/CMakeLists.txt create mode 100644 src/homemade_controller/include/homemade_controller/forward_kinematics.hpp create mode 100644 src/homemade_controller/include/homemade_controller/rppico_comms.hpp create mode 100644 src/homemade_controller/package.xml create mode 100644 src/homemade_controller/src/homemade_controller.cpp diff --git a/src/homemade_controller/CMakeLists.txt b/src/homemade_controller/CMakeLists.txt new file mode 100644 index 0000000..affc653 --- /dev/null +++ b/src/homemade_controller/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.8) +project(homemade_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + geometry_msgs + rclcpp +) + + + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + + +# install +install(DIRECTORY include/ + DESTINATION include +) + + +add_executable(homemade_controller src/homemade_controller.cpp) +ament_target_dependencies(homemade_controller + rclcpp + geometry_msgs + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +target_link_libraries(homemade_controller serial) + +install(TARGETS + homemade_controller + DESTINATION lib/${PROJECT_NAME} +) + + +ament_package() diff --git a/src/homemade_controller/include/homemade_controller/forward_kinematics.hpp b/src/homemade_controller/include/homemade_controller/forward_kinematics.hpp new file mode 100644 index 0000000..c5173b4 --- /dev/null +++ b/src/homemade_controller/include/homemade_controller/forward_kinematics.hpp @@ -0,0 +1,12 @@ +#ifndef FORWARD_KINEMATICS_HPP +#define FORWARD_KINEMATICS_HPP + +/* +forward kinematics of our robot +*/ + + + + + +#endif // FORWARD_KINEMATICS_HPP \ No newline at end of file diff --git a/src/homemade_controller/include/homemade_controller/rppico_comms.hpp b/src/homemade_controller/include/homemade_controller/rppico_comms.hpp new file mode 100644 index 0000000..a7a16ad --- /dev/null +++ b/src/homemade_controller/include/homemade_controller/rppico_comms.hpp @@ -0,0 +1,121 @@ +#ifndef OMNIDRIVE_RPPICO__OMNIBOT_COMMS_HPP_ +#define OMNIDRIVE_RPPICO__OMNIBOT_COMMS_HPP_ + +#include +#include +#include + + +LibSerial::BaudRate convert_baud_rate(int baud_rate) +{ + // Just handle some common baud rates + switch (baud_rate) + { + case 1200: return LibSerial::BaudRate::BAUD_1200; + case 1800: return LibSerial::BaudRate::BAUD_1800; + case 2400: return LibSerial::BaudRate::BAUD_2400; + case 4800: return LibSerial::BaudRate::BAUD_4800; + case 9600: return LibSerial::BaudRate::BAUD_9600; + case 19200: return LibSerial::BaudRate::BAUD_19200; + case 38400: return LibSerial::BaudRate::BAUD_38400; + case 57600: return LibSerial::BaudRate::BAUD_57600; + case 115200: return LibSerial::BaudRate::BAUD_115200; + case 230400: return LibSerial::BaudRate::BAUD_230400; + default: + std::cout << "Error! Baud rate " << baud_rate << " not supported! Default to 57600" << std::endl; + return LibSerial::BaudRate::BAUD_57600; + } +} + +class RpPicoComs +{ + +public: + + RpPicoComs() = default; + + void connect(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) + { + timeout_ms_ = timeout_ms; + serial_conn_.Open(serial_device); + serial_conn_.SetBaudRate(convert_baud_rate(baud_rate)); + } + + void disconnect() + { + serial_conn_.Close(); + } + + bool connected() const + { + return serial_conn_.IsOpen(); + } + + + std::string send_msg(const std::string &msg_to_send, bool print_output = false) + { + serial_conn_.FlushIOBuffers(); // Just in case + serial_conn_.Write(msg_to_send); + + std::string response = ""; + try + { + // Responses end with \r\n so we will read up to (and including) the \n. + serial_conn_.ReadLine(response, '\n', timeout_ms_); + } + catch (const LibSerial::ReadTimeout&) + { + std::cerr << "The ReadByte() call has timed out." << std::endl ; + } + + if (print_output) + { + std::cout << "Sent: " << msg_to_send << " Recv: " << response << std::endl; + } + + return response; + } + + + void send_empty_msg() + { + std::string response = send_msg("\r"); + } + + void read_encoder_values(int &val_1, int &val_2,int &val_3, int &val_4) + { + std::string response = send_msg("e\r"); + + std::string delimiter = " "; + size_t del_pos = response.find(delimiter); + std::string token_1 = response.substr(0, del_pos); + std::string token_2 = response.substr(del_pos + delimiter.length()); + std::string token_3 = response.substr(del_pos + delimiter.length()); + std::string token_4 = response.substr(del_pos + delimiter.length()); + + val_1 = std::atoi(token_1.c_str()); + val_2 = std::atoi(token_2.c_str()); + val_3 = std::atoi(token_3.c_str()); + val_4 = std::atoi(token_4.c_str()); + + } + void set_motor_values(int val_1, int val_2, int val_3, int val_4) + { + std::stringstream ss; + ss << "m " << val_1 << " " << val_2 << " " << val_3 << " " << val_4 << "\r"; + send_msg(ss.str()); + } + + void set_pid_values(int k_p, int k_d, int k_i, int k_o) + { + std::stringstream ss; + ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r"; + send_msg(ss.str()); + } + +private: + LibSerial::SerialPort serial_conn_; + int timeout_ms_; +}; + +#endif // OMNIDRIVE_RPPICO__OMNIBOT_SYSTEM_HPP_ \ No newline at end of file diff --git a/src/homemade_controller/package.xml b/src/homemade_controller/package.xml new file mode 100644 index 0000000..b4f2aff --- /dev/null +++ b/src/homemade_controller/package.xml @@ -0,0 +1,20 @@ + + + + homemade_controller + 0.0.0 + TODO: Package description + vincent + TODO: License declaration + + ament_cmake + + libserial-dev + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/homemade_controller/src/homemade_controller.cpp b/src/homemade_controller/src/homemade_controller.cpp new file mode 100644 index 0000000..47dad80 --- /dev/null +++ b/src/homemade_controller/src/homemade_controller.cpp @@ -0,0 +1,84 @@ + +// rasperry pi pico communications +#include "homemade_controller/rppico_comms.hpp" + +// ros2 +#include +#include + + +using namespace std::chrono_literals; + +/* +This node is responsible for getting the cmd_vel (from any source) and converting it to speed orders for the rasoerry pi pico +and then sending it to the pico via serial. +Parameters (wheel radius, wheel base, max speed, max acceleration, max deceleration, max angular speed, max angular acceleration, max angular deceleration, +update rate) + are stored in a yaml file. It is given in the launch file. + +*/ + + + +class HomemadeController : public rclcpp::Node +{ + public: + HomemadeController() + : Node("HomemadeController"){ + // get parameters from the command line arguments or the yaml file + this->declare_parameter("wheel_diameter", 0.06); + this->declare_parameter("wheel_base", 0.25); + this->declare_parameter("max_speed", 1.0); + this->declare_parameter("max_acceleration", 1.0); + this->declare_parameter("max_deceleration", 1.0); + this->declare_parameter("max_angular_speed", 1.0); // rad.s-1 + this->declare_parameter("max_angular_acceleration", 1.0); + this->declare_parameter("max_angular_deceleration", 1.0); + this->declare_parameter("update_rate", 50); //Hz + this->declare_parameter("serial_port", "/dev/ttyACM0"); + this->declare_parameter("serial_baudrate", "115200"); + + + timer_ = this->create_wall_timer( + 1000ms, std::bind(&HomemadeController::timer_callback, this)); + + subscription_ = this->create_subscription( + "cmd_vel", 10, std::bind(&HomemadeController::cmd_vel_callback, this, std::placeholders::_1)); + + } + + void timer_callback(){ + //print the latests /cmd_vel topic values in the console for now + RCLCPP_INFO(this->get_logger(), "Latest heard: '%f'", linear_x); + } + + void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg){ + // convert the twist message to speed orders for the pico + // send the speed orders to the pico + // print the speed orders in the console for now + RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg->linear.x); + RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg->angular.z); + linear_x = msg->linear.x; + + + + } + + private: + rclcpp::TimerBase::SharedPtr timer_; + size_t count_; + RpPicoComs rppico_comms_; + rclcpp::Subscription::SharedPtr subscription_; + double linear_x; + + +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file