MOOS程序解析记录（2）CMOOSBehaviour底层代码分析

系列文章目录

这里将我看代码学习到的关于CMOOSBehaviour底层代码分析的理解进行记录

---

---

前言

先从.h文件进行分析，再分析一下.cpp文件

---

一、CMOOSBehaviour.h

///
//
//   MOOS - Mission Oriented Operating Suite
//
//   A suit of Applications and Libraries for Mobile Robotics Research
//   Copyright (C) 2001-2005 Massachusetts Institute of Technology and
//   Oxford University.
//
//   This software was written by Paul Newman and others
//   at MIT 2001-2002 and Oxford University 2003-2005.
//   email: pnewman@robots.ox.ac.uk.
//
//   This file is part of a  MOOS Basic (Common) Application.
//
//   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 2 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, write to the Free Software
//   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
//   02111-1307, USA.
//
//    END_GPL    //
// MOOSBehaviour.h: interface for the CMOOSBehaviour class.
//
//

#if !defined(AFX_MOOSBEHAVIOUR_H__1C10DD47_7690_4AEF_9174_0B0EA068A77D__INCLUDED_)
#define AFX_MOOSBEHAVIOUR_H__1C10DD47_7690_4AEF_9174_0B0EA068A77D__INCLUDED_

#include <MOOSLIB/MOOSLib.h>
#include <MOOSUtilityLib/PitchZPID.h>
#include "PathAction.h"


#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include "MOOSTaskDefaults.h"
#include <string>
using namespace std;

typedef  list<string> STRING_LIST;

//unless set all task will time out after this time...
//very important for safety
#define MOOS_DEFUALT_TIMEOUT 100
enum DeviceType//  
{
    DEVICE_FOG=0,
    DEVICE_PAYLOADPC,
    DEVICE_MODEM,
    DEVICE_DEPTH,
    DEVICE_NETBRIDGE,
    DEVICE_DVL,
    DEVICE_IRADIUM,
    DEVICE_PROPULTION,
    DEVICE_CTD,
    DEVICE_LOAD,
    DEVICE_STROBE,
    DEVICE_GPS,
    DEVICE_TCM,
    DEVICE_RADIO,
    DEVICE_EMERGENCY,
    DEVICE_SONAR,
};
class CMOOSBehaviour
{
public:
    class CXYPoint
    {

    public:
        double GetY();
        double GetX();
        double GetZ();
        void SetZ(double dfZ);
        void SetY(double dfY);
        void SetX(double dfX);
        CXYPoint();
        double m_dfY;
        double m_dfX;
        double m_dfZ;
    };
class CControllerGains
    {
    public:
        CControllerGains();
        //yaw controller gains
        double m_dfYawKp;
        double m_dfYawKd;
        double m_dfYawKi;
        double m_dfYawKiMax;

        //Z controller gains
        double m_dfZToPitchKp;
        double m_dfZToPitchKd;
        double m_dfZToPitchKi;
        double m_dfZToPitchKiMax;

        //Pitch contoller gains
        double m_dfPitchKp;
        double m_dfPitchKd;
        double m_dfPitchKi;
        double m_dfPitchKiMax;

        //endstops
        double m_dfMaxPitch;
        double m_dfMaxRudder;
        double m_dfMaxElevator;
        double m_dfMaxThrust;
        double m_dfMaxMotorAngle;
        double m_dfConElevator;

    };
    
public:
	void addCondition(string svalue);
    virtual void SetTime(double dfTimeNow);
    virtual bool ReInitialise();
    bool Start();
    void SetPriority(int nPriority);
    int GetPriority();
    void SetName(string sName);
    bool SetGains(CControllerGains NewGains);
    double GetCreationTime();
    double GetStartTime();
    bool HasNewRegistration();
    void SetMissionFileReader(CProcessConfigReader* pMissionFileReader);
    string GetName();
    virtual bool SetParam(string sParam, string sVal);
    virtual bool GetNotifications(MOOSMSG_LIST & List);
    virtual bool GetRegistrations(STRING_LIST &List);
    virtual bool OnNewMail(MOOSMSG_LIST & NewMail);
    virtual bool RegularMailDelivery(double dfTimeNow)=0;
    virtual bool Run(CPathAction & DesiredAction);
    CMOOSBehaviour();
    virtual ~CMOOSBehaviour();
    bool ShouldRun();
	bool checkConditions();


protected:
    virtual bool OnStart();
    virtual bool OnBegin();
    bool OnError(string sReason);
    bool DebugNotify(const string & sStr);
    double m_dfIterateTime;
    double GetTimeNow(){return m_dfIterateTime;};
    virtual bool OnTimeOut();
    virtual bool OnEvent(const string & sReason="",bool bVerbalNotify = true);
    virtual void Stop(const string & sReason="DONE");

    virtual bool OnComplete();
    bool PeekMail(MOOSMSG_LIST & Mail,const string & sKey,CMOOSMsg & Msg);
    unsigned int m_nPriority;

    //list of messages to be sent to the outside world
    //when given the chance
    MOOSMSG_LIST m_Notifications;


    CProcessConfigReader* m_pMissionFileReader;

    class ControlledDOF
    {
    public:
        ControlledDOF();
        bool IsStale(double dfTimeNow,double dfTaskStartTime,double dfTimeOut=10.0);
        bool IsValid();
        double GetCurrent();
        double GetDesired();

        double GetErrorTime();
        double GetError();
        void SetTolerance(double dfTol);
        void SetDesired(double dfDesired);
        double GetDT();
        void SetCurrent(double dfCurrent, double dfTime);

