modelec/ros2_roboclaw_driver
1
0
Fork 0
mirror of https://github.com/modelec/ros2_roboclaw_driver.git synced 2026-01-18 16:47:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Initial checkin

Browse Source
This commit is contained in:
Michael Wimble
2021-09-23 13:29:58 -07:00
parent eb93c82482
commit 2a1b3f2ca0
13 changed files with 2085 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
CMakeLists.txt Executable file
View File

@@ -0,0 +1,97 @@
cmake_minimum_required(VERSION 3.5)
project(ros2_roboclaw_driver)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
add_compile_options(-g)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
set(msg_files
"msg/RoboClawStatus.msg"
)
set(srv_files
"srv/ResetEncoders.srv"
)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/RoboClawStatus.msg"
"srv/ResetEncoders.srv"
DEPENDENCIES builtin_interfaces std_msgs
)
add_executable(
ros2_roboclaw_driver_node
src/motor_driver_node.cpp
src/motor_driver.cpp
src/roboclaw.cpp
)
ament_target_dependencies(
ros2_roboclaw_driver_node
rclcpp
geometry_msgs
nav_msgs
std_msgs
tf2_msgs
tf2_ros
)
rosidl_target_interfaces(
ros2_roboclaw_driver_node
${PROJECT_NAME}
"rosidl_typesupport_cpp"
)
# Install config files.
install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}/
)
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/
)
install(TARGETS
ros2_roboclaw_driver_node
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_dependencies(rosidl_default_runtime)
ament_package()

259
README.md
View File

@@ -1,2 +1,259 @@
# ros2_roboclaw_driver
A driver for the RoboClaw family of devices for use under ROS2
A driver for the RoboClaw family of devices for use under ROS2.
This driver is hard coded to use a pair of motors, presumed to be in a
differential drive configuration. In various places, they are refered to
as ***m1*** and ***m2***
This code does not publish odometry information. Wheel odometry is often so
wildly wrong that it is expected that a robot builder will derive it in some way
outside of the scope of this driver. There should be enough hooks in here to use
the wheel odometry from the RoboClaw and publish any odometry information you need.
# Prerequisites
- [ROS 2](https://docs.ros.org/en/rolling/Installation.html) (Foxy or newer)
- git
# Build from source
It is recommented to create a new overlay workspace on top of your curreent ROS 2 installation, although you could just clone this into the src directory of an existing workspace. To create an overlay workspace:
```
mkdir -p ~/ros2_roboclaw_driver/src
cd ~/rosbag_ws_src
git clone https://github.com/wimblerobotics/ros2_roboclaw_driver.git
cd ..
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
```
As with any ROS 2 overlay, you need to make the overlay known to the system. Execute the following and possibly add it to your *~/.bashrc* file.
```
source ~/ros2_roboclaw_driver/install/local_setup.bash
```
## Notes for build
- The name ***~/ros2_roboclaw_driver*** is just an example. Create whatever workspace name you want.
- By default, a debug version of the code is created. If you don't want this, in the *CmakeLists.txt* file, remove the line
```
add_compile_options(-g)
```
- The *--symlink-install* argument is optional. It makes a build where you can change, e.g., the yaml configuration file in the workspace without having to rebuild the workspace.
# ROS topics
You must specify in the configuration yaml file a topic name to be used
to publish a status message for the RoboClaw device, which are mostly values pulled from various registers on the RoboClaw device. The message is
(currently) hard-coded to be published at 20 times per second.
The specification of that message is
```
float32 m1_p
float32 m1_i
float32 m1_d
uint32 m1_qpps
int32 m1_current_speed
float32 m1_motor_current
uint32 m1_encoder_value
uint8 m1_encoder_status
float32 m2_p
float32 m2_i
float32 m2_d
uint32 m2_qpps
int32 m2_current_speed
float32 m2_motor_current
uint32 m2_encoder_value
uint8 m2_encoder_status
float32 main_battery_voltage
float32 logic_battery_voltage
float32 temperature
string error_string
```
**NOTE** The current error interpretation may not be in agreement with the
latest RoboClaw firmware.
The error_string value is an interpretation of the RoboClaw unit status and
can include the concatenation of the following strings
- [M2 Home]
- [M1 Home]
- [Temperature2 Warning]
- [Temperature Warning]
- [Main Battery Low Warning]
- [Main Battery High Warning]
- [M1 Driver Fault]
- [M2 Driver Fault]
- [Logic Battery Low Error]
- [Logic Battery High Error]
- [Main Battery High Error]
- [Temperature2 Error]
- [Temperature Error]
- [E-Stop]
- [M2 OverCurrent Warning]
- [M1 OverCurrent Warning]
There are two topics subscribed
- /cmd_vel
Messages are expected to come from the naviation stack, keyboard teleop,
joystick_teleop, etc. and are the external commands to move the motors.
A continuous and frequent stream of commands is expected.
When a motor movement command is issued to the RoboClaw by the driver,
the maximum possible distance of travel is computed using the commanded
velocity and the
yaml configuration parameter ***max_seconds_uncommanded_travel*** and
will be used to limit how far the robot will be allowed to travel before
another command must be issued. If commands come in less frequently than
max_seconds_uncommanded_travel seconds beween commands, the motors will
stop. This is a safety feature.
- /odom
*** NOTE: REMOVE odom stuff
# Run the node
You need to change the yaml configuration file to match your hardward setup.
```
ros2 launch ros2_roboclaw_driver ros2_roboclaw_driver.launch.py
```
# Example launch file
An example launch file called ***ros2_roboclaw_driver.launch.py*** is provided.
```
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
import launch_ros.actions
from launch_ros.actions import Node
def generate_launch_description():
# The following 'my_package_name' should be changed to the
# package name in your robot workspace that will contain
# your own yaml configuration file.
#
# Also the 'motor_driver.yaml' should be changed to the yaml
# configuration file name in your robot package. The example
# launch file here assumes that your configuration file will
# be under the 'config' directory within the my_package_name
# folder.
#
# The name 'ros2_roboclaw_driver'
# here will point to this workspace and its example configuration
# file which is unlikely to work out-of-the-box for your hardware
# so it is expected that your will also create a yaml file to
# match your hardware. See the README file for more information.
my_package_name = 'ros2_roboclaw_driver'
configFilePath = os.path.join(
get_package_share_directory(my_package_name),
'config',
'motor_driver.yaml'
)
# Extract the relevant configuration parameters from the yaml file.
# Doing it this way allows you, for example, to include the configuration
# parameters in a larger yaml file that also provides parameters for
# other packages. See the example yaml file provided and the README
# file for more information.
with open(configFilePath, 'r') as file:
configParams = yaml.safe_load(file)['motor_driver_node']['ros__parameters']
ld = LaunchDescription()
# The 'emulate_tty' here helps colorize the log output to the console
motor_driver_node = Node(
emulate_tty=True,
executable='ros2_roboclaw_driver_node',
package='ros2_roboclaw_driver',
parameters=[configParams],
#prefix=['xterm -e gdb -ex run --args'],
respawn=True,
output='screen')
ld.add_action(motor_driver_node)
return ld
```
# Configuration file
An example configuration file called ***motor_driver.yaml*** is provided.
```
# The configuration file provides values for the two, differential
# drive motors, 'm1' and 'm2'. See the article:
# https://resources.basicmicro.com/auto-tuning-with-motion-studio/
# for a description of how to derive the p, i, d and qpps values.
#
# The two strings ***motor_driver_node*** and ***ros_parameters*** must match
# the code in the launch file.
motor_driver_node:
ros__parameters:
# Incremental acceleration to use in quadrature pulses per second.
accel_quad_pulses_per_second: 1000
# The device name to be opened.
device_name: "/dev/roboclaw"
# The assigned port for the device (as configured on the RoboClaw).
device_port: 128
# The P, I, D and maximum quadrature pulses per second for both motors.
m1_p: 7.26239
m1_i: 1.36838
m1_d: 0.0
m1_qpps: 2437
m2_p: 7.26239
m2_i: 1.28767
m2_d: 0.0
m2_qpps: 2437
# The maximum expected current (in amps) for both motors.
# A motor will be commanded to stop if it exceeds this current.
m1_max_current: 6.0
m2_max_current: 6.0
# Rate limiting commands. The driver will clip any value
# exceeding these limits.
max_angular_velocity: 0.07
max_linear_velocity: 0.3
# If no new motor commands is received since the last motor
# command in this number of seconds, stop the motors.
max_seconds_uncommanded_travel: 0.25
# Based upon your particular motor gearing and encoders.
# These values are used to scale cmd_vel commands
# and encoder values to motor commands and odometry, respectfully.
quad_pulses_per_meter: 1566
quad_pulses_per_revolution: 1000
# Based upon y our particular robot model.
# The wheel separation and radius, in meters.
wheel_radius: 0.10169
wheel_separation: 0.345
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# See: http://unixwiz.net/techtips/termios-vmin-vtime.html
vmin: 1
vtime: 2
```
# Miscellaneous notes
The driver keeps track of the encoder values for the left and right
motors. Whenever the driver starts, the existing encoder values captured
in the RoboClaw itself are reset to zero.
This was done to decrease the probability of encoder value overflow and
underflow over a period of time, which is not dealt with especially
by this package.
Do not expect the encoder
values to be the same between node instantiations for this driver package.

3
TODO Normal file
View File

@@ -0,0 +1,3 @@
Remove odom transform code, move to static
Fix roboclaw status to error string mapping
Add service handler to reset encoders

59
config/motor_driver.yaml Executable file
View File

@@ -0,0 +1,59 @@
# The configuration file provides values for the two, differential
# drive motors, 'm1' and 'm2'. See the article:
# https://resources.basicmicro.com/auto-tuning-with-motion-studio/
# for a description of how to derive the p, i, d and qpps values.
motor_driver_node:
ros__parameters:
# Incremental acceleration to use in quadrature pulses per second.
accel_quad_pulses_per_second: 1000
# The device name to be opened.
device_name: "/dev/roboclaw"
# The assigned port for the device (as configured on the RoboClaw).
device_port: 128
# The P, I, D and maximum quadrature pulses per second for both motors.
m1_p: 7.26239
m1_i: 1.36838
m1_d: 0.0
m1_qpps: 2437
m2_p: 7.26239
m2_i: 1.28767
m2_d: 0.0
m2_qpps: 2437
# The maximum expected current (in amps) for both motors.
# Used just to signal warnings.
m1_max_current: 6.0
m2_max_current: 6.0
# Rate limiting commands. The driver will clip any value
# exceeding these limits.
max_angular_velocity: 0.07
max_linear_velocity: 0.3
# If no new motor commands is received since the last motor
# command in this number of seconds, stop the motors.
max_seconds_uncommanded_travel: 0.25
# Based upon your particular motor gearing and encoders.
# These values are used to scale cmd_vel commands
# and encoder values to motor commands and odometry, respectfully.
quad_pulses_per_meter: 1566
quad_pulses_per_revolution: 1000
# Based upon y our particular robot model.
# The wheel separation and radius, in meters.
wheel_radius: 0.10169
wheel_separation: 0.345
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# See: http://unixwiz.net/techtips/termios-vmin-vtime.html
vmin: 1
vtime: 2

54
launch/ros2_roboclaw_driver.launch.py Normal file
View File

@@ -0,0 +1,54 @@
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
import launch_ros.actions
from launch_ros.actions import Node
def generate_launch_description():
# The following 'my_package_name' should be changed to the
# package name in your robot workspace that will contain
# your own yaml configuration file.
#
# Also the 'motor_driver.yaml' should be changed to the yaml
# configuration file name in your robot package. The example
# launch file here assumes that your configuration file will
# be under the 'config' directory within the my_package_name
# folder.
#
# The name 'ros2_roboclaw_driver'
# here will point to this workspace and its example configuration
# file which is unlikely to work out-of-the-box for your hardware
# so it is expected that your will also create a yaml file to
# match your hardware. See the README file for more information.
my_package_name = 'ros2_roboclaw_driver'
configFilePath = os.path.join(
get_package_share_directory(my_package_name),
'config',
'motor_driver.yaml'
)
# Extract the relevant configuration parameters from the yaml file.
# Doing it this way allows you, for example, to include the configuration
# parameters in a larger yaml file that also provides parameters for
# other packages. See the example yaml file provided and the README
# file for more information.
with open(configFilePath, 'r') as file:
configParams = yaml.safe_load(file)['motor_driver_node']['ros__parameters']
ld = LaunchDescription()
# The 'emulate_tty' here helps colorize the log output to the console
motor_driver_node = Node(
emulate_tty=True,
executable='ros2_roboclaw_driver_node',
package='ros2_roboclaw_driver',
parameters=[configParams],
#prefix=['xterm -e gdb -ex run --args'],
respawn=True,
output='screen')
ld.add_action(motor_driver_node)
return ld

22
msg/RoboClawStatus.msg Executable file
View File

@@ -0,0 +1,22 @@
float32 m1_p
float32 m1_i
float32 m1_d
uint32 m1_qpps
int32 m1_current_speed
float32 m1_motor_current
uint32 m1_encoder_value
uint8 m1_encoder_status
float32 m2_p
float32 m2_i
float32 m2_d
uint32 m2_qpps
int32 m2_current_speed
float32 m2_motor_current
uint32 m2_encoder_value
uint8 m2_encoder_status
float32 main_battery_voltage
float32 logic_battery_voltage
float32 temperature
string error_string

31
package.xml Executable file
View File

@@ -0,0 +1,31 @@
<?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>ros2_roboclaw_driver</name>
<version>1.0.0</version>
<description>A driver for RoboClaw motor controller devices for use with ROS2</description>
<maintainer email="mike@wimblerobotics.com">Michael Wimble</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>buildin_interfaces</depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

160
src/motor_driver.cpp Executable file
View File

@@ -0,0 +1,160 @@
#include <algorithm>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <math.h>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rcutils/logging_macros.h>
#include <string>
#include <stdint.h>
#include <tf2_ros/transform_broadcaster.h>
#include "motor_driver.h"
#include "roboclaw.h"
MotorDriver::MotorDriver()
: Node("motor_driver_node"), device_name_("foo_bar"), wheel_radius_(0.10169), wheel_separation_(0.345)
{
this->declare_parameter<int>("accel_quad_pulses_per_second", 600);
this->declare_parameter<std::string>("device_name", "roboclaw");
this->declare_parameter<int>("device_port", 123);
this->declare_parameter<float>("m1_p", 0.0);
this->declare_parameter<float>("m1_i", 0.0);
this->declare_parameter<float>("m1_d", 0.0);
this->declare_parameter<int>("m1_qpps", 0);
this->declare_parameter<float>("m1_max_current", 0.0);
this->declare_parameter<float>("m2_p", 0.0);
this->declare_parameter<float>("m2_i", 0.0);
this->declare_parameter<float>("m2_d", 0.0);
this->declare_parameter<int>("m2_qpps", 0);
this->declare_parameter<float>("max_angular_velocity", 0.0);
this->declare_parameter<float>("max_linear_velocity", 0.0);
this->declare_parameter<float>("m2_max_current", 0.0);
this->declare_parameter<float>("max_seconds_uncommanded_travel", 0.0);
this->declare_parameter<int>("quad_pulses_per_meter", 0);
this->declare_parameter<int>("quad_pulses_per_revolution", 0);
this->declare_parameter<int>("vmin", 1);
this->declare_parameter<int>("vtime", 2);
this->declare_parameter<float>("wheel_radius", 0.0);
this->declare_parameter<float>("wheel_separation", 0.0);
}
void MotorDriver::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = rclcpp::Node::now();
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "t265";
transformStamped.transform.translation.x = msg->pose.pose.position.x;
transformStamped.transform.translation.y = msg->pose.pose.position.y;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation.x = 0.0;
transformStamped.transform.rotation.y = 0.0;
transformStamped.transform.rotation.z = msg->pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg->pose.pose.orientation.w;
broadcaster_->sendTransform(transformStamped);
// RCUTILS_LOG_INFO("odom published");
}
void MotorDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const
{
double x_velocity = std::min(std::max((float) msg->linear.x, -max_linear_velocity_), max_linear_velocity_);
double yaw_velocity = std::min(std::max((float) msg->angular.z, -max_angular_velocity_), max_angular_velocity_);
if ((msg->linear.x == 0) && (msg->angular.z == 0))
{
roboclaw_->doMixedSpeedDist(0, 0, 0, 0);
}
else if ((fabs(x_velocity) > 0.01) || (fabs(yaw_velocity) > 0.01))
{
const double m1_desired_velocity = x_velocity - (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const double m2_desired_velocity = x_velocity + (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const int32_t m1_quad_pulses_per_second = m1_desired_velocity * quad_pulses_per_meter_;
const int32_t m2_quad_pulses_per_second = m2_desired_velocity * quad_pulses_per_meter_;
const int32_t m1_max_distance = fabs(m1_quad_pulses_per_second * max_seconds_uncommanded_travel_);
const int32_t m2_max_distance = fabs(m2_quad_pulses_per_second * max_seconds_uncommanded_travel_);
roboclaw_->doMixedSpeedAccelDist(accel_quad_pulses_per_second_, m1_quad_pulses_per_second, m1_max_distance, m2_quad_pulses_per_second, m2_max_distance);
}
}
void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
{
node_ = node;
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
this->get_parameter("accel_quad_pulses_per_second", accel_quad_pulses_per_second_);
this->get_parameter("device_name", device_name_);
this->get_parameter("device_port", device_port_);
this->get_parameter("m1_p", m1_p_);
this->get_parameter("m1_i", m1_i_);
this->get_parameter("m1_d", m1_d_);
this->get_parameter("m1_qpps", m1_qpps_);
this->get_parameter("m1_max_current", m1_max_current_);
this->get_parameter("m2_p", m2_p_);
this->get_parameter("m2_i", m2_i_);
this->get_parameter("m2_d", m2_d_);
this->get_parameter("m2_qpps", m2_qpps_);
this->get_parameter("m2_max_current", m2_max_current_);
max_angular_velocity_ = -12.34;
this->get_parameter("max_angular_velocity", max_angular_velocity_);
this->get_parameter("max_linear_velocity", max_linear_velocity_);
this->get_parameter("max_seconds_uncommanded_travel", max_seconds_uncommanded_travel_);
this->get_parameter("quad_pulses_per_meter", quad_pulses_per_meter_);
this->get_parameter("quad_pulses_per_revolution", quad_pulses_per_revolution_);
this->get_parameter("vmin", vmin_);
this->get_parameter("vtime", vtime_);
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
RCUTILS_LOG_INFO("accel_quad_pulses_per_second: %d", accel_quad_pulses_per_second_);
RCUTILS_LOG_INFO("device_name: %s", device_name_.c_str());
RCUTILS_LOG_INFO("device_port: %d", device_port_);
RCUTILS_LOG_INFO("m1_p: %f", m1_p_);
RCUTILS_LOG_INFO("m1_i: %f", m1_i_);
RCUTILS_LOG_INFO("m1_d: %f", m1_d_);
RCUTILS_LOG_INFO("m1_qpps: %d", m1_qpps_);
RCUTILS_LOG_INFO("m1_max_current: %f", m1_max_current_);
RCUTILS_LOG_INFO("m2_p: %f", m2_p_);
RCUTILS_LOG_INFO("m2_i: %f", m2_i_);
RCUTILS_LOG_INFO("m2_d: %f", m2_d_);
RCUTILS_LOG_INFO("m2_qpps: %d", m2_qpps_);
RCUTILS_LOG_INFO("m2_max_current: %f", m2_max_current_);
RCUTILS_LOG_INFO("max_angular_velocity: %f", max_angular_velocity_);
RCUTILS_LOG_INFO("max_linear_velocity: %f", max_linear_velocity_);
RCUTILS_LOG_INFO("max_seconds_uncommanded_travel: %f", max_seconds_uncommanded_travel_);
RCUTILS_LOG_INFO("quad_pulses_per_meter: %d", quad_pulses_per_meter_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %d", quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("vmin: %d", vmin_);
RCUTILS_LOG_INFO("vtime: %d", vtime_);
RCUTILS_LOG_INFO("wheel_radius: %f", wheel_radius_);
RCUTILS_LOG_INFO("wheel_separation: %f", wheel_separation_);
RoboClaw::TPIDQ m1Pid = {m1_p_, m1_i_, m1_d_, m1_qpps_, m1_max_current_};
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, m2_qpps_, m2_max_current_};
roboclaw_ = new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_, device_name_.c_str(), device_port_, vmin_, vtime_);
RCUTILS_LOG_INFO("Main battery: %f", roboclaw_->getMainBatteryLevel());
rclcpp::QoS bestEffortQos(10);
bestEffortQos.keep_last(10);
bestEffortQos.best_effort();
bestEffortQos.durability_volatile();
cmdVelSub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", bestEffortQos, std::bind(&MotorDriver::cmdVelCallback, this, std::placeholders::_1));
odomSub_ = node->create_subscription<nav_msgs::msg::Odometry>(
"/odom", bestEffortQos, std::bind(&MotorDriver::odomCallback, this, std::placeholders::_1));
}
MotorDriver &MotorDriver::singleton()
{
if (!g_singleton)
{
g_singleton = new MotorDriver();
}
return *g_singleton;
}
MotorDriver *MotorDriver::g_singleton = nullptr;

59
src/motor_driver.h Executable file
View File

@@ -0,0 +1,59 @@
#pragma once
#include <stdint.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include "roboclaw.h"
class MotorDriver : public rclcpp::Node {
public:
static MotorDriver &singleton();
void onInit(rclcpp::Node::SharedPtr node);
RoboClaw &roboClaw() { return *roboclaw_; }
private:
MotorDriver();
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
uint32_t accel_quad_pulses_per_second_;
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
std::string device_name_;
uint8_t device_port_;
float m1_p_;
float m1_i_;
float m1_d_;
uint32_t m1_qpps_;
float m1_max_current_;
float m2_p_;
float m2_i_;
float m2_d_;
uint32_t m2_qpps_;
float m2_max_current_;
float max_angular_velocity_; // Maximum allowed angular velocity.
float max_linear_velocity_; // Maximum allowed linear velocity.
double max_seconds_uncommanded_travel_;
uint32_t quad_pulses_per_meter_;
uint32_t quad_pulses_per_revolution_;
RoboClaw *roboclaw_;
uint8_t vmin_;
uint8_t vtime_;
double wheel_radius_;
double wheel_separation_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdVelSub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
static MotorDriver *g_singleton;
};

114
src/motor_driver_node.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include <rclcpp/rclcpp.hpp>
#include <string>
#include "motor_driver.h"
#include "ros2_roboclaw_driver/msg/robo_claw_status.hpp"
#include "roboclaw.h"
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ros2_roboclaw_driver");
MotorDriver &motorDriver = MotorDriver::singleton();
motorDriver.onInit(node);
auto qos = rclcpp::QoS(
rclcpp::QoSInitialization(
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
10));
qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
qos.avoid_ros_namespace_conventions(false);
std::string statusTopicName;
node->declare_parameter<std::string>("roboclaw_status_topic", "roboclaw_status");
node->get_parameter("roboclaw_status_topic", statusTopicName);
RCUTILS_LOG_INFO("[motor_driver_node] roboclaw_status_topic: %s", statusTopicName.c_str());
rclcpp::Publisher<ros2_roboclaw_driver::msg::RoboClawStatus>::SharedPtr statusPublisher = node->create_publisher<ros2_roboclaw_driver::msg::RoboClawStatus>(statusTopicName, qos);
ros2_roboclaw_driver::msg::RoboClawStatus roboClawStatus;
rclcpp::WallRate loop_rate(20);
while (rclcpp::ok())
{
try
{
roboClawStatus.logic_battery_voltage = motorDriver.roboClaw().getLogicBatteryLevel();
roboClawStatus.main_battery_voltage = motorDriver.roboClaw().getMainBatteryLevel();
RoboClaw::TMotorCurrents motorCurrents = motorDriver.roboClaw().getMotorCurrents();
roboClawStatus.m1_motor_current = motorCurrents.m1Current;
roboClawStatus.m2_motor_current = motorCurrents.m2Current;
RoboClaw::TPIDQ pidq = motorDriver.roboClaw().getPIDQ(RoboClaw::kGETM1PID);
roboClawStatus.m1_p = pidq.p / 65536.0;
roboClawStatus.m1_i = pidq.i / 65536.0;
roboClawStatus.m1_d = pidq.d / 65536.0;
roboClawStatus.m1_qpps = pidq.qpps;
pidq = motorDriver.roboClaw().getPIDQ(RoboClaw::kGETM2PID);
roboClawStatus.m2_p = pidq.p / 65536.0;
roboClawStatus.m2_i = pidq.i / 65536.0;
roboClawStatus.m2_d = pidq.d / 65536.0;
roboClawStatus.m2_qpps = pidq.qpps;
roboClawStatus.temperature = motorDriver.roboClaw().getTemperature();
{
RoboClaw::EncodeResult encoder = motorDriver.roboClaw().getEncoderCommandResult(RoboClaw::kGETM1ENC);
roboClawStatus.m1_encoder_value = encoder.value;
roboClawStatus.m1_encoder_status = encoder.status;
}
{
RoboClaw::EncodeResult encoder = motorDriver.roboClaw().getEncoderCommandResult(RoboClaw::kGETM2ENC);
roboClawStatus.m2_encoder_value = encoder.value;
roboClawStatus.m2_encoder_status = encoder.status;
}
roboClawStatus.m1_current_speed = motorDriver.roboClaw().getVelocity(RoboClaw::kGETM1SPEED);
roboClawStatus.m2_current_speed = motorDriver.roboClaw().getVelocity(RoboClaw::kGETM2SPEED);
roboClawStatus.error_string = motorDriver.roboClaw().getErrorString();
statusPublisher->publish(roboClawStatus);
if (motorDriver.roboClaw().motorAlarms() != 0)
{
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM1_OVER_CURRENT)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M1_OVER_CURRENT");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM2_OVER_CURRENT)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M2_OVER_CURRENT");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM1_OVER_CURRENT_ALARM)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M1_OVER_CURRENT_ALARM");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM2_OVER_CURRENT_ALARM)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M2_OVER_CURRENT_ALARM");
}
}
}
catch (RoboClaw::TRoboClawException *e)
{
RCUTILS_LOG_ERROR("[motor_driver_node] Exception: %s", e->what());
}
catch (...)
{
RCUTILS_LOG_ERROR("[motor_driver_node] Uncaught exception !!!");
}
rclcpp::spin_some(node);
loop_rate.sleep();
}
rclcpp::shutdown();
return 0;
}

