change mode service setup

diff --git a/pioneer_workstation_ws/Makefile b/pioneer_workstation_ws/Makefile
index 51d7667393d6e4188ea6381cbdfcc9b1892636fc..15a10f4d5b0efa5d8d06d90cdbaa0b8d3736914d 100644 (file)
--- 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 66599ccdebef3cdaabd481f4fdc01ed8bde6cfd3..ee8dec139bfa65081e8f6727bbec8ad36d88ece2 100644 (file)
--- 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 1b6afdec4d0b3d65a1ffabab6cc0209fb5bfcea1..b70614419465b7cf33f6308207b9f7a2b7b405a6 100644 (file)
--- 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 96d0a482f9283666484e7f9b1048751c4610383d..536b99b3b34d423d9fe7bf16c815e644c4699e13 100644 (file)
--- 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 23edd9d0e1cbe77b3d8ed843b06fe0f26a8aec09..f8e01f1c832e2f6fc5d6eefcc8f6b39872800098 100644 (file)
--- 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 9e4284da4085f702606fdf0e636a2c7f97628a19..6e4819b954fb176698ffe1e899f395714aa8189b 100644 (file)
--- 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_Node> p3dx_controller = nullptr;
+
 int32_t main(int32_t argc, const char *argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<P3DX_Controller_Node>());
+  p3dx_controller = std::make_shared<P3DX_Controller_Node>();
+  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 fb1e4f6b9459a5ba800fe5faa93d4809a87cc5e5..bfc315743c4086c25c891a35c2b2438ed1c305ee 100644 (file)
--- 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<nav_msgs::msg::Odometry>("odometry", 10)),
-  subscriber_command_velocity(this->create_subscription<geometry_msgs::msg::Twist>("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<geometry_msgs::msg::Twist>("p3dx_command_velocity", 10,
+                                                                           std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity,
+                                                                                     this, std::placeholders::_1))),
+  service_change_mode(this->create_service<pioneer3dx_controller::srv::ChangeMode>("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<pioneer3dx_controller::srv::ChangeMode::Request> request,
+                                                                  std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Response> 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 f860f6c4a9d52e7b46ca02814bd5fbec262cedfa..94fb4f4fad06fa4b524c84d7cef34484ce4a6b94 100644 (file)
--- 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 <memory>
+
 #include <rclcpp/rclcpp.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <pioneer3dx_controller/srv/change_mode.hpp>
+#include <pioneer3dx_controller/srv/config_pid.hpp>
+#include <pioneer3dx_controller/srv/config_led.hpp>
+#include <pioneer3dx_controller/srv/config_robot.hpp>
+
 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<nav_msgs::msg::Odometry>::SharedPtr publisher_odometry;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscriber_command_velocity;
-  rclcpp::TimerBase::SharedPtr publisher_odometry_timer;
   bool stm32_running;
 
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr                 publisher_odometry;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr            subscriber_velocity;
+  rclcpp::Service<pioneer3dx_controller::srv::ChangeMode>::SharedPtr    service_change_mode;
+  rclcpp::Service<pioneer3dx_controller::srv::ConfigPid>::SharedPtr     service_config_pid;
+  rclcpp::Service<pioneer3dx_controller::srv::ConfigLed>::SharedPtr     service_config_led;
+  rclcpp::Service<pioneer3dx_controller::srv::ConfigRobot>::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<pioneer3dx_controller::srv::ChangeMode::Request> request,
+                                                     std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Response> response);
 
   void stm32_message_print(const FMW_Message *msg);
 };
 
+extern std::shared_ptr<P3DX_Controller_Node> p3dx_controller;
 
 #endif