    protected:
        double  m_dfDesired;
        double  m_dfCurrent;
        double  m_dfCurrentTime;
        double  m_dfTolerance;
    };

    //status variables
    string      m_sName;
    string      m_sLogPath;
    bool        m_bActive;
    bool        m_bComplete;
    bool        m_bNewRegistrations;

	unsigned short m_dDeviceFlag;
	unsigned short m_dDeviceValue;
	unsigned long  m_dIOValue;
	bool m_bIoValueSet;

    STRING_LIST      m_StartFlags;
    STRING_LIST      m_CompleteFlags;
    STRING_LIST      m_EventFlags;
	std::vector<std::string> m_condition_vars;
    std::vector<std::string> m_condition_vals;
	std::vector<std::string> m_bEnableRun;
    double      m_dfTimeOut;
    double      m_dfStartTime;
    double      m_dfCreationTime;


    //PID controllers
    CPitchZPID m_ZPID;
    CScalarPID m_YawPID;

    CControllerGains m_Gains;


};

typedef list<CMOOSBehaviour*> TASK_LIST;


#endif // !defined(AFX_MOOSBEHAVIOUR_H__1C10DD47_7690_4AEF_9174_0B0EA068A77D__INCLUDED_)

上述是源码里关于MOOSbehavior.h文件里的具体内容。

    virtual bool SetParam(string sParam, string sVal);
    virtual bool GetNotifications(MOOSMSG_LIST & List);
    virtual bool GetRegistrations(STRING_LIST &List);
    virtual bool OnNewMail(MOOSMSG_LIST & NewMail);
    virtual bool RegularMailDelivery(double dfTimeNow)=0;
    virtual bool Run(CPathAction & DesiredAction);

对于这几个常用的固定格式的函数都要定义为虚函数，虚函数最关键的特点是“动态联编”，它可以在运行时判断指针指向的对象，并自动调用相应的函数。这样可以保证在调用该函数的时候，指针指向的函数是用户所编写的函数，而不是父类的子函数。

二、CMOOSBehaviour.cpp

1.源代码

代码如下（示例）：

CMOOSBehaviour::CControllerGains::CControllerGains()
{

    m_dfPitchKp    =PITCH_PID_KP;
    m_dfPitchKd    =PITCH_PID_KD;
    m_dfPitchKi    =PITCH_PID_KI;
    m_dfPitchKiMax =PITCH_PID_INTEGRAL_LIMIT;


    m_dfYawKp = YAW_PID_KP;
    m_dfYawKd = YAW_PID_KD;
    m_dfYawKi = YAW_PID_KI;
    m_dfYawKiMax = YAW_PID_INTEGRAL_LIMIT;

    m_dfZToPitchKp = Z_TO_PITCH_PID_KP;
    m_dfZToPitchKd = Z_TO_PITCH_PID_KD;
    m_dfZToPitchKi = Z_TO_PITCH_PID_KI;
    m_dfZToPitchKiMax = Z_TO_PITCH_PID_INTEGRAL_LIMIT;

    m_dfMaxPitch = PITCH_MAX;
    m_dfMaxRudder = RUDDER_MAX;
    m_dfMaxElevator = ELEVATOR_MAX;
    m_dfMaxThrust = THRUST_MAX;

}

CControllerGains类的构造函数，对CControllerGains类进行初始化操作，将所有的变量赋予初值，后面的PITCH_PID_KP等变量的值的定义在文件MOOSTaskDefaults.h处可以找到，下面是文件内容：

#ifndef AUVPARAMH
#define AUVPARAMH


#ifndef PI
#define PI 3.141592653589
#endif

#define YAW_PID_KP 0.5
#define YAW_PID_KD 0.2
#define YAW_PID_KI 0.00
#define YAW_PID_INTEGRAL_LIMIT 2


#define PITCH_PID_KP 10
#define PITCH_PID_KD 50
#define PITCH_PID_KI 0.1
#define PITCH_PID_INTEGRAL_LIMIT 10

#define Z_TO_PITCH_PID_KP 10
#define Z_TO_PITCH_PID_KD 50
#define Z_TO_PITCH_PID_KI 0.1
#define Z_TO_PITCH_PID_INTEGRAL_LIMIT 10

#define PITCH_MAX MOOSDeg2Rad(10)


#define ELEVATOR_MAX 30
#define ELEVATOR_MIN -30
#define RUDDER_MAX 20
#define RUDDER_MIN -20
#define THRUST_MAX 100
#define THRUST_MIN -100


#endif

下面的代码是CMOOSBehaviour的构造函数和析构函数，构造函数对CMOOSBehaviour.h文件中定义的CMOOSBehaviour类的状态变量进行初始化操作，析构函数没有任何操作。

CMOOSBehaviour::CMOOSBehaviour()
{
    m_bComplete = false;
    m_bActive = false;

    m_sName="NotSet";

    m_pMissionFileReader = NULL;

    m_bNewRegistrations = true;

    m_dfTimeOut=MOOS_DEFUALT_TIMEOUT;
    m_dfStartTime=-1;

    m_dfCreationTime = MOOSTime();

    m_dfIterateTime = m_dfCreationTime;

    m_nPriority = 3;

	m_bIoValueSet = false;

}

CMOOSBehaviour::~CMOOSBehaviour()
{

}

2.读入数据

代码如下（示例）：

data = pd.read_csv(
    'https://labfile.oss.aliyuncs.com/courses/1283/adult.data.csv')
print(data.head())

该处使用的url网络请求的数据。

---

总结

提示：这里对文章进行总结：
例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。