diffbot_hw_interface.h

Go to the documentation of this file.

#ifndef DIFFBOT_HW_INTERFACE_H
#define DIFFBOT_HW_INTERFACE_H
 
 
// ROS
#include <ros/ros.h>
#include <urdf/model.h>
#include <diffbot_msgs/EncodersStamped.h>
#include <diffbot_msgs/WheelsCmdStamped.h>
#include <diffbot_msgs/AngularVelocitiesStamped.h>
#include <sensor_msgs/JointState.h>
 
// ROS Controls
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
 
#include <diffbot_base/pid.h>
 
 
namespace diffbot_base
{
    const unsigned int NUM_JOINTS = 2;
 
    struct JointState
    {
        float angular_position_;
        float angular_velocity_;
    };
 
    class DiffBotHWInterface : public hardware_interface::RobotHW
    {
    public:
        DiffBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model = NULL);
 
        virtual ~DiffBotHWInterface() {}
 
        virtual bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh);
 
        virtual void read(const ros::Time& time, const ros::Duration& period) override;
 
        virtual void write(const ros::Time& time, const ros::Duration& period);
 
        bool isReceivingMeasuredJointStates(const ros::Duration &timeout=ros::Duration(1));
 
        virtual void printState();
        std::string printStateHelper();
 
        std::string printCommandHelper();
 
    protected:
 
        virtual void loadURDF(const ros::NodeHandle& nh, std::string param_name);
 
        void encoderTicksCallback(const diffbot_msgs::EncodersStamped::ConstPtr& msg_encoders);
 
        void measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states);
 
        double ticksToAngle(const int &ticks) const;
 
        double normalizeAngle(double &angle) const;
 
 
        // The following functions are currently unused
        // DiffBot directly calculates normalized angles from encoder ticks
        // The joint_commands from ros_control are mapped to percentage values for the motor driver
        // The following comments are incorrect
        double linearToAngular(const double &distance) const;
        double angularToLinear(const double &angle) const;
        
 
        // Short name of this class
        std::string name_;
 
        // Startup and shutdown of the internal node inside a roscpp program
        ros::NodeHandle nh_;
 
        // Hardware interfaces
        // hardware_interface::JointStateInterface gives read access to all joint values 
        // without conflicting with other controllers.
        hardware_interface::JointStateInterface joint_state_interface_;
        // hardware_interface::VelocityJointInterface inherits from 
        // hardware_interface::JointCommandInterface and is used for reading and writing
        // joint velocities. Because this interface reserves the joints for write access,
        // conflicts with other controllers writing to the same joints might occure.
        // To only read joint velocities, avoid conflicts using 
        // hardware_interface::JointStateInterface.
        hardware_interface::VelocityJointInterface velocity_joint_interface_;
 
        // Configuration
        std::vector<std::string> joint_names_;
        std::size_t num_joints_;
        urdf::Model *urdf_model_;
 
        double wheel_radius_;
        double wheel_diameter_;
        double max_velocity_;
 
        // Hardware related parameters to hold values from the parameter server
        // The parameters are defined in diffbot_base/config/base.yaml
        double encoder_resolution_;
        // Parameters for the gain trim model
        double gain_;
        double trim_;
        // Assume the same motor constant for both motors
        double motor_constant_;
        double pwm_limit_;
 
        // Enable/Disable debug output
        // Setting only possible during first start of the hw interface
        // to avoid reading permanently form the parameter server
        bool debug_;
 
 
        // Data member array to store the controller commands which are sent to the 
        // robot's resources (joints, actuators)
        // The diff_drive_controller uses the hardware_interface::VelocityJointInterface
        // It provides semantic meaning to the wheel joints describing that 
        // they require velocity commands.
        double joint_velocity_commands_[NUM_JOINTS];
 
        // Data member arrays to store the state of the robot's resources (joints, sensors)
        // These values are filled in the read() method and are registered to the 
        // joint_state_interface_ of type hardware_interface::JointStateInterface.
        double joint_positions_[NUM_JOINTS];
        double joint_velocities_[NUM_JOINTS];
        double joint_efforts_[NUM_JOINTS];
 
        ros::ServiceServer srv_start_;
        ros::ServiceServer srv_stop_;
 
        // Declare publishers for the motor driver
        ros::Publisher pub_left_motor_value_;
        ros::Publisher pub_right_motor_value_;
 
 
        // Declare publishers for angular wheel joint velocities
        ros::Publisher pub_wheel_cmd_velocities_;
 
        // Declare publisher to reset the wheel encoders
        // used during first launch of hardware interface to avoid large difference in encoder ticks from a previous run
        ros::Publisher pub_reset_encoders_;
        // Declare subscriber for the wheel encoders
        // This subscriber receives the encoder ticks in the custom diffbot_msgs/Encoder message
        ros::Subscriber sub_encoder_ticks_;
        // Declare subscriber for the measured joint states (absolute angular position (rad) and angular velocity (rad/s) of the wheel)
        // This subscriber receives the measured angular wheel joint velocity in the custom diffbot_msgs/AngularVelocitiesStamped message
        ros::Subscriber sub_measured_joint_states_;
 
        // Array to store the received encoder tick values from the \ref sub_encoder_ticks_ subscriber
        int encoder_ticks_[NUM_JOINTS];
        JointState measured_joint_states_[NUM_JOINTS];
 
        PID pids_[NUM_JOINTS];
    };  // class DiffBotHWInterface
 
}  // namespace
 
#endif // DIFFBOT_HW_INTERFACE_H