commit 94b2281895cc9ab97ade9200ba3ebe2d0465cba0 Author: zhanli <719901725@qq.com> Date: Wed Jul 3 17:19:25 2024 +0800 上传ROS驱动 diff --git a/README.md b/README.md new file mode 100644 index 0000000..b88802b --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +## 自主机器人笨笨 + +采用无刷电机的大型机器人框架，目前ROS驱动暂时没有验证。 \ No newline at end of file diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/CMakeLists.txt b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/CMakeLists.txt new file mode 100644 index 0000000..226a769 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hoverboard_driver) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + hardware_interface + controller_manager + dynamic_reconfigure + control_toolbox + rosparam_shortcuts +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + +generate_dynamic_reconfigure_options( + config/Hoverboard.cfg +) + +catkin_package( + INCLUDE_DIRS include +) + +include_directories( + include + include/hoverboard_driver + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +add_executable(${PROJECT_NAME} + src/hoverboard.cpp + src/pid.cpp + src/main.cpp +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_dependencies(hoverboard_driver ${PROJECT_NAME}_gencfg) + diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/LICENSE b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/LICENSE new file mode 100644 index 0000000..bd9124d --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Alex Makarov + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/README.md b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/README.md new file mode 100644 index 0000000..0eef285 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/README.md @@ -0,0 +1,26 @@ + + +## Usage + +1. Hoverboard port can be set as a "port" parameter in the node namespace. Check `hoverboard.launch` and `4x4.launch` for examples of 2WD and 4WD configurations. 4WD config is purely for illustration on how to use several ports, but was **not** tested with two hoverboards. +2. `roslaunch hoverboard_driver hoverboard.launch` +3. Use any tool (keyboard_teleop, rqt) to send speed commands to `hoverboard_velocity_controller/cmd_vel`. + +In 4x4 configuration, if you want to control both axis with the same `cmd_vel`, you can apply remapping as following: +```xml + + + + + + + + + + + + +``` + +## DISCLAIMER +I bear **no responsibility** for any damage, direct or indirect, caused by using this project. Hoverboards are powerful and can be dangerous! Make sure you take all safety precautions! diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/Hoverboard.cfg b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/Hoverboard.cfg new file mode 100644 index 0000000..119fe5b --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/Hoverboard.cfg @@ -0,0 +1,18 @@ +#!/usr/bin/env python +# Desc: Allows PID parameters and feed forward gain, etc to be tuned in realtime using dynamic reconfigure +PACKAGE = "hoverboard_driver" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Name Type Level Description Default Min Max +gen.add( "f" , double_t, 1, "Feed forward gain.", 10.0, -100, 100) +gen.add( "p" , double_t, 1, "Proportional gain.", 10.0, -100, 100) +gen.add( "i" , double_t, 1, "Integral gain.", 0.1, -100, 100) +gen.add( "d" , double_t, 1, "Derivative gain.", 1.0, -100, 100) +gen.add( "i_clamp_min" , double_t, 1, "Min bounds for the integral windup", -10.0, -100, 0) +gen.add( "i_clamp_max" , double_t, 1, "Max bounds for the integral windup", 10.0, 0, 100) +gen.add( "antiwindup" , bool_t, 1, "Antiwindup.", False) + # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py +exit(gen.generate(PACKAGE, "hoverboard_driver", "Hoverboard")) diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/controllers.yaml b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/controllers.yaml new file mode 100644 index 0000000..9e8959e --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/controllers.yaml @@ -0,0 +1,51 @@ +hoverboard_joint_publisher: + type: "joint_state_controller/JointStateController" + publish_rate: 50 + left_wheel : 'left_wheel' + right_wheel : 'right_wheel' + +hoverboard_velocity_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel' + right_wheel : 'right_wheel' + publish_rate: 50.0 # default: 50 + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.32 + wheel_radius : 0.0825 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 0.5 + + # Base frame_id + base_frame_id: base_footprint #default: base_link + allow_multiple_cmd_vel_publishers: true + odom_frame_id: '/raw_odom' + enable_odom_tf: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 1.0 # m/s + has_acceleration_limits: true + max_acceleration : 0.8 # m/s^2 + has_jerk_limits : false + max_jerk : 0.0 # m/s^3 + angular: + z: + has_velocity_limits : true # was true + max_velocity : 6.28 # rad/s + has_acceleration_limits: true # was true + max_acceleration : 3.14 # rad/s^2 + has_jerk_limits : true + max_jerk : 3.14 # rad/s^3 diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/hardware.yaml b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/hardware.yaml new file mode 100644 index 0000000..86cc5bd --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/config/hardware.yaml @@ -0,0 +1,7 @@ +robaka: + direction: 1 + hardware_interface: + loop_hz: 50 # hz + joints: + - left_wheel + - right_wheel diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/how to start.txt b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/how to start.txt new file mode 100644 index 0000000..c8564c5 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/how to start.txt @@ -0,0 +1,18 @@ +Usage +Hoverboard port can be set as a "port" parameter in the node namespace. Check hoverboard.launch and 4x4.launch for examples of 2WD and 4WD configurations. 4WD config is purely for illustration on how to use several ports, but was not tested with two hoverboards. +roslaunch hoverboard_driver hoverboard.launch +Use any tool (keyboard_teleop, rqt) to send speed commands to hoverboard_velocity_controller/cmd_vel. +In 4x4 configuration, if you want to control both axis with the same cmd_vel, you can apply remapping as following: + + + + + + + + + + + + + \ No newline at end of file diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/config.h b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/config.h new file mode 100644 index 0000000..0aa338c --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/config.h @@ -0,0 +1,10 @@ +#pragma once + +#define DEFAULT_PORT "/dev/ttyTHS1" + +#define ENCODER_MIN 0 +#define ENCODER_MAX 9000 +#define ENCODER_LOW_WRAP_FACTOR 0.3 +#define ENCODER_HIGH_WRAP_FACTOR 0.7 + +#define TICKS_PER_ROTATION 90 diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/hoverboard.h b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/hoverboard.h new file mode 100644 index 0000000..13f3723 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/hoverboard.h @@ -0,0 +1,74 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "hoverboard_driver/HoverboardConfig.h" +#include "hoverboard_driver/pid.h" +#include "protocol.h" + +class HoverboardAPI; + +class Hoverboard : public hardware_interface::RobotHW { +public: + Hoverboard(); + ~Hoverboard(); + + void read(); + void write(const ros::Time& time, const ros::Duration& period); + void tick(); + private: + void protocol_recv (char c); + void on_encoder_update (int16_t right, int16_t left); + + hardware_interface::JointStateInterface joint_state_interface; + hardware_interface::VelocityJointInterface velocity_joint_interface; + + // The units for wheels are radians (pos), radians per second (vel,cmd), and Netwton metres (eff) + struct Joint { + std_msgs::Float64 pos; + std_msgs::Float64 vel; + std_msgs::Float64 eff; + std_msgs::Float64 cmd; + } joints[2]; + + // Publishers + ros::NodeHandle nh; + ros::Publisher vel_pub[2]; + ros::Publisher pos_pub[2]; + ros::Publisher cmd_pub[2]; + ros::Publisher voltage_pub; + ros::Publisher temp_pub; + ros::Publisher connected_pub; + + double wheel_radius; + double max_velocity = 0.0; + int direction_correction = 1; + std::string port; + + ros::Time last_read; + // Last known encoder values + int16_t last_wheelcountR; + int16_t last_wheelcountL; + // Count of full encoder wraps + int multR; + int multL; + // Thresholds for calculating the wrap + int low_wrap; + int high_wrap; + + // Hoverboard protocol + int port_fd; + int msg_len = 0; + char prev_byte = 0; + uint16_t start_frame = 0; + char* p; + SerialFeedback msg, prev_msg; + + PID pids[2]; +}; diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/pid.h b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/pid.h new file mode 100644 index 0000000..38ea6e6 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/pid.h @@ -0,0 +1,134 @@ +// This file is based on work by Franz Pucher, Diffbot, (2020), GitHub repository, https://github.com/fjp/diffbot +#include +#include +// Dynamic reconfigure +#include +#include "hoverboard_driver/HoverboardConfig.h" +#include + +class PID : public control_toolbox::Pid +{ + public: + /** + * @brief Construct a new PID object + * + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param out_min The min computed output. + * @param out_max The max computed output. + */ + PID(double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=0.0, bool antiwindup=false, double out_max=0.0, double out_min=0.0); + + /** + * @brief Initialize the + * + * @param nh ROS node handle with possible namespace describing the purpose of the PID controller. + * @param kF The feed forward gain. + * @param kP The proportional gain. + * @param kI The integral gain. + * @param kD The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup + * @param out_min The min computed output. + * @param out_max The max computed output. + */ + void init(ros::NodeHandle& nh, double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min); + + /** + * @brief Compute PID output value from error using process value, set point and time period + * + * @param measured_value The process value measured by sensors. + * @param setpoint The desired target value. + * @param dt The delta time or period since the last call. + * @return double Computed PID output value. + */ + double operator()(const double &measured_value, const double &setpoint, const ros::Duration &dt); + + /** + * @brief Get the FPID parameters + * + * @param f The feed forward gain. + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup Enable or disable antiwindup check. + */ + void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min); + void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); + + /** + * @brief Set the Parameters using the Gains object of control_toolbox::Pid + * + * @param f The feed forward gain. + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup Enable or disable antiwindup check. + */ + void setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + + /** + * @brief Set the Output Limits of the PID controller + * + * @param out_min + * @param out_max + */ + void setOutputLimits(double out_min, double out_max); + + /** + * @brief Clam given value to upper and lower limits. + * + * @param value Input value that's possibly clamped. + * @param lower_limit Lower limit which the value must not exceed. + * @param upper_limit Upper limit which the value must not exceed. + * @return double Clamped value to range in between [lower_limit, upper_limit]. + */ + double clamp(const double& value, const double& lower_limit, const double& upper_limit); + + /** + * @brief Get the current error. + * + * @return double The current error computed from the measured and target value. + */ + inline double getError() const { return error_; }; + + /** + * @brief Start the dynamic reconfigure node and load the default values + * @param node - a node handle where dynamic reconfigure services will be published + */ + void initDynamicReconfig(ros::NodeHandle &node); + + /** + * @brief Set Dynamic Reconfigure's gains to PID's values + */ + void updateDynamicReconfig(); + void updateDynamicReconfig(Gains gains_config); + void updateDynamicReconfig(hoverboard_driver::HoverboardConfig config); + + /** + * \brief Update the PID parameters from dynamics reconfigure + */ + void dynamicReconfigCallback(hoverboard_driver::HoverboardConfig &config, uint32_t /*level*/); + + private: + double f_; + double error_; + double out_min_; + double out_max_; + + // Dynamic reconfigure + bool dynamic_reconfig_initialized_; + typedef dynamic_reconfigure::Server DynamicReconfigServer; + boost::shared_ptr param_reconfig_server_; + DynamicReconfigServer::CallbackType param_reconfig_callback_; + + boost::recursive_mutex param_reconfig_mutex_; +}; diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/protocol.h b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/protocol.h new file mode 100644 index 0000000..9a65d32 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/include/hoverboard_driver/protocol.h @@ -0,0 +1,33 @@ +// Protocol support file +// Based on Arduino sample https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/master/Arduino/hoverserial/hoverserial.ino +// From Emanuel FERU's hoverboard-firmware-hack-FOC firmware + +#ifndef _FOC_PROTOCOL_H +#define _FOC_PROTOCOL_H + +#define START_FRAME 0xABCD + +// 串口命令信息 +typedef struct { + uint16_t start; // 开始帧头 + int16_t steer; + int16_t speed; // + uint16_t checksum; +} SerialCommand; + +// 串口的返回信息 +typedef struct { + uint16_t start; // 开始帧头 + int16_t cmd1; // cmd1 + int16_t cmd2; // cmd2 + int16_t speedR_meas; // 右轮测速 + int16_t speedL_meas; // 左轮测速 + int16_t wheelR_cnt; // 右轮计数 + int16_t wheelL_cnt; // 左轮计数 + int16_t batVoltage; // 电池电压 + int16_t boardTemp; // 板子温度 + uint16_t cmdLed; // LED命令 + uint16_t checksum; // 校验位 +} SerialFeedback; + +#endif diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/4x4.launch b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/4x4.launch new file mode 100644 index 0000000..dd69123 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/4x4.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/hoverboard.launch b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/hoverboard.launch new file mode 100644 index 0000000..b26481f --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/launch/hoverboard.launch @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/package.xml b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/package.xml new file mode 100644 index 0000000..d39e975 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/package.xml @@ -0,0 +1,26 @@ + + + hoverboard_driver + 0.0.1 + Driver for hoverboard motors + road.and.track@gmail.com + BSD + catkin + roscpp + std_msgs + hardware_interface + controller_manager + control_toolbox + rosparam_shortcuts + roscpp + std_msgs + roscpp + std_msgs + hardware_interface + controller_manager + rosparam_shortcuts + control_toolbox + dynamic_reconfigure + + + diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/hoverboard.cpp b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/hoverboard.cpp new file mode 100644 index 0000000..ddac2f7 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/hoverboard.cpp @@ -0,0 +1,277 @@ +#include "config.h" +#include "hoverboard.h" + +#include +#include +#include +#include +#include + +Hoverboard::Hoverboard() { + hardware_interface::JointStateHandle left_wheel_state_handle("left_wheel", + &joints[0].pos.data, + &joints[0].vel.data, + &joints[0].eff.data); + hardware_interface::JointStateHandle right_wheel_state_handle("right_wheel", + &joints[1].pos.data, + &joints[1].vel.data, + &joints[1].eff.data); + joint_state_interface.registerHandle (left_wheel_state_handle); + joint_state_interface.registerHandle (right_wheel_state_handle); + registerInterface(&joint_state_interface); + + hardware_interface::JointHandle left_wheel_vel_handle( + joint_state_interface.getHandle("left_wheel"), + &joints[0].cmd.data); + hardware_interface::JointHandle right_wheel_vel_handle( + joint_state_interface.getHandle("right_wheel"), + &joints[1].cmd.data); + velocity_joint_interface.registerHandle (left_wheel_vel_handle); + velocity_joint_interface.registerHandle (right_wheel_vel_handle); + registerInterface(&velocity_joint_interface); + + // These publishers are only for debugging purposes + vel_pub[0] = nh.advertise("hoverboard/left_wheel/velocity", 3); + vel_pub[1] = nh.advertise("hoverboard/right_wheel/velocity", 3); + pos_pub[0] = nh.advertise("hoverboard/left_wheel/position", 3); + pos_pub[1] = nh.advertise("hoverboard/right_wheel/position", 3); + cmd_pub[0] = nh.advertise("hoverboard/left_wheel/cmd", 3); + cmd_pub[1] = nh.advertise("hoverboard/right_wheel/cmd", 3); + voltage_pub = nh.advertise("hoverboard/battery_voltage", 3); + temp_pub = nh.advertise("hoverboard/temperature", 3); + connected_pub = nh.advertise("hoverboard/connected", 3); + + std::size_t error = 0; + error += !rosparam_shortcuts::get("hoverboard_driver", nh, "hoverboard_velocity_controller/wheel_radius", wheel_radius); + error += !rosparam_shortcuts::get("hoverboard_driver", nh, "hoverboard_velocity_controller/linear/x/max_velocity", max_velocity); + error += !rosparam_shortcuts::get("hoverboard_driver", nh, "robaka/direction", direction_correction); + rosparam_shortcuts::shutdownIfError("hoverboard_driver", error); + + if (!rosparam_shortcuts::get("hoverboard_driver", nh, "port", port)) { + port = DEFAULT_PORT; + ROS_WARN("Port is not set in config, using default %s", port.c_str()); + } else { + ROS_INFO("Using port %s", port.c_str()); + } + + // Convert m/s to rad/s + max_velocity /= wheel_radius; + + low_wrap = ENCODER_LOW_WRAP_FACTOR*(ENCODER_MAX - ENCODER_MIN) + ENCODER_MIN; + high_wrap = ENCODER_HIGH_WRAP_FACTOR*(ENCODER_MAX - ENCODER_MIN) + ENCODER_MIN; + last_wheelcountR = last_wheelcountL = 0; + multR = multL = 0; + + ros::NodeHandle nh_left(nh, "pid/left"); + ros::NodeHandle nh_right(nh, "pid/right"); + // Init PID controller + pids[0].init(nh_left, 1.0, 0.0, 0.0, 0.01, 1.5, -1.5, true, max_velocity, -max_velocity); + pids[0].setOutputLimits(-max_velocity, max_velocity); + pids[1].init(nh_right, 1.0, 0.0, 0.0, 0.01, 1.5, -1.5, true, max_velocity, -max_velocity); + pids[1].setOutputLimits(-max_velocity, max_velocity); + + if ((port_fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0) { + ROS_FATAL("Cannot open serial port to hoverboard"); + exit(-1); + } + + // CONFIGURE THE UART -- connecting to the board + // The flags (defined in /usr/include/termios.h - see http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html): + struct termios options; + tcgetattr(port_fd, &options); + options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; // 0 && i++ < 1024) + protocol_recv(c); + + if (i > 0) + last_read = ros::Time::now(); + + if (r < 0 && errno != EAGAIN) + ROS_ERROR("Reading from serial %s failed: %d", port.c_str(), r); + } + + if ((ros::Time::now() - last_read).toSec() > 1) { + ROS_FATAL("Timeout reading from serial %s failed", port.c_str()); + + //publish false when not receiving serial data + std_msgs::Bool b; + b.data = false; + connected_pub.publish(b); + } else { + //we must be connected - publish true + std_msgs::Bool b; + b.data = true; + connected_pub.publish(b); + } +} + +// 协议读取函数 +void Hoverboard::protocol_recv (char byte) { + start_frame = ((uint16_t)(byte) << 8) | (uint8_t)prev_byte; + + // Read the start frame + if (start_frame == START_FRAME) { + p = (char*)&msg; + *p++ = prev_byte; + *p++ = byte; + msg_len = 2; + } else if (msg_len >= 2 && msg_len < sizeof(SerialFeedback)) { + // Otherwise just read the message content until the end + *p++ = byte; + msg_len++; + } + + if (msg_len == sizeof(SerialFeedback)) { + uint16_t checksum = (uint16_t)( + msg.start ^ + msg.cmd1 ^ + msg.cmd2 ^ + msg.speedR_meas ^ + msg.speedL_meas ^ + msg.wheelR_cnt ^ + msg.wheelL_cnt ^ + msg.batVoltage ^ + msg.boardTemp ^ + msg.cmdLed); + + // 校验帧头和校验位 + if (msg.start == START_FRAME && msg.checksum == checksum) { + std_msgs::Float64 f; + + // 获取电压信息并发布 + f.data = (double)msg.batVoltage/100.0; + voltage_pub.publish(f); + + // 获取板子信息 + f.data = (double)msg.boardTemp/10.0; + temp_pub.publish(f); + + // 转换 RPM to RAD/S + joints[0].vel.data = direction_correction * (abs(msg.speedL_meas) * 0.10472); + joints[1].vel.data = direction_correction * (abs(msg.speedR_meas) * 0.10472); + + // 发布x和y方向的速度 + vel_pub[0].publish(joints[0].vel); + vel_pub[1].publish(joints[1].vel); + + // Process encoder values and update odometry + on_encoder_update (msg.wheelR_cnt, msg.wheelL_cnt); + } else { + ROS_WARN("Hoverboard checksum mismatch: %d vs %d", msg.checksum, checksum); + } + msg_len = 0; + } + prev_byte = byte; +} + +void Hoverboard::write(const ros::Time& time, const ros::Duration& period) { + if (port_fd == -1) { + ROS_ERROR("Attempt to write on closed serial"); + return; + } + + // Inform interested parties about the commands we've got + // 把命令发送出去(速度信息发布) + cmd_pub[0].publish(joints[0].cmd); + cmd_pub[1].publish(joints[1].cmd); + + // 计算PID输出 + double pid_outputs[2]; + pid_outputs[0] = pids[0](joints[0].vel.data, joints[0].cmd.data, period); + pid_outputs[1] = pids[1](joints[1].vel.data, joints[1].cmd.data, period); + + // Convert PID outputs in RAD/S to RPM + double set_speed[2] = { + pid_outputs[0] / 0.10472, + pid_outputs[1] / 0.10472 + }; + + // Calculate steering from difference of left and right + // 根据左右差计算转向 + const double speed = (set_speed[0] + set_speed[1]) / 2.0; + const double steer = (set_speed[0] - speed)*2.0; + + SerialCommand command; + command.start = (uint16_t)START_FRAME; + command.steer = (int16_t)steer; + command.speed = (int16_t)speed; + command.checksum = (uint16_t)(command.start ^ command.steer ^ command.speed); + + int rc = ::write(port_fd, (const void*)&command, sizeof(command)); + if (rc < 0) { + ROS_ERROR("Error writing to hoverboard serial port"); + } +} + +void Hoverboard::on_encoder_update (int16_t right, int16_t left) { + double posL = 0.0, posR = 0.0; + + // Calculate wheel position in ticks, factoring in encoder wraps + if (right < low_wrap && last_wheelcountR > high_wrap) + multR++; + else if (right > high_wrap && last_wheelcountR < low_wrap) + multR--; + posR = right + multR*(ENCODER_MAX-ENCODER_MIN); + last_wheelcountR = right; + + if (left < low_wrap && last_wheelcountL > high_wrap) + multL++; + else if (left > high_wrap && last_wheelcountL < low_wrap) + multL--; + posL = left + multL*(ENCODER_MAX-ENCODER_MIN); + last_wheelcountL = left; + + // When the board shuts down and restarts, wheel ticks are reset to zero so the robot can be suddently lost + // This section accumulates ticks even if board shuts down and is restarted + static double lastPosL = 0.0, lastPosR = 0.0; + static double lastPubPosL = 0.0, lastPubPosR = 0.0; + static bool nodeStartFlag = true; + + //IF there has been a pause in receiving data AND the new number of ticks is close to zero, indicates a board restard + //(the board seems to often report 1-3 ticks on startup instead of zero) + //reset the last read ticks to the startup values + if((ros::Time::now() - last_read).toSec() > 0.2 + && abs(posL) < 5 && abs(posR) < 5){ + lastPosL = posL; + lastPosR = posR; + } + double posLDiff = 0; + double posRDiff = 0; + + //if node is just starting keep odom at zeros + if(nodeStartFlag){ + nodeStartFlag = false; + }else{ + posLDiff = posL - lastPosL; + posRDiff = posR - lastPosR; + } + + lastPubPosL += posLDiff; + lastPubPosR += posRDiff; + lastPosL = posL; + lastPosR = posR; + + // Convert position in accumulated ticks to position in radians + joints[0].pos.data = 2.0*M_PI * lastPubPosL/(double)TICKS_PER_ROTATION; + joints[1].pos.data = 2.0*M_PI * lastPubPosR/(double)TICKS_PER_ROTATION; + + pos_pub[0].publish(joints[0].pos); + pos_pub[1].publish(joints[1].pos); +} diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/main.cpp b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/main.cpp new file mode 100644 index 0000000..8cd3721 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/main.cpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include "hoverboard.h" + +int main(int argc, char **argv) { + ros::init(argc, argv, "hoverboard_driver"); + + Hoverboard hoverboard; + controller_manager::ControllerManager cm(&hoverboard); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::Time prev_time = ros::Time::now(); + ros::Rate rate(100.0); + + while (ros::ok()) { + const ros::Time time = ros::Time::now(); + const ros::Duration period = time - prev_time; + prev_time = time; + + hoverboard.read(); + cm.update(time, period); + hoverboard.write(time, period); + + rate.sleep(); + } + + return 0; +} diff --git a/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/pid.cpp b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/pid.cpp new file mode 100644 index 0000000..d312571 --- /dev/null +++ b/Software/ROS_Driver/ROS1-driver/hoverboard_driver/src/pid.cpp @@ -0,0 +1,161 @@ +// Based on work by Franz Pucher, Diffbot, (2020), GitHub repository, https://github.com/fjp/diffbot +#include "hoverboard_driver/pid.h" + +PID::PID(double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min) + : control_toolbox::Pid() + , dynamic_reconfig_initialized_(false) +{ + f_ = 0.0; + initPid(p, i, d, i_max, i_min, antiwindup); + error_ = 0.0; + + out_min_ = out_min; + out_max_ = out_max; +} + +void PID::init(ros::NodeHandle& nh, double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min) +{ + ROS_INFO("Initialize PID"); + f_ = f; + initPid(p, i, d, i_max, i_min, antiwindup); + error_ = 0.0; + + out_min_ = out_min; + out_max_ = out_max; + + initDynamicReconfig(nh); + + Gains gains = getGains(); + ROS_INFO_STREAM("Initialized PID: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_); + +} + +double PID::operator()(const double &measured_value, const double &setpoint, const ros::Duration &dt) +{ + // Compute error terms + error_ = setpoint - measured_value; + ROS_DEBUG_STREAM_THROTTLE(1, "Error: " << error_); + + // Reset the i_error in case the p_error and the setpoint is zero + // Otherwise there will always be a constant i_error_ that won't vanish + if (0.0 == setpoint && 0.0 == error_) + { + // reset() will reset + // p_error_last_ = 0.0; + // p_error_ = 0.0; + // i_error_ = 0.0; + // d_error_ = 0.0; + // cmd_ = 0.0; + reset(); + } + + // Use control_toolbox::Pid::computeCommand() + double output = computeCommand(error_, dt); + ROS_DEBUG_STREAM_THROTTLE(1, "PID computed command: " << output); + + // Compute final output including feed forward term + output = f_ * setpoint + output; + //output = clamp(output, out_min_, out_max_); + + return output; +} + +void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min) +{ + bool antiwindup; + getParameters(f, p, i, d, i_max, i_min, antiwindup); +} + +void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) +{ + f = f_; + // Call getGains from control_toolbox + getGains(p, i, d, i_max, i_min, antiwindup); +} + +void PID::setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + f_ = f; + setGains(p, i, d, i_max, i_min, antiwindup); + + Gains gains = getGains(); + ROS_INFO_STREAM("Update PID Gains: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_); +} + +void PID::setOutputLimits(double output_max, double output_min) +{ + out_max_ = output_max; + out_min_ = output_min; + ROS_INFO_STREAM("Update PID output limits: lower=" << out_min_ << ", upper=" << out_max_); +} + + +double PID::clamp(const double& value, const double& lower_limit, const double& upper_limit) +{ + if (value > upper_limit) + { + ROS_DEBUG_STREAM_THROTTLE(1, "Clamp " << value << " to upper limit " << upper_limit); + return upper_limit; + } + else if (value < lower_limit) + { + ROS_DEBUG_STREAM_THROTTLE(1, "Clamp " << value << " to lower limit " << upper_limit); + return lower_limit; + } + + return value; +} + +void PID::initDynamicReconfig(ros::NodeHandle &node) +{ + ROS_INFO_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace " + << node.getNamespace()); + + // Start dynamic reconfigure server + param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node)); + dynamic_reconfig_initialized_ = true; + + // Set Dynamic Reconfigure's gains to Pid's values + updateDynamicReconfig(); + + // Set callback + param_reconfig_callback_ = boost::bind(&PID::dynamicReconfigCallback, this, _1, _2); + param_reconfig_server_->setCallback(param_reconfig_callback_); + ROS_INFO_NAMED("pid", "Initialized dynamic reconfigure"); +} + +void PID::updateDynamicReconfig() +{ + // Make sure dynamic reconfigure is initialized + if(!dynamic_reconfig_initialized_) + return; + + // Get starting values + hoverboard_driver::HoverboardConfig config; + config.f = f_; + + // Get starting values + getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); + + updateDynamicReconfig(config); +} + +void PID::updateDynamicReconfig(hoverboard_driver::HoverboardConfig config) +{ + // Make sure dynamic reconfigure is initialized + if(!dynamic_reconfig_initialized_) + return; + + // Set starting values, using a shared mutex with dynamic reconfig + param_reconfig_mutex_.lock(); + param_reconfig_server_->updateConfig(config); + param_reconfig_mutex_.unlock(); +} + +void PID::dynamicReconfigCallback(hoverboard_driver::HoverboardConfig &config, uint32_t /*level*/) +{ + ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); + + // Set the gains + setParameters(config.f, config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); +} \ No newline at end of file diff --git a/Software/ROS_Driver/ROS2-driver/CMakeLists.txt b/Software/ROS_Driver/ROS2-driver/CMakeLists.txt new file mode 100644 index 0000000..8f171c7 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.16) +project(hoverboard_driver LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + control_toolbox +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +## COMPILE +add_library( + hoverboard_driver + SHARED + hardware/hoverboard_driver.cpp + hardware/pid.cpp +) +target_compile_features(hoverboard_driver PUBLIC cxx_std_17) +target_include_directories(hoverboard_driver PUBLIC +$ +$ +) + +ament_target_dependencies( + hoverboard_driver PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "HOVERBOARD_DRIVER_BUILDING_DLL") + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface hoverboard_driver.xml) + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/hoverboard_driver +) +install( + DIRECTORY description/launch description/ros2_control description/urdf + DESTINATION share/hoverboard_driver +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/hoverboard_driver +) +install(TARGETS hoverboard_driver + EXPORT export_hoverboard_driver + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_targets(export_hoverboard_driver HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() \ No newline at end of file diff --git a/Software/ROS_Driver/ROS2-driver/README.md b/Software/ROS_Driver/ROS2-driver/README.md new file mode 100644 index 0000000..388c393 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/README.md @@ -0,0 +1,43 @@ + +# Hints +Connect Hoverboard PCB (USART3, GND, TX, RX, no VCC!!) to USB-TTL converter (or UART interface of your SBC). +Set the serial port according to your setup in hoverboard_driver.ros2_control.xacro file + +# Launch +``` +ros2 launch hoverboard_driver diffbot.launch.py +``` + +# Classes +The entire package consists of three main classes: +1. hoverboard_driver_node +2. hoverboard_driver +3. pid + +whereas the first two are implemented in the same file named hoverboard_driver.cpp (blame on me) + +## hoverboard_driver +This is the main class and implements the hardare interface for diff_drove_controller itself. This class is responsible for read/write to hardware. + +## hoverboard_driver_node +This class is a member/attribute of hoverboard_driver class and acts as a helper class. As hoverboard_driver itselfs derives from hardware_interface::SystemInterface class, hoverboard_driver class is not able to define publishers or settable parameters. To overcome this, hoverboard_driver_node has been introduced. This class derives from rclcpp::Node and can therefore publish topics and also can define reconfigurable parameters. + +This class publishes state of Hoverboard PCB like velocity, pose, command, voltage, temperature, battery level,current consumption of each motor and the state of serial interface(connected, not connected). + +Also this class provides dynamic parameters for PID controller + +## pid +The PID class is defined as /attribute of overboard_Driver class (array, of PIDs, one per wheel). +It works in general but it's not active now as I idn't find proper PID settings so far. +To activate, change this code section in hoverboard_driver.cpp + +``` // Convert PID outputs in RAD/S to RPM + //double set_speed[2] = { + // pid_outputs[0] / 0.10472, + // pid_outputs[1] / 0.10472}; + + double set_speed[2] = { + hw_commands_[left_wheel] / 0.10472, + hw_commands_[right_wheel] / 0.10472 + }; + ``` diff --git a/Software/ROS_Driver/ROS2-driver/bringup/config/hoverboard_controllers.yaml b/Software/ROS_Driver/ROS2-driver/bringup/config/hoverboard_controllers.yaml new file mode 100644 index 0000000..eb838ac --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/bringup/config/hoverboard_controllers.yaml @@ -0,0 +1,57 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + hoverboard_base_controller: + type: diff_drive_controller/DiffDriveController + +hoverboard_base_controller: + ros__parameters: + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] + + wheel_separation: 0.32 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.0825 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/Software/ROS_Driver/ROS2-driver/bringup/launch/diffbot.launch.py b/Software/ROS_Driver/ROS2-driver/bringup/launch/diffbot.launch.py new file mode 100644 index 0000000..121e5cc --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/bringup/launch/diffbot.launch.py @@ -0,0 +1,122 @@ +# Copyright 2020 ros2_control Development Team +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("hoverboard_driver"), "urdf", "diffbot.urdf.xacro"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("hoverboard_driver"), + "config", + "hoverboard_controllers.yaml", + ] + ) + # rviz_config_file = PathJoinSubstitution( + # [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] + # ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + remappings=[ + ("/hoverboard_base_controller/cmd_vel_unstamped", "/cmd_vel"), + ], + ) + #rviz_node = Node( + # package="rviz2", + # executable="rviz2", + # name="rviz2", + # output="log", + # arguments=["-d", rviz_config_file], + # condition=IfCondition(gui), + #) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["hoverboard_base_controller", "--controller-manager", "/controller_manager"], + ) + + # Delay rviz start after `joint_state_broadcaster` + #delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=joint_state_broadcaster_spawner, + # on_exit=[rviz_node], + # ) + #) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + # delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/Software/ROS_Driver/ROS2-driver/description/launch/view_robot.launch.py b/Software/ROS_Driver/ROS2-driver/description/launch/view_robot.launch.py new file mode 100644 index 0000000..c69793e --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/description/launch/view_robot.launch.py @@ -0,0 +1,111 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("hoverboard_driver"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/Software/ROS_Driver/ROS2-driver/description/ros2_control/hoverboard_driver.ros2_control.xacro b/Software/ROS_Driver/ROS2-driver/description/ros2_control/hoverboard_driver.ros2_control.xacro new file mode 100644 index 0000000..658b558 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/description/ros2_control/hoverboard_driver.ros2_control.xacro @@ -0,0 +1,27 @@ + + + + + + + + hoverboard_driver/hoverboard_driver + 0.0825 + 1.0 + /dev/serial/by-id/usb-FTDI_Quad_RS232-HS-if01-port0 + + + + + + + + + + + + + + + + diff --git a/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.materials.xacro b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.materials.xacro new file mode 100644 index 0000000..2b49da5 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.materials.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.urdf.xacro b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.urdf.xacro new file mode 100644 index 0000000..5660e85 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot.urdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot_description.urdf.xacro b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot_description.urdf.xacro new file mode 100644 index 0000000..263be60 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/description/urdf/diffbot_description.urdf.xacro @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Software/ROS_Driver/ROS2-driver/doc/diffbot.png b/Software/ROS_Driver/ROS2-driver/doc/diffbot.png new file mode 100644 index 0000000..1c4fc24 Binary files /dev/null and b/Software/ROS_Driver/ROS2-driver/doc/diffbot.png differ diff --git a/Software/ROS_Driver/ROS2-driver/doc/userdoc.rst b/Software/ROS_Driver/ROS2-driver/doc/userdoc.rst new file mode 100644 index 0000000..4bc6f34 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/doc/userdoc.rst @@ -0,0 +1,105 @@ + +********* +DiffBot +********* + +*DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. +The robot is basically a box moving according to differential drive kinematics. + +For *example_2*, the hardware interface plugin is implemented having only one interface. + +* The communication is done using proprietary API to communicate with the robot control box. +* Data for all joints is exchanged at once. + +The *DiffBot* URDF files can be found in ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst + +Tutorial steps +-------------------------- + +1. To check that *DiffBot* description is working properly use following launch commands + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_2 view_robot.launch.py + + .. warning:: + Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. + This happens because ``joint_state_publisher_gui`` node need some time to start. + + .. image:: diffbot.png + :width: 400 + :alt: Differential Mobile Robot + +2. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_2 diffbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. + In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. + This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. + + If you can see an orange box in *RViz* everything has started properly. + Still, to be sure, let's introspect the control system before moving *DiffBot*. + +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + You should get + + .. code-block:: shell + + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity + + The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. + +4. Check if controllers are running + + .. code-block:: shell + + ros2 control list_controllers + + You should get + + .. code-block:: shell + + diffbot_base_controller[diff_drive_controller/DiffDriveController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + +5. If everything is fine, now you can send a command to *Diff Drive Controller* using ROS 2 CLI interface: + + .. code-block:: shell + + ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.0" + + You should now see an orange box circling in *RViz*. + Also, you should see changing states in the terminal where launch file is started. + + .. code-block:: shell + + [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! + [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'! + +Files used for this demos +-------------------------- + diff --git a/Software/ROS_Driver/ROS2-driver/hardware/hoverboard_driver.cpp b/Software/ROS_Driver/ROS2-driver/hardware/hoverboard_driver.cpp new file mode 100644 index 0000000..62e8d95 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/hoverboard_driver.cpp @@ -0,0 +1,547 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "hoverboard_driver/hoverboard_driver.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace hoverboard_driver +{ + + hoverboard_driver_node::hoverboard_driver_node() : Node("hoverboard_driver_node") + { + // These publishers are only for debugging purposes + + vel_pub[left_wheel] = this->create_publisher("hoverboard/left_wheel/velocity", 3); + vel_pub[right_wheel] = this->create_publisher("hoverboard/right_wheel/velocity", 3); + pos_pub[left_wheel] = this->create_publisher("hoverboard/left_wheel/position", 3); + pos_pub[right_wheel] = this->create_publisher("hoverboard/right_wheel/position", 3); + cmd_pub[left_wheel] = this->create_publisher("hoverboard/left_wheel/cmd", 3); + cmd_pub[right_wheel] = this->create_publisher("hoverboard/right_wheel/cmd", 3); + voltage_pub = this->create_publisher("hoverboard/battery_voltage", 3); + temp_pub = this->create_publisher("hoverboard/temperature", 3); + curr_pub[left_wheel] = this->create_publisher("hoverboard/left_wheel/dc_current", 3); + curr_pub[right_wheel] = this->create_publisher("hoverboard/right_wheel/dc_current", 3); + connected_pub = this->create_publisher("hoverboard/connected", 3); + + declare_parameter("f", 10.2); + declare_parameter("p", 1.0); + declare_parameter("i", 0.1); + declare_parameter("d", 1.0); + declare_parameter("i_clamp_min", -10.0); + declare_parameter("i_clamp_max", 10.0); + declare_parameter("antiwindup", false); + get_parameter("f", pid_config.f); + get_parameter("p", pid_config.p); + get_parameter("i", pid_config.i); + get_parameter("d", pid_config.d); + get_parameter("i_clamp_min", pid_config.i_clamp_min); + get_parameter("i_clamp_max", pid_config.i_clamp_max); + get_parameter("antiwindup", pid_config.antiwindup); + + // register parameter change callback handle + callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&hoverboard_driver_node::parametersCallback, this, std::placeholders::_1)); + } + + void hoverboard_driver_node::publish_vel(int wheel, double message) + { + std_msgs::msg::Float64 f; + f.data = message; + vel_pub[wheel]->publish(f); + } + + void hoverboard_driver_node::publish_pos(int wheel, double message) + { + std_msgs::msg::Float64 f; + f.data = message; + pos_pub[wheel]->publish(f); + } + + void hoverboard_driver_node::publish_cmd(int wheel, double message) + { + std_msgs::msg::Float64 f; + f.data = message; + cmd_pub[wheel]->publish(f); + } + + void hoverboard_driver_node::publish_curr(int wheel, double message) + { + std_msgs::msg::Float64 f; + f.data = message; + curr_pub[wheel]->publish(f); + } + + void hoverboard_driver_node::publish_voltage(double message) + { + std_msgs::msg::Float64 f; + f.data = message; + voltage_pub->publish(f); + } + + void hoverboard_driver_node::publish_temp(double message) + { + std_msgs::msg::Float64 f; + f.data = message; + temp_pub->publish(f); + } + + void hoverboard_driver_node::publish_connected(bool message) + { + std_msgs::msg::Bool f; + f.data = message; + connected_pub->publish(f); + } + + rcl_interfaces::msg::SetParametersResult hoverboard_driver_node::parametersCallback( + const std::vector ¶meters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + // Here update class attributes, do some actions, etc. + for (const auto ¶m : parameters) + { + if (param.get_name() == "p") + { + pid_config.p = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID P: %f", pid_config.p); + } + if (param.get_name() == "i") + { + pid_config.i = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID I: %f", pid_config.i); + } + if (param.get_name() == "d") + { + pid_config.d = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID D: %f", pid_config.d); + } + if (param.get_name() == "f") + { + pid_config.p = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID F: %f", pid_config.f); + } + if (param.get_name() == "i_clamp_min") + { + pid_config.i_clamp_min = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID I_CLAMP_MIN: %f", pid_config.i_clamp_min); + } + if (param.get_name() == "i_clamp_max") + { + pid_config.i_clamp_max = param.as_double(); + RCLCPP_INFO(get_logger(), "new value for PID I_CLAMP_MAX: %f", pid_config.i_clamp_max); + } + if (param.get_name() == "antiwindup") + { + pid_config.antiwindup = param.as_bool(); + RCLCPP_INFO(get_logger(), "new value for PID ANTIWINDUP: %i", pid_config.antiwindup); + } + + // HINT: the PID configuration itself will be set by timer callback of hoverboard_driver class + } + + return result; + } + + hardware_interface::CallbackReturn hoverboard_driver::on_init( + const hardware_interface::HardwareInfo &info) + { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // read parameter from hoverboard_driver.ros2_control.xacro file + wheel_radius = std::stod(info_.hardware_parameters["wheel_radius"]); + max_velocity = std::stod(info_.hardware_parameters["max_velocity"]); + port = info_.hardware_parameters["device"]; + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo &joint : info_.joints) + { + // DiffBotSystem has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("HoverBoardSystemHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("HoverBoardSystemHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("HoverBoardSystemHardware"), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("HoverBoardSystemHardware"), + "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("HoverBoardSystemHardware"), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + hardware_publisher = std::make_shared(); // fire up the publisher node + + return hardware_interface::CallbackReturn::SUCCESS; + } + + std::vector hoverboard_driver::export_state_interfaces() + { + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + } + + return state_interfaces; + } + + std::vector hoverboard_driver::export_command_interfaces() + { + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + } + + return command_interfaces; + } + + hardware_interface::CallbackReturn hoverboard_driver::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) + { + + RCLCPP_INFO(rclcpp::get_logger("hoverboard_driver"), "Using port %s", port.c_str()); + + // Convert m/s to rad/s + max_velocity /= wheel_radius; + + low_wrap = ENCODER_LOW_WRAP_FACTOR * (ENCODER_MAX - ENCODER_MIN) + ENCODER_MIN; + high_wrap = ENCODER_HIGH_WRAP_FACTOR * (ENCODER_MAX - ENCODER_MIN) + ENCODER_MIN; + last_wheelcountR = last_wheelcountL = 0; + multR = multL = 0; + + first_read_pass_ = true; + + // Init PID controller + pids[0].init(hardware_publisher->pid_config.f, hardware_publisher->pid_config.p, + hardware_publisher->pid_config.i, hardware_publisher->pid_config.d, + hardware_publisher->pid_config.i_clamp_max, hardware_publisher->pid_config.i_clamp_min, + hardware_publisher->pid_config.antiwindup, max_velocity, -max_velocity); + pids[0].setOutputLimits(-max_velocity, max_velocity); + pids[1].init(hardware_publisher->pid_config.f, hardware_publisher->pid_config.p, + hardware_publisher->pid_config.i, hardware_publisher->pid_config.d, + hardware_publisher->pid_config.i_clamp_max, hardware_publisher->pid_config.i_clamp_min, + hardware_publisher->pid_config.antiwindup, max_velocity, -max_velocity); + pids[1].setOutputLimits(-max_velocity, max_velocity); + + if ((port_fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0) + { + RCLCPP_FATAL(rclcpp::get_logger("hoverboard_driver"), "Cannot open serial port to hoverboard"); + exit(-1); + } + + // CONFIGURE THE UART -- connecting to the board + // The flags (defined in /usr/include/termios.h - see http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html): + struct termios options; + tcgetattr(port_fd, &options); + options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; // 0 && i++ < 1024) + protocol_recv(time, c); + + if (i > 0) + last_read = time; + + if (r < 0 && errno != EAGAIN) + RCLCPP_ERROR(rclcpp::get_logger("hoverboard_driver"), "Reading from serial %s failed: %d", port.c_str(), r); + } + + if ((time - last_read).seconds() > 1) + { + // ROS_FATAL("Timeout reading from serial %s failed", port.c_str()); + + // publish false when not receiving serial data + hardware_publisher->publish_connected(false); + } + else + { + // we must be connected - publish true + hardware_publisher->publish_connected(true); + } + return hardware_interface::return_type::OK; + } + + void hoverboard_driver::protocol_recv(const rclcpp::Time &time, char byte) + { + start_frame = ((uint16_t)(byte) << 8) | (uint8_t)prev_byte; + + // Read the start frame + if (start_frame == START_FRAME) + { + p = (char *)&msg; + *p++ = prev_byte; + *p++ = byte; + msg_len = 2; + } + else if (msg_len >= 2 && msg_len < sizeof(SerialFeedback)) + { + // Otherwise just read the message content until the end + *p++ = byte; + msg_len++; + } + + if (msg_len == sizeof(SerialFeedback)) + { + uint16_t checksum = (uint16_t)(msg.start ^ + msg.cmd1 ^ + msg.cmd2 ^ + msg.speedR_meas ^ + msg.speedL_meas ^ + msg.wheelR_cnt ^ + msg.wheelL_cnt ^ + msg.left_dc_curr ^ + msg.right_dc_curr ^ + msg.batVoltage ^ + msg.boardTemp ^ + msg.cmdLed); + + if (msg.start == START_FRAME && msg.checksum == checksum) + { + hardware_publisher->publish_voltage((double)msg.batVoltage / 100.0); + hardware_publisher->publish_temp((double)msg.boardTemp / 10.0); + ; + hardware_publisher->publish_curr(left_wheel, (double)msg.left_dc_curr / 100.0); + hardware_publisher->publish_curr(right_wheel, (double)msg.right_dc_curr / 100.0); + + // Convert RPM to RAD/S + hw_velocities_[left_wheel] = direction_correction * (abs(msg.speedL_meas) * 0.10472); + hw_velocities_[right_wheel] = direction_correction * (abs(msg.speedR_meas) * 0.10472); + hardware_publisher->publish_vel(left_wheel, hw_velocities_[left_wheel]); + hardware_publisher->publish_vel(right_wheel, hw_velocities_[right_wheel]); + + // Process encoder values and update odometry + on_encoder_update(time, msg.wheelR_cnt, msg.wheelL_cnt); + } + else + { + RCLCPP_WARN(rclcpp::get_logger("hoverboard_driver"), "Hoverboard checksum mismatch: %d vs %d", msg.checksum, checksum); + } + msg_len = 0; + } + prev_byte = byte; + } + + hardware_interface::return_type hoverboard_driver::hoverboard_driver::write( + const rclcpp::Time &time, const rclcpp::Duration &period) + { + if (port_fd == -1) + { + RCLCPP_ERROR(rclcpp::get_logger("hoverboard_driver"), "Attempt to write on closed serial"); + return hardware_interface::return_type::ERROR; + } + // Inform interested parties about the commands we've got + hardware_publisher->publish_cmd(left_wheel, hw_commands_[left_wheel]); + hardware_publisher->publish_cmd(right_wheel, hw_commands_[right_wheel]); + + // Set PID Parameters + pids[0].setParameters(hardware_publisher->pid_config.f, hardware_publisher->pid_config.p, + hardware_publisher->pid_config.i, hardware_publisher->pid_config.d, + hardware_publisher->pid_config.i_clamp_max, hardware_publisher->pid_config.i_clamp_min, + hardware_publisher->pid_config.antiwindup); + pids[1].setParameters(hardware_publisher->pid_config.f, hardware_publisher->pid_config.p, + hardware_publisher->pid_config.i, hardware_publisher->pid_config.d, + hardware_publisher->pid_config.i_clamp_max, hardware_publisher->pid_config.i_clamp_min, + hardware_publisher->pid_config.antiwindup); + + // calculate PID values + double pid_outputs[2]; + pid_outputs[0] = pids[0](hw_velocities_[left_wheel], hw_commands_[left_wheel], period); + pid_outputs[1] = pids[1](hw_velocities_[left_wheel], hw_commands_[right_wheel], period); + + // Convert PID outputs in RAD/S to RPM + //double set_speed[2] = { + // pid_outputs[0] / 0.10472, + // pid_outputs[1] / 0.10472}; + + double set_speed[2] = { + hw_commands_[left_wheel] / 0.10472, + hw_commands_[right_wheel] / 0.10472 + }; + + // Calculate steering from difference of left and right + const double speed = (set_speed[0] + set_speed[1]) / 2.0; + const double steer = (set_speed[0] - speed) * 2.0; + + SerialCommand command; + command.start = (uint16_t)START_FRAME; + command.steer = (int16_t)steer; + command.speed = (int16_t)speed; + command.checksum = (uint16_t)(command.start ^ command.steer ^ command.speed); + + int rc = ::write(port_fd, (const void *)&command, sizeof(command)); + if (rc < 0) + { + RCLCPP_ERROR(rclcpp::get_logger("hoverboard_driver"), "Error writing to hoverboard serial port"); + } + return hardware_interface::return_type::OK; + } + + void hoverboard_driver::on_encoder_update(const rclcpp::Time &time, int16_t right, int16_t left) + { + double posL = 0.0, posR = 0.0; + + // Calculate wheel position in ticks, factoring in encoder wraps + if (right < low_wrap && last_wheelcountR > high_wrap) + multR++; + else if (right > high_wrap && last_wheelcountR < low_wrap) + multR--; + posR = right + multR * (ENCODER_MAX - ENCODER_MIN); + last_wheelcountR = right; + + if (left < low_wrap && last_wheelcountL > high_wrap) + multL++; + else if (left > high_wrap && last_wheelcountL < low_wrap) + multL--; + posL = left + multL * (ENCODER_MAX - ENCODER_MIN); + last_wheelcountL = left; + + // When the board shuts down and restarts, wheel ticks are reset to zero so the robot can be suddently lost + // This section accumulates ticks even if board shuts down and is restarted + static double lastPosL = 0.0, lastPosR = 0.0; + static double lastPubPosL = 0.0, lastPubPosR = 0.0; + static bool nodeStartFlag = true; + + // IF there has been a pause in receiving data AND the new number of ticks is close to zero, indicates a board restard + //(the board seems to often report 1-3 ticks on startup instead of zero) + // reset the last read ticks to the startup values + if ((time - last_read).seconds() > 0.2 && abs(posL) < 5 && abs(posR) < 5) + { + lastPosL = posL; + lastPosR = posR; + } + double posLDiff = 0; + double posRDiff = 0; + + // if node is just starting keep odom at zeros + if (nodeStartFlag) + { + nodeStartFlag = false; + } + else + { + posLDiff = posL - lastPosL; + posRDiff = posR - lastPosR; + } + + lastPubPosL += posLDiff; + lastPubPosR += posRDiff; + lastPosL = posL; + lastPosR = posR; + + // Convert position in accumulated ticks to position in radians + hw_positions_[left_wheel] = 2.0 * M_PI * lastPubPosL / (double)TICKS_PER_ROTATION; + hw_positions_[right_wheel] = 2.0 * M_PI * lastPubPosR / (double)TICKS_PER_ROTATION; + + hardware_publisher->publish_pos(left_wheel, hw_positions_[left_wheel]); + hardware_publisher->publish_pos(right_wheel, hw_positions_[right_wheel]); + } + +} // namespace hoverboard_driver + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + hoverboard_driver::hoverboard_driver, hardware_interface::SystemInterface) diff --git a/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/config.hpp b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/config.hpp new file mode 100644 index 0000000..e238550 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/config.hpp @@ -0,0 +1,9 @@ +#pragma once + + +#define ENCODER_MIN 0 +#define ENCODER_MAX 9000 +#define ENCODER_LOW_WRAP_FACTOR 0.3 +#define ENCODER_HIGH_WRAP_FACTOR 0.7 + +#define TICKS_PER_ROTATION 90 diff --git a/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/hoverboard_driver.hpp b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/hoverboard_driver.hpp new file mode 100644 index 0000000..6b9867b --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/hoverboard_driver.hpp @@ -0,0 +1,188 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HOVERBOARD_DRIVER_HPP_ +#define HOVERBOARD_DRIVER_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "hoverboard_driver/config.hpp" +#include "hoverboard_driver/protocol.hpp" +#include "hoverboard_driver/pid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/bool.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +namespace hoverboard_driver +{ + + enum + { + /// @brief left wheel + left_wheel, + /// @brief right wheel + right_wheel + }; + + /// @brief Helper class to get a node interface. This class is used to build publishers and dynamic parameters + /// for hoverboard_driver class. + class hoverboard_driver_node : public rclcpp::Node + { + public: + hoverboard_driver_node(); + /// @brief publish velocity data for debugging + /// @param wheel left or right wheel + /// @param message value to publish + void publish_vel(int wheel, double message); + + /// @brief publish pose data for debugging + /// @param wheel left or right wheel + /// @param message value to publish + void publish_pos(int wheel, double message); + + /// @brief publish command data for debugging + /// @param wheel left or right wheel + /// @param message value to publish + void publish_cmd(int wheel, double message); + + /// @brief publish battery voltage + /// @param message value to publish + void publish_voltage(double message); + + /// @brief publish current (power consumption) of motor + /// @param wheel left or right wheel + /// @param message value to publish + void publish_curr(int wheel, double message); + + /// @brief publish PCB temperature + /// @param message value to publish + void publish_temp(double message); + + + /// @brief publish state of PCB (on or off) + /// @param message value to publish + void publish_connected(bool message); + + /// @brief parameter callback method. + /// @param parameters + /// @return + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector ¶meters); + + /// @brief PID configuration structure + struct + { + double p; + double i; + double d; + double f; + double i_clamp_min; + double i_clamp_max; + bool antiwindup; + } pid_config; + + private: + // Publishers + rclcpp::Publisher::SharedPtr vel_pub[2]; + rclcpp::Publisher::SharedPtr pos_pub[2]; + rclcpp::Publisher::SharedPtr cmd_pub[2]; + rclcpp::Publisher::SharedPtr voltage_pub; + rclcpp::Publisher::SharedPtr curr_pub[2]; + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr connected_pub; + + // Parameter Callback handle + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + }; + + /// @brief Hardware Interface class to communicate with Hoverboard PCB + class hoverboard_driver : public hardware_interface::SystemInterface + { + public: + RCLCPP_SHARED_PTR_DEFINITIONS(hoverboard_driver); + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo &info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + // HOVERBOARD_DRIVER_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + std::shared_ptr hardware_publisher; // make the publisher node a member + private: + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_positions_; + std::vector hw_velocities_; + + void protocol_recv(const rclcpp::Time &time, char c); + void on_encoder_update(const rclcpp::Time &time, int16_t right, int16_t left); + + double wheel_radius; + double max_velocity = 0.0; + int direction_correction = 1; + std::string port; + + rclcpp::Time last_read; + bool first_read_pass_; + // Last known encoder values + int16_t last_wheelcountR; + int16_t last_wheelcountL; + // Count of full encoder wraps + int multR; + int multL; + // Thresholds for calculating the wrap + int low_wrap; + int high_wrap; + + // Hoverboard protocol + int port_fd; + unsigned long msg_len = 0; + char prev_byte = 0; + uint16_t start_frame = 0; + char *p; + SerialFeedback msg, prev_msg; + + PID pids[2]; + }; + +} // namespace hoverboard_driver + +#endif // HOVERBOARD_DRIVER_HPP_ diff --git a/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/pid.hpp b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/pid.hpp new file mode 100644 index 0000000..277f7f4 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/pid.hpp @@ -0,0 +1,131 @@ + +#include +#include +#include + +class PID : public control_toolbox::Pid +{ + public: + /** + * @brief Construct a new PID object + * + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param out_min The min computed output. + * @param out_max The max computed output. + */ + PID(double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=0.0, bool antiwindup=false, double out_max=0.0, double out_min=0.0); + + /** + * @brief Initialize the + * + * @param kF The feed forward gain. + * @param kP The proportional gain. + * @param kI The integral gain. + * @param kD The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup + * @param out_min The min computed output. + * @param out_max The max computed output. + */ + void init(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min); + + /** + * @brief Compute PID output value from error using process value, set point and time period + * + * @param measured_value The process value measured by sensors. + * @param setpoint The desired target value. + * @param dt The delta time or period since the last call. + * @return double Computed PID output value. + */ + double operator()(const double &measured_value, const double &setpoint, const rclcpp::Duration &dt); + + /** + * @brief Get the FPID parameters + * + * @param f The feed forward gain. + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup Enable or disable antiwindup check. + */ + void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min); + void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); + + /** + * @brief Set the Parameters using the Gains object of control_toolbox::Pid + * + * @param f The feed forward gain. + * @param p The proportional gain. + * @param i The integral gain. + * @param d The derivative gain. + * @param i_max The max integral windup. + * @param i_min The min integral windup. + * @param antiwindup Enable or disable antiwindup check. + */ + void setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + + /** + * @brief Set the Output Limits of the PID controller + * + * @param out_min + * @param out_max + */ + void setOutputLimits(double out_min, double out_max); + + /** + * @brief Clam given value to upper and lower limits. + * + * @param value Input value that's possibly clamped. + * @param lower_limit Lower limit which the value must not exceed. + * @param upper_limit Upper limit which the value must not exceed. + * @return double Clamped value to range in between [lower_limit, upper_limit]. + */ + double clamp(const double& value, const double& lower_limit, const double& upper_limit); + + /** + * @brief Get the current error. + * + * @return double The current error computed from the measured and target value. + */ + inline double getError() const { return error_; }; + + /** + * @brief Start the dynamic reconfigure node and load the default values + * @param node - a node handle where dynamic reconfigure services will be published + */ + // void initDynamicReconfig(ros::NodeHandle &node); + + /** + * @brief Set Dynamic Reconfigure's gains to PID's values + */ + //void updateDynamicReconfig(); + //void updateDynamicReconfig(Gains gains_config); + //void updateDynamicReconfig(hoverboard_driver::HoverboardConfig config); + + /** + * \brief Update the PID parameters from dynamics reconfigure + */ + //void dynamicReconfigCallback(hoverboard_driver::HoverboardConfig &config, uint32_t /*level*/); + + private: + double f_; + double error_; + double out_min_; + double out_max_; + rclcpp::Node::SharedPtr node_; + + // Dynamic reconfigure + bool dynamic_reconfig_initialized_; + // typedef dynamic_reconfigure::Server DynamicReconfigServer; + //boost::shared_ptr param_reconfig_server_; + //DynamicReconfigServer::CallbackType param_reconfig_callback_; + + // boost::recursive_mutex param_reconfig_mutex_; +}; diff --git a/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/protocol.hpp b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/protocol.hpp new file mode 100644 index 0000000..d08e921 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/include/hoverboard_driver/protocol.hpp @@ -0,0 +1,30 @@ + +#ifndef _FOC_PROTOCOL_H +#define _FOC_PROTOCOL_H + +#define START_FRAME 0xABCD + +typedef struct { + uint16_t start; + int16_t steer; + int16_t speed; + uint16_t checksum; +} SerialCommand; + +typedef struct { + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t wheelR_cnt; + int16_t wheelL_cnt; + int16_t left_dc_curr; + int16_t right_dc_curr; + int16_t batVoltage; + int16_t boardTemp; + uint16_t cmdLed; + uint16_t checksum; +} SerialFeedback; + +#endif diff --git a/Software/ROS_Driver/ROS2-driver/hardware/pid.cpp b/Software/ROS_Driver/ROS2-driver/hardware/pid.cpp new file mode 100644 index 0000000..c85fa90 --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hardware/pid.cpp @@ -0,0 +1,107 @@ + +#include "hoverboard_driver/pid.hpp" + +PID::PID(double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min) + : control_toolbox::Pid() +{ + f_ = 0.0; + initPid(p, i, d, i_max, i_min, antiwindup); + error_ = 0.0; + + out_min_ = out_min; + out_max_ = out_max; +} + +void PID::init(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min) +{ + RCLCPP_INFO(rclcpp::get_logger("hoverboard_driver"), "Initialize PID"); + f_ = f; + initPid(p, i, d, i_max, i_min, antiwindup); + error_ = 0.0; + + out_min_ = out_min; + out_max_ = out_max; + + Gains gains = getGains(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Initialized PID: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_); +} + +double PID::operator()(const double &measured_value, const double &setpoint, const rclcpp::Duration &dt) +{ + // Compute error terms + error_ = setpoint - measured_value; + // rclcpp::Clock clock; // NOTA: Puede mamar + // RCLCPP_DEBUG_STREAM_THROTTLE( + // node_->get_logger(), clock, 1000000000, "Error: " << error_); + + // Reset the i_error in case the p_error and the setpoint is zero + // Otherwise there will always be a constant i_error_ that won't vanish + if (0.0 == setpoint && 0.0 == error_) + { + // reset() will reset + // p_error_last_ = 0.0; + // p_error_ = 0.0; + // i_error_ = 0.0; + // d_error_ = 0.0; + // cmd_ = 0.0; + reset(); + } + + // Use control_toolbox::Pid::computeCommand() + double output = computeCommand(error_, dt.nanoseconds()); + // RCLCPP_DEBUG_STREAM_THROTTLE(node_->get_logger(), clock, dt.nanoseconds(), + // "PID computed command: " << output); + + // Compute final output including feed forward term + output = f_ * setpoint + output; + //output = clamp(output, out_min_, out_max_); + + return output; +} + +void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min) +{ + bool antiwindup; + getParameters(f, p, i, d, i_max, i_min, antiwindup); +} + +void PID::getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) +{ + f = f_; + // Call getGains from control_toolbox + getGains(p, i, d, i_max, i_min, antiwindup); +} + +void PID::setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup) +{ + f_ = f; + setGains(p, i, d, i_max, i_min, antiwindup); + + Gains gains = getGains(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Update PID Gains: F=" << f << ", P=" << gains.p_gain_ << ", I=" << gains.i_gain_ << ", D=" << gains.d_gain_ << ", out_min=" << out_min_ << ", out_max=" << out_max_); +} + +void PID::setOutputLimits(double output_max, double output_min) +{ + out_max_ = output_max; + out_min_ = output_min; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("hoverboard_driver"), "Update PID output limits: lower=" << out_min_ << ", upper=" << out_max_); +} + + +double PID::clamp(const double& value, const double& lower_limit, const double& upper_limit) +{ + rclcpp::Clock clock; + if (value > upper_limit){ + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("hoverboard_driver"), clock, 1000000000, + "Clamp " << value << " to upper limit " << upper_limit); + return upper_limit; + }else if (value < lower_limit){ + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("hoverboard_driver"), clock, 1000000000, + "Clamp " << value << " to lower limit " << upper_limit); + return lower_limit; + } + return value; +} \ No newline at end of file diff --git a/Software/ROS_Driver/ROS2-driver/hoverboard_driver.xml b/Software/ROS_Driver/ROS2-driver/hoverboard_driver.xml new file mode 100644 index 0000000..0a3fe6e --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/hoverboard_driver.xml @@ -0,0 +1,9 @@ + + + + The ros2_control DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots. + + + diff --git a/Software/ROS_Driver/ROS2-driver/package.xml b/Software/ROS_Driver/ROS2-driver/package.xml new file mode 100644 index 0000000..11f77db --- /dev/null +++ b/Software/ROS_Driver/ROS2-driver/package.xml @@ -0,0 +1,42 @@ + + + + hoverboard_driver + 0.0.0 + Demo package of `ros2_control` hardware for DiffBot. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + + controller_manager + diff_drive_controller + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + control_toolbox + + ament_cmake_gtest + + + ament_cmake + +