├── roboteq.repos
├── config
    └── roboteq.yaml
├── package.xml
├── LICENSE
├── launch
    └── roboteq_ros2_driver.launch.py
├── CMakeLists.txt
├── README.md
├── include
    └── roboteq_ros2_driver
    │   └── roboteq_ros2_driver.hpp
└── src
    └── driver_dev.cpp


/roboteq.repos:
--------------------------------------------------------------------------------
 1 | repositories:
 2 |   ros2_serial:
 3 |     type: git
 4 |     url: https://github.com/CJdev99/serial.git
 5 |     version: ros2
 6 |   ros2_serial_utils:
 7 |     type: git
 8 |     url: https://github.com/wjwwood/serial_utils.git
 9 |     version: ros2    
10 | 


--------------------------------------------------------------------------------
/config/roboteq.yaml:
--------------------------------------------------------------------------------
 1 | roboteq_ros2_driver:
 2 |   ros__parameters:
 3 |     pub_odom_tf: false
 4 |     odom_frame: "odom"
 5 |     base_frame: "base_link"
 6 |     cmdvel_topic: "cmdvel"
 7 |     odom_topic: "odom"
 8 |     port: "/dev/ttyACM0"
 9 |     baud: 115200
10 |     open_loop: true
11 |     wheel_circumference: 0.3192
12 |     track_width: 0.85
13 |     encoder_ppr: 1024
14 |     encoder_cpr: 4096
15 |     max_amps: 5
16 |     max_rpm: 100
17 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>roboteq_ros2_driver</name>
 5 |   <version>0.0.0</version>
 6 |   <description>TODO: Package description</description>
 7 |   <maintainer email="cjdevitt@gmail.com">chase</maintainer>
 8 |   <license>TODO: License declaration</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <depend>io_context</depend>
13 |   <depend>rclcpp</depend>
14 |   <depend>std_msgs</depend>
15 |   <depend>sensor_msgs</depend>
16 |   <depend>nav_msgs</depend>
17 |   <depend>geometry_msgs</depend>
18 |   <depend>tf2_geometry_msgs</depend>
19 |   <depend>tf2_ros</depend>
20 |   <depend>tf2</depend>
21 |   <build_depend>serial</build_depend>
22 |   <test_depend>ament_lint_auto</test_depend>
23 |   <test_depend>ament_lint_common</test_depend>
24 | 
25 |   <test_depend>ament_lint_auto</test_depend>
26 |   <test_depend>ament_lint_common</test_depend>
27 | 
28 |   <export>
29 |     <build_type>ament_cmake</build_type>
30 |   </export>
31 | </package>
32 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2023 Chase Devitt
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/launch/roboteq_ros2_driver.launch.py:
--------------------------------------------------------------------------------
 1 | # MIT License
 2 | #
 3 | # Copyright (c) []
 4 | #
 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | # of this software and associated documentation files (the "Software"), to deal
 7 | # in the Software without restriction, including without limitation the rights
 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | # copies of the Software, and to permit persons to whom the Software is
10 | # furnished to do so, subject to the following conditions:
11 | #
12 | # The above copyright notice and this permission notice shall be included in all
13 | # copies or substantial portions of the Software.
14 | #
15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | # SOFTWARE.
22 | 
23 | import os
24 | from ament_index_python.packages import get_package_share_directory
25 | from launch import LaunchDescription
26 | from launch_ros.actions import Node
27 | 
28 | 
29 | def generate_launch_description():
30 |     # ROS packages
31 |     pkg_roboteq = get_package_share_directory('roboteq_ros2_driver')
32 | 
33 |     # Config
34 |     roboteq_config = os.path.join(pkg_roboteq, 'config/roboteq',
35 |         'roboteq.yaml')
36 | 
37 |     # Nodes
38 |     roboteq_ros2_driver = Node(
39 |         package='roboteq_ros2_driver',
40 |         executable='driver_dev',
41 |         name='roboteq_ros2_driver',
42 |         output='screen',
43 |         parameters=[roboteq_config],
44 |     )
45 | 
46 |     return LaunchDescription([
47 |         # Nodes
48 |         roboteq_ros2_driver,
49 |     ])
50 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | project(roboteq_ros2_driver)
 3 | 
 4 | # Default to C99
 5 | if(NOT CMAKE_C_STANDARD)
 6 |   set(CMAKE_C_STANDARD 99)
 7 | endif()
 8 | 
 9 | # Default to C++14
