Reimplement controller node in C++

launch/cartographer.py

@@ -1,5 +1,3 @@
-import sys
-
 from launch import LaunchDescription
 from launch_ros.actions import Node
 # from launch_ros.substitutions import FindPackageShare
@@ -51,8 +49,7 @@ def generate_launch_description():
     )
 
     controller_node = Node(
-        executable = sys.executable,
-        arguments=['nodes/controller.py'],
+        executable = "meson-build-debug/nodes/controller",
     )
 
     cartographer_node = Node(

meson.build

@@ -2,10 +2,20 @@ project('JetBot', 'cpp',
     default_options : [
         'cpp_std=c++20',
         'warning_level=3',  # -Wall -Wextra -Wpedantic
+        'default_library=static',
     ])
 
 inc = include_directories('inc')
 
+py = import('python').find_installation('python3.10')
+
+# https://mesonbuild.com/Wrapdb-projects.html
+# https://github.com/scipy/scipy/blob/main/scipy/meson.build#L93-L119
+# https://github.com/mesonbuild/meson/issues/4677
+pybind11 = declare_dependency(
+    include_directories : include_directories('../ext/pybind11/include'),
+)
+
 subdir('nodes')
 subdir('pywrapcc')
 subdir('tests')

nodes/controller.cc

@@ -0,0 +1,126 @@
+/**
+* Foxglove teleop panel settings
+* - Publish rate: 5.00
+* - Topic: /cmd_vel
+* - Up   : linear-x   0.56
+* - Down : linear-x  -0.56
+* - Left : angular-z  8.00
+* - Right: angular-z -8.00
+* Refer to https://foxglove.dev/docs/studio/panels/teleop
+* 
+* Alternatives
+* - ROS1: https://wiki.ros.org/diff_drive_controller
+* - ROS2: https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html
+*/
+
+#include <algorithm>
+#include <chrono>
+#include <numbers>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/embed.h>
+
+// FIXME
+inline constexpr auto MAX_RPM = 180; // Roughly measured
+inline constexpr auto MAX_MAV = MAX_RPM * 2 * std::numbers::pi / 60; // Max motor angular velocity (rad/s)
+inline constexpr auto MIN_MAV = -MAX_MAV; // Min motor angular velocity (rad/s)
+inline constexpr auto WHEEL_RADIUS = 0.03; // m
+inline constexpr auto WHEEL_SEPERATION = 0.1; // m
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+namespace py = pybind11;
+
+// TODO: https://stackoverflow.com/q/60927251/20015297
+py::scoped_interpreter guard{};
+
+
+class Controller : public rclcpp::Node {
+public:
+  Controller()
+  : Node("controller") {
+    // Period allowed between two successive velocity commands.
+    // After this delay, a zero speed command will be sent to the wheels.
+    cmd_vel_timeout_ = this->create_wall_timer(
+    300ms, std::bind(&Controller::brake, this)); // FIXME: Expose the timeout time
+
+    // FIXME: QoS profile
+    cmd_vel_ = this->create_subscription<geometry_msgs::msg::Twist>(
+      "/cmd_vel", 10, std::bind(&Controller::cmd_vel, this, _1));
+
+    duck_detected_ = this->create_subscription<std_msgs::msg::Bool>(
+      "/duck_detected", 10, std::bind(&Controller::duck_detected, this, _1));
+  }
+
+private:
+  void
+  cmd_vel(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    cmd_vel_timeout_->reset();
+
+    // https://answers.ros.org/question/185427/make-sure-geometry_msgstwist-or-cmd_vel-units/
+    auto 𝑣 = msg->linear.x; // Max 0.56 m/s (3 revolutions of the motor)
+    auto 𝜔 = msg->angular.z; // rad/s
+    auto 𝑟 = WHEEL_RADIUS;
+    auto 𝑏 = WHEEL_SEPERATION;
+
+    // https://en.wikipedia.org/wiki/Differential_wheeled_robot#Kinematics_of_Differential_Drive_Robots
+    auto 𝜔ᴸ = (𝑣 - 𝜔 * 𝑏 / 2) / 𝑟;
+    auto 𝜔ᴿ = (𝑣 + 𝜔 * 𝑏 / 2) / 𝑟;
+
+    𝜔ᴸ = std::clamp(𝜔ᴸ, MIN_MAV, MAX_MAV);
+    𝜔ᴿ = std::clamp(𝜔ᴿ, MIN_MAV, MAX_MAV);
+
+    /**
+     * FIXME: These are very basic motors, and have no built-inencoders, speed
+     * control or positional feedback. Voltage goes in, rotation goes out!
+     *
+     * Normalise to [-1, 1] range
+     * https://en.wikipedia.org/wiki/Normalization_(statistics)
+     * https://en.wikipedia.org/wiki/Feature_scaling
+     * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
+     */
+    lmotor.attr("throttle") = (𝜔ᴸ - MIN_MAV) * (1 - -1) / (MAX_MAV - MIN_MAV) + -1;
+    rmotor.attr("throttle") = (𝜔ᴿ - MIN_MAV) * (1 - -1) / (MAX_MAV - MIN_MAV) + -1;
+  }
+
+  void
+  brake() {
+    lmotor.attr("throttle") = 0.0;
+    rmotor.attr("throttle") = 0.0;
+  }
+
+  // TODO: Two fns for unsub and resub /cmd_vel
+  void
+  duck_detected(const std_msgs::msg::Bool::SharedPtr detected) {
+    if (detected->data == true) {
+      brake();
+      cmd_vel_.reset(); // Unsubscribe /cmd_vel
+    } else {
+      // Resubscribe /cmd_vel
+      cmd_vel_ = this->create_subscription<geometry_msgs::msg::Twist>(
+      "/cmd_vel", 10, std::bind(&Controller::cmd_vel, this, _1));
+    }
+  }
+
+  // https://adafruit.com/product/2927
+  // https://adafruit.com/product/3777
+  py::object MotorKit = py::module::import("adafruit_motorkit").attr("MotorKit");
+  py::object kit = MotorKit();
+  py::object lmotor = kit.attr("motor1");
+  py::object rmotor = kit.attr("motor2");
+
+  rclcpp::TimerBase::SharedPtr cmd_vel_timeout_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr duck_detected_;
+};
+
+
+auto main(int argc, char * argv[]) -> int {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Controller>());
+  rclcpp::shutdown();
+}

