diff --git a/launch/cartographer.py b/launch/cartographer.py index 413de5a..7f92962 100644 --- a/launch/cartographer.py +++ b/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( diff --git a/meson.build b/meson.build index f64c2b7..93d57ee 100644 --- a/meson.build +++ b/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') diff --git a/nodes/controller.cc b/nodes/controller.cc new file mode 100644 index 0000000..fb4b53d --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +#include +#include + +// 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( + "/cmd_vel", 10, std::bind(&Controller::cmd_vel, this, _1)); + + duck_detected_ = this->create_subscription( + "/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( + "/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::SharedPtr cmd_vel_; + rclcpp::Subscription::SharedPtr duck_detected_; +}; + + +auto main(int argc, char * argv[]) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/nodes/controller.py b/nodes/controller.py deleted file mode 100644 index 2452bb7..0000000 --- a/nodes/controller.py +++ /dev/null @@ -1,97 +0,0 @@ -""" -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 -""" - -from math import pi - -import numpy -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy -from geometry_msgs.msg import Twist - -# https://adafruit.com/product/2927 -# https://adafruit.com/product/3777 -from adafruit_motorkit import MotorKit -_kit = MotorKit() -lmotor = _kit.motor1 -rmotor = _kit.motor2 -MAX_RPM = 180 # Roughly measured -MAX_MAV = MAX_RPM * 2 * pi / 60 # Max motor angular velocity (rad/s) -MIN_MAV = -MAX_MAV # Min motor angular velocity (rad/s) - - -class Controller(Node): - def __init__(self) -> None: - super().__init__("controller") - - self.declare_parameters( - namespace='', - parameters=[ - ("wheel_separation", 0.1), # meter - ("wheel_radius", 0.03), # meter - ("cmd_vel_timeout", 0.3)]) # second - - self.wheel_separation = self.get_parameter("wheel_separation").value - self.wheel_radius = self.get_parameter("wheel_radius").value - - # Allowed period (in s) allowed between two successive velocity commands. - # After this delay, a zero speed command will be sent to the wheels. - cmd_vel_timeout = self.get_parameter("cmd_vel_timeout").value - self.cmd_vel_timeout_timer = self.create_timer(cmd_vel_timeout, self.brake) - - self.cmd_vel_sub = self.create_subscription(Twist, "/cmd_vel", self.cmd_vel, QoSProfile(depth=10, durability=DurabilityPolicy.VOLATILE)) - - def cmd_vel(self, msg: Twist): - self.cmd_vel_timeout_timer.reset() - - # https://answers.ros.org/question/185427/make-sure-geometry_msgstwist-or-cmd_vel-units/ - 𝑣 = msg.linear.x # Max 0.56 m/s (3 revolutions of the motor) - 𝜔 = msg.angular.z # rad/s - 𝑟 = self.wheel_radius - 𝑏 = self.wheel_separation - - # https://en.wikipedia.org/wiki/Differential_wheeled_robot#Kinematics_of_Differential_Drive_Robots - 𝜔ᴸ = (𝑣 - 𝜔 * 𝑏 / 2) / 𝑟 - 𝜔ᴿ = (𝑣 + 𝜔 * 𝑏 / 2) / 𝑟 - - 𝜔ᴸ = numpy.clip(𝜔ᴸ, MIN_MAV, MAX_MAV) - 𝜔ᴿ = numpy.clip(𝜔ᴿ, MIN_MAV, MAX_MAV) - - # 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 - 𝜔ᴸ = (𝜔ᴸ - MIN_MAV) * (1 - -1) / (MAX_MAV - MIN_MAV) + -1 - 𝜔ᴿ = (𝜔ᴿ - MIN_MAV) * (1 - -1) / (MAX_MAV - MIN_MAV) + -1 - - # FIXME: These are very basic motors, and have no built-inencoders,speed - # control or positional feedback. Voltage goes in, rotation goes out! - lmotor.throttle = 𝜔ᴸ - rmotor.throttle = -𝜔ᴿ - - def brake(self): - lmotor.throttle = 0.0 - rmotor.throttle = 0.0 - -def process(args=None): - rclpy.init(args=args) - teleop = Controller() - rclpy.spin(teleop) - teleop.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - process() diff --git a/nodes/example.cc b/nodes/example.cc deleted file mode 100644 index c5dbdef..0000000 --- a/nodes/example.cc +++ /dev/null @@ -1,42 +0,0 @@ -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -class ExamplePublisher : public rclcpp::Node -{ -public: - ExamplePublisher() - : Node("example"), count_(0) - { - publisher_ = this->create_publisher("example", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&ExamplePublisher::timer_callback, this)); - } - -private: - void timer_callback() - { - auto msg = std_msgs::msg::String(); - msg.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str()); - publisher_->publish(msg); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/nodes/meson.build b/nodes/meson.build index 8ce8510..fc712d7 100644 --- a/nodes/meson.build +++ b/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', diff --git a/pywrapcc/meson.build b/pywrapcc/meson.build index ab3709b..f463c15 100644 --- a/pywrapcc/meson.build +++ b/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()], )