From: LeonardoBizzoni Date: Sun, 15 Mar 2026 09:04:09 +0000 (+0100) Subject: change mode service setup X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=52348097834cf2bdcc4ecda98b25397d9c7c59d9;p=pioneer-stm32 change mode service setup --- diff --git a/pioneer_workstation_ws/Makefile b/pioneer_workstation_ws/Makefile index 51d7667..15a10f4 100644 --- a/pioneer_workstation_ws/Makefile +++ b/pioneer_workstation_ws/Makefile @@ -1,11 +1,21 @@ -.PHONY = build +.PHONY: build SHELL := /bin/bash +build: + TERM=dumb colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo + run: colcon build source install/setup.bash && ros2 run pioneer3dx_controller Pioneer3dx_Controller -debug: - colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo +debug: build source install/setup.bash && ros2 run --prefix 'gdb -ex start --args' pioneer3dx_controller Pioneer3dx_Controller + +mode_config: + ros2 service call /p3dx_command_change_mode pioneer3dx_controller/srv/ChangeMode "{mode: 1}" +mode_run: + ros2 service call /p3dx_command_change_mode pioneer3dx_controller/srv/ChangeMode "{mode: 2}" +mode_invalid: + ros2 service call /p3dx_command_change_mode pioneer3dx_controller/srv/ChangeMode "{mode: 3}" + ros2 service call /p3dx_command_change_mode pioneer3dx_controller/srv/ChangeMode "{mode: 0}" diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ChangeMode.srv b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ChangeMode.srv index 66599cc..ee8dec1 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ChangeMode.srv +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ChangeMode.srv @@ -1,3 +1,3 @@ -byte mode +uint8 mode --- -byte status_response \ No newline at end of file +uint8 status_response \ No newline at end of file diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigLed.srv b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigLed.srv index 1b6afde..b706144 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigLed.srv +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigLed.srv @@ -3,4 +3,4 @@ float32 voltage_orange float32 voltage_hysteresis uint32 update_period --- -byte status_response +uint8 status_response diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigPid.srv b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigPid.srv index 96d0a48..536b99b 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigPid.srv +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigPid.srv @@ -2,4 +2,4 @@ float32[3] left float32[3] right float32[3] cross --- -byte status_response \ No newline at end of file +uint8 status_response \ No newline at end of file diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigRobot.srv b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigRobot.srv index 23edd9d..f8e01f1 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigRobot.srv +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/services/ConfigRobot.srv @@ -4,4 +4,4 @@ float32 wheel_circumference_right uint32 ticks_per_revolution_left uint32 ticks_per_revolution_right --- -byte status_response \ No newline at end of file +uint8 status_response \ No newline at end of file diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp index 9e4284d..6e4819b 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/main.cpp @@ -4,10 +4,14 @@ #include "main.hpp" #include "p3dx_node.hpp" +std::shared_ptr p3dx_controller = nullptr; + int32_t main(int32_t argc, const char *argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + p3dx_controller = std::make_shared(); + assert(p3dx_controller != nullptr); + rclcpp::spin(p3dx_controller); rclcpp::shutdown(); } diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp index fb1e4f6..bfc3157 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp @@ -1,17 +1,18 @@ #include "p3dx_node.hpp" -#include "pioneer3dx_controller/msg/change_mode.hpp" - using namespace std::chrono_literals; P3DX_Controller_Node::P3DX_Controller_Node() : - rclcpp::Node("P3DX_Controller_Node"), serial_fd(serial_open("/dev/ttyACM0")), + rclcpp::Node("P3DX_Controller_Node"), + serial_fd(serial_open("/dev/ttyACM0")), + stm32_running(false), 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))), - stm32_running(false) + subscriber_velocity(this->create_subscription("p3dx_command_velocity", 10, + std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity, + this, std::placeholders::_1))), + service_change_mode(this->create_service("p3dx_command_change_mode", + &P3DX_Controller_Node::callback_service_command_change_mode_s)), + timer_publisher_odometry(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))) { assert(this->serial_fd != -1); @@ -22,7 +23,8 @@ P3DX_Controller_Node::P3DX_Controller_Node() : FMW_Message response, msg_run; msg_run.header.type = FMW_MessageType_ModeChange_Run; msg_run.header.crc = (uint32_t)-1; - (void)::write(this->serial_fd, &msg_run, sizeof(FMW_Message)); + size_t bytes_written = ::write(this->serial_fd, &msg_run, sizeof(FMW_Message)); + (void)bytes_written; for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { ssize_t n = ::read(this->serial_fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); if (n >= 0) { bytes_read += (uint32_t)n; } @@ -40,7 +42,8 @@ void P3DX_Controller_Node::callback_publish_odometry(void) FMW_Message response, msg_get_status; msg_get_status.header.type = FMW_MessageType_Run_GetStatus; msg_get_status.header.crc = (uint32_t)-1; - (void)::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message)); + size_t bytes_written = ::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message)); + (void)bytes_written; for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { ssize_t n = ::read(this->serial_fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); if (n >= 0) { bytes_read += (uint32_t)n; } @@ -77,6 +80,31 @@ void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_ms (void)cmd; } +void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::shared_ptr request, + std::shared_ptr response) +{ + (void)response; + + RCLCPP_INFO(p3dx_controller->get_logger(), "%d", request->mode); + P3DX_Cmd_ChangeMode_Kind mode = (P3DX_Cmd_ChangeMode_Kind)request->mode; + if (mode == P3DX_Cmd_ChangeMode_Kind::None || mode >= P3DX_Cmd_ChangeMode_Kind::COUNT) { + // TODO(lb): respond with an error + RCLCPP_INFO(p3dx_controller->get_logger(), "invalid mode received"); + } else { + switch (mode) { + case P3DX_Cmd_ChangeMode_Kind::Config: { + RCLCPP_INFO(p3dx_controller->get_logger(), "switching to config mode"); + } break; + case P3DX_Cmd_ChangeMode_Kind::Run: { + RCLCPP_INFO(p3dx_controller->get_logger(), "switching to run mode"); + } break; + default: { + assert(false && "unreachable"); + } break; + } + } +} + void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg) { static const char *message_types[] = { @@ -116,5 +144,5 @@ void P3DX_Controller_Node::stm32_message_print(const FMW_Message *msg) assert(false && "unreachable"); } break; } - RCLCPP_INFO(this->get_logger(), "%s\n}", buffer); + // RCLCPP_INFO(this->get_logger(), "%s\n}", buffer); } diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp index f860f6c..94fb4f4 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp @@ -1,29 +1,51 @@ #ifndef P3DX_NODE_HPP #define P3DX_NODE_HPP +#include + #include #include +#include +#include +#include +#include + extern "C" { #include "stm32_messages.h" } #include "main.hpp" +enum P3DX_Cmd_ChangeMode_Kind: uint8_t { + None = 0, + Config = 1, + Run = 2, + COUNT = 3, +}; + 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; bool stm32_running; + rclcpp::Publisher::SharedPtr publisher_odometry; + rclcpp::Subscription::SharedPtr subscriber_velocity; + rclcpp::Service::SharedPtr service_change_mode; + rclcpp::Service::SharedPtr service_config_pid; + rclcpp::Service::SharedPtr service_config_led; + rclcpp::Service::SharedPtr service_config_robot; + rclcpp::TimerBase::SharedPtr timer_publisher_odometry; + P3DX_Controller_Node(void); void callback_publish_odometry(void); void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd); + static void callback_service_command_change_mode_s(const std::shared_ptr request, + std::shared_ptr response); void stm32_message_print(const FMW_Message *msg); }; +extern std::shared_ptr p3dx_controller; #endif