diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..e526d5a --- /dev/null +++ b/CMakeLists.txt @@ -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() diff --git a/README.md b/README.md index cf8578b..c6b404e 100644 --- a/README.md +++ b/README.md @@ -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. \ No newline at end of file diff --git a/TODO b/TODO new file mode 100644 index 0000000..b49e32b --- /dev/null +++ b/TODO @@ -0,0 +1,3 @@ +Remove odom transform code, move to static +Fix roboclaw status to error string mapping +Add service handler to reset encoders \ No newline at end of file diff --git a/config/motor_driver.yaml b/config/motor_driver.yaml new file mode 100755 index 0000000..ceba926 --- /dev/null +++ b/config/motor_driver.yaml @@ -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 + diff --git a/launch/ros2_roboclaw_driver.launch.py b/launch/ros2_roboclaw_driver.launch.py new file mode 100644 index 0000000..e0eebeb --- /dev/null +++ b/launch/ros2_roboclaw_driver.launch.py @@ -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 diff --git a/msg/RoboClawStatus.msg b/msg/RoboClawStatus.msg new file mode 100755 index 0000000..a796342 --- /dev/null +++ b/msg/RoboClawStatus.msg @@ -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 \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100755 index 0000000..443b332 --- /dev/null +++ b/package.xml @@ -0,0 +1,31 @@ + + + + ros2_roboclaw_driver + 1.0.0 + A driver for RoboClaw motor controller devices for use with ROS2 + Michael Wimble + MIT + + ament_cmake + + buildin_interfaces + rclcpp + + geometry_msgs + nav_msgs + std_msgs + tf2_msgs + tf2_ros + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp new file mode 100755 index 0000000..35ea419 --- /dev/null +++ b/src/motor_driver.cpp @@ -0,0 +1,160 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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("accel_quad_pulses_per_second", 600); + this->declare_parameter("device_name", "roboclaw"); + this->declare_parameter("device_port", 123); + this->declare_parameter("m1_p", 0.0); + this->declare_parameter("m1_i", 0.0); + this->declare_parameter("m1_d", 0.0); + this->declare_parameter("m1_qpps", 0); + this->declare_parameter("m1_max_current", 0.0); + this->declare_parameter("m2_p", 0.0); + this->declare_parameter("m2_i", 0.0); + this->declare_parameter("m2_d", 0.0); + this->declare_parameter("m2_qpps", 0); + this->declare_parameter("max_angular_velocity", 0.0); + this->declare_parameter("max_linear_velocity", 0.0); + this->declare_parameter("m2_max_current", 0.0); + this->declare_parameter("max_seconds_uncommanded_travel", 0.0); + this->declare_parameter("quad_pulses_per_meter", 0); + this->declare_parameter("quad_pulses_per_revolution", 0); + this->declare_parameter("vmin", 1); + this->declare_parameter("vtime", 2); + this->declare_parameter("wheel_radius", 0.0); + this->declare_parameter("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(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( + "/cmd_vel", bestEffortQos, std::bind(&MotorDriver::cmdVelCallback, this, std::placeholders::_1)); + odomSub_ = node->create_subscription( + "/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; \ No newline at end of file diff --git a/src/motor_driver.h b/src/motor_driver.h new file mode 100755 index 0000000..bf57eec --- /dev/null +++ b/src/motor_driver.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#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 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::SharedPtr cmdVelSub_; + rclcpp::Subscription::SharedPtr odomSub_; + + static MotorDriver *g_singleton; +}; diff --git a/src/motor_driver_node.cpp b/src/motor_driver_node.cpp new file mode 100644 index 0000000..231f640 --- /dev/null +++ b/src/motor_driver_node.cpp @@ -0,0 +1,114 @@ +#include +#include + +#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("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::SharedPtr statusPublisher = node->create_publisher(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; +} \ No newline at end of file diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp new file mode 100755 index 0000000..267e722 --- /dev/null +++ b/src/roboclaw.cpp @@ -0,0 +1,966 @@ +#include "roboclaw.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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); +} diff --git a/src/roboclaw.h b/src/roboclaw.h new file mode 100755 index 0000000..de9c69e --- /dev/null +++ b/src/roboclaw.h @@ -0,0 +1,258 @@ +#pragma once + +#include "ros2_roboclaw_driver/srv/reset_encoders.hpp" +#include +#include + +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); +}; diff --git a/srv/ResetEncoders.srv b/srv/ResetEncoders.srv new file mode 100755 index 0000000..86e69af --- /dev/null +++ b/srv/ResetEncoders.srv @@ -0,0 +1,4 @@ +int64 left +int64 right +--- +bool ok \ No newline at end of file