diff_drive_controller.h

Go to the documentation of this file.

/*
 * Author: Yam Geva
 */


#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>

#include <nav_msgs/Odometry.h>
#include <tf/tfMessage.h>

#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>

#include <robotican_controllers/odometry.h>
#include <robotican_controllers/speed_limiter.h>

namespace diff_drive_controller{

    class DiffDriveController
            : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
    {
    public:
        DiffDriveController();

        bool init(hardware_interface::VelocityJointInterface* hw,
                  ros::NodeHandle& root_nh,
                  ros::NodeHandle &controller_nh);

        void update(const ros::Time& time, const ros::Duration& period);

        void starting(const ros::Time& time);

        void stopping(const ros::Time& /*time*/);

    private:
        std::string name_;

        ros::Duration publish_period_;
        ros::Time last_state_publish_time_;
        bool open_loop_;

        std::vector<hardware_interface::JointHandle> left_wheel_joints_;
        std::vector<hardware_interface::JointHandle> right_wheel_joints_;

        struct Commands
        {
            double lin;
            double ang;
            ros::Time stamp;

            Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
        };
        realtime_tools::RealtimeBuffer<Commands> command_;
        Commands command_struct_;
        ros::Subscriber sub_command_;

        boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
        boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
        Odometry odometry_;

        double wheel_separation_;

        double wheel_radius_;

        double wheel_separation_multiplier_;
        double wheel_radius_multiplier_;

        double cmd_vel_timeout_;

        bool allow_multiple_cmd_vel_publishers_;

        std::string base_frame_id_;

        std::string odom_frame_id_;

        bool enable_odom_tf_;

        size_t wheel_joints_size_;

        Commands last1_cmd_;
        Commands last0_cmd_;
        SpeedLimiter limiter_lin_;
        SpeedLimiter limiter_ang_;

    private:
        void brake();

        void cmdVelCallback(const geometry_msgs::Twist& command);

        bool getWheelNames(ros::NodeHandle& controller_nh,
                           const std::string& wheel_param,
                           std::vector<std::string>& wheel_names);

        bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
                                   const std::string& left_wheel_name,
                                   const std::string& right_wheel_name,
                                   bool lookup_wheel_separation,
                                   bool lookup_wheel_radius);

        void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);

    };

    PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);
} // namespace diff_drive_controller