From 42def6262786c72bf59b555ddf552ae0ad79c996 Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Sat, 14 Mar 2026 07:39:38 +0100 Subject: [PATCH] initialized ros2 project for workstation --- pioneer_workstation/Makefile | 16 - pioneer_workstation/src/main.c | 316 ------------------ pioneer_workstation_ws/Makefile | 11 + .../src/pioneer3DX_controller/CMakeLists.txt | 29 ++ .../src/pioneer3DX_controller/package.xml | 20 ++ .../src/pioneer3DX_controller/src/main.cpp | 41 +++ .../src/pioneer3DX_controller/src/main.hpp | 9 + .../pioneer3DX_controller/src/p3dx_node.cpp | 46 +++ .../pioneer3DX_controller/src/p3dx_node.hpp | 21 ++ 9 files changed, 177 insertions(+), 332 deletions(-) delete mode 100644 pioneer_workstation/Makefile delete mode 100644 pioneer_workstation/src/main.c create mode 100644 pioneer_workstation_ws/Makefile create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/CMakeLists.txt create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/package.xml create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/src/main.cpp create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/src/main.hpp create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp create mode 100644 pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp diff --git a/pioneer_workstation/Makefile b/pioneer_workstation/Makefile deleted file mode 100644 index 5940498..0000000 --- a/pioneer_workstation/Makefile +++ /dev/null @@ -1,16 +0,0 @@ -CCFLAGS = -I../pioneer_controller/Core/Inc \ - -pedantic -Wall -Werror -Wextra \ - -Wconversion -Wdouble-promotion \ - -Wundef -Wcast-qual -Wmissing-declarations \ - -Wredundant-decls -Wno-unused-function -Wno-override-init \ - -Wno-unused-variable - -debug: CCFLAGS += -O0 -g3 -ggdb -fsanitize=address,undefined,leak \ - -fsanitize-trap -fstack-protector-strong -debug: main - -release: CCFLAGS += -O3 -s -march=native -flto -D_FORTIFY_SOURCE=2 -release: main - -main: src/main.c - $(CC) -std=gnu11 $^ -o $@ $(CCFLAGS) diff --git a/pioneer_workstation/src/main.c b/pioneer_workstation/src/main.c deleted file mode 100644 index 265c89c..0000000 --- a/pioneer_workstation/src/main.c +++ /dev/null @@ -1,316 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include "firmware/fmw_messages.h" - -// TODO(lb): implement CRC - -static int serial_open(char *portname) { - int fd = open(portname, O_RDWR | O_NOCTTY); - assert(fd >= 0); - - struct termios tty; - tcgetattr(fd, &tty); - - cfsetispeed(&tty, B115200); - cfsetospeed(&tty, B115200); - - tty.c_cflag &= (tcflag_t)~PARENB; - tty.c_cflag &= (tcflag_t)~CSTOPB; - tty.c_cflag &= (tcflag_t)~CRTSCTS; - tty.c_cflag |= CS8 | CLOCAL | CREAD; - - tty.c_lflag = 0; - tty.c_iflag = 0; - tty.c_oflag = 0; - - tty.c_cc[VMIN] = 1; - tty.c_cc[VTIME] = 0; - - sleep(2); // NOTE(lb): required to make flush work, for some reason - tcflush(fd, TCIOFLUSH); - tcsetattr(fd, TCSANOW, &tty); - - return fd; -} - -static void pprint_message(const FMW_Message *msg) { - static const char *message_types[] = { -#define X(Variant) #Variant, - FMW_MESSAGE_TYPE_VARIANTS(X) -#undef X - }; - - static const char *result_types[] = { -#define X(Variant) #Variant, - FMW_RESULT_VARIANTS(X) -#undef X - }; - - printf("FMW_Message {" - "\n header.type = %s" - "\n header.crc = %u", - message_types[msg->header.type], - msg->header.crc); - switch (msg->header.type) { - case FMW_MessageType_Response: { - printf("\n response: %s", result_types[msg->response.result]); - printf("\n delta_millis: %d", msg->response.delta_millis); - printf("\n ticks_left: %d", msg->response.ticks_left); - printf("\n ticks_right: %d", msg->response.ticks_right); - } break; - default: { - assert(0 && "unreachable"); - } break; - } - printf("\n}\n"); -} - -int main(void) { - int fd = serial_open("/dev/ttyACM0"); - assert(fd); - - FMW_Message response = {0}; - -#if 0 - FMW_Message msgs[] = { - { - .header = { - .type = FMW_MessageType_Config_Robot, - .crc = (uint32_t)-1, - }, - .config_robot = { - .baseline = 2.3f, - .wheel_circumference_left = 4.f, - .wheel_circumference_right = 3.2f, - .ticks_per_revolution_left = 250, - .ticks_per_revolution_right = 350, - }, - }, - { - .header = { - .type = FMW_MessageType_Config_PID, - .crc = (uint32_t)-1, - }, - .config_pid = { - .left = { - .proportional = 0.2f, - .integral = 0.2f, - .derivative = 0.2f, - }, - .right = { - .proportional = 0.3f, - .integral = 0.3f, - .derivative = 0.3f, - }, - .cross = { - .proportional = 0.4f, - .integral = 0.4f, - .derivative = 0.4f, - }, - }, - }, - { - .header = { - .type = FMW_MessageType_Config_Robot, - .crc = (uint32_t)-1, - }, - .config_robot = { - .baseline = 2.3f, - .wheel_circumference_left = 4.f, - .wheel_circumference_right = 3.2f, - .ticks_per_revolution_left = 250, - .ticks_per_revolution_right = 350, - }, - }, - }; - write(fd, &msgs, sizeof(msgs)); - for (int i = 0; i < 2; ++i) { - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - pprint_message(&response); - } -#elif 0 - FMW_Message msg_config_robot = { - .header = { - .type = FMW_MessageType_Config_Robot, - .crc = (uint32_t)-1, - }, - .config_robot = { - .baseline = 2.3f, - .wheel_circumference_left = 4.f, - .wheel_circumference_right = 3.2f, - .ticks_per_revolution_left = 250, - .ticks_per_revolution_right = 350, - }, - }; - FMW_Message msg_config_pid = { - .header = { - .type = FMW_MessageType_Config_PID, - .crc = (uint32_t)-1, - }, - .config_pid = { - .left = { - .proportional = 0.2f, - .integral = 0.2f, - .derivative = 0.2f, - }, - .right = { - .proportional = 0.3f, - .integral = 0.3f, - .derivative = 0.3f, - }, - .cross = { - .proportional = 0.4f, - .integral = 0.4f, - .derivative = 0.4f, - }, - }, - }; - - write(fd, &msg_config_robot, sizeof(FMW_Message)); - write(fd, &msg_config_pid, sizeof(FMW_Message)); - for (int i = 0; i < 2; ++i) { - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - pprint_message(&response); - } -#else - FMW_Message msg_config_robot = { - .header = { - .type = FMW_MessageType_Config_Robot, - .crc = (uint32_t)-1, - }, - .config_robot = { - .baseline = 2.3f, - .wheel_circumference_left = 4.f, - .wheel_circumference_right = 3.2f, - .ticks_per_revolution_left = 250, - .ticks_per_revolution_right = 350, - }, - }; - write(fd, &msg_config_robot, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - printf("\tconfig:robot\n"); - pprint_message(&response); - assert(response.header.type == FMW_MessageType_Response); - assert(response.response.result == FMW_Result_Ok); - - FMW_Message msg_config_pid = { - .header = { - .type = FMW_MessageType_Config_PID, - .crc = (uint32_t)-1, - }, - .config_pid = { - .left = { - .proportional = 0.2f, - .integral = 0.2f, - .derivative = 0.2f, - }, - .right = { - .proportional = 0.3f, - .integral = 0.3f, - .derivative = 0.3f, - }, - .cross = { - .proportional = 0.4f, - .integral = 0.4f, - .derivative = 0.4f, - }, - }, - }; - write(fd, &msg_config_pid, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - printf("\tconfig:pid\n"); - pprint_message(&response); - assert(response.response.result == FMW_Result_Ok); - - FMW_Message msg_run = { - .header = { - .type = FMW_MessageType_ModeChange_Run, - .crc = (uint32_t)-1, - }, - }; - write(fd, &msg_run, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - printf("\trun\n"); - pprint_message(&response); - assert(response.response.result == FMW_Result_Ok); - - FMW_Message msg_get_status = { - .header = { - .type = FMW_MessageType_Run_GetStatus, - .crc = (uint32_t)-1, - }, - }; - write(fd, &msg_get_status, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - printf("\trun:get_status\n"); - pprint_message(&response); - - for (int i = 0; i < 100; ++i) { - FMW_Message msg_set_speed = { - .header = { - .type = FMW_MessageType_Run_SetVelocity, - .crc = (uint32_t)-1, - }, - .run_set_velocity = { - .linear = 10.f, - .angular = 2.5f, - }, - }; - write(fd, &msg_set_speed, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - printf("\trun:set_speed\n"); - pprint_message(&response); - } - - FMW_Message msg_init = { - .header = { - .type = FMW_MessageType_ModeChange_Config, - .crc = (uint32_t)-1, - }, - }; - write(fd, &msg_init, sizeof(FMW_Message)); - for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { - ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); - if (n >= 0) { bytes_read += (uint32_t)n; } - } - assert(response.header.type == FMW_MessageType_Response); - printf("\tinit\n"); - pprint_message(&response); -#endif - - close(fd); -} diff --git a/pioneer_workstation_ws/Makefile b/pioneer_workstation_ws/Makefile new file mode 100644 index 0000000..81ed6d5 --- /dev/null +++ b/pioneer_workstation_ws/Makefile @@ -0,0 +1,11 @@ +.PHONY = build + +SHELL := /bin/bash + +run: + colcon build + source install/setup.bash && ros2 run pioneer3DX_controller Pioneer3DX_Controller + +debug: + colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo + source install/setup.bash && ros2 run --prefix 'gdb -ex start --args' pioneer3DX_controller Pioneer3DX_Controller diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/CMakeLists.txt b/pioneer_workstation_ws/src/pioneer3DX_controller/CMakeLists.txt new file mode 100644 index 0000000..afa82c8 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(pioneer3DX_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) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) + +add_executable(Pioneer3DX_Controller src/main.cpp src/p3dx_node.cpp) +ament_target_dependencies(Pioneer3DX_Controller rclcpp nav_msgs) +install(TARGETS Pioneer3DX_Controller DESTINATION lib/${PROJECT_NAME}) + +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() + +ament_package() diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/package.xml b/pioneer_workstation_ws/src/pioneer3DX_controller/package.xml new file mode 100644 index 0000000..ab2ebe4 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/package.xml @@ -0,0 +1,20 @@ + + + + pioneer3DX_controller + 0.0.0 + TODO: Package description + leo + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.cpp b/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.cpp new file mode 100644 index 0000000..9e4284d --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.cpp @@ -0,0 +1,41 @@ +#include +#include + +#include "main.hpp" +#include "p3dx_node.hpp" + +int32_t main(int32_t argc, const char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} + +[[nodiscard]] int32_t serial_open(const char *portname) +{ + int32_t fd = open(portname, O_RDWR | O_NOCTTY); + assert(fd >= 0); + + struct termios tty = {}; + tcgetattr(fd, &tty); + + cfsetispeed(&tty, B115200); + cfsetospeed(&tty, B115200); + + tty.c_cflag &= (tcflag_t)~PARENB; + tty.c_cflag &= (tcflag_t)~CSTOPB; + tty.c_cflag &= (tcflag_t)~CRTSCTS; + tty.c_cflag |= CS8 | CLOCAL | CREAD; + + tty.c_lflag = 0; + tty.c_iflag = 0; + tty.c_oflag = 0; + + tty.c_cc[VMIN] = 1; + tty.c_cc[VTIME] = 0; + + tcflush(fd, TCIOFLUSH); + tcsetattr(fd, TCSANOW, &tty); + + return fd; +} diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.hpp b/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.hpp new file mode 100644 index 0000000..fba410c --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/main.hpp @@ -0,0 +1,9 @@ +#ifndef MAIN_HPP +#define MAIN_HPP + +#include +#include + +[[nodiscard]] int32_t serial_open(const char *portname); + +#endif diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp new file mode 100644 index 0000000..661d4a0 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.cpp @@ -0,0 +1,46 @@ +#include "p3dx_node.hpp" + +using namespace std::chrono_literals; + +P3DX_Controller_Node::P3DX_Controller_Node() : + rclcpp::Node("P3DX_Controller_Node"), serial_fd(serial_open("/dev/ttyACM0")), + publisher_odometry(this->create_publisher("odometry", 10)), + subscriber_command_velocity(this->create_subscription("p3dx_command_velocity", 10, + std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity, + this, std::placeholders::_1))), + publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))) +{ + this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry")); + assert(this->serial_fd != -1); +} + +void P3DX_Controller_Node::callback_publish_odometry(void) +{ + nav_msgs::msg::Odometry output; + output.header.frame_id = this->get_parameter("frame_id_odometry").as_string(); + // output.header.stamp = ; // TODO(lb): generate some kind of current timestamp + + // output.pose.pose.position.x = ; // TODO(lb): fill with pioneer data + // output.pose.pose.position.y = ; // TODO(lb): fill with pioneer data + // output.pose.pose.orientation.x = ; // TODO(lb): fill with pioneer data + // output.pose.pose.orientation.y = ; // TODO(lb): fill with pioneer data + + // output.twist.twist.linear.x = ; // TODO(lb): fill with pioneer data + // output.twist.twist.linear.y = ; // TODO(lb): fill with pioneer data + // output.twist.twist.angular.x = ; // TODO(lb): fill with pioneer data + // output.twist.twist.angular.y = ; // TODO(lb): fill with pioneer data + + assert(output.pose.pose.position.z == 0); + assert(output.pose.pose.orientation.z == 0); + assert(output.pose.pose.orientation.w == 1); + assert(output.twist.twist.linear.z == 0); + assert(output.twist.twist.angular.z == 0); + + this->publisher_odometry->publish(output); + RCLCPP_INFO(this->get_logger(), "Hello, World!"); +} + +void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd) +{ + (void)cmd; +} diff --git a/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp new file mode 100644 index 0000000..164a193 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3DX_controller/src/p3dx_node.hpp @@ -0,0 +1,21 @@ +#ifndef P3DX_NODE_HPP +#define P3DX_NODE_HPP + +#include +#include + +#include "main.hpp" + +struct P3DX_Controller_Node: public rclcpp::Node { + const int32_t serial_fd; + rclcpp::Publisher::SharedPtr publisher_odometry; + rclcpp::Subscription::SharedPtr subscriber_command_velocity; + rclcpp::TimerBase::SharedPtr publisher_odometry_timer; + + P3DX_Controller_Node(void); + + void callback_publish_odometry(void); + void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd); +}; + +#endif -- 2.52.0