10 | if(NOT CMAKE_CXX_STANDARD)
11 |   set(CMAKE_CXX_STANDARD 17)
12 | endif()
13 | 
14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
15 |   add_compile_options(-Wall -Wextra -Wpedantic)
16 | endif()
17 | 
18 | # find dependencies
19 | find_package(ament_cmake REQUIRED)
20 | find_package(rclcpp REQUIRED)
21 | find_package(sensor_msgs REQUIRED)
22 | find_package(std_msgs REQUIRED)
23 | find_package(geometry_msgs REQUIRED)
24 | find_package(nav_msgs REQUIRED)
25 | find_package(tf2 REQUIRED)
26 | find_package(tf2_ros REQUIRED)
27 | find_package(tf2_geometry_msgs REQUIRED)
28 | find_package(serial REQUIRED)
29 | find_package(Boost 1.71 CONFIG REQUIRED COMPONENTS thread)
30 | # uncomment the following section in order to fill in
31 | # further dependencies manually.
32 | # find_package(<dependency> REQUIRED)
33 | 
34 | #add_executable(roboteq_diff_driver src/driver.cpp)
35 | add_executable(roboteq_ros2_driver src/driver_dev.cpp)
36 | 
37 | ament_target_dependencies(
38 |   roboteq_ros2_driver serial
39 |   rclcpp
40 |   sensor_msgs
41 |   std_msgs
42 |   geometry_msgs
43 |   nav_msgs
44 |   tf2
45 |   tf2_ros
46 |   tf2_geometry_msgs
47 | )
48 | 
49 | target_include_directories(roboteq_ros2_driver PUBLIC
50 |   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
51 |   $<INSTALL_INTERFACE:include>)
52 | 
53 | install(
54 |   DIRECTORY launch config
55 |   DESTINATION share/${PROJECT_NAME}/
56 | )
57 | 
58 | install(TARGETS roboteq_ros2_driver
59 |   DESTINATION lib/${PROJECT_NAME})
60 |   
61 | if(BUILD_TESTING)
62 |   find_package(ament_lint_auto REQUIRED)
63 |   # the following line skips the linter which checks for copyrights
64 |   # uncomment the line when a copyright and license is not present in all source files
65 |   #set(ament_cmake_copyright_FOUND TRUE)
66 |   # the following line skips cpplint (only works in a git repo)
67 |   # uncomment the line when this package is not in a git repo
68 |   #set(ament_cmake_cpplint_FOUND TRUE)
69 |   ament_lint_auto_find_test_dependencies()
70 | endif()
71 | 
72 | ament_package()


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # roboteq_ros2_driver
 2 | 
 3 | 
 4 | ROS2 driver for the Roboteq SDC21xx, HDC24xx family of motor controllers in a differential-drive configuration.
 5 | Initially developed for SDC21xx and HDC24xx, but could work with other roboteq dual-channel motor drivers.
 6 | 
 7 | Subscribes to cmd_vel, publishes to odom
 8 | 
 9 | 
10 | Does not require any MicroBasic script to operate.
11 | 
12 | ## Usage
13 | 
14 | Clone to src directory of ros2 workspace, then `colcon build` 
15 | 
16 | Requires serial package, which is not available as deb in ROS2. If not already installed, install ros2 branch of serial:
17 | 
18 | Get the code:
19 |     
20 |     git clone -b ros2 https://github.com/SunnyApp-Robotics/serial.git
21 | 
22 | Build:
23 | 
24 |     make
25 | 
26 | Install:
27 | 
28 |     make install
29 |     
30 |     
31 | Sample launch files in roboteq_ros2_driver/launch, or run `ros2 run roboteq_ros2_driver roboteq_ros2_driver`
32 | 
33 | ## Motor Power Connections
34 | 
35 | This driver assumes right motor is connected to channel 1 (M1) of motor controller, and left motor is connected to channel 2 (M2). It also assumes a positive speed command will result in forward motion of each motor. Best to test motor directions using the roboteq utility software.
36 | 
37 | 
38 | ## TODO
39 | 
40 | - [X] Initial ROS2 release with motor commands and odometry stream
41 | - [ ] Implement transform broadcasting with tf2
42 | - [ ] Add roboteq/voltage, roboteq/current, roboteq/energy, and roboteq/temperature publishers
43 | - [ ] Make topic names and frames configuration parameters configurable at runtime.
44 | - [ ] Make robot configuration parameters configurable at runtime.
45 | - [ ] Make motor controller device configuration parameters configurable at runtime.
46 | - [ ] Make miscellaneous motor controller configuration parameters configurable at runtime.
47 | - [ ] Implement dynamically enabled self-test mode to verify correct motor power and encoder connections and configuration.
48 | 
49 | ### Note: I do not have access to Roboteq hardware anymore - feel free to contribute!
50 | [original work for ROS1](https://github.com/ecostech/roboteq_diff_driver)
51 | ## Authors
52 | 
53 | * **Chad Attermann** - *Initial work* - [Ecos Technologies](https://github.com/ecostech)
54 | * **Chase Devitt** - *ROS2 Migration*
55 | 


--------------------------------------------------------------------------------
/include/roboteq_ros2_driver/roboteq_ros2_driver.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef ROBOTEQ_ROS2_DRIVER__ROBOTEQ_ROS2_DRIVER_HPP_
  2 | #define ROBOTEQ_ROS2_DRIVER__ROBOTEQ_ROS2_DRIVER_HPP_
  3 | 
  4 | #include <math.h>
  5 | #include <unistd.h>
  6 | 
  7 | #include <cstdio>
  8 | #include <iostream>
  9 | #include <memory>
 10 | #include <rclcpp/rclcpp.hpp>
 11 | #include <vector>
 12 | 
 13 | #include "geometry_msgs/msg/twist.hpp"
 14 | #include "std_msgs/msg/header.hpp"
 15 | #include "nav_msgs/msg/odometry.hpp"
 16 | 
 17 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 18 | 
 19 | namespace Roboteq
 20 | {
 21 | class Roboteq : public rclcpp::Node
 22 | {
 23 |   public:
 24 |   explicit Roboteq(); //(nodeOptions options?)
 25 |   ~Roboteq();
 26 | 
 27 |   private:
 28 |   // class atributes
 29 |   //rclcpp::Node::SharedPtr nh{};
 30 | 
 31 |   uint32_t starttime{};
 32 |   uint32_t hstimer{};
 33 |   uint32_t mstimer{};
 34 |   uint32_t lstimer{};
 35 |   rclcpp::TimerBase::SharedPtr timer_;
 36 | 
 37 |   // buffer for reading encoder counts
 38 |   unsigned int odom_idx{};
 39 |   char odom_buf[24]{};
 40 | 
 41 |   // toss out initial encoder readings
 42 |   char odom_encoder_toss{};
 43 | 
 44 |   int32_t odom_encoder_left{};
 45 |   int32_t odom_encoder_right{};
 46 | 
 47 |   float odom_x{};
 48 |   float odom_y{};
 49 |   float odom_yaw{};
 50 |   float odom_last_x{};
 51 |   float odom_last_y{};
 52 |   float odom_last_yaw{};
 53 | 
 54 |   uint32_t odom_last_time{};
 55 | 
 56 | 
 57 |   // settings
 58 |   bool pub_odom_tf{};
 59 |   std::string odom_frame{};
 60 |   std::string base_frame{};
 61 |   std::string cmdvel_topic{};
 62 |   std::string odom_topic{};
 63 |   std::string port{};
 64 |   int baud{};
 65 |   bool open_loop{};
 66 |   double wheel_circumference{};
 67 |   double track_width{};
 68 |   int encoder_ppr{};
 69 |   int encoder_cpr{};
 70 |   double max_amps{};
 71 |   int max_rpm{};
 72 |   // Test different odom msg memory
 73 |   //nav_msgs::msg::Odometry odom_msg{};
 74 |   nav_msgs::msg::Odometry odom_msg{};
 75 |   //geometry_msgs::msg::Twist twist_msg{};
 76 | 
 77 | 
 78 | 
 79 |     //
 80 |   // cmd_vel subscriber
 81 |   //
 82 |   void cmdvel_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg);
 83 |   void cmdvel_setup();
 84 |   void cmdvel_loop();
 85 |   void cmdvel_run();
 86 |  
 87 | 
 88 | 
 89 |   //
 90 |   // odom publisher
 91 |   //
 92 |   void odom_setup();
 93 |   void odom_stream(); 
 94 |   void odom_loop();
 95 |   //void odom_hs_run();
 96 |   void odom_ms_run();
 97 |   void odom_ls_run();
 98 |   void odom_publish();
 99 |   void connect();
100 | 
101 |   void update_parameters();
102 |   int run();
103 | 
104 |   //subscriber
105 |   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdvel_sub;
106 | 
107 |   //publisher
108 |   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
109 | 
110 | 
111 | 
112 | };
113 | 
114 | }
115 | 
116 | 
117 | 
118 | 
119 | 
120 | #endif
121 | 


--------------------------------------------------------------------------------
/src/driver_dev.cpp:
--------------------------------------------------------------------------------
  1 | #include "roboteq_ros2_driver/roboteq_ros2_driver.hpp"
  2 | 
  3 | 
  4 | #include <chrono> 
  5 | #include <functional> 
  6 | #include <memory>     
  7 | #include <string>     
  8 | 
  9 | #include "rclcpp/rclcpp.hpp"
 10 | #include "rclcpp/clock.hpp"
 11 | #include <iostream>
 12 | 
 13 | #include "std_msgs/msg/string.hpp"
 14 | 
 15 | // dependencies for ROS
 16 | #include <serial/serial.h>
 17 | #include <signal.h>
 18 | #include <string>
 19 | #include <sstream>
 20 | 
 21 | #define DELTAT(_nowtime, _thentime) ((_thentime > _nowtime) ? ((0xffffffff - _thentime) + _nowtime) : (_nowtime - _thentime))
 22 | 
 23 | 
 24 | // Define following to enable cmdvel debug output
 25 | #define _CMDVEL_DEBUG
 26 | 
 27 | 
 28 | 
 29 | 
 30 | // Define following to enable odom debug output
 31 | #define _ODOM_DEBUG
 32 | 
 33 | // Define following to publish additional sensor information; comment out to not publish (TODO: write custom roboteq messages to support other reportable data from MC
 34 | 
 35 | // #define _ODOM_SENSORS
 36 | 
 37 | // Define following to enable service for returning covariance
 38 | // #define _ODOM_COVAR_SERVER
 39 | 
 40 | #define NORMALIZE(_z) atan2(sin(_z), cos(_z))
 41 | 
 42 | #include <tf2_ros/transform_broadcaster.h>
 43 | #include <geometry_msgs/msg/transform_stamped.hpp>
 44 | #include <tf2/LinearMath/Quaternion.h>
 45 | 
 46 | 
 47 | serial::Serial controller;
 48 | uint32_t millis()
 49 | {
 50 |     auto now = std::chrono::system_clock::now();
 51 |     auto duration = now.time_since_epoch();
 52 |     return std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
 53 | }
 54 | 
 55 | namespace Roboteq
 56 | {
 57 | Roboteq::Roboteq() : Node("roboteq_ros2_driver")
 58 | // initialize parameters and variables
 59 | {
 60 |     pub_odom_tf = this->declare_parameter("pub_odom_tf", false);
 61 |     odom_frame = this->declare_parameter("odom_frame", "odom");
 62 |     base_frame = this->declare_parameter("base_frame", "base_link");
 63 |     cmdvel_topic = this->declare_parameter("cmdvel_topic", "cmd_vel");
 64 |     odom_topic = this->declare_parameter("odom_topic", "odom");
 65 |     port = this->declare_parameter("port", "/dev/ttyACM0");
 66 |     baud = this->declare_parameter("baud", 115200);
 67 |     open_loop = this->declare_parameter("open_loop", true);
 68 |     wheel_circumference = this->declare_parameter("wheel_circumference", 0.55);
 69 |     track_width = this->declare_parameter("track_width", 0.89);
 70 |     encoder_ppr = this->declare_parameter("encoder_ppr", 1024);
 71 |     encoder_cpr = this->declare_parameter("encoder_cpr", 4096);
 72 |     max_amps = this->declare_parameter("max_amps", 5.0);
 73 |     max_rpm = this->declare_parameter("max_rpm", 100);
 74 | 
 75 |     starttime = 0;
 76 |     hstimer = 0;
 77 |     mstimer = 0;
 78 |     odom_idx = 0;
 79 |     odom_encoder_toss = 5;
 80 |     odom_encoder_left = 0;
 81 |     odom_encoder_right = 0;
 82 |     odom_x = 0.0;
 83 |     odom_y = 0.0;
 84 |     odom_yaw = 0.0;
 85 |     odom_last_x = 0.0;
 86 |     odom_last_y = 0.0;
 87 |     odom_last_yaw = 0.0;
 88 |     odom_last_time = 0;
 89 |     
 90 |     //odom_msg = std::make_shared<nav_msgs::msg::Odometry>();
 91 |     odom_msg = nav_msgs::msg::Odometry();
 92 | 
 93 |     serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
 94 |     controller.setPort(port);
 95 |     controller.setBaudrate(baud);
 96 |     controller.setTimeout(timeout);
 97 |     // connect to serial port
 98 |     connect();
 99 |     // configure motor controller
100 |     cmdvel_setup();
101 |     odom_setup();
102 | //
103 | //  odom publisher
104 | //
105 |     odom_pub = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic, 1000);
106 | //
107 | // cmd_vel subscriber
108 | //
109 | 
110 |     cmdvel_sub = this->create_subscription<geometry_msgs::msg::Twist>(
111 |         cmdvel_topic, // topic name
112 |         1000,         // QoS history depth
113 |         std::bind(&Roboteq::cmdvel_callback, this, std::placeholders::_1));
114 |     using namespace std::chrono_literals;
115 |     // set odometry publishing loop timer at 10Hz
116 |     timer_ = this->create_wall_timer(10ms,std::bind(&Roboteq::run, this));
117 |     // enable modifying params at run-time
118 |     /*    
119 |     using namespace std::chrono_literals;
120 | 
121 |     param_update_timer =
122 |       this->create_wall_timer(1000ms, std::bind(&Roboteq::update_params, this));
123 |     */
124 | }
125 | 
126 | void Roboteq::update_parameters()
127 | {
128 |     RCLCPP_INFO(this->get_logger(), "Parameters updated ...");
129 |     this->get_parameter("pub_odom_tf", pub_odom_tf);
130 |     this->get_parameter("odom_frame", odom_frame);
131 |     this->get_parameter("base_frame", base_frame);
132 |     this->get_parameter("cmdvel_topic", cmdvel_topic);
133 |     this->get_parameter("odom_topic", odom_topic);
134 |     this->get_parameter("port", port);
135 |     this->get_parameter("baud", baud);
136 |     this->get_parameter("open_loop", open_loop);
137 |     this->get_parameter("wheel_circumference", wheel_circumference);
138 |     this->get_parameter("track_width", track_width);
139 |     this->get_parameter("encoder_ppr", encoder_ppr);
140 |     this->get_parameter("encoder_cpr", encoder_cpr);
141 |     this->get_parameter("max_amps", max_amps);
142 |     this->get_parameter("max_rpm", max_rpm);
143 | }
144 | 
145 | void Roboteq::connect(){
146 |     RCLCPP_INFO_STREAM(this->get_logger(),"Opening serial port on " << port << " at " << baud << "..." );
147 |     try
148 |     {
149 |         controller.open();
150 |         if (controller.isOpen())
151 |         {
152 |             RCLCPP_INFO(this->get_logger(), "Successfully opened serial port");
153 |             return; 
154 |             
155 |         }
156 |     }
157 |     catch (serial::IOException &e)
158 |     {
159 |         RCLCPP_WARN_STREAM(this->get_logger(), "serial::IOException: ");
160 |         throw;
161 |     }
162 |     RCLCPP_WARN(this->get_logger(),"Failed to open serial port");
163 |     sleep(5);
164 | 
165 | }
166 | 
167 | 
168 | void Roboteq::cmdvel_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg) // const???
169 | {
170 |     // wheel speed (m/s)
171 |     float right_speed = twist_msg->linear.x + track_width * twist_msg->angular.z / 2.0;
172 |     float left_speed = twist_msg->linear.x - track_width * twist_msg->angular.z / 2.0;
173 | 
174 |     std::stringstream right_cmd;
175 |     std::stringstream left_cmd;
176 |     
177 | 
178 |     if (open_loop)
179 |     {
180 |         // motor power (scale 0-1000)
181 |         RCLCPP_INFO_STREAM(this->get_logger(),"open loop");
182 |         int32_t right_power = right_speed / wheel_circumference * 60.0 / max_rpm * 1000.0;
183 |         int32_t left_power = left_speed / wheel_circumference * 60.0 / max_rpm * 1000.0;
184 |         /*
185 |         // set minimum to overcome friction if cmd_vel too low
186 |         if (right_power < 150 && left_power > 0){
187 |             right_power = 150
188 |         }
189 |         if (left_power < 150 && left_power > 0){
190 |             left_power = 150
191 |         }
192 |         */
193 |         
194 |         right_cmd << "!G 1 " << right_power << "\r";
195 |         left_cmd << "!G 2 " << left_power << "\r";
196 |     }
197 |     else
198 |     {
199 |         // motor speed (rpm)
200 |         int32_t right_rpm = right_speed / wheel_circumference * 60.0;
201 |         int32_t left_rpm = left_speed / wheel_circumference * 60.0;
202 |         
203 |         right_cmd << "!S 1 " << right_rpm << "\r";
204 |         left_cmd << "!S 2 " << left_rpm << "\r";
205 |     }
206 | //write cmd to motor controller
207 | #ifndef _CMDVEL_FORCE_RUN
208 |   controller.write(right_cmd.str());
209 |   controller.write(left_cmd.str());
210 |   controller.flush();
211 | #endif
212 | }
213 | void Roboteq::cmdvel_setup()
214 | {
215 |     RCLCPP_INFO(this->get_logger(), "configuring motor controller...");
216 | 
217 |     // stop motors
218 |     controller.write("!G 1 0\r");
219 |     controller.write("!G 2 0\r");
220 |     controller.write("!S 1 0\r");
221 |     controller.write("!S 2 0\r");
222 |     controller.flush();
223 | 
224 |     // disable echo
225 |     controller.write("^ECHOF 1\r");
226 |     controller.flush();
227 | 
228 |     // enable watchdog timer (1000 ms)
229 |     controller.write("^RWD 1000\r");
230 | 
231 |     // set motor operating mode (1 for closed-loop speed)
232 |     if (open_loop)
233 |     {
234 |         // open-loop speed mode
235 |         controller.write("^MMOD 1 0\r");
236 |         controller.write("^MMOD 2 0\r");
237 |     }
238 |     else
239 |     {
240 |         // closed-loop speed mode
241 |         controller.write("^MMOD 1 1\r");
242 |         controller.write("^MMOD 2 1\r");
243 |     }
244 | 
245 |     // set motor amps limit (A * 10)
246 |     std::stringstream right_ampcmd;
247 |     std::stringstream left_ampcmd;
248 |     right_ampcmd << "^ALIM 1 " << (int)(max_amps * 10) << "\r";
249 |     left_ampcmd << "^ALIM 2 " << (int)(max_amps * 10) << "\r";
250 |     controller.write(right_ampcmd.str());
251 |     controller.write(left_ampcmd.str());
252 | 
253 |     // set max speed (rpm) for relative speed commands
254 |     std::stringstream right_rpmcmd;
255 |     std::stringstream left_rpmcmd;
256 |     right_rpmcmd << "^MXRPM 1 " << max_rpm << "\r";
257 |     left_rpmcmd << "^MXRPM 2 " << max_rpm << "\r";
258 |     controller.write(right_rpmcmd.str());
259 |     controller.write(left_rpmcmd.str());
260 | 
261 |     // set max acceleration rate (2000 rpm/s * 10)
262 |     controller.write("^MAC 1 20000\r");
263 |     controller.write("^MAC 2 20000\r");
264 | 
265 |     // set max deceleration rate (2000 rpm/s * 10)
266 |     controller.write("^MDEC 1 20000\r");
267 |     controller.write("^MDEC 2 20000\r");
268 | 
269 |     // set PID parameters (gain * 10)
270 |     controller.write("^KP 1 10\r");
271 |     controller.write("^KP 2 10\r");
272 |     controller.write("^KI 1 80\r");
273 |     controller.write("^KI 2 80\r");
274 |     controller.write("^KD 1 0\r");
275 |     controller.write("^KD 2 0\r");
276 | 
277 |     // set encoder mode (18 for feedback on motor1, 34 for feedback on motor2)
278 |     controller.write("^EMOD 1 18\r");
279 |     controller.write("^EMOD 2 34\r");
280 | 
281 |     // set encoder counts (ppr)
282 |     std::stringstream right_enccmd;
283 |     std::stringstream left_enccmd;
284 |     right_enccmd << "^EPPR 1 " << encoder_ppr << "\r";
285 |     left_enccmd << "^EPPR 2 " << encoder_ppr << "\r";
286 |     controller.write(right_enccmd.str());
287 |     controller.write(left_enccmd.str());
288 | 
289 |     controller.flush();
290 | }
291 | 
292 | void Roboteq::cmdvel_loop()
293 | {
294 | }
295 | 
296 | void Roboteq::cmdvel_run()
297 | {
298 | #ifdef _CMDVEL_FORCE_RUN
299 |     if (open_loop)
300 |     {
301 |     controller.write("!G 1 100\r");
302 |     controller.write("!G 2 100\r");
303 |     }
304 |     else
305 |     {
306 |     std::stringstream right_cmd;
307 |     std::stringstream left_cmd;
308 |     right_cmd << "!S 1 " << (int)(max_rpm * 0.1) << "\r";
309 |     left_cmd << "!S 2 " << (int)(max_rpm * 0.1) << "\r";
310 |     controller.write(right_cmd.str());
311 |     controller.write(left_cmd.str());
312 |     }
313 |     controller.flush();
314 | #endif
315 | }
316 | 
317 | 
318 | void Roboteq::odom_setup()
319 | {
320 |     RCLCPP_INFO(this->get_logger(),"setting up odom...");
321 |     if (pub_odom_tf)
322 |     {
323 |         //TODO: implement tf2 broadcaster
324 |         
325 |     }
326 | 
327 |     // maybe use this-> instead of
328 | 
329 | 
330 |     odom_msg.header.stamp = this->get_clock()->now();
331 |     
332 |     odom_msg.header.frame_id = odom_frame;
333 |     odom_msg.child_frame_id = base_frame;
334 | 
335 |     // Set up the pose covariance
336 |     for (size_t i = 0; i < 36; i++)
337 |     {
338 |         odom_msg.pose.covariance[i] = 0;
339 |         odom_msg.twist.covariance[i] = 0;
340 |     }
341 | 
342 |     odom_msg.pose.covariance[7] = 0.001;
343 |     odom_msg.pose.covariance[14] = 1000000;
344 |     odom_msg.pose.covariance[21] = 1000000;
345 |     odom_msg.pose.covariance[28] = 1000000;
346 |     odom_msg.pose.covariance[35] = 1000;
347 | 
348 |     // Set up the twist covariance
349 |     odom_msg.twist.covariance[0] = 0.001;
350 |     odom_msg.twist.covariance[7] = 0.001;
351 |     odom_msg.twist.covariance[14] = 1000000;
352 |     odom_msg.twist.covariance[21] = 1000000;
353 |     odom_msg.twist.covariance[28] = 1000000;
354 |     odom_msg.twist.covariance[35] = 1000;
355 | 
356 |     // Set up the transform message: move to odom_publish
357 |     /*
358 |     tf2::Quaternion q;
359 |     q.setRPY(0, 0, odom_yaw);
360 | 
361 |     tf_msg.transform.translation.x = x;
362 |     tf_msg.transform.translation.y = y;
363 |     tf_msg.transform.translation.z = 0.0;
364 |     tf_msg.transform.rotation.x = q.x();
365 |     tf_msg.transform.rotation.y = q.y();
366 |     tf_msg.transform.rotation.z = q.z();
367 |     tf_msg.transform.rotation.w = q.w();
368 |     */
369 | 
370 |     // start encoder streaming
371 |     RCLCPP_INFO_STREAM(this->get_logger(),"covariance set");
372 |     RCLCPP_INFO_STREAM(this->get_logger(),"odometry stream starting...");
373 |     odom_stream();
374 |     
375 |     odom_last_time = millis();
376 | #ifdef _ODOM_SENSORS
377 |     current_last_time = millis();
378 | #endif
379 | }
380 | 
381 | // Odom msg streams
382 | 
383 | void Roboteq::odom_stream()
384 | {
385 | 
386 | #ifdef _ODOM_SENSORS
387 |     // start encoder and current output (30 hz)
388 |     // doubling frequency since one value is output at each cycle
389 |     //  controller.write("# C_?CR_?BA_# 17\r");
390 |     // start encoder, current and voltage output (30 hz)
391 |     // tripling frequency since one value is output at each cycle
392 |     controller.write("# C_?CR_?BA_?V_# 11\r");
393 | #else
394 |     //  // start encoder output (10 hz)
395 |     //  controller.write("# C_?CR_# 100\r");
396 |     // start encoder output (30 hz)
397 |     controller.write("# C_?CR_# 33\r");
398 | 
399 | #endif
400 |     controller.flush();
401 | }
402 | 
403 | void Roboteq::odom_loop()
404 | {
405 |     
406 |     uint32_t nowtime = millis();
407 | 
408 |     // if we haven't received encoder counts in some time then restart streaming
409 |     if (DELTAT(nowtime, odom_last_time) >= 1000)
410 |     {
411 |         odom_stream();
412 |         odom_last_time = nowtime;
413 |     }
414 | 
415 |     // read sensor data stream from motor controller
416 |     // maybe use while loop to improve cpu usage?
417 |     if (controller.available())
418 |     {
419 |         char ch = 0;
420 |         if (controller.read((uint8_t *)&ch, 1) == 0)
421 |             return;
422 |         if (ch == '\r')
423 |         {
424 |             odom_buf[odom_idx] = 0;
425 |             // CR= is encoder counts
426 |             if (odom_buf[0] == 'C' && odom_buf[1] == 'R' && odom_buf[2] == '=')
427 |             {
428 |                 unsigned int delim;
429 |                 for (delim = 3; delim < odom_idx; delim++)
430 |                 {
431 |                     if (odom_encoder_toss > 0)
432 |                     {
433 |                         --odom_encoder_toss;
434 |                         break;
435 |                     }
436 |                     if (odom_buf[delim] == ':')
437 |                     {
438 |                         odom_buf[delim] = 0;
439 |                         odom_encoder_right = (int32_t)strtol(odom_buf + 3, NULL, 10);
440 |                         odom_encoder_left = (int32_t)strtol(odom_buf + delim + 1, NULL, 10);
441 | 
442 |                         odom_publish();
443 |                         break;
444 |                     }
445 |                 }
446 |             }
447 | 
448 |             odom_idx = 0;
449 |         }
450 |         else if (odom_idx < (sizeof(odom_buf) - 1))
451 |         {
452 |             odom_buf[odom_idx++] = ch;
453 |         }
454 |     }
455 | }
456 | void Roboteq::odom_publish()
457 | {
458 | 
459 |     // determine delta time in seconds
460 |     uint32_t nowtime = millis();
461 |     float dt = (float)DELTAT(nowtime, odom_last_time) / 1000.0;
462 |     odom_last_time = nowtime;
463 | 
464 |     // determine deltas of distance and angle
465 |     float linear = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference + (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / 2.0;
466 |     //  float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width * -1.0;
467 |     float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width;
468 | 
469 |     // Update odometry
470 |     odom_x += linear * cos(odom_yaw);         // m
471 |     odom_y += linear * sin(odom_yaw);         // m
472 |     odom_yaw = NORMALIZE(odom_yaw + angular); // rad
473 | 
474 |     // Calculate velocities
475 |     float vx = (odom_x - odom_last_x) / dt;
476 |     float vy = (odom_y - odom_last_y) / dt;
477 |     float vyaw = (odom_yaw - odom_last_yaw) / dt;
478 | 
479 |     odom_last_x = odom_x;
480 |     odom_last_y = odom_y;
481 |     odom_last_yaw = odom_yaw;
482 |     // convert yaw to quat;
483 |     tf2::Quaternion tf2_quat;
484 |     tf2_quat.setRPY(0, 0, odom_yaw);
485 |     // Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
486 |     geometry_msgs::msg::Quaternion quat = tf2::toMsg(tf2_quat);
487 | 
488 | 
489 | 
490 |     //tf2::Quaternion quat = tf2::createQuaternionMsgFromYaw(odom_yaw);
491 |     // TODO: set up tf2_ros
492 |     //update odom msg
493 | 
494 |     //odom_msg->header.seq++; //? not used in ros2 ?
495 |     odom_msg.header.stamp = this->get_clock()->now();
496 |     odom_msg.pose.pose.position.x = odom_x;
497 |     odom_msg.pose.pose.position.y = odom_y;
498 |     odom_msg.pose.pose.position.z = 0.0;
499 |     odom_msg.pose.pose.orientation = quat;
500 |     odom_msg.twist.twist.linear.x = vx;
501 |     odom_msg.twist.twist.linear.y = vy;
502 |     odom_msg.twist.twist.linear.z = 0.0;
503 |     odom_msg.twist.twist.angular.x = 0.0;
504 |     odom_msg.twist.twist.angular.y = 0.0;
505 |     odom_msg.twist.twist.angular.z = vyaw;
506 |     odom_pub->publish(odom_msg);
507 |     // odom_pub.publish(odom_msg); ROS1
508 | }
509 | 
510 | int Roboteq::run()
511 | {
512 | 
513 |     starttime = millis();
514 |     hstimer = starttime;
515 |     mstimer = starttime;
516 |     lstimer = starttime;
517 |         
518 |      cmdvel_loop();
519 |      odom_loop();
520 |      cmdvel_run();
521 |     return 0;
522 | }
523 | 
524 | Roboteq::~Roboteq()
525 | {
526 | 
527 |     if (controller.isOpen()){
528 |         controller.close();
529 |     }
530 |     // rclcpp::shutdown(); // uncomment if node doesnt destroy properly
531 | 
532 | }
533 | 
534 | } // end of namespace
535 | 
536 | int main(int argc, char* argv[])
537 | {
538 | 
539 |     rclcpp::init(argc, argv);
540 |     
541 |     rclcpp::executors::SingleThreadedExecutor exec;
542 |     rclcpp::NodeOptions options;
543 |     auto node = std::make_shared<Roboteq::Roboteq>();
544 |     exec.add_node(node);
545 |     exec.spin();
546 |     rclcpp::shutdown();
547 |     return 0;
548 | 
549 |    
550 | }
551 | 


--------------------------------------------------------------------------------