Ported over odometry and control using new serial communication

toid_control/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(library_name toid_control)
+
+set(PACKAGE_DEPS
+  rclcpp
+  pluginlib
+  hardware_interface
+  rclcpp_lifecycle
+  rclcpp_components
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+
+add_library(
+  ${library_name}
+  SHARED
+  src/toid_control.cpp
+)
+
+target_include_directories(
+  ${library_name}
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(${library_name}
+  ${PACKAGE_DEPS}
+)
+
+pluginlib_export_plugin_description_file(hardware_interface toid_control_interfaces.xml)
+
+install(
+  TARGETS ${library_name} 
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY 
+    launch 
+    params
+    rviz
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${PACKAGE_DEPS})
+
+ament_package()

toid_control/include/toid_control/toid_control.hpp

@@ -0,0 +1,45 @@
+#ifndef MG_WHEEL_INTERFACE_HPP_
+#define MG_WHEEL_INTERFACE_HPP_
+#include <vector>
+#include <string>
+
+#include "boost/asio.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace toid {
+    namespace asio = boost::asio;
+
+    class StepperInterface : public hardware_interface::SystemInterface {
+      public:
+
+        CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& info) override;
+        CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
+        CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override;
+
+        std::vector<hardware_interface::StateInterface>   export_state_interfaces() override;
+        std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+        hardware_interface::return_type read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
+        hardware_interface::return_type write(const rclcpp::Time& /*time*/,
+                                              const rclcpp::Duration& /*period*/) override;
+
+      private:
+        std::string serial_port_name_;
+        asio::io_context io_context_;
+        asio::serial_port serial_port_ = asio::serial_port(io_context_);
+
+        double left_wheel_vel_cmd_    = 0;
+        double left_wheel_pos_state_  = 0;
+        double right_wheel_vel_cmd_   = 0;
+        double right_wheel_pos_state_ = 0;
+    };
+}
+
+PLUGINLIB_EXPORT_CLASS(toid::StepperInterface, hardware_interface::SystemInterface)
+
+#endif

toid_control/launch/toid.launch.py

@@ -0,0 +1,120 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition 
+from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
+from launch_ros.actions import Node, LifecycleNode
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+
+    pkg_share = FindPackageShare("").find('toid_control')
+    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')
+
+    description_pkg_share = FindPackageShare("").find('toid_bot_description')
+    default_model_path = os.path.join(
+        description_pkg_share, 
+        'src', 
+        'toid_bot_description.urdf'
+    )
+
+    visualize = LaunchConfiguration("visualize")
+
+    visualize_arg = DeclareLaunchArgument(
+        'visualize',
+        default_value='False',
+        description="Whether to launch rviz2" 
+    )
+
+    use_mock = LaunchConfiguration("use_mock")
+
+    use_mock_arg = DeclareLaunchArgument(
+        'use_mock',
+        default_value='True',
+        description="Whether to use mock controller" 
+    )
+
+    # odom_broadcast = Node(
+    #     package='tf2_ros',
+    #     executable='static_transform_publisher',
+    #     name='map_to_odom_broadcaster',
+    #     arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+    #     condition=IfCondition(LaunchConfiguration('visualize'))
+    # )
+
+    odom_broadcast = Node(
+        package='toid_odometry',
+        executable='toid_odometry',
+        name='map_to_odom_broadcaster',
+        arguments={'use_mock': use_mock},
+        condition=IfCondition(LaunchConfiguration('visualize'))
+    )
+
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
+    )
+
+    controller_manager = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        output='screen',
+        parameters=[params]
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=["joint_state_broadcaster"]
+    )
+
+    diffbot_base_controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='both',
+        arguments=[
+            "diffdrive_controller",
+            "-p",
+            params,
+            "--controller-ros-args",
+            "-r diffdrive_controller/cmd_vel:=/cmd_vel",
+            "--controller-ros-args",
+            "-r diffdrive_controller/odom:=/odom",
+            "--controller-ros-args",
+            IfElseSubstitution(use_mock, 
+                "--param odom_frame_id:=odom", 
+                "--param odom_frame_id:=odom_expected"
+            ),
+            "--controller-ros-args",
+            IfElseSubstitution(use_mock, 
+                "--param enable_odom_tf:=true", 
+                "--param enable_odom_tf:=false"
+            ),
+        ]
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', default_rviz_config_path],
+        condition=IfCondition(visualize)
+    )
+
+    return LaunchDescription([
+        visualize_arg,
+        use_mock_arg,
+        odom_broadcast,
+        robot_state_publisher,
+        controller_manager,
+        joint_state_broadcaster,
+        diffbot_base_controller,
+        rviz_node
+    ])

