├── CMakeLists.txt ├── LICENSE ├── README.md ├── controllers └── robot_controller_example.yaml ├── fake_robot_hardware.xml ├── include └── diffdrive_arduino │ ├── arduino_comms.h │ ├── config.h │ ├── diffdrive_arduino.h │ ├── fake_robot.h │ └── wheel.h ├── launch ├── fake_robot.launch.py └── test_robot.launch.py ├── package.xml ├── robot_hardware.xml └── src ├── arduino_comms.cpp ├── diffdrive_arduino.cpp ├── diffdrive_robot.cpp ├── fake_robot.cpp └── wheel.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(diffdrive_arduino) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(serial REQUIRED) 16 | find_package(hardware_interface REQUIRED) 17 | find_package(controller_manager REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(pluginlib REQUIRED) 20 | 21 | 22 | 23 | 24 | 25 | 26 | add_library(diffdrive_arduino SHARED src/diffdrive_arduino.cpp src/wheel.cpp src/arduino_comms.cpp) 27 | 28 | target_include_directories( 29 | diffdrive_arduino 30 | PRIVATE 31 | include 32 | ) 33 | ament_target_dependencies( 34 | diffdrive_arduino 35 | hardware_interface 36 | controller_manager 37 | serial 38 | rclcpp 39 | pluginlib 40 | ) 41 | 42 | pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml) 43 | 44 | 45 | 46 | add_library(fake_robot SHARED src/fake_robot.cpp src/wheel.cpp) 47 | set_property(TARGET fake_robot PROPERTY POSITION_INDEPENDENT_CODE ON) 48 | 49 | target_include_directories( 50 | fake_robot 51 | PRIVATE 52 | include 53 | ) 54 | ament_target_dependencies( 55 | fake_robot 56 | hardware_interface 57 | controller_manager 58 | rclcpp 59 | pluginlib 60 | ) 61 | 62 | pluginlib_export_plugin_description_file(hardware_interface fake_robot_hardware.xml) 63 | 64 | 65 | 66 | 67 | install( 68 | TARGETS diffdrive_arduino fake_robot 69 | DESTINATION lib 70 | ) 71 | 72 | 73 | 74 | 75 | install( 76 | DIRECTORY controllers launch/ 77 | DESTINATION share/${PROJECT_NAME} 78 | ) 79 | 80 | 81 | ament_export_libraries( 82 | diffdrive_arduino 83 | fake_robot 84 | ) 85 | ament_export_dependencies( 86 | hardware_interface 87 | controller_manager 88 | serial 89 | rclcpp 90 | pluginlib 91 | ) 92 | 93 | ament_package() 94 | 95 | 96 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Josh Newans 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # diffdrive_arduino 2 | 3 | 4 | This node is designed to provide an interface between a `diff_drive_controller` from `ros_control` and an Arduino running firmware from `ros_arduino_bridge`. -------------------------------------------------------------------------------- /controllers/robot_controller_example.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 # Hz 4 | 5 | diff_controller: 6 | type: diff_drive_controller/DiffDriveController 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | diff_controller: 12 | ros__parameters: 13 | publish_rate: 50.0 14 | left_wheel_names: ['left_wheel_joint'] 15 | right_wheel_names: ['right_wheel_joint'] 16 | wheels_per_side: 1 17 | wheel_separation: 0.3 18 | wheel_radius: 0.05 19 | base_frame_id: base_link 20 | use_stamped_vel: false 21 | 22 | linear.x.has_velocity_limits: false 23 | linear.x.has_acceleration_limits: false 24 | linear.x.has_jerk_limits: false 25 | linear.x.max_velocity: 0.0 26 | linear.x.min_velocity: 0.0 27 | linear.x.max_acceleration: 0.0 28 | linear.x.min_acceleration: 0.0 29 | linear.x.max_jerk: 0.0 30 | linear.x.min_jerk: 0.0 31 | 32 | angular.z.has_velocity_limits: false 33 | angular.z.has_acceleration_limits: false 34 | angular.z.has_jerk_limits: false 35 | angular.z.max_velocity: 0.0 36 | angular.z.min_velocity: 0.0 37 | angular.z.max_acceleration: 0.0 38 | angular.z.min_acceleration: 0.0 39 | angular.z.max_jerk: 0.0 40 | angular.z.min_jerk: 0.0 41 | 42 | # joint_state_broadcaster: 43 | # ros__parameters: 44 | -------------------------------------------------------------------------------- /fake_robot_hardware.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /include/diffdrive_arduino/arduino_comms.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H 2 | #define DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H 3 | 4 | #include 5 | #include 6 | 7 | class ArduinoComms 8 | { 9 | 10 | 11 | public: 12 | 13 | ArduinoComms() 14 | { } 15 | 16 | ArduinoComms(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) 17 | : serial_conn_(serial_device, baud_rate, serial::Timeout::simpleTimeout(timeout_ms)) 18 | { } 19 | 20 | void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms); 21 | void sendEmptyMsg(); 22 | void readEncoderValues(int &val_1, int &val_2); 23 | void setMotorValues(int val_1, int val_2); 24 | void setPidValues(float k_p, float k_d, float k_i, float k_o); 25 | 26 | bool connected() const { return serial_conn_.isOpen(); } 27 | 28 | std::string sendMsg(const std::string &msg_to_send, bool print_output = false); 29 | 30 | 31 | private: 32 | serial::Serial serial_conn_; ///< Underlying serial connection 33 | }; 34 | 35 | #endif // DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H -------------------------------------------------------------------------------- /include/diffdrive_arduino/config.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVE_ARDUINO_CONFIG_H 2 | #define DIFFDRIVE_ARDUINO_CONFIG_H 3 | 4 | #include 5 | 6 | 7 | struct Config 8 | { 9 | std::string left_wheel_name = "left_wheel"; 10 | std::string right_wheel_name = "right_wheel"; 11 | float loop_rate = 30; 12 | std::string device = "/dev/ttyUSB0"; 13 | int baud_rate = 57600; 14 | int timeout = 1000; 15 | int enc_counts_per_rev = 1920; 16 | }; 17 | 18 | 19 | #endif // DIFFDRIVE_ARDUINO_CONFIG_H -------------------------------------------------------------------------------- /include/diffdrive_arduino/diffdrive_arduino.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVE_ARDUINO_REAL_ROBOT_H 2 | #define DIFFDRIVE_ARDUINO_REAL_ROBOT_H 3 | 4 | #include 5 | #include "rclcpp/rclcpp.hpp" 6 | 7 | #include "hardware_interface/base_interface.hpp" 8 | #include "hardware_interface/system_interface.hpp" 9 | #include "hardware_interface/handle.hpp" 10 | #include "hardware_interface/hardware_info.hpp" 11 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 12 | #include "hardware_interface/types/hardware_interface_status_values.hpp" 13 | 14 | #include "config.h" 15 | #include "wheel.h" 16 | #include "arduino_comms.h" 17 | 18 | 19 | using hardware_interface::return_type; 20 | 21 | class DiffDriveArduino : public hardware_interface::BaseInterface 22 | { 23 | 24 | 25 | public: 26 | DiffDriveArduino(); 27 | 28 | return_type configure(const hardware_interface::HardwareInfo & info) override; 29 | 30 | std::vector export_state_interfaces() override; 31 | 32 | std::vector export_command_interfaces() override; 33 | 34 | return_type start() override; 35 | 36 | return_type stop() override; 37 | 38 | return_type read() override; 39 | 40 | return_type write() override; 41 | 42 | 43 | 44 | private: 45 | 46 | Config cfg_; 47 | ArduinoComms arduino_; 48 | 49 | Wheel l_wheel_; 50 | Wheel r_wheel_; 51 | 52 | rclcpp::Logger logger_; 53 | 54 | std::chrono::time_point time_; 55 | 56 | }; 57 | 58 | 59 | #endif // DIFFDRIVE_ARDUINO_REAL_ROBOT_H -------------------------------------------------------------------------------- /include/diffdrive_arduino/fake_robot.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVE_ARDUINO_FAKE_ROBOT_H 2 | #define DIFFDRIVE_ARDUINO_FAKE_ROBOT_H 3 | 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | 7 | #include "hardware_interface/base_interface.hpp" 8 | #include "hardware_interface/system_interface.hpp" 9 | #include "hardware_interface/handle.hpp" 10 | #include "hardware_interface/hardware_info.hpp" 11 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 12 | #include "hardware_interface/types/hardware_interface_status_values.hpp" 13 | 14 | #include "config.h" 15 | #include "wheel.h" 16 | 17 | using hardware_interface::return_type; 18 | 19 | class FakeRobot : public hardware_interface::BaseInterface 20 | { 21 | 22 | 23 | public: 24 | FakeRobot(); 25 | 26 | return_type configure(const hardware_interface::HardwareInfo & info) override; 27 | 28 | std::vector export_state_interfaces() override; 29 | 30 | std::vector export_command_interfaces() override; 31 | 32 | return_type start() override; 33 | 34 | return_type stop() override; 35 | 36 | return_type read() override; 37 | 38 | return_type write() override; 39 | 40 | 41 | 42 | private: 43 | 44 | Config cfg_; 45 | 46 | Wheel l_wheel_; 47 | Wheel r_wheel_; 48 | 49 | rclcpp::Logger logger_; 50 | 51 | std::chrono::time_point time_; 52 | 53 | }; 54 | 55 | 56 | #endif // DIFFDRIVE_ARDUINO_FAKE_ROBOT_H -------------------------------------------------------------------------------- /include/diffdrive_arduino/wheel.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVE_ARDUINO_WHEEL_H 2 | #define DIFFDRIVE_ARDUINO_WHEEL_H 3 | 4 | #include 5 | 6 | 7 | 8 | class Wheel 9 | { 10 | public: 11 | 12 | std::string name = ""; 13 | int enc = 0; 14 | double cmd = 0; 15 | double pos = 0; 16 | double vel = 0; 17 | double eff = 0; 18 | double velSetPt = 0; 19 | double rads_per_count = 0; 20 | 21 | Wheel() = default; 22 | 23 | Wheel(const std::string &wheel_name, int counts_per_rev); 24 | 25 | void setup(const std::string &wheel_name, int counts_per_rev); 26 | 27 | double calcEncAngle(); 28 | 29 | 30 | 31 | }; 32 | 33 | 34 | #endif // DIFFDRIVE_ARDUINO_WHEEL_H -------------------------------------------------------------------------------- /launch/fake_robot.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 ROS2-Control Development Team (2020) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch_ros.actions import Node 21 | 22 | import xacro 23 | 24 | 25 | def generate_launch_description(): 26 | 27 | # Get URDF via xacro 28 | robot_description_path = os.path.join( 29 | get_package_share_directory('diffdrive_arduino'), 30 | 'description', 31 | 'robot.urdf') 32 | robot_description_config = xacro.process_file(robot_description_path) 33 | robot_description = {'robot_description': robot_description_config.toxml()} 34 | 35 | test_controller = os.path.join( 36 | get_package_share_directory('diffdrive_arduino'), 37 | 'controllers', 38 | 'fake_robot_controller.yaml' 39 | ) 40 | 41 | return LaunchDescription([ 42 | Node( 43 | package='controller_manager', 44 | executable='ros2_control_node', 45 | parameters=[robot_description, test_controller], 46 | output={ 47 | 'stdout': 'screen', 48 | 'stderr': 'screen', 49 | }, 50 | ) 51 | 52 | ]) 53 | -------------------------------------------------------------------------------- /launch/test_robot.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 ROS2-Control Development Team (2020) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch_ros.actions import Node 21 | 22 | import xacro 23 | 24 | 25 | def generate_launch_description(): 26 | 27 | # Get URDF via xacro 28 | robot_description_path = os.path.join( 29 | get_package_share_directory('diffdrive_arduino'), 30 | 'description', 31 | 'robot.urdf') 32 | robot_description_config = xacro.process_file(robot_description_path) 33 | robot_description = {'robot_description': robot_description_config.toxml()} 34 | 35 | test_controller = os.path.join( 36 | get_package_share_directory('diffdrive_arduino'), 37 | 'controllers', 38 | 'robot_controller.yaml' 39 | ) 40 | 41 | return LaunchDescription([ 42 | Node( 43 | package='controller_manager', 44 | executable='ros2_control_node', 45 | parameters=[robot_description, test_controller], 46 | output={ 47 | 'stdout': 'screen', 48 | 'stderr': 'screen', 49 | }, 50 | ) 51 | 52 | ]) 53 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | diffdrive_arduino 4 | 0.0.1 5 | Provides an interface between diff_drive_controller and an Arduino. 6 | 7 | Josh Newans 8 | Josh Newans 9 | BSD 10 | 11 | 12 | 13 | https://github.com/joshnewans/diffdrive_arduino/issues 14 | https://github.com/joshnewans/diffdrive_arduino 15 | 16 | 17 | 18 | ament_cmake 19 | 20 | hardware_interface 21 | controller_manager 22 | pluginlib 23 | rclcpp 24 | serial 25 | 26 | 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /robot_hardware.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Hardware Interface for Differential Drive Arduino controller. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/arduino_comms.cpp: -------------------------------------------------------------------------------- 1 | #include "diffdrive_arduino/arduino_comms.h" 2 | // #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | void ArduinoComms::setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) 9 | { 10 | serial_conn_.setPort(serial_device); 11 | serial_conn_.setBaudrate(baud_rate); 12 | serial::Timeout tt = serial::Timeout::simpleTimeout(timeout_ms); 13 | serial_conn_.setTimeout(tt); // This should be inline except setTimeout takes a reference and so needs a variable 14 | serial_conn_.open(); 15 | // serial_conn_.(serial_device, baud_rate, serial::Timeout::simpleTimeout(timeout_ms)); 16 | 17 | } 18 | 19 | 20 | void ArduinoComms::sendEmptyMsg() 21 | { 22 | std::string response = sendMsg("\r"); 23 | } 24 | 25 | void ArduinoComms::readEncoderValues(int &val_1, int &val_2) 26 | { 27 | std::string response = sendMsg("e\r"); 28 | 29 | std::string delimiter = " "; 30 | size_t del_pos = response.find(delimiter); 31 | std::string token_1 = response.substr(0, del_pos); 32 | std::string token_2 = response.substr(del_pos + delimiter.length()); 33 | 34 | val_1 = std::atoi(token_1.c_str()); 35 | val_2 = std::atoi(token_2.c_str()); 36 | } 37 | 38 | void ArduinoComms::setMotorValues(int val_1, int val_2) 39 | { 40 | std::stringstream ss; 41 | ss << "m " << val_1 << " " << val_2 << "\r"; 42 | sendMsg(ss.str(), false); 43 | } 44 | 45 | void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o) 46 | { 47 | std::stringstream ss; 48 | ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r"; 49 | sendMsg(ss.str()); 50 | } 51 | 52 | std::string ArduinoComms::sendMsg(const std::string &msg_to_send, bool print_output) 53 | { 54 | serial_conn_.write(msg_to_send); 55 | std::string response = serial_conn_.readline(); 56 | 57 | if (print_output) 58 | { 59 | // RCLCPP_INFO_STREAM(logger_,"Sent: " << msg_to_send); 60 | // RCLCPP_INFO_STREAM(logger_,"Received: " << response); 61 | } 62 | 63 | return response; 64 | } -------------------------------------------------------------------------------- /src/diffdrive_arduino.cpp: -------------------------------------------------------------------------------- 1 | #include "diffdrive_arduino/diffdrive_arduino.h" 2 | 3 | 4 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 5 | 6 | 7 | 8 | 9 | DiffDriveArduino::DiffDriveArduino() 10 | : logger_(rclcpp::get_logger("DiffDriveArduino")) 11 | {} 12 | 13 | 14 | 15 | 16 | 17 | return_type DiffDriveArduino::configure(const hardware_interface::HardwareInfo & info) 18 | { 19 | if (configure_default(info) != return_type::OK) { 20 | return return_type::ERROR; 21 | } 22 | 23 | RCLCPP_INFO(logger_, "Configuring..."); 24 | 25 | time_ = std::chrono::system_clock::now(); 26 | 27 | cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"]; 28 | cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"]; 29 | cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]); 30 | cfg_.device = info_.hardware_parameters["device"]; 31 | cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]); 32 | cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]); 33 | cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]); 34 | 35 | // Set up the wheels 36 | l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); 37 | r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); 38 | 39 | // Set up the Arduino 40 | arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); 41 | 42 | RCLCPP_INFO(logger_, "Finished Configuration"); 43 | 44 | status_ = hardware_interface::status::CONFIGURED; 45 | return return_type::OK; 46 | } 47 | 48 | std::vector DiffDriveArduino::export_state_interfaces() 49 | { 50 | // We need to set up a position and a velocity interface for each wheel 51 | 52 | std::vector state_interfaces; 53 | 54 | state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel)); 55 | state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos)); 56 | state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel)); 57 | state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos)); 58 | 59 | return state_interfaces; 60 | } 61 | 62 | std::vector DiffDriveArduino::export_command_interfaces() 63 | { 64 | // We need to set up a velocity command interface for each wheel 65 | 66 | std::vector command_interfaces; 67 | 68 | command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd)); 69 | command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd)); 70 | 71 | return command_interfaces; 72 | } 73 | 74 | 75 | return_type DiffDriveArduino::start() 76 | { 77 | RCLCPP_INFO(logger_, "Starting Controller..."); 78 | 79 | arduino_.sendEmptyMsg(); 80 | // arduino.setPidValues(9,7,0,100); 81 | // arduino.setPidValues(14,7,0,100); 82 | arduino_.setPidValues(30, 20, 0, 100); 83 | 84 | status_ = hardware_interface::status::STARTED; 85 | 86 | return return_type::OK; 87 | } 88 | 89 | return_type DiffDriveArduino::stop() 90 | { 91 | RCLCPP_INFO(logger_, "Stopping Controller..."); 92 | status_ = hardware_interface::status::STOPPED; 93 | 94 | return return_type::OK; 95 | } 96 | 97 | hardware_interface::return_type DiffDriveArduino::read() 98 | { 99 | 100 | // TODO fix chrono duration 101 | 102 | // Calculate time delta 103 | auto new_time = std::chrono::system_clock::now(); 104 | std::chrono::duration diff = new_time - time_; 105 | double deltaSeconds = diff.count(); 106 | time_ = new_time; 107 | 108 | 109 | if (!arduino_.connected()) 110 | { 111 | return return_type::ERROR; 112 | } 113 | 114 | arduino_.readEncoderValues(l_wheel_.enc, r_wheel_.enc); 115 | 116 | double pos_prev = l_wheel_.pos; 117 | l_wheel_.pos = l_wheel_.calcEncAngle(); 118 | l_wheel_.vel = (l_wheel_.pos - pos_prev) / deltaSeconds; 119 | 120 | pos_prev = r_wheel_.pos; 121 | r_wheel_.pos = r_wheel_.calcEncAngle(); 122 | r_wheel_.vel = (r_wheel_.pos - pos_prev) / deltaSeconds; 123 | 124 | 125 | 126 | return return_type::OK; 127 | 128 | 129 | } 130 | 131 | hardware_interface::return_type DiffDriveArduino::write() 132 | { 133 | 134 | if (!arduino_.connected()) 135 | { 136 | return return_type::ERROR; 137 | } 138 | 139 | arduino_.setMotorValues(l_wheel_.cmd / l_wheel_.rads_per_count / cfg_.loop_rate, r_wheel_.cmd / r_wheel_.rads_per_count / cfg_.loop_rate); 140 | 141 | 142 | 143 | 144 | return return_type::OK; 145 | 146 | 147 | 148 | } 149 | 150 | 151 | 152 | #include "pluginlib/class_list_macros.hpp" 153 | 154 | PLUGINLIB_EXPORT_CLASS( 155 | DiffDriveArduino, 156 | hardware_interface::SystemInterface 157 | ) -------------------------------------------------------------------------------- /src/diffdrive_robot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "diffdrive_arduino/diffdrive_arduino.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "diffdrive_robot"); 8 | ros::NodeHandle n("~"); 9 | 10 | DiffDriveArduino::Config robot_cfg; 11 | 12 | // Attempt to retrieve parameters. If they don't exist, the default values from the struct will be used 13 | n.getParam("left_wheel_name", robot_cfg.left_wheel_name); 14 | n.getParam("right_wheel_name", robot_cfg.right_wheel_name); 15 | n.getParam("baud_rate", robot_cfg.baud_rate); 16 | n.getParam("device", robot_cfg.device); 17 | n.getParam("enc_counts_per_rev", robot_cfg.enc_counts_per_rev); 18 | n.getParam("robot_loop_rate", robot_cfg.loop_rate); 19 | 20 | 21 | DiffDriveArduino robot(robot_cfg); 22 | controller_manager::ControllerManager cm(&robot); 23 | 24 | ros::AsyncSpinner spinner(1); 25 | spinner.start(); 26 | 27 | ros::Time prevTime = ros::Time::now(); 28 | 29 | ros::Rate loop_rate(10); 30 | 31 | while (ros::ok()) 32 | { 33 | robot.read(); 34 | cm.update(robot.get_time(), robot.get_period()); 35 | robot.write(); 36 | 37 | loop_rate.sleep(); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/fake_robot.cpp: -------------------------------------------------------------------------------- 1 | #include "diffdrive_arduino/fake_robot.h" 2 | 3 | 4 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 5 | 6 | 7 | FakeRobot::FakeRobot() 8 | : logger_(rclcpp::get_logger("FakeRobot")) 9 | {} 10 | 11 | 12 | 13 | return_type FakeRobot::configure(const hardware_interface::HardwareInfo & info) 14 | { 15 | if (configure_default(info) != return_type::OK) { 16 | return return_type::ERROR; 17 | } 18 | 19 | RCLCPP_INFO(logger_, "Configuring..."); 20 | 21 | time_ = std::chrono::system_clock::now(); 22 | 23 | cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"]; 24 | cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"]; 25 | 26 | 27 | // Set up the wheels 28 | // Note: It doesn't matter that we haven't set encoder counts per rev 29 | // since the fake robot bypasses the encoder code completely 30 | 31 | l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); 32 | r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); 33 | 34 | RCLCPP_INFO(logger_, "Finished Configuration"); 35 | 36 | status_ = hardware_interface::status::CONFIGURED; 37 | return return_type::OK; 38 | } 39 | 40 | std::vector FakeRobot::export_state_interfaces() 41 | { 42 | // We need to set up a position and a velocity interface for each wheel 43 | 44 | std::vector state_interfaces; 45 | 46 | state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel)); 47 | state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos)); 48 | state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel)); 49 | state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos)); 50 | 51 | return state_interfaces; 52 | } 53 | 54 | std::vector FakeRobot::export_command_interfaces() 55 | { 56 | // We need to set up a velocity command interface for each wheel 57 | 58 | std::vector command_interfaces; 59 | 60 | command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd)); 61 | command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd)); 62 | 63 | return command_interfaces; 64 | } 65 | 66 | 67 | return_type FakeRobot::start() 68 | { 69 | RCLCPP_INFO(logger_, "Starting Controller..."); 70 | status_ = hardware_interface::status::STARTED; 71 | 72 | return return_type::OK; 73 | } 74 | 75 | return_type FakeRobot::stop() 76 | { 77 | RCLCPP_INFO(logger_, "Stopping Controller..."); 78 | status_ = hardware_interface::status::STOPPED; 79 | 80 | return return_type::OK; 81 | } 82 | 83 | hardware_interface::return_type FakeRobot::read() 84 | { 85 | 86 | // TODO fix chrono duration 87 | 88 | // Calculate time delta 89 | auto new_time = std::chrono::system_clock::now(); 90 | std::chrono::duration diff = new_time - time_; 91 | double deltaSeconds = diff.count(); 92 | time_ = new_time; 93 | 94 | 95 | // Force the wheel position 96 | l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; 97 | r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; 98 | 99 | return return_type::OK; 100 | 101 | 102 | } 103 | 104 | hardware_interface::return_type FakeRobot::write() 105 | { 106 | 107 | // Set the wheel velocities to directly match what is commanded 108 | 109 | l_wheel_.vel = l_wheel_.cmd; 110 | r_wheel_.vel = r_wheel_.cmd; 111 | 112 | 113 | return return_type::OK; 114 | } 115 | 116 | 117 | 118 | #include "pluginlib/class_list_macros.hpp" 119 | 120 | PLUGINLIB_EXPORT_CLASS( 121 | FakeRobot, 122 | hardware_interface::SystemInterface 123 | ) -------------------------------------------------------------------------------- /src/wheel.cpp: -------------------------------------------------------------------------------- 1 | #include "diffdrive_arduino/wheel.h" 2 | 3 | #include 4 | 5 | 6 | Wheel::Wheel(const std::string &wheel_name, int counts_per_rev) 7 | { 8 | setup(wheel_name, counts_per_rev); 9 | } 10 | 11 | 12 | void Wheel::setup(const std::string &wheel_name, int counts_per_rev) 13 | { 14 | name = wheel_name; 15 | rads_per_count = (2*M_PI)/counts_per_rev; 16 | } 17 | 18 | double Wheel::calcEncAngle() 19 | { 20 | return enc * rads_per_count; 21 | } --------------------------------------------------------------------------------