966
src/roboclaw.cpp Executable file
View File

@@ -0,0 +1,966 @@
#include "roboclaw.h"
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <rcutils/logging_macros.h>
#include <stdio.h>
#include <sys/types.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include <boost/assign.hpp>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
#define SetDWORDval(arg) \
(uint8_t)(arg >> 24), (uint8_t)(arg >> 16), (uint8_t)(arg >> 8), (uint8_t)arg
RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name,
uint8_t device_port, uint8_t vmin, uint8_t vtime)
: device_port_(device_port),
maxCommandRetries_(3),
maxM1Current_(m1MaxCurrent),
maxM2Current_(m2MaxCurrent),
device_name_(device_name),
portAddress_(128),
vmin_(vmin),
vtime_(vtime) {
openPort();
RCUTILS_LOG_INFO("[RoboClaw::RoboClaw] RoboClaw software version: %s",
getVersion().c_str());
setM1PID(m1Pid.p, m1Pid.i, m1Pid.d, m1Pid.qpps);
setM2PID(m2Pid.p, m2Pid.i, m2Pid.d, m2Pid.qpps);
ros2_roboclaw_driver::srv::ResetEncoders::Request resetRequest;
resetRequest.left = 0;
resetRequest.right = 0;
ros2_roboclaw_driver::srv::ResetEncoders::Response response;
resetEncoders(resetRequest, response);
}
RoboClaw::~RoboClaw() {}
void RoboClaw::doMixedSpeedDist(int32_t m1_quad_pulses_per_second,
int32_t m1_max_distance,
int32_t m2_quad_pulses_per_second,
int32_t m2_max_distance) {
writeN(true, 19, portAddress_, kMIXEDSPEEDDIST,
SetDWORDval(m1_quad_pulses_per_second), SetDWORDval(m1_max_distance),
SetDWORDval(m2_quad_pulses_per_second), SetDWORDval(m2_max_distance),
1 /* Cancel any previous command. */
);
}
void RoboClaw::doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second,
int32_t m1_quad_pulses_per_second,
uint32_t m1_max_distance,
int32_t m2_quad_pulses_per_second,
uint32_t m2_max_distance) {
writeN(true, 23, portAddress_, kMIXEDSPEEDACCELDIST,
SetDWORDval(accel_quad_pulses_per_second),
SetDWORDval(m1_quad_pulses_per_second), SetDWORDval(m1_max_distance),
SetDWORDval(m2_quad_pulses_per_second), SetDWORDval(m2_max_distance),
1 /* Cancel any previous command. */
);
}
RoboClaw::EncodeResult RoboClaw::getEncoderCommandResult(WHICH_ENC command) {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, command);
writeN(false, 2, portAddress_, command);
EncodeResult result = {0, 0};
uint8_t datum = readByteWithTimeout();
result.value |= datum << 24;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result.value |= datum << 16;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result.value |= datum << 8;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result.value |= datum;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result.status |= datum;
updateCrc(crc, datum);
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
}
RCUTILS_LOG_ERROR(
"[RoboClaw::getEncoderCommandResult] Expected CRC of: 0x%02X, but "
"got: 0x%02X",
int(crc), int(responseCrc));
throw new TRoboClawException(
"[RoboClaw::getEncoderCommandResult] INVALID CRC");
}
uint16_t RoboClaw::getErrorStatus() {
for (int retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, kGETERROR);
writeN(false, 2, portAddress_, kGETERROR);
unsigned short result = (unsigned short)getULongCont(crc);
uint16_t responseCrc = 0;
uint16_t datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::getPIDQ] invalid CRC expected: 0x%02X, got: "
"0x%02X",
crc, responseCrc);
}
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getErrorStatus] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getErrorStatus] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getErrorStatus] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::getErrorStatus] RETRY COUNT EXCEEDED");
}
std::string RoboClaw::getErrorString() {
uint16_t errorStatus = getErrorStatus();
if (errorStatus == 0)
return "normal";
else {
std::stringstream errorMessage;
if (errorStatus & 0x8000) {
errorMessage << "[M2 Home] ";
}
if (errorStatus & 0x4000) {
errorMessage << "[M1 Home] ";
}
if (errorStatus & 0x2000) {
errorMessage << "[Temperature2 Warning] ";
}
if (errorStatus & 0x1000) {
errorMessage << "[Temperature Warning] ";
}
if (errorStatus & 0x800) {
errorMessage << "[Main Battery Low Warning] ";
}
if (errorStatus & 0x400) {
errorMessage << "[Main Battery High Warning] ";
}
if (errorStatus & 0x200) {
errorMessage << "[M1 Driver Fault] ";
}
if (errorStatus & 0x100) {
errorMessage << "[M2 Driver Fault] ";
}
if (errorStatus & 0x80) {
errorMessage << "[Logic Battery Low Error] ";
}
if (errorStatus & 0x40) {
errorMessage << "[Logic Battery High Error] ";
}
if (errorStatus & 0x20) {
errorMessage << "[Main Battery High Error] ";
}
if (errorStatus & 0x10) {
errorMessage << "[Temperature2 Error] ";
}
if (errorStatus & 0x08) {
errorMessage << "[Temperature Error] ";
}
if (errorStatus & 0x04) {
errorMessage << "[E-Stop] ";
}
if (errorStatus & 0x02) {
errorMessage << "[M2 OverCurrent Warning] ";
motorAlarms_ |= kM2_OVER_CURRENT_ALARM;
} else {
motorAlarms_ &= ~kM2_OVER_CURRENT_ALARM;
}
if (errorStatus & 0x01) {
errorMessage << "[M1 OverCurrent Warning] ";
motorAlarms_ |= kM1_OVER_CURRENT_ALARM;
} else {
motorAlarms_ &= ~kM1_OVER_CURRENT_ALARM;
}
return errorMessage.str();
}
}
float RoboClaw::getLogicBatteryLevel() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
float result = ((float)get2ByteCommandResult(kGETLBATT)) / 10.0;
return result;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getLogicBatteryLevel] Exception: %s, retry "
"number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getLogicBatteryLevel] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getLogicBatteryLevel] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::getLogicBatteryLevel] RETRY COUNT EXCEEDED");
}
int32_t RoboClaw::getM1Encoder() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
EncodeResult result = getEncoderCommandResult(kGETM1ENC);
return result.value;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getM1Encoder] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getM1Encoder] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getM1Encoder] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getM1Encoder] RETRY COUNT EXCEEDED");
}
float RoboClaw::getMainBatteryLevel() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
float result = ((float)get2ByteCommandResult(kGETMBATT)) / 10.0;
return result;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR_EXPRESSION(
"[RoboClaw::getMainBatteryLevel] Exception: %s, retry number: "
"%d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getMainBatteryLevel] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getMainBatteryLevel] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::getMainBatteryLevel] RETRY COUNT EXCEEDED");
}
unsigned short RoboClaw::get2ByteCommandResult(uint8_t command) {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, command);
writeN(false, 2, portAddress_, command);
unsigned short result = 0;
uint8_t datum = readByteWithTimeout();
result |= datum << 8;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum;
updateCrc(crc, datum);
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::get2ByteCommandResult] invalid CRC expected: "
"0x%02X, got: 0x%02X",
crc, responseCrc);
throw new TRoboClawException(
"[RoboClaw::get2ByteCommandResult] INVALID CRC");
return 0;
}
}
RoboClaw::TMotorCurrents RoboClaw::getMotorCurrents() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
TMotorCurrents result;
unsigned long currentPair = getUlongCommandResult(kGETCURRENTS);
result.m1Current = ((int16_t)(currentPair >> 16)) * 0.010;
result.m2Current = ((int16_t)(currentPair & 0xFFFF)) * 0.010;
if (result.m1Current > maxM1Current_) {
motorAlarms_ |= kM1_OVER_CURRENT_ALARM;
RCUTILS_LOG_ERROR(
"[RoboClaw::getMotorCurrents] Motor 1 over current. Max "
"allowed: %6.3f, found: %6.3f",
maxM1Current_, result.m1Current);
stop();
} else {
motorAlarms_ &= ~kM1_OVER_CURRENT_ALARM;
}
if (result.m2Current > maxM2Current_) {
motorAlarms_ |= kM2_OVER_CURRENT_ALARM;
RCUTILS_LOG_ERROR(
"[RoboClaw::getMotorCurrents] Motor 2 over current. Max "
"allowed: %6.3f, found: %6.3f",
maxM2Current_, result.m2Current);
stop();
} else {
motorAlarms_ &= ~kM2_OVER_CURRENT_ALARM;
}
return result;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getMotorCurrents] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getMotorCurrents] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("RoboClaw::getMotorCurrents] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::getMotorCurrents] RETRY COUNT EXCEEDED");
}
RoboClaw::TPIDQ RoboClaw::getPIDQ(WHICH_MOTOR whichMotor) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
TPIDQ result;
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, whichMotor);
writeN(false, 2, portAddress_, whichMotor);
result.p = (int32_t)getULongCont(crc);
result.i = (int32_t)getULongCont(crc);
result.d = (int32_t)getULongCont(crc);
result.qpps = (int32_t)getULongCont(crc);
uint16_t responseCrc = 0;
uint16_t datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::getPIDQ] invalid CRC expected: 0x%2X, got: "
"0x%2X",
crc, responseCrc);
}
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR("[RoboClaw::getPIDQ] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getPIDQ] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getPIDQ] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getPIDQ] RETRY COUNT EXCEEDED");
}
float RoboClaw::getTemperature() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, kGETTEMPERATURE);
writeN(false, 2, portAddress_, kGETTEMPERATURE);
uint16_t result = 0;
uint8_t datum = readByteWithTimeout();
updateCrc(crc, datum);
result = datum << 8;
datum = readByteWithTimeout();
updateCrc(crc, datum);
result |= datum;
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result / 10.0;
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::getTemperature] invalid CRC expected: 0x%2X, "
"got: 0x%2X",
crc, responseCrc);
}
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getTemperature] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getTemperature] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getTemperature] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::getTemperature] RETRY COUNT EXCEEDED");
}
unsigned long RoboClaw::getUlongCommandResult(uint8_t command) {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, command);
writeN(false, 2, portAddress_, command);
unsigned long result = 0;
uint8_t datum = readByteWithTimeout();
result |= datum << 24;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 16;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 8;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum;
updateCrc(crc, datum);
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
}
RCUTILS_LOG_ERROR(
"[RoboClaw::getUlongCommandResult] Expected CRC of: 0x%02X, but "
"got: 0x%02X",
int(crc), int(responseCrc));
throw new TRoboClawException("[RoboClaw::getUlongCommandResult] INVALID CRC");
return 0;
}
uint32_t RoboClaw::getULongCont(uint16_t &crc) {
uint32_t result = 0;
uint8_t datum = readByteWithTimeout();
result |= datum << 24;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 16;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 8;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum;
updateCrc(crc, datum);
return result;
}
int32_t RoboClaw::getVelocity(WHICH_VELOCITY whichVelocity) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint32_t result = getVelocityResult(whichVelocity);
return result;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getVelocity] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getVelocity] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("RoboClaw::getVelocity] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getVelocity] RETRY COUNT EXCEEDED");
}
int32_t RoboClaw::getVelocityResult(uint8_t command) {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, command);
writeN(false, 2, portAddress_, command);
int32_t result = 0;
uint8_t datum = readByteWithTimeout();
result |= datum << 24;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 16;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum << 8;
updateCrc(crc, datum);
datum = readByteWithTimeout();
result |= datum;
updateCrc(crc, datum);
uint8_t direction = readByteWithTimeout();
updateCrc(crc, direction);
if (direction != 0) result = -result;
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return result;
}
RCUTILS_LOG_ERROR(
"[RoboClaw::getVelocityResult] Expected CRC of: 0x%02X, but got: "
"0x%02X",
int(crc), int(responseCrc));
throw new TRoboClawException("[RoboClaw::getVelocityResult] INVALID CRC");
return 0;
}
int32_t RoboClaw::getM2Encoder() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
EncodeResult result = getEncoderCommandResult(kGETM2ENC);
return result.value;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getM2Encoder] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getM2Encoder] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getM2Encoder] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getM2Encoder] RETRY COUNT EXCEEDED");
}
std::string RoboClaw::getVersion() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, kGETVERSION);
writeN(false, 2, portAddress_, kGETVERSION);
uint8_t i;
uint8_t datum;
std::stringstream version;
for (i = 0; i < 48; i++) {
datum = readByteWithTimeout();
updateCrc(crc, datum);
if (datum == 0) {
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return version.str();
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::getVersion] invalid CRC expected: 0x%02X, "
"got: 0x%02X",
crc, responseCrc);
}
} else {
version << (char)datum;
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] unexpected long string");
throw new TRoboClawException(
"[RoboClaw::getVersion] unexpected long string");
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getVersion] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getVersion] RETRY COUNT EXCEEDED");
}
void RoboClaw::openPort() {
RCUTILS_LOG_INFO("[RoboClaw::openPort] about to open port: %s",
device_name_.c_str());
device_port_ = open(device_name_.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
if (device_port_ < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to open USB port: %s, errno: (%d) "
"%s",
device_name_.c_str(), errno, strerror(errno));
throw new TRoboClawException(
"[RoboClaw::openPort] Unable to open USB port");
}
// Fetch the current port settings.
struct termios portOptions;
int ret = 0;
ret = tcgetattr(device_port_, &portOptions);
if (ret < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to get terminal options "
"(tcgetattr), error: %d: %s",
errno, strerror(errno));
// throw new TRoboClawException("[RoboClaw::openPort] Unable to get
// terminal options (tcgetattr)");
}
// c_cflag contains a few important things- CLOCAL and CREAD, to prevent
// this program from "owning" the port and to enable receipt of data.
// Also, it holds the settings for number of data bits, parity, stop bits,
// and hardware flow control.
portOptions.c_cflag |= CLOCAL; // Prevent changing ownership.
portOptions.c_cflag |= CREAD; // Enable reciever.
portOptions.c_cflag &= ~CRTSCTS; // Disable hardware CTS/RTS flow control.
portOptions.c_cflag |= CS8; // Enable 8 bit characters.
portOptions.c_cflag &= ~CSIZE; // Remove size flag.
portOptions.c_cflag &= ~CSTOPB; // Disable 2 stop bits.
portOptions.c_cflag |=
HUPCL; // Enable lower control lines on close - hang up.
portOptions.c_cflag &= ~PARENB; // Disable parity.
// portOptions.c_iflag |= BRKINT;
portOptions.c_iflag &= ~IGNBRK; // Disable ignoring break.
portOptions.c_iflag &= ~IGNCR; // Disable ignoring CR;
portOptions.c_iflag &= ~(IGNPAR | PARMRK); // Disable parity checks.
// portOptions.c_iflag |= IGNPAR;
portOptions.c_iflag &= ~(INLCR | ICRNL); // Disable translating NL <-> CR.
portOptions.c_iflag &= ~INPCK; // Disable parity checking.
portOptions.c_iflag &= ~ISTRIP; // Disable stripping 8th bit.
portOptions.c_iflag &= ~(IXON | IXOFF); // disable XON/XOFF flow control
portOptions.c_lflag &= ~ECHO; // Disable echoing characters.
portOptions.c_lflag &= ~ECHONL; // ??
portOptions.c_lflag &= ~ICANON; // Disable canonical mode - line by line.
portOptions.c_lflag &= ~IEXTEN; // Disable input processing
portOptions.c_lflag &= ~ISIG; // Disable generating signals.
portOptions.c_lflag &= ~NOFLSH; // Disable flushing on SIGINT.
portOptions.c_oflag &= ~OFILL; // Disable fill characters.
portOptions.c_oflag &= ~(ONLCR | OCRNL); // Disable translating NL <-> CR.
portOptions.c_oflag &= ~OPOST; // Disable output processing.
portOptions.c_cc[VKILL] = 8;
portOptions.c_cc[VMIN] = vmin_;
portOptions.c_cc[VTIME] = vtime_;
if (cfsetispeed(&portOptions, B38400) < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to set terminal speed "
"(cfsetispeed)");
throw new TRoboClawException(
"[RoboClaw::openPort] Unable to set terminal speed "
"(cfsetispeed)");
}
if (cfsetospeed(&portOptions, B38400) < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to set terminal speed "
"(cfsetospeed)");
throw new TRoboClawException(
"[RoboClaw::openPort] Unable to set terminal speed "
"(cfsetospeed)");
}
// Now that we've populated our options structure, let's push it back to the
// system.
if (tcsetattr(device_port_, TCSANOW, &portOptions) < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to set terminal options "
"(tcsetattr)");
throw new TRoboClawException(
"[RoboClaw::openPort] Unable to set terminal options "
"(tcsetattr)");
}
}
uint8_t RoboClaw::readByteWithTimeout() {
struct pollfd ufd[1];
ufd[0].fd = device_port_;
ufd[0].events = POLLIN;
int retval = poll(ufd, 1, 11);
if (retval < 0) {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Poll failed (%d) %s",
errno, strerror(errno));
throw new TRoboClawException("[RoboClaw::readByteWithTimeout] Read error");
} else if (retval == 0) {
std::stringstream ev;
ev << "[RoboClaw::readByteWithTimeout] TIMEOUT revents: " << std::hex
<< ufd[0].revents;
RCUTILS_LOG_ERROR(ev.str().c_str());
throw new TRoboClawException("[RoboClaw::readByteWithTimeout] TIMEOUT");
} else if (ufd[0].revents & POLLERR) {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Error on socket");
restartPort();
throw new TRoboClawException(
"[RoboClaw::readByteWithTimeout] Error on socket");
} else if (ufd[0].revents & POLLIN) {
unsigned char buffer[1];
ssize_t bytesRead = ::read(device_port_, buffer, sizeof(buffer));
if (bytesRead != 1) {
RCUTILS_LOG_ERROR(
"[RoboClaw::readByteWithTimeout] Failed to read 1 byte, read: "
"%d",
(int)bytesRead);
throw TRoboClawException(
"[RoboClaw::readByteWithTimeout] Failed to read 1 byte");
}
static const bool kDEBUG_READBYTE = true;
if (kDEBUG_READBYTE) {
if ((buffer[0] < 0x21) || (buffer[0] > 0x7F)) {
// RCUTILS_LOG_INFO("..> char: ?? 0x%02X <--", buffer[0]);
// fprintf(stderr, "..> char: ?? 0x%02X <--", buffer[0]);
} else {
// fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0], buffer[0]);
}
}
return buffer[0];
} else {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Unhandled case");
throw new TRoboClawException(
"[RoboClaw::readByteWithTimeout] Unhandled case");
}
return 0;
}
bool RoboClaw::resetEncoders(
ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response) {
try {
SetEncoder(kSETM1ENCODER, request.left);
SetEncoder(kSETM2ENCODER, request.right);
response.ok = true;
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::resetEncoders] uncaught exception");
}
return true;
}
void RoboClaw::restartPort() {
close(device_port_);
usleep(200000);
openPort();
}
void RoboClaw::SetEncoder(ROBOCLAW_COMMAND command, long value) {
int retry;
if ((command != kSETM1ENCODER) && (command != kSETM2ENCODER)) {
RCUTILS_LOG_ERROR("[RoboClaw::SetEncoder] Invalid command value");
throw new TRoboClawException(
"[RoboClaw::SetEncoder] Invalid command value");
}
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
writeN(true, 6, portAddress_, command, SetDWORDval(value));
return;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::SetEncoder] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::SetEncoder] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::SetEncoder] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::SetEncoder] RETRY COUNT EXCEEDED");
}
void RoboClaw::setM1PID(float p, float i, float d, uint32_t qpps) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint32_t kp = int(p * 65536.0);
uint32_t ki = int(i * 65536.0);
uint32_t kd = int(d * 65536.0);
writeN(true, 18, portAddress_, kSETM1PID, SetDWORDval(kd),
SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps));
return;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED");
}
void RoboClaw::setM2PID(float p, float i, float d, uint32_t qpps) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint32_t kp = int(p * 65536.0);
uint32_t ki = int(i * 65536.0);
uint32_t kd = int(d * 65536.0);
writeN(true, 18, portAddress_, kSETM2PID, SetDWORDval(kd),
SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps));
return;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED");
}
void RoboClaw::stop() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
writeN(true, 6, portAddress_, kMIXEDDUTY, 0, 0, 0, 0);
RCUTILS_LOG_INFO("[RoboClaw::stop] Stop requested"); //#####
return;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR("[RoboClaw::stop] Exception: %s, retry number: %d",
e->what(), retry);
restartPort();
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::stop] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::stop] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::stop] RETRY COUNT EXCEEDED");
}
void RoboClaw::updateCrc(uint16_t &crc, uint8_t data) {
crc = crc ^ ((uint16_t)data << 8);
for (int i = 0; i < 8; i++) {
if (crc & 0x8000)
crc = (crc << 1) ^ 0x1021;
else
crc <<= 1;
}
}
void RoboClaw::writeByte(uint8_t byte) {
ssize_t result = ::write(device_port_, &byte, 1);
// RCUTILS_LOG_INFO("--> write: 0x%02X, result: %ld", byte, result); //####
// fprintf(stderr, "--> write: 0x%02X, result: %ld", byte, result); //####
if (result != 1) {
RCUTILS_LOG_ERROR(
"[RoboClaw::writeByte] Unable to write one byte, result: %d, "
"errno: %d)",
(int)result, errno);
restartPort();
throw new TRoboClawException(
"[RoboClaw::writeByte] Unable to write one byte");
}
}
void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) {
uint16_t crc = 0;
va_list marker;
va_start(marker, cnt);
int origFlags = fcntl(device_port_, F_GETFL, 0);
fcntl(device_port_, F_SETFL, origFlags & ~O_NONBLOCK);
for (uint8_t i = 0; i < cnt; i++) {
uint8_t byte = va_arg(marker, int);
writeByte(byte);
updateCrc(crc, byte);
}
va_end(marker);
if (sendCRC) {
writeByte(crc >> 8);
writeByte(crc);
uint8_t response = readByteWithTimeout();
if (response != 0xFF) {
RCUTILS_LOG_ERROR("[RoboClaw::writeN] Invalid ACK response");
throw new TRoboClawException("[RoboClaw::writeN] Invalid ACK response");
}
}
fcntl(device_port_, F_SETFL, origFlags);
}

