tetrapod_plugin.h

Go to the documentation of this file.

/*******************************************************************/
/*    AUTHOR: Paal Arthur S. Thorseth                              */
/*    ORGN:   Dept of Eng Cybernetics, NTNU Trondheim              */
/*    FILE:   tetrapod_plugin.h                                    */
/*    DATE:   Feb 4, 2021                                          */
/*                                                                 */
/* Copyright (C) 2021 Paal Arthur S. Thorseth,                     */
/*                    Adrian B. Ghansah                            */
/*                                                                 */
/* This program is free software: you can redistribute it          */
/* and/or modify it under the terms of the GNU General             */
/* Public License as published by the Free Software Foundation,    */
/* either version 3 of the License, or (at your option) any        */
/* later version.                                                  */
/*                                                                 */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty     */
/* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         */
/* See the GNU General Public License for more details.            */
/*                                                                 */
/* You should have received a copy of the GNU General Public       */
/* License along with this program. If not, see                    */
/* <https://www.gnu.org/licenses/>.                                */
/*                                                                 */
/*******************************************************************/
 
#pragma once
 
// Gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
 
// ROS
#include "ros/ros.h"
#include "ros/package.h"
#include "ros/console.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "eigen_conversions/eigen_msg.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "sensor_msgs/JointState.h"
#include "std_srvs/Empty.h"
 
// ROS Package Libraries
#include<math_utils/angle_utils.h>
 
// Eigen
#include <Eigen/Core>
 
// Standard library
#include <thread>
#include <vector>
#include <map>
 
namespace gazebo
{
    static constexpr unsigned int NUMJOINTS = 12;
 
    using JointVelocities = Eigen::Matrix<double, 12, 1>;
 
    
    class TetrapodPlugin : public ModelPlugin
    {
        public: enum ControlMode { position = 1, velocity = 2, torque = 3 };
 
        public: TetrapodPlugin();
        
        public: virtual ~TetrapodPlugin();
 
        public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
 
        public: Eigen::Matrix<double, 6, 1> GetBasePose();
 
        public: Eigen::Matrix<double, 6, 1> GetBaseTwist();
 
        public: double GetJointForce(const std::string &_joint_name);
 
        public: Eigen::Matrix<double, 12, 1> GetJointForces();
 
        public: double GetJointVelocity(const std::string &_joint_name);
 
        public: Eigen::Matrix<double, 12, 1> GetJointVelocities();
 
        public: double GetJointPosition(const std::string &_joint_name);
 
        public: Eigen::Matrix<double, 12, 1> GetJointPositions();
 
        public: void SetJointForce(const std::string &_joint_name, const double &_force);
 
        public: void SetJointForces(const std::vector<double> &_forces);
 
        public: void SetJointVelocity(const std::string &_joint_name, const double &_vel);
 
        public: void SetJointVelocities(const std::vector<double> &_vel);
        
        public: void SetJointPosition(const std::string &_joint_name, const double &_pos);
 
        public: void SetJointPositions(const std::vector<double> &_pos);
 
        public: void OnJointStateMsg(const sensor_msgs::JointStateConstPtr &_msg);
 
        public: void OnFlJointStateMsg(const sensor_msgs::JointStateConstPtr &_msg);
 
        public: void OnForceMsg(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
        public: void OnVelMsg(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
        public: void OnPosMsg(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
        public: void ProcessQueueThread();
 
        public: void PublishQueueThread();
 
        // TODO REMOVE
        public: void printMap(const std::map<std::string,physics::JointPtr> &_map);
 
        protected: void InitRos();
 
        protected: void InitRosQueueThreads();
 
        protected: bool LoadParametersRos();
 
        protected: void InitJointControllers();
 
        protected: void InitJointConfiguration();
 
        public: bool ResetSimulation(const std_srvs::Empty::Request &_req,
                                     std_srvs::Empty::Response &_res);
 
        private: physics::ModelPtr model;
 
        private: physics::WorldPtr world;
 
        private: std::string model_name;
 
        private: physics::LinkPtr base;
 
        //private: std::vector<physics::JointPtr> joints;
        //private: physics::Joint_V joints;
        private: std::map<std::string, physics::JointPtr> joints;
 
        private: std::vector<std::string> joint_names;
 
        private: std::vector<double> joint_config;
 
        private: std::vector<double> vel_p_gains;
 
        private: std::vector<double> vel_i_gains;
 
        private: std::vector<double> vel_d_gains;
 
        private: std::vector<double> pos_p_gains;
 
        private: std::vector<double> pos_i_gains;
 
        private: std::vector<double> pos_d_gains;
 
        private: ControlMode controlMode;
 
        private: std::unique_ptr<ros::NodeHandle> rosNode;
 
        private: ros::ServiceServer resetSimService;
 
        private: ros::Publisher genCoordPub;
 
        private: ros::Publisher genVelPub;
 
        private: ros::Publisher basePosePub;
 
        private: ros::Publisher baseTwistPub;
 
        private: ros::Publisher jointForcesPub;
 
        private: ros::Subscriber jointStateCmdSub;
 
        private: ros::Publisher jointStatePub;
 
        private: ros::Subscriber forceSub;
 
        private: ros::Subscriber velSub;
 
        private: ros::Subscriber posSub;
 
        private: ros::CallbackQueue rosProcessQueue;
 
        private: ros::CallbackQueue rosPublishQueue;
 
        private: std::thread rosProcessQueueThread;
 
        private: std::thread rosPublishQueueThread;
    };
 
    // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
    GZ_REGISTER_MODEL_PLUGIN(TetrapodPlugin)
} // namespace gazebo