nodes/meson.build

@@ -3,11 +3,17 @@
 rclcpp = dependency('rclcpp', method : 'cmake')
 std_msgs = dependency('std_msgs', method : 'cmake')
 sensor_msgs = dependency('sensor_msgs', method : 'cmake')
+geometry_msgs = dependency('geometry_msgs', method : 'cmake')
 
-executable('example_publisher',
-    sources : 'example.cc',
-    # include_directories :
-    dependencies : [rclcpp, std_msgs],
+executable('controller',
+    sources : 'controller.cc',
+    dependencies : [
+        rclcpp,
+        sensor_msgs,
+        geometry_msgs,
+        # https://mesonbuild.com/Python-module.html#dependency
+        pybind11,
+        py.dependency(embed:true)],
 )
 
 executable('lsm303d_publisher',

pywrapcc/meson.build

@@ -1,19 +1,11 @@
-py = import('python').find_installation('python3.10')
-
-# https://github.com/scipy/scipy/blob/main/scipy/meson.build#L93-L119
-# https://github.com/mesonbuild/meson/issues/4677
-pybind11 = declare_dependency(
-    include_directories : include_directories('../ext/pybind11/include'),
-)
-
 py.extension_module('duppa_i2c_encoder_mini',
     sources : 'duppa_i2c_encoder_mini.cc',
     include_directories : inc,
-    dependencies : [py.dependency(), pybind11],
+    dependencies : [pybind11, py.dependency()],
 )
 
 py.extension_module('lsm303d',
     sources : 'lsm303d.cc',
     include_directories : inc,
-    dependencies : [py.dependency(), pybind11],
+    dependencies : [pybind11, py.dependency()],
 )