motor_hardware.h

Go to the documentation of this file.

#ifndef MOTORHARDWARE_H
#define MOTORHARDWARE_H

#include "hardware_interface/joint_command_interface.h"
#include "hardware_interface/joint_state_interface.h"
#include "hardware_interface/robot_hw.h"
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"

#include "std_msgs/Int32.h"
#include "std_msgs/UInt32.h"

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

#include <gtest/gtest_prod.h>

class MotorHardware : public hardware_interface::RobotHW {
public:
    MotorHardware(ros::NodeHandle nh, CommsParams serial_params,
                  FirmwareParams firmware_params);
    virtual ~MotorHardware();
    void readInputs();
    void writeSpeeds();
    void requestVersion();
    void setParams(FirmwareParams firmware_params);
    void sendParams();
    void setDeadmanTimer(int32_t deadman);
    void setDebugLeds(bool led1, bool led2);

    int firmware_version;

private:
    void _addOdometryRequest(std::vector<MotorMessage>& commands) const;
    void _addVelocityRequest(std::vector<MotorMessage>& commands) const;

    int16_t calculateTicsFromRadians(double radians) const;
    double calculateRadiansFromTics(int16_t tics) const;

    hardware_interface::JointStateInterface joint_state_interface_;
    hardware_interface::VelocityJointInterface velocity_joint_interface_;

    FirmwareParams pid_params;
    FirmwareParams prev_pid_params;

    int32_t deadman_timer;

    int32_t sendPid_count;

    struct Joint {
        double position;
        double velocity;
        double effort;
        double velocity_command;

        Joint() : position(0), velocity(0), effort(0), velocity_command(0) {}
    } joints_[2];

    ros::Publisher leftError;
    ros::Publisher rightError;

    ros::Publisher pubU50;
    ros::Publisher pubS50;
    ros::Publisher pubU51;
    ros::Publisher pubS51;
    ros::Publisher pubU52;
    ros::Publisher pubS52;
    ros::Publisher pubU53;
    ros::Publisher pubS53;
    ros::Publisher pubU54;
    ros::Publisher pubS54;
    ros::Publisher pubU55;
    ros::Publisher pubS55;
    ros::Publisher pubU56;
    ros::Publisher pubS56;
    ros::Publisher pubU57;
    ros::Publisher pubS57;
    ros::Publisher pubU58;
    ros::Publisher pubS58;
    ros::Publisher pubU59;
    ros::Publisher pubS59;

    MotorSerial* motor_serial_;

    FRIEND_TEST(MotorHardwareTests, nonZeroWriteSpeedsOutputs);
    FRIEND_TEST(MotorHardwareTests, odomUpdatesPosition);
    FRIEND_TEST(MotorHardwareTests, odomUpdatesPositionMax);
};

#endif