toid_control/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_control</name>
+  <version>0.0.1</version>
+  <description>Ros2 control compatible drivers for magrob</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ros2launch</depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp_components</depend>
+  <depend>pluginlib </depend>
+
+  <depend>ros2_control</depend>
+  <depend>hardware_interface</depend>
+
+  <depend>controller_manager</depend>
+  <depend>diff_drive_controller</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>joint_state_broadcaster</depend>
+
+  <depend>boost</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_control/params/toid_general_params.yaml

@@ -0,0 +1,22 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 200
+
+    diffdrive_controller:
+      type: diff_drive_controller/DiffDriveController
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+diffdrive_controller:
+  ros__parameters:
+    left_wheel_names: ["drivewhl_l_joint"]
+    right_wheel_names: ["drivewhl_r_joint"]
+    
+    enable_odom_tf: true 
+    odom_frame_id: odom
+    base_frame_id: base_footprint
+
+    open_loop: true 
+
+    wheel_separation: 0.192
+    wheel_radius: 0.032

toid_control/rviz/rviz.rviz

@@ -0,0 +1,194 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 1144
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        drivewhl_l_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        drivewhl_r_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_caster:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_caster:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.1797966957092285
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.1635823249816895
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1662
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f000000574fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000005740000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000574fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000005740000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000005efc0100000002fb0000000800540069006d0065010000000000000b400000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000007dd0000057400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2880
+  X: 0
+  Y: 64

toid_control/src/toid_control.cpp

@@ -0,0 +1,87 @@
+#include "toid_control/toid_control.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include <cmath>
+
+CallbackReturn toid::StepperInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& info) {
+    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+        return CallbackReturn::ERROR;
+    }
+
+    if (info_.hardware_parameters.find("device_path") != info.hardware_info.hardware_parameters.end()) {
+        serial_port_name_ = info_.hardware_parameters["device_path"];
+    } else {
+        serial_port_name_ = "/dev/ttyACM0";
+    }
+
+    left_wheel_pos_state_  = 0;
+    right_wheel_pos_state_ = 0;
+    left_wheel_vel_cmd_    = 0;
+    right_wheel_vel_cmd_   = 0;
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn toid::StepperInterface::on_configure(const rclcpp_lifecycle::State&) {
+    try {
+        serial_port_.open(serial_port_name_);
+    } catch (const std::exception& er) {
+        RCLCPP_ERROR(rclcpp::get_logger("StepperInterface"),
+                     "Failed to open serial port (Is the stepper driver plugged in)");
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn toid::StepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
+    if (serial_port_.is_open()) {
+        boost::system::error_code ec;
+        ec = serial_port_.close(ec);
+    }
+    return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> toid::StepperInterface::export_state_interfaces() {
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+    state_interfaces.reserve(2);
+    state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state_);
+    state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state_);
+
+    return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> toid::StepperInterface::export_command_interfaces() {
+    std::vector<hardware_interface::CommandInterface> command_interfaces;
+    command_interfaces.reserve(2);
+    command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd_);
+    command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd_);
+
+    return command_interfaces;
+}
+
+hardware_interface::return_type toid::StepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
+    return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
+    using namespace asio::buffer_literals;
+
+    try {
+        double values[] = {
+            -left_wheel_vel_cmd_ / (2 * M_PI),
+            right_wheel_vel_cmd_ / (2 * M_PI),
+        };
+        asio::write(serial_port_, "s;"_buf);
+        asio::write(serial_port_, asio::buffer(values, sizeof(values)));
+    } catch (const std::runtime_error& e) { 
+        RCLCPP_ERROR_THROTTLE(
+            this->get_logger(), 
+            *this->get_clock(), 
+            5000, 
+            "Failed to write to serial port: %s",
+            e.what()
+        );
+        boost::system::error_code ec;
+        ec = serial_port_.close(ec);
+        ec = serial_port_.open(serial_port_name_, ec);
+    }
+    return hardware_interface::return_type::OK;
+}

toid_control/toid_control_interfaces.xml

@@ -0,0 +1,7 @@
+    <library path="toid_control">
+        <class 
+            name="toid_control/StepperInterface"
+            type="toid::StepperInterface" 
+            base_class_type="hardware_interface::SystemInterface"
+        />
+    </library>