Skip to content

Commit

Permalink
Motor hardware for ROS 2
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatjazBostic
MatjazBostic committed Aug 20, 2024
1 parent 962583b commit dde8e0d
Show file tree
Hide file tree
Showing 4 changed files with 1,708 additions and 1,480 deletions.
232 changes: 46 additions & 186 deletions include/ubiquity_motor/motor_hardware.h
View file
Original file line number Diff line number Diff line change
@@ -1,146 +1,31 @@
/**
Copyright (c) 2016, Ubiquity Robotics
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of ubiquity_motor nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/

#ifndef MOTORHARDWARE_H
#define MOTORHARDWARE_H

#include "rclcpp/rclcpp.hpp"
//#include "hardware_interface/joint_command_interface.h"
//#include "hardware_interface/joint_state_interface.hpp"
// #include "ros2_control/robot_hw.hpp"
// #include "hardware_interface/robot_hw.hpp"
// #include <hardware_interface/robot_hw.h>

#include "zenoh.hxx"

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/u_int32.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/battery_state.hpp"
// #include "ubiquity_motor_ros2/msg/motor_state.hpp"

#include <diagnostic_updater/update_functions.hpp>
#ifndef MOTOR_HARDWARE_H
#define MOTOR_HARDWARE_H

#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <std_msgs/msg/float32.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>

#include <ubiquity_motor/motor_parameters.h>
//#include <ubiquity_motor/motor_serial.h>

#include <gtest/gtest_prod.h>

// Mininum hardware versions required for various features
#define MIN_HW_OPTION_SWITCH 50

struct MotorDiagnostics {
MotorDiagnostics()
: odom_update_status(
diagnostic_updater::FrequencyStatusParam(&odom_min_freq, &odom_max_freq)) {}
// Communication Statuses
int firmware_version = 0;
int firmware_date = 0;
int firmware_options = 0;

// These are for diagnostic topic output
int fw_pid_proportional = 0;
int fw_pid_integral = 0;
int fw_pid_derivative = 0;
int fw_pid_control = 0;
int fw_pid_velocity = 0;
int fw_pid_denominator = 0;
int fw_pid_moving_buffer_size = 0;
int fw_max_pwm = 0;

double odom_max_freq = 1000;
double odom_min_freq = 50;
diagnostic_updater::FrequencyStatus odom_update_status;

// Limits
bool left_pwm_limit = false;
bool right_pwm_limit = false;
bool left_integral_limit = false;
bool right_integral_limit = false;
bool left_max_speed_limit = false;
bool right_max_speed_limit = false;
bool param_limit_in_firmware = false;
#include "motor_parameters.h"

// Power supply statuses
float battery_voltage = 0.0;
float battery_voltage_low_level = 22.5;
float battery_voltage_critical = 21.0;

// Wheel current states
double motorCurrentLeft = 0.0;
double motorCurrentRight = 0.0;

// ADC count for zero current. We could calibrate this if required.
// Nominally 1024 and goes up from there this lower value is used.
double motorAmpsZeroAdcCount = 1015;

int motorPwmDriveLeft = 0;
int motorPwmDriveRight = 0;

/* For later implementation (firmware support)
bool main_5V_error = false;
bool main_5V_ol = false;
bool main_12V_error = false;
bool main_12V_ol = false;
bool aux_5V_error = false;
bool aux_5V_ol = false;
bool aux_12V_error = false;
bool aux_12V_ol = false;
*/

bool estop_motor_power_off = false; // for Diagnostic reporting of ESTOP switch

void firmware_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void limit_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void battery_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_power_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_p_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_i_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_d_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_v_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_max_pwm_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void firmware_options_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void firmware_date_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
};

