ModuleController.hh

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Author: Edward Yunkai Cui
// Description: Thie is a customized model plugin class, in which we 
//              implemented the low level controllers of each individual
//              model. Those controllers including joint position
//              control, joint speed control, model plane motion control,
//              model plane position and orientation control
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#ifndef _GAZEBO_MODEL_CONTROLLER_HH_
#define _GAZEBO_MODEL_CONTROLLER_HH_

#include <iostream>
#include <cmath>
#include <boost/bind.hpp>

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
// Classes for communication between different plugins
#include "collision_message.pb.h"
#include "command_message.pb.h"
// Library for colored log text
#include "ColorLog.hh"
// The head file which contains the joint_plus structure
#include "SmoresJoint.hh"

#define PI 3.1415926
#define EXECUTIONERROR 0.08

using std::vector;
using std::string;
typedef const boost::shared_ptr<const gazebo::msgs::GzString> GzStringPtr;
typedef const boost::shared_ptr<const gazebo::msgs::Pose> PosePtr;
typedef const boost::shared_ptr<const command_message::msgs::CommandMessage> 
    CommandMessagePtr;

namespace gazebo
{

class ModuleController : public ModelPlugin 
{
 public:
  ModuleController();
  ~ModuleController();

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+  Useful functions to get model status and other tool functions  +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  double RevolutionSpeedCal(physics::JointPtr joint, const int axis_index);

  math::Pose GetModelCentralCoor(void);

  math::Angle AngleCalculation2Points(math::Vector2d start_point, 
      math::Vector2d end_point);

  double ComplementaryFilter(double filtering_value, 
      double complement_filter_par, double *value_history);

  double ComplementaryFilter(double filtering_value, double *value_history);

  SmoresJoint & GetJointPlus(int node_ID);

  int GetJointAxis(int node_ID);

  math::Angle GetJointAngle(physics::JointPtr current_joint, int rot_axis);

 private:

  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);

  void WelcomInfoProcessor(GzStringPtr &msg);

  void SystemInitialization(physics::ModelPtr parent_model);
  void OnSystemRunning(const common::UpdateInfo & /*_info*/);

  void CollisionPubAndSubInitialization(void);

  void CollisionReceivingCallback(GzStringPtr &msg);

  void CommandDecoding(CommandMessagePtr &msg);


  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+            Low level model control functions                    +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  void SetJointAngleForce(physics::JointPtr current_joint, int rot_axis, 
      double position);

  void JointPIDController(double angle_desired_radian, 
      double desire_speed, SmoresJoint *current_joint);

  void JointPIDController(double angle_desired_radian, SmoresJoint *current_joint);
  void JointAngleUpdateInJointPlus(void);

  void SetJointSpeed(physics::JointPtr CurrentJoint, int RotAxis, 
      double SpeedDesired);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+            Command Execution Tracking Functions                 +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  void JointAngleTracking(void);

  void PositionTracking(void);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+          Functions that control the model's planar motion       +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  void AnglePIDController(math::Angle desired_angle, math::Angle current_angle, 
      math::Vector2d current_speed);

  void Move2Point(math::Vector2d DesiredPoint, math::Angle DesiredOrientation);
 public:
  physics::ModelPtr model;
  transport::PublisherPtr collisionInfoToServer;
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                       Model Pramaters                           +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  math::Vector3 jointAngleKPID; 

  math::Vector3 modelAngleKPID;   
  double maxiRotationRate;  // Maximium rotation rate of each joint
  double accelerationRate;
  double planarMotionStopThreshold;
  double wheelRadius;
 private: 
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+       Pointers of Model and joints and a emhanced struct        +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  physics::JointPtr jointWR;
  physics::JointPtr jointWL;
  physics::JointPtr jointWF;
  physics::JointPtr jointCB;
  SmoresJoint jointWRP;
  SmoresJoint jointWLP;
  SmoresJoint jointWFP;
  SmoresJoint jointCBP;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                     Model plugin events                         +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  event::ConnectionPtr updateConnection;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+         Communication Publishers and Subscribers                +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  transport::SubscriberPtr welcomeInfoSub;
  transport::SubscriberPtr commandSub;
  transport::PublisherPtr  commandPub;
  transport::SubscriberPtr linkCollisonSub[4];

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                       Useful Buffers                            +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  vector<string> nameOfConnectedModels;

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+                 Model Execution Variables                       +
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  int executionState; 
  double jointAngleShouldBe[4];
  double jointSpeeds[4];
  math::Pose targetPosition;
  double lftWheelSpeed;
  double rgtWheelSpeed;
  bool startExecution;
  // TODO: command id should be implemented in the future
}; // class ModuleController
} // namespace gazebo
#endif