258
src/roboclaw.h Executable file
View File

@@ -0,0 +1,258 @@
#pragma once
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
#include <rclcpp/logger.hpp>
#include <string>
class RoboClaw
{
public:
// Bit positions used to build alarms.
enum
{
kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high.
kM2_OVER_CURRENT = 0x02, // Motor 2 current sense is too high.
kM1_OVER_CURRENT_ALARM = 0x04, // Motor 1 controller over current alarm.
kM2_OVER_CURRENT_ALARM = 0x08, // Motor 2 controller over current alarm.
};
// Referencing which encoder in the RoboClaw
typedef enum WHICH_ENC
{
kGETM1ENC = 16,
kGETM2ENC = 17,
} WHICH_ENC;
// Referencing which motor in the RoboClaw
typedef enum WHICH_MOTOR
{
kGETM1PID = 55,
kGETM2PID = 56,
} WHICH_MOTOR;
// Referencing which velocity in the RoboClaw
typedef enum WHICH_VELOCITY
{
kGETM1SPEED = 18,
kGETM2SPEED = 19,
} WHICH_VELOCITY;
// A convenience struction to pass around configuration information.
typedef struct
{
float p;
float i;
float d;
uint32_t qpps;
float max_current;
} TPIDQ;
// For a custom exception message.
struct TRoboClawException : public std::exception
{
std::string s;
TRoboClawException(std::string ss) : s(ss) {}
~TRoboClawException() throw() {}
const char *what() const throw() { return s.c_str(); }
};
// Holds RoboClaw encoder result.
typedef struct
{
int32_t value;
uint8_t status;
} EncodeResult;
// Constructor.
RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent, float m2MaxCurrent, std::string device_name, uint8_t device_port, uint8_t vmin, uint8_t vtime);
~RoboClaw();
void doMixedSpeedDist(int32_t m1_quad_pulses_per_second, int32_t m1_max_distance, int32_t m2_quad_pulses_per_second, int32_t m2_max_distance);
void doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second, int32_t m1_quad_pulses_per_second, uint32_t m1_max_distance, int32_t m2_quad_pulses_per_second, uint32_t m2_max_distance);
EncodeResult getEncoderCommandResult(WHICH_ENC command);
// Get RoboClaw error status bits.
uint16_t getErrorStatus();
// Get RoboClaw error status as a string.
std::string getErrorString();
float getLogicBatteryLevel();
float getMainBatteryLevel();
// Get the encoder value for motor 1.
int32_t getM1Encoder();
// Get the encoder value for motor 2.
int32_t getM2Encoder();
// Convenience structure to hold a pair of current values.
typedef struct
{
float m1Current;
float m2Current;
} TMotorCurrents;
TMotorCurrents getMotorCurrents();
TPIDQ getPIDQ(WHICH_MOTOR whichMotor);
int32_t getM1Speed();
TPIDQ getM2PIDQ();
int32_t getM2Speed();
float getTemperature();
// Get velocity (speed) of a motor.
int32_t getVelocity(WHICH_VELOCITY whichVelocity);
// Get RoboClaw software versions.
std::string getVersion();
int motorAlarms() { return motorAlarms_; }
// Stop motion.
void stop();
private:
typedef struct
{
unsigned long p1;
unsigned long p2;
} ULongPair;
enum
{
kERROR_NORMAL = 0x00,
kM1OVERCURRENT = 0x01,
kM2OVERCURRENT = 0x02,
kESTOP = 0x04,
kTEMPERATURE = 0x08,
kMAINBATTERYHIGH = 0x10,
kMAINBATTERYLOW = 0x20,
kLOGICBATTERYHIGH = 0x40,
kLOGICBATTERYLOW = 0x80
};
// Enum values without a 'k' prefix have not been used in code.
typedef enum ROBOCLAW_COMMAND
{
M1FORWARD = 0,
M1BACKWARD = 1,
SETMINMB = 2,
SETMAXMB = 3,
M2FORWARD = 4,
M2BACKWARD = 5,
M17BIT = 6,
M27BIT = 7,
MIXEDFORWARD = 8,
MIXEDBACKWARD = 9,
MIXEDRIGHT = 10,
MIXEDLEFT = 11,
MIXEDFB = 12,
MIXEDLR = 13,
RESETENC = 20,
kGETVERSION = 21,
kSETM1ENCODER = 22,
kSETM2ENCODER = 23,
kGETMBATT = 24,
kGETLBATT = 25,
SETMINLB = 26,
SETMAXLB = 27,
kSETM1PID = 28,
kSETM2PID = 29,
GETM1ISPEED = 30,
GETM2ISPEED = 31,
M1DUTY = 32,
M2DUTY = 33,
kMIXEDDUTY = 34,
M1SPEED = 35,
M2SPEED = 36,
kMIXEDSPEED = 37,
M1SPEEDACCEL = 38,
M2SPEEDACCEL = 39,
MIXEDSPEEDACCEL = 40,
M1SPEEDDIST = 41,
M2SPEEDDIST = 42,
kMIXEDSPEEDDIST = 43,
M1SPEEDACCELDIST = 44,
M2SPEEDACCELDIST = 45,
kMIXEDSPEEDACCELDIST = 46,
GETBUFFERS = 47,
SETPWM = 48,
kGETCURRENTS = 49,
MIXEDSPEED2ACCEL = 50,
MIXEDSPEED2ACCELDIST = 51,
M1DUTYACCEL = 52,
M2DUTYACCEL = 53,
MIXEDDUTYACCEL = 54,
kGETTEMPERATURE = 82,
kGETERROR = 90,
WRITENVM = 94,
GETM1MAXCURRENT = 135
} ROBOCLAW_COMMAND;
int device_port_; // Unix file descriptor for RoboClaw connection.
float m1p_;
float m1i_;
float m1d_;
int m1qpps_;
float m2p_;
float m2i_;
float m2d_;
int m2qpps_;
int maxCommandRetries_; // Maximum number of times to retry a RoboClaw command.
float maxM1Current_; // Maximum allowed M1 current.
float maxM2Current_; // Maximum allowed M2 current.
int motorAlarms_; // Motors alarms. Bit-wise OR of contributors.
std::string device_name_; // Device name of RoboClaw device.
int portAddress_; // Port number of RoboClaw device under control
int vmin_; // Terminal control value.
int vtime_; // Terminal control value.
// Get velocity (speed) result from the RoboClaw controller.
int32_t getVelocityResult(uint8_t command);
unsigned long getUlongCommandResult(uint8_t command);
uint32_t getULongCont(uint16_t &crc);
unsigned short get2ByteCommandResult(uint8_t command);
// Open the RoboClaw USB port.
void openPort();
// Read one byte from device with timeout.
uint8_t readByteWithTimeout();
// Perform error recovery to re-open a failed device port.
void restartPort();
// Reset the encoders.
bool resetEncoders(ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response);
// Set the PID for motor M1.
void setM1PID(float p, float i, float d, uint32_t qpps);
// Set the PID for motor M1.
void setM2PID(float p, float i, float d, uint32_t qpps);
// Update the running CRC result.
void updateCrc(uint16_t &crc, uint8_t data);
// Write one byte to the device.
void writeByte(uint8_t byte);
// Write a stream of bytes to the device.
void writeN(bool sendCRC, uint8_t cnt, ...);
void SetEncoder(ROBOCLAW_COMMAND command, long value);
};

4
srv/ResetEncoders.srv Executable file
View File

@@ -0,0 +1,4 @@
int64 left
int64 right
---
bool ok