class MotorHardware : public hardware_interface::RobotHW {
class MotorHardware : public hardware_interface::SystemInterface, public rclcpp_lifecycle::LifecycleNode {
public:
MotorHardware(rclcpp::Node &nh, NodeParams node_params, CommsParams serial_params,
FirmwareParams firmware_params);
MotorHardware(const rclcpp::NodeOptions &options);
virtual ~MotorHardware();

hardware_interface::return_type configure(const hardware_interface::HardwareInfo &info) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
hardware_interface::return_type start() override;
hardware_interface::return_type stop() override;
hardware_interface::return_type read() override;
hardware_interface::return_type write() override;

void closePort();
bool openPort();
void clearCommands();
Expand All @@ -149,58 +34,41 @@ class MotorHardware : public hardware_interface::RobotHW {
void writeSpeedsInRadians(double left_radians, double right_radians);
void publishFirmwareInfo();
float calculateBatteryPercentage(float voltage, int cells, const float* type);
int areWheelSpeedsLower(double wheelSpeedRadPerSec);
int areWheelSpeedsLower(double wheelSpeedRadPerSec);
void requestFirmwareVersion();
void requestFirmwareDate();
void setParams(FirmwareParams firmware_params);
void sendParams();
void forcePidParamUpdates();
float getBatteryVoltage(void);
float getBatteryVoltage();
void setDeadmanTimer(int32_t deadman);
void setDeadzoneEnable(int32_t deadzone_enable);
void setDebugLeds(bool led1, bool led2);
void setHardwareVersion(int32_t hardware_version);
void setEstopPidThreshold(int32_t estop_pid_threshold);
void setEstopDetection(int32_t estop_detection);
bool getEstopState(void);
bool getEstopState();
void setMaxFwdSpeed(int32_t max_speed_fwd);
void setMaxRevSpeed(int32_t max_speed_rev);
void setMaxPwm(int32_t max_pwm);
void setWheelType(int32_t wheel_type);
void setWheelGearRatio(double wheel_gear_ratio);
double getWheelGearRatio(void);
double getWheelTicksPerRadian(void);
double getWheelGearRatio();
double getWheelTicksPerRadian();
void setDriveType(int32_t drive_type);
void setPidControl(int32_t pid_control);
void nullWheelErrors(void);
void nullWheelErrors();
void setWheelDirection(int32_t wheel_direction);
void getMotorCurrents(double &currentLeft, double &currentRight);
int getOptionSwitch(void);
int getPidControlWord(void);
int getOptionSwitch();
int getPidControlWord();
void setOptionSwitchReg(int32_t option_switch);
void requestSystemEvents();
void setSystemEvents(int32_t system_events);
void getWheelJointPositions(double &leftWheelPosition, double &rightWheelPosition);
void setWheelJointVelocities(double leftWheelVelocity, double rightWheelVelocity);
void publishMotorState(void);
int firmware_version;
int firmware_date;
int firmware_options;
int num_fw_params; // This is used for sendParams as modulo count
int hardware_version;
int estop_pid_threshold;
int max_speed_fwd;
int max_speed_rev;
int max_pwm;
int pid_control;
int deadman_enable;
int system_events;
int wheel_type;
double wheel_gear_ratio;
int drive_type;
void publishMotorState();


diagnostic_updater::Updater diag_updater;
private:
void _addOdometryRequest(std::vector<MotorMessage>& commands) const;
void _addVelocityRequest(std::vector<MotorMessage>& commands) const;
Expand All @@ -215,12 +83,9 @@ class MotorHardware : public hardware_interface::RobotHW {
FirmwareParams prev_fw_params;

int32_t deadman_timer;

double ticks_per_radian; // Odom ticks per radian for wheel encoders in use

double ticks_per_radian;
int32_t sendPid_count;

bool estop_motor_power_off; // Motor power inactive, most likely from ESTOP switch
bool estop_motor_power_off;

struct Joint {
double position;
Expand All @@ -231,39 +96,34 @@ class MotorHardware : public hardware_interface::RobotHW {
Joint() : position(0), velocity(0), effort(0), velocity_command(0) {}
} joints_[2];

// MessageTypes enum for refering to motor or wheel number
enum MotorOrWheelNumber {
Motor_M1 = 1,
Motor_M2 = 2
};

// MessageTypes enum in class to avoid global namespace pollution
enum WheelJointLocation {
Left = 0,
Right = 1
};

ros::Publisher leftError;
ros::Publisher rightError;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr leftError;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rightError;

ros::Publisher leftCurrent;
ros::Publisher rightCurrent;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr leftCurrent;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr rightCurrent;

ros::Publisher leftTickInterval;
ros::Publisher rightTickInterval;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr leftTickInterval;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rightTickInterval;

ros::Publisher firmware_state;
ros::Publisher battery_state;
ros::Publisher motor_power_active;
ros::Publisher motor_state;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr firmware_state;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr motor_power_active;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr motor_state;

MotorSerial* motor_serial_;

MotorDiagnostics motor_diag_;

FRIEND_TEST(MotorHardwareTests, nonZeroWriteSpeedsOutputs);
FRIEND_TEST(MotorHardwareTests, odomUpdatesPosition);
FRIEND_TEST(MotorHardwareTests, odomUpdatesPositionMax);
diagnostic_updater::Updater diag_updater_;
};

#endif
#endif // MOTOR_HARDWARE_HPP
Loading

0 comments on commit dde8e0d

Please sign in to comment.