Linux下MOOS编程(以应急程序为例)

原创 2016年08月28日 16:36:48

特定术语

应用

我们在MOOS平台下新编写的程序,在这里称为应用,比如传我们的应急程序,又比如GPS采集程序。

消息

消息是MOOSDB中的一条记录,该记录包括这里写图片描述

变量
变量即消息中的NAME和Val两个值,它属于消息。

邮件
邮件是打包好的一条或者多条消息。在我们的应用中,我们会事先注册变量,以及定义跟MOOSDB通信的频率,当到了需要通信的时间节点后,MOOSDB会将我们订阅的消息中,距离上次读取之后,又更新的那些消息打包成一封邮件发给我们的应用。

MOOS简介

MOOS结构

MOOS应用了星型拓扑结构,MOOSDB是整个拓扑结构的核心,在MOOS中,每一个应用都会连接到MOOSDB。所有的通信都依赖于这个中心发生。

该拓扑结构有如下特点:
1. 没有点到点的通信。即,应用和应用之间不能直接进行通信,只能通过MOOSDB通信。
2. 所有的应用到服务器的通信都是由应用发起的。
3. 每一个应用都有特定的名字。
4. 各个应用之间互不了解。

CMOOSApp类

MOOS中有一个重要的类——CMOOSApp,我们编写应用,主要就是用的该类。它可以使我们快速的编写一个新的MOOS应用。在CMOOSApp下是一个循环,它会不断的调用一个函数——Iterate()。这个函数默认是不执行任何操作的,所以我们的工作之一就是充实Iterate()函数,让它来完成特定的工作。我们另一个重要的工作是处理我们新编写的应用所收到的邮件,这就涉及到另一个重要的虚函数——OnNewmail()

下面的图表总结了CMOOSApp类的工作流程。

CMOOSApp类的源代码见附件1。

CMOOSApp介绍

首先,我们来看一下利用CMOOSApp来新建一个应用的步骤:

  1. 新建一个”main.cpp”
  2. 新建一个CMOOSApp的派生类
  3. 在main()函数中对上一步骤中的派生类实例化
  4. 在此对象中调用 ::Run() 函数

    ::Iterate()
    此函数需要完成应用的主要工作。

    ::OnNewMail()
    当新邮件到达时,调用这个函数。

    ::OnConnectToServer()
    当与MOOS数据库建立连接时,调用这个函数

    ::OnStartUp()
    当应用开始运行时,调用这个函数

重要虚函数介绍

Iterate

首先,Iterate函数会被基类周期性调用(被CMOOSApp::Run()调用),所以我们一般把机器人需要执行的任务代码放在这个函数里(因为,机器人执行的任务一般不是只执行一次)。所以,通过在CMOOSApp的派生类中重写Iterate函数,可以使用Iterate函数来完成机器人特定的工作

  • 不要在此代码中进入一个等待数据的无线循环
  • 可以通过下面两种方法配置Iterate被调用的频率f:
    • 调用MOOSApp::SetAppFreq(double dfFreq)函数
      f=(1/dfFreq)Hz
    • 在配置文件(.moos)中,设置AppTick值
      f=(1/AppTick)Hz
  • 通过上面两种方法设置的Iterate被调用的频率,是Iterate被调用的最大频率。比如,如果Iterate中的代码执行完成需要1s,那么即使你设置的频率大于1Hz,Iterate也不可能按照你设置的频率运行。
  • 如果想要Iterate运行频率越大越好,那么可以SetAppFreq(0)
  • 可以通过MOOSTime()函数返回一个浮点类型的Unix时间戳。
    • 例如 double dfTimeNow = MOOSTime();

OnNewMail

当你把你感兴趣的消息注册后,其他应用发布了这些消息,那么函数OnNewMail将会在CMOOSApp::Run()函数中被调用。其中,新到来的消息是被存储在std::list< CMOOSMsg >中。 比如当新邮件到达时,我们需要根据邮件内容来改变一些成员变量的值,就需要在此函数中操作。

OnConnectToServer

与Iterate和OnNewMail两个函数不同的是,OnConnectToServer并不会直接被CMOOSApp::Run()直接调用。OnConnectToServer实际上是m_Comms对象线程中一个回调。m_Comms对象是CMOOSCommsObject类的实例,用来处理所有的进程间通信(IPC)。

这个回调发生在应用和MOOSDB服务器连接时。在OnConnectToServer中我们用m_Comms.Register注册变量,注册变量的意思是告诉MOOSDB哪些消息是我们想要的,当其它应用发布这些消息时,现在的应用会收到这些消息。

OnDisconnectFromServer

这是OnConnectToServer的相反函数。当应用和MOOSDB服务器的连接因为正常和非正常原因断开时,这个函数将被调用。所以,这个函数中需要放置一些当连接断开时需要执行的代码。如果当连接断开没有什么想要执行的代码,不要在CMOOSApp的派生类中添加这个函数。

OnStartUp

当CMOOSApp::Run进入循环之前,CMOOSApp::Run调用OnStartUp函数。因此,这个函数中需要实现的是参数的初始化。在OnStartUp中实现参数的初始化有两个方法:

  • 直接赋值
  • 读取配置文件(.moos)中的值(后文会又详细介绍)

例一

下面我们来看一下根据以上知识写的应急程序:


//main.cpp
#include "Emergency.h"

int main(int argc, char *argc[])
{
    //如果运行时无输入参数,那么使用默认的配置文件和应用名称
    const char *sMissionFile = "Mission.moos";
    const char *sMOOSName = "Emergency";

    //如果运行时有输入参数,那么把第一个参数作为配置文件名称
    //第二个参数作为应用名称
    //比如:  Emergency Emergency.moos pEmergency
    //       则运行后配置文件是Emergency.moos
    //              应用名称是pEmergency
    switch(argc)
    {
    case 3:
        sMOOSName = argv[2];
    case 2:
        sMissionFile = argv[1];
    }

    Emergency TheApp;

    TheApp.Run(sMOOSName, sMissionFile);

    return 0;
}


//Emergency.h
#ifndef _EMERGENCY_H_
#define _EMERGENCY_H_

#include <MOOSLIB/MOOSApp.h>

class Emergency : public CMOOSApp
{
public:
    Emergency();
    virtual ~Emergency();

protect:
    bool OnNewMail(MOOSMSG_LIST &NewMail);
    bool Iterate();
    bool OnConnectToServer();
    bool OnStartUp();
};

#endif


//Emergency.cpp
#include "Emergency.h"

//默认构造函数
Emergency::Emergency()
{
}

//默认析构函数
Emergency::~Emergency()
{
}

bool OnNewMail(MOOSMSG_LIST &NewMail)
{
    return true;
}

bool Iterate()
{
    return true;
}

bool OnConnectToServer()
{
    return true;
}

bool OnStartUp()
{
    return true;
}

在linux下,我们还需要写一个CMakeLists.txt文件。CMakeLists.txt的目的是要求CMake根据指定的源文件生成可执行文件。

CMake是一个跨平台的软件,在很多平台可以使用。一般在windows下,我们会直接使用VS生成项目,在linux下面,我们也可以使用QT Creater生成项目,但是两个不同平台上面的项目不能相互移植。这就有了CMake的用武之地,我们可以先编写一个CMakeLists.txt文件,将需要的.h和.cpp文件包含进来,然后在不同的平台使用CMake调用各自的编译器生成各自的工程。

下面给出该应用的CMakeLists.txt内容:


//CMakeLists.txt
# Set System Specific Libraries
if (${WIN32})
  SET(SYSTEM_LIBS
    wsock32)
else (${WIN32})
  SET(SYSTEM_LIBS
    m
    pthread)
endif (${WIN32})

SET(SRC
   Emergency.cpp
   main.cpp
)

//设置可执行文件名称
//值得注意的是,该名称并不是应用名称
//应用名称是在main文件中定义的名称
ADD_EXECUTABLE(Emergency ${SRC})

TARGET_LINK_LIBRARIES(Emergency
   MOOS
   MOOSGen
   MOOSUtility
   contacts
   mbutil
   geometry
   ${SYSTEM_LIBS})

# Install Targets
INSTALL(TARGETS Emergency
  RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
)

除此之外,我们还需要修改源代码主目录下的CMakeLists.txt文件,在SET(IVP_NON_GUI_APPS){}中加入我们在上个CMakeLists.txt中生成的可执行文件名称,如果应用是pMarineViewerUVL等有界面的应用,我们应该把名称加到SET(IVP_GUI_APPS){}中,我们的应急程序没有界面,所以加到SET(IVP_NON_GUI_APPS){}中。

此处我们先不涉及配置文件(.moos)的编写与解析,下文中会介绍。

在src路径下新建一个Emergency文件夹,将上面的Emergency.cpp,Emergency.h,CMakeLists.txt放到文件夹下,并且修改src文件夹下的CMakeLists.txt文件,我们就可以进行编译了。

myoung@myoung-linux:~/off0714/UVLMiniAuv/moos-ivp-12.2$ ./build-ivp.sh 

在例一的代码中,我们的应用运行后,先是和MOOSDB建立连接,然后规律性的和MOOSDB对话,其它的什么功能也没有,对话中也没有订阅和发布消息。

我们先来假设应急程序的功能,此例中只实现两个功能,那就是定深超时和尾舱泄露。

定深超时的意思就是我们期望AUV运行时不能超过某一个深度MAX_DEPTH,如果超过该深度后一定时间DEPTH_TIME,就会发送消息ACT给MOOSDB要求AUV上浮FLOAT。其中当前深度是由传感器采集程序得到的,假设包含当前深度的消息名称为CURRENT_DEPTH。

这里写图片描述

尾舱泄露的意思就是当传感器检测到AUV的尾舱发生泄露时,就会发送消息ACT给MOOSDB要求AUV STOPALL(此处只是为了和定深超时的FLOAT区分,并不是严谨的要求STOPALL)。假设包含尾舱泄露的消息名称为AUV_STATUS,泄露时值为LEAK。

这里写图片描述

这就涉及到订阅消息、发布消息和解析消息的知识了。下面我们先来看一下订阅消息、发布消息和解析消息的基本知识。

订阅消息

CMOOSCommClient类的对象m_Comms可以让我们通过函数m_Comms.Register()来订阅消息。(根据我的理解m_Comms应该是在MOOS其他线程中建立的)

如果我们要订阅”current_depth”的注册语句m_Comms.Register("CURRENT_DEPTH", 0.25)来说,它的最大读取频率为4Hz,原因在上文中说过;如果写作对m_Comms.Register("CURRENT_DEPTH", 0),它将会以尽可能大的频率读取新消息。

我们需要在两个地方注册消息,一是OnStartUp()的末尾,二是在OnConnectToServer()中。原因如下:

  • 在应用开始的时候,我们就需要决定哪些消息是我们需要的,所以我们需要在OnStartUp()中注册。这也是为什么我们要在OnStartUp()中解析配置文件。
  • 应用和MOOSDB的连接是异步的,他们的连接取决于是否有其它的应用在和MOOSDB连接。所以,OnConnecToServer()可能在OnStartUp()的之前或者之后被调用,如果是之前调用,那么我们希望注册消息发生在OnStartUp()的末尾,如果是之后调用,那么我们希望注册的消息发生在OnConnectToServer()中。

发布消息

CMOOSCommClient类的对象m_Comms可以让我们通过函数m_Comms.Notify()来订阅消息。

格式为m_Comms.Notify("ACT",sName);,其中ACT为我们发送的消息名称,而sName是我们发送的消息值。值得注意的是,sName应该为string或者double型变量。

解析消息

我们的程序中,最重要的函数就是OnNewMail(),其中我们需要在OnNewMail()中根据新邮件解析消息值,并用这些消息值改变一些成员变量。

parseString(string, char)

// Procedure: parseString(string, char)
// Example: svector = parseString(“apples, pears,banannas”, ‘,’);
// svector[0] = “apples”
// svector[1] = ” pears”
// svector[2] = “bananas”

parseStringQ(string, char)

// Procedure: parseStringQ(string, char)
// Notes: Differs from “parseString” in that the separator is
// ignored if it is found in an open-quote mode. That is
// after an odd # of double-quote characters have appeared.
// Example: str = “left=apples # left=pears # left=\”bana#nas\””
// w/out Q: svector = parseString(str, ‘#’);
// svector[0] = [left=apples ]
// svector[1] = [ left=pears ]
// svector[2] = [ left=”bana#]
// svector[3] = [nas”]
// Compare: svector = parseStringQ(str, ‘#’);
// svector[0] = [left=apples ]
// svector[1] = [ left=pears ]
// svector[2] = [ left=”bana#nas”]

parseString(string, string)

// Procedure: parseString(string, string)
// Example: svector = parseString(“apples @ pears @ banannas”, “@”);
// svector[0] = “apples ”
// svector[1] = ” pears ”
// svector[2] = ” bananas”

parseQuotedString(string, char)

// Procedure: parseQuotedString(string, char)
// Example: svector = parseQuotedString(“”apples,pears”,banannas”, ‘,’);
// svector[0] = “apples,pears”
// svector[1] = “bananas”

chompString(const string&, char)

// Procedure: chompString(const string&, char)
// Example: svector = chompString(“apples, pears, bananas”, ‘,’)
// svector[0] = “apples”
// svector[1] = ” pears, bananas”

removeWhite(const string&)

// Procedure: removeWhite(const string&)
// Example: input_str = “apples, pears, bananas ”
// str = removeWhite(input_str)
// str = “apples,pears,bananas”

biteString(const string&, char)

// Procedure: biteString(const string&, char)
// Example: input_str = “apples, pears, bananas”
// str = biteString(input_str, ‘,’)
// str = “apples”
// input_str = ” pears, bananas”

用::PeekMail()来拣选消息

我们在文章最开头的时候说过,邮件包含一条或者多条消息,那么当新邮件到来时,我们应该怎么把邮件中包含的每条消息对应到各自的处理程序呢?当然我们可以使用for循环来处理。如下:


bool CSimpleApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
    MOOSMSG_LIST::iterator p;

    for(p=NewMail.begin(); p!=NewMail.end(); p++)
    {
        CMOOSMsg &rMsg = *p;

        if(MOOSStrCmp(rMsg.GetKey(), "CURRENT_DEPTH"))
        {
            DEPTH(rMsg);
        }
        else if(MOOSStrCmp(rMsg.GetKey(), "AUV_STATUS"))
        {
            STATUS(rMsg);
        }
    }

    return true;
}

除了使用上述方法外,我们还可以使用CMOOSCommsClient::PeekMail()函数来做这件事情。这个函数可以从到来的所有消息中提取特定的消息(我们订阅过的消息)。而且,这个函数还可以提取最新的消息值,意思就是如果CURRENT_DEPTH消息在距离上次提取时已经更新过好几次了,所以我们的应用中的MOOSMSG_LIST中已经保存了好几个该消息,我们使用CMOOSCommsClient::PeekMail()函数可以只提取最新的一个消息值。 并且,使用CMOOSCommsClient::PeekMail()的话,是从MOOSMSG_LIST中剪切出此条消息,而不是复制。

相反的,如果使用for循环来检测消息,那么如果距离上次收到邮件,某消息在MOOSDB中又更新了N次,那么我们在下次解析邮件时,又会对该消息解析N次,并且会相应的处理N次该消息。

检查陈旧的消息

当一个新的应用启动并且注册变量后,它并不知道MOOSDB中的情况,比如该应用注册了消息”CURRENT_DEPTH”,而”CURRENT_DEPTH”消息在该应用启动前几个小时更新过并且在MOOSDB中,那么当OnNewMail第一次调用,应用会收到这个陈旧的消息,然而有时候我们并不希望收到这条陈旧的消息。为了避免这种情况,一个简单的方法就是检查每条消息的时间,然后只接受最近的消息。

我们用CMOOSMsg::IsSkewed函数来检查这个,例如下面的代码:


bool CSimpleApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
    CMOOSMsg Msg;
    double dfNow = MOOSTime();

    if (m_Comms.PeekMail(NewMail, "CURRENT_DEPTH", Msg, false, true))
    {
        if (!Msg.IsSkewed(dfNow))
        {
            DEPTH(Msg);
        }
    }

    return true;
}

例二

下面我们根据上面的知识给出完善后的应急程序代码:


//main.cpp
#include "Emergency.h"

int main(int argc, char *argv[])
{
    //如果运行时无输入参数,那么使用默认的配置文件和应用名称
    const char *sMissionFile = "Mission.moos";
    const char *sMOOSName = "Emergency";

    //如果运行时有输入参数,那么把第一个参数作为配置文件名称
    //第二个参数作为应用名称
    //比如:  Emergency Emergency.moos pEmergency
    //       则运行后配置文件是Emergency.moos
    //              应用名称是pEmergency
    switch(argc)
    {
    case 3:
        sMOOSName = argv[2];
    case 2:
        sMissionFile = argv[1];
    }

    Emergency TheApp;

    TheApp.Run(sMOOSName, sMissionFile);

    return 0;
}


//Emergency.h
#ifndef _EMERGENCY_H_
#define _EMERGENCY_H_

#include <MOOSLIB/MOOSApp.h>

class Emergency : public CMOOSApp
{
public:
    Emergency();
    virtual ~Emergency();

protected:
    bool OnNewMail(MOOSMSG_LIST &NewMail);
    bool Iterate();
    bool OnConnectToServer();
    bool OnStartUp();

    //注册变量
    void DoRegistrations();

    double m_current_depth;
    double m_bKeepingDepth;
    double m_dfKeepingDepthStartTime;
    double m_dfKeepDepthMaxTime;
    bool m_bKeepingDepthFlag;
};
#endif


//Emergency.cpp
#include <stdio.h>
#include "Emergency.h"
#include <MOOSGenLib/MOOSGenLibGlobalHelper.h>

using namespace std;

Emergency::Emergency()
{
    m_current_depth = 0.0;
    //m_bKeepingDepth = 1000.0;
    m_dfKeepingDepthStartTime = -1.0;
    //m_dfKeepDepthMaxTime = 30.0;
    m_bKeepingDepthFlag = false;
}

Emergency::~Emergency()
{
}

bool Emergency::OnNewMail(MOOSMSG_LIST &NewMail)
{
    MOOSMSG_LIST::iterator p;

    for(p=NewMail.begin(); p!=NewMail.end(); p++)
    {
        CMOOSMsg &rMsg = *p;

        string key = rMsg.m_sKey;
        MOOSTrace("key is %s\n", key.c_str());
        string sdata = rMsg.m_sVal;
        MOOSTrace("sdata is %s\n", sdata.c_str());

        m_current_depth = rMsg.GetDouble();

        if (key == "CURRENT_DEPTH")
        {
            m_bKeepingDepthFlag = true;
            MOOSTrace("current depth is:%s\n",sdata.c_str());
        }

        else if (key == "AUV_STATUS")
        {
            MOOSTrace("AUV_STATUS is:%s\n",sdata.c_str());

            if (sdata == "LEAK")
            {
                m_Comms.Notify("ACT", "STOPALL");
            }
        }
    }

    return true;
}

bool Emergency::OnConnectToServer()
{
    //上文说过,注册消息,一是OnStartUp()的末尾
    //二是在OnConnectToServer()中
    DoRegistrations();

    return true;
}

bool Emergency::Iterate()
{
    double dfNowTime = MOOSTime();

    if (m_bKeepingDepthFlag)
    {
        if (m_current_depth > m_bKeepingDepth)
        {
            if (m_dfKeepingDepthStartTime < 0.0)
            {
                m_dfKeepingDepthStartTime = dfNowTime;
            }

            double time = dfNowTime - m_dfKeepingDepthStartTime;

            if (time > m_dfKeepDepthMaxTime)
            {
                m_Comms.Notify("ACT", "FLOAT");
            }
        }

        else if (m_current_depth < m_bKeepingDepth)
        {
            m_dfKeepingDepthStartTime = -1.0;
        }
    }
    return true;
}

bool Emergency::OnStartUp()
{
    //上文说过,注册消息,一是OnStartUp()的末尾
    //二是在OnConnectToServer()中

    if(m_MissionReader.GetValue("KeepingDepth",m_bKeepingDepth))
    {      
        MOOSTrace("从配置文件中读取持续水深为%lf\n", m_bKeepingDepth);
    }
    else
    {
        m_bKeepingDepth = 1000;
        MOOSTrace("使用默认持续水深为%lf\n", m_bKeepingDepth);
    }

    if(m_MissionReader.GetValue("KeepDepthMaxTime",m_dfKeepDepthMaxTime))
    {       
        MOOSTrace("从配置文件中读取持续水深%lf的最大时间为%lf\n", m_bKeepingDepth, m_dfKeepDepthMaxTime);
    }
    else
    {        
        m_dfKeepDepthMaxTime = 30;
        MOOSTrace("使用默认持续水深%lf的最大时间为%lf", m_bKeepingDepth, m_dfKeepDepthMaxTime);
    }
    DoRegistrations();

    return true;
}


//在此函数内,用m_Comms.Register("sName", 0)注册我们感兴趣的消息
void Emergency::DoRegistrations()
{
    m_Comms.Register("CURRENT_DEPTH", 0);
    m_Comms.Register("AUV_STATUS", 0.25);

    return;
}


//Mission.moos(放到bin目录,也就是可执行文件的目录下)
SeverPort = 9000
Severhost = localhost

ProcessConfig = Emergency
{
    KeepingDepth = 2000
    KeepDepthMaxTime = 10
}


//CMakeLists.txt
# Set System Specific Libraries
if (${WIN32})
  SET(SYSTEM_LIBS
    wsock32)
else (${WIN32})
  SET(SYSTEM_LIBS
    m
    pthread)
endif (${WIN32})

SET(SRC
   Emergency.cpp
   main.cpp
)

ADD_EXECUTABLE(Emergency ${SRC})

TARGET_LINK_LIBRARIES(Emergency
   MOOS
   MOOSGen
   MOOSUtility
   contacts
   mbutil
   geometry
   ${SYSTEM_LIBS})

# Install Targets
INSTALL(TARGETS Emergency
  RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
)

除上述五个之外,还要记得修改src文件夹下的CMakeLists.txt文件。

测试新应用

当我们写完了我们自己的应用,接下来应该怎么测试呢?通常,我们需要在系统中运行,但是大部分情况下我们的系统都是虚拟的,并不会真的接上机器人,并且也没有其它的应用会去发布”CURRENT_DEPTH”和”AUV_STATUS”消息。

针对这种情况,我们有以下两种方法解决:

  • 另外写一个应用,发布”CURRENT_DEPTH”和”AUV_STATUS”消息
  • 利用MOOS中自带的一个工具”uMS”向MOOSDB中发送数据。

通过uMS测试

利用uMS测试的步骤如下:

  1. 启动MOOSDB(如果之前已经启动过MOOSDB可以忽略此步骤,启动方法为在终端中直接输入MOOSDB,回车即可)
    这里写图片描述
  2. 启动uMS(启动方法为在终端中直接输入uMS,回车即可),然后在uMS图形话界面中点击”Connect”

    这里写图片描述
    这里写图片描述

  3. 启动你新写的应用(启动完成后,你会看到此应用在MOOSDB和uMS中可以被检测到)
    这里写图片描述
  4. 在uMS中发布”Heading”或者”VehicleStatus”变量。两种方法如下:
    a.在uMS中ctrl+左击一个空行,然后依次会出现让你输入变量名称、变量类型和变量值的对话框。
    这里写图片描述
    这里写图片描述
    这里写图片描述
    b.在终端中通过uPokeDB直接发布消息。
    myoung@myoung-linux:~$ uPokeDB VehicleStatus=stopall
    然后几个选项都默认回车就可以。
    这里写图片描述
    然后可以看到uMS中已经收到消息并且更新数据了
    这里写图片描述
    5.不管通过以上哪一种方法,我们都可以在启动我们应用的终端看到收到”VehicleStatus”的打印提示。

通过另一个应用测试

比起通过uMS测试,更有意思的事情是通过另外一个应用来配合测试。另外一个应用负责发布”VehicleStatus”和”Heading”变量。和例二不同的是,新的应用利用m_Comms的Notify方法来发布变量而不是订阅变量了。源代码如下:


//main.cpp
#include "Simulator.h"

int main(int argc, char* argv[])
{
    const char* sMissionFile = "Mission.moos";
    const char* sMOOSName = "MyMOOSApp2";

    switch (argc)
    {
    case 3:
        sMOOSName = argv[2];
    case 2:
        sMissionFile = argv[1];
    }

    CSimulator TheApp;

    TheApp.Run(sMOOSName, sMissionFile);

    return 0;
}


//Simulator.h
#ifndef _SIMULATOR_H_
#define _SIMULATOR_H_

#include <MOOSLIB/MOOSApp.h>

class CSimulator:public CMOOSApp
{
public:
    CSimulator();
    virtual ~CSimulator();

protected:
    bool OnNewMail(MOOSMSG_LIST &NewMail);
    bool Iterate();
    bool OnConnectToServer();
    bool OnStartUp();
};

#endif


//Simulator.cpp
#include "Simulator.h"
#include <math.h>

CSimulator::CSimulator()
{
}

CSimulator::~CSimulator()
{
}

bool CSimulator::OnNewMail(MOOSMSG_LIST &NewMail)
{
    return true;
}

bool CSimulator::OnConnectToServer()
{
    return true;
}

bool CSimulator::Iterate()
{
    static int k = 0;

    if (k++%10 == 0)
    {
        static double dfHeading = 0;
        dfHeading+=MOOSWhiteNoise(0.1);

        m_Comms.Notify("Heading", dfHeading, MOOSTime());        
    }

    if (k%35 == 0)
    {
        static double dfVolts = 100;
        dfVolts-=fabs(MOOSWhiteNoise(0.1));

        std::string sStatus = MOOSFormat("Status=%s, BatteryVoltage=%.2f, Bilge = %s",
            dfVolts > 50.0? "Good":"Bad",
            dfVolts,
            k%100 > 50? "On":"Off");

        m_Comms.Notify("VehicleStatus", sStatus, MOOSTime());
    }

    return true;
}

bool CSimulator::OnStartUp()
{
    return true;
}


//Mission.moos
ServerPort = 9000
Serverhost = localhost

ProcessConfig = Simulator
{
    //设置Iterate调用频率
    AppTick = 10

    //设置与MOOSDB交流频率
    CommsTick = 10

    //机器人名称
    VehicleName = AUV

    //其它参数
    InitialLocation = [3*1]{0, 1, 2}
    InitialConditions = Bilge = off, BatteryVoltage = 101,
    Heading = 0.57
}

CMOOSMsg

在MOOS中,数据是在客户端和MOOSDB中传送的,不允许客户端和客户端之间传送数据。值得注意的是,MOOS只允许数据以”string”或者”double”类型传送。数据打包成消息(CMOOSMsg类),这个消息中还包括其它一些重要的信息,如下图所示:
这里写图片描述

使用字符串格式虽然会使数据量增大,但是它有易读等其它优点,事实证明使用字符串的优点远远大于缺点。(具体可以查看英文文档)

但是,数字数据并不需要转换成字符串格式。例如:机器人当前深度,这就是一个数字格式。在这种情况下,数据就会以”MOOS_DOUBLE”的类型传送。

6重要的CMOOSApp思想

6.1从配置文件中读取配置信息

每一个MOOS应用都可以从配置文件中读取配置参数,配置文件的扩展名用”.moos”来区别。

我们可以为每一个MOOS应用写一个配置文件,也可以让所有的MOOS应用共用一个配置文件。当第二种情况时,MOOS应用会在共用的配置文件中通过”ProcessConfig”寻找自己的语句块,这个语句块格式如下:


ProcessConfig = ProcessName
{
    //参数值
    ParameterName = Value
}

每一个MOOS应用在和MOOSDB交流时,都会有一个唯一的”ProcessName”来区别于其他应用。因此语句块通过”ProcessName”来区分,并且通过一个大括号来限定,在大括号里面写参数的初始值。

在这些参数中,有两个非常重要并且必须要配置的是:CommTickAppTick

  • CommTick决定了该应用和MOOSDB通信的频率

    • f = 1/CommTick Hz
  • AppTick决定了Iterate被调用的频率(大约频率)

    • f = 1/AppTick Hz

6.2 解析配置文件

在MOOSGENLin库中包含了许多类和函数,用来帮助我们解析配置文件。特别的是,CMOOSApp类中有一个CMOOSProcessConfiguration的对象叫做m_MissionReader。当函数OnStartUp被调用的时候,m_MissionReader已经解析完了配置文件,并且知道配置文件中哪一个程序块是该应用所拥有的。下面的代码中,我们修改了Simulator程序,使它可以读取配置文件。


#include "Simulator.h"
#include <math.h>

CSimulator::CSimulator()
{
}

CSimulator::~CSimulator()
{
}

bool CSimulator::OnNewMail(MOOSMSG_LIST &NewMail)
{
    return true;
}

bool CSimulator::OnConnectToServer()
{
    return true;
}

bool CSimulator::Iterate()
{
    static int k = 0;

    if (k++%10 == 0)
    {
        m_dfHeading += MOOSWhiteNoise(0.1);

        std::string sVarName = m_sVehicleName+"_Heading";

        m_Comms.Notify(sVarName, m_dfHeading, MOOSTime());
    }

    if (k%35 == 0)
    {
        m_dfBatteryVoltage -= fabs(MOOSWhiteNoise(0.1));

        std::string sStatus = MOOSFormat("Status = %s,
            BatteryVoltage = %.2f, bilge = %s",
            m_dfBatteryVoltage > 50.0? "Good":"Bad",
            m_dfBatteryVoltage,
            m_sBilge.c_str());

        std::string sVarName = m_sVehicleName + "_Status";

        m_Comms.Notify(m_sVehicleName, sStatus, MOOSTime());
    }

    return true;
}

bool CSimulator::OnStartUp()
{
    m_sVehicleName = "UnNamed";

    if (!m_MissionReader.GetConfigurationParam("VehicleName",
         m_sVehicleName))
    {
        MOOSTrace("Warning parameter \"VehicleName\" not 
            specified. Using default of \"%s\" \n", 
            m_sVehicleName.c_str());
    }

    std::vector<double> vInitialLocation(3, 0.0);
    int nRows = vInitialLocation.size();
    int nCols = 1;

    if (!m_MissionReader.GetConfigurationParam("InitialLocation", 
        vInitialLocation, nRows, nCols))
    {
        MOOSTrace("Warning parameter \"InitialLocation\" not 
            specified. Using default of \"%s\" \n", 
            DoubleVector2String(vInitialLocation).c_str());
    }

    std::string sComplex;

    if (m_MissionReader.GetConfigurationParam("InitialConditions", sComplex))
    {
        m_sBilge = "off";
        MOOSValFromString(m_sBilge, sComplex, "bilge");

        m_dfBatteryVoltage = 100.0;
        MOOSValFromString(m_dfBatteryVoltage, sComplex, "BatteryVoltage");

        m_dfHeading = 0;
        MOOSValFromString(m_dfHeading, sComplex, "Heading");
    }
    else
    {
        return MOOSFail("no \"InitialConditions\" 
            specified in mission file (compulsory)\n");
    }

    MOOSTrace("Verbose Summary:\n");
    MOOSTrace("\tVehicle is called %s\n", m_sVehicleName.c_str());
    MOOSTrace("\tInitial Location is : %s\n", 
        DoubleVector2String(vInitialLocation).c_str());
    MOOSTrace("\tHeading is : %f\n", m_dfHeading);
    MOOSTrace("\tBatteryVoltage is %s\n", m_sBilge.c_str());

    return true;
}

SeverPort = 9000
Severhost = localhost

ProcessConfig = Simulator
{
    AppTick = 10
    CommsTick = 10
    VehicleName = TheGoodShipMOOS
    InitialLocation = [3*1]{0, 1, 2}
    InitialConditions = Bilge = off, BatteryVoltage = 101, Heading = 0.57
}

6.3 AppTick 和 CommsTick 的作用

CMOOSApp的每一个派生类,都可以在配置文件中配置AppTick和CommsTick的值。

  • CommsTick决定了该应用与MOOSDB通信的频率
    • f = 1/CommsTick Hz
  • AppTick决定了Iterate函数被调用的频率
    • f = 1/AppTick Hz

如果设置AppTick为0,那么Iterate将会以尽可能大的频率调用,也就是说,CMOOSApp中的函数::Run()将会不停的循环调用,不会有任何休眠时间。

7 CMOOSVariables

CMOOSApp还提供了另外一个非常有用的功能,用来创建和管理变量。我们的应用要保持一个系统状态,有时候需要很多的变量,这些变量的值都要通过一系列MOOS消息来决定。比如,一个航行应用可能需要以下这些变量:heading,speed,fuel,enginespeed,headwind,current,depth等等。用面向对象的思想来看,我们需要把这一系列变量看成一个对象。当一个应用需要的变量不是太多时,这可能很容易实现,但是当一个应用需要几十甚至上百个变量时,消息处理函数就会变得冗长而且巨大,同时,对应的类也会变得杂乱无章。

对于这个问题,我们用以下几个函数来解决。

//摘自MOOSApp.h
/*
添加一个动态变量
参数: 
sName   该动态变量的变量名
sSubscribeName  更新变量时MOOSDB中的变量名(使用方法见下面的更新变量)
sPublishName    发布变量时MOOSDB中的变量名(使用方法见下面的发布变量)
CommsTime       更新频率(1/CommsTime)
如果是更新变量,则"sPublishName"为空
如果是发布变量,则"sSubscribeName"为空
*/
bool AddMOOSVariable(std::string sName, std::string sSubscribeName,
     std::string sPublishName, double dfCommsTime);

//返回变量sName的指针
CMOOSVariable * GetMOOSVar(std::string sName);

//更新变量时用,用以注册MOOSDB中的变量
bool RegisterMOOSVariables();

//更新变量时用,用以更新本地的变量值
bool UpdateMOOSVariables(MOOSMSG_LIST &NewMail);

//当动态变量是"double"类型时,使用此函数为该变量赋值
bool SetMOOSVar(const std::string & sName, const std::string &sVal, double dfTime);

//当动态变量是"string"类型时,使用此函数为该变量赋值
bool SetMOOSVar(const std::string & sVarName, double dfVal, double dfTime);

//发布变量时使用,用以更新MOOSDB中的变量值
bool PublishFreshMOOSVariables();

发布变量

发布变量,即我们想把本地的变量发布到MOOSDB中。假设,我们的GPS传感器测到的值为X(变量X不需要在任何头文件中声明,用到时候动态创建就可以,比如X的值可能直接是从一个字符串中解析得来的),我们想把此变量发布到MOOSDB中,假设在此变量在MOOSDB中的变量名为GPS_X,分为以下几个步骤:

  1. AddMOOSVariable(“X”, “”, “GPS_X”, 0);
    建立”X”和”GPS_X”的对应关系。
  2. PublishFreshMOOSVariables();

更新变量

和发布变量相反的是更新变量,也就是我们想从MOOSDB中读取变量到本地。假设我们本地的变量名字为Temp(MOOSVariable类型),MOOSDB中的变量为ENGINE_TEMP,即我们想把ENGINE_TEMP赋值给Temp,分为以下几个步骤:

  1. AddMOOSVariable(“Temp”, “ENGINE_TEMP”, “”, 0);
    建立”Temp”和”ENGINE_TEMP”的对应关系。
  2. RegisterMOOSVariables();
    确保我们的应用注册了所有的消息。
    ???????????????????
    是注册所有的MOOSDB中的消息吗?
    MOOSDB消息和MOOSDB变量有什么关系?
    如果一个MOOSDB消息中包含该变量,使用该方法可以吗?
  3. UpdateMOOSVariables(NewMail);
    调用此函数后,当消息列表MOOSMsg中出现ENGINE_TEMP变量后,就会自动用ENGINE_TEMP变量更新Temp变量。

8两个附加功能

状态字符串

每几秒钟,CMOOSApp都会发布一个状态字符串。如果这个进程的MOOS名字为XYZ,那么它发布的状态字符串名字就是XYZ_STATUS。这个状态字符串中包括以下三部分:
1. 进程运行时间
2. 到目前为止发布过的所有消息名
3. 当前正在发布的所有消息名

对于状态字符串中的内容,我们可以通过重载std::string CMOOSApp::MakeStatusString()来自定义状态字符串包含的内容,CMOOSApp会优先选择重载的此函数。

功能二

附件

附件一


//CMOOSApp.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 at MIT 2001-2002 and Oxford
//   University 2003-2005. email: pnewman@robots.ox.ac.uk.
//
//   This file is part of a  MOOS Core Component.
//
//   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    //////////////////////////////////
// MOOSApp.h: interface for the CMOOSApp class.
//
//////////////////////////////////////////////////////////////////////

#ifndef MOOSAPPH
#define MOOSAPPH

#include <MOOSGenLib/MOOSGenLib.h>
#include "MOOSCommClient.h"


#define DEFAULT_MOOS_APP_COMMS_FREQ 5
#define DEFAULT_MOOS_APP_FREQ 5
#define MOOS_MAX_APP_FREQ 50
#define MOOS_MAX_COMMS_FREQ 200
#define STATUS_PERIOD 2

#include "MOOSVariable.h"
#include <set>
#include <map>

typedef std::map<std::string,CMOOSVariable> MOOSVARMAP;

/** This is a class from which all MOOS component applications can be derived
main() will typically end with a call to MOOSAppDerivedClass::Run(). It provides
automatic connection to the MOOSDB, provides slots for Mail Processing and application
work, callbacks for connection/disconnection to MOOSDB, Configuration file reading and
dynamic (runtime) variables. Definately worth getting to know. */
class CMOOSApp
{
public:

    CMOOSApp();
    virtual ~CMOOSApp();


protected:


    /////////////////////////////////////////////////////////////////////////////////////////////
    //                       THE MOST IMPORTANT AND USEFUL MEMBERS
    /////////////////////////////////////////////////////////////////////////////////////////////
public:
    /**called to start the application
    @param sName The name of this application (used to read configuration from a mission file and if sSubscribeName is NULL, to register with the MOOSDB)
    @param the name of the mission file
    @param the subscribe name of the application. If NULL then sName
    */
    bool Run( const char * sName,const char * sMissionFile,const char * sSubscribeName);
    bool Run( const char * sName,const char * sMissionFile);

    /** Called when the class has succesully connected to the server. Overload this function
    and place use it to register for notification when variables of interest change */
    virtual bool OnConnectToServer();

    /** Called when the class has disconnects from  the server. Put code you want to run when this happens in a virtual version of this method*/
    virtual bool OnDisconnectFromServer();

    /** requests the MOOSApp to quit (i.e return from Run)*/
    bool RequestQuit();


protected:
    /** called when the application should iterate. Overload this function in a derived class
    and within it write all the application specific code. It will be called at approximately
    nFreq = 1/AppTick Hz*/
    virtual bool Iterate();

    /** called when new mail has arrived. Overload this method in a derived class to process new mail.
    It will be called at approximately 1/CommsTick Hz. In this function you'll most likely interate over the
    collection of mail message received or call a m_Comms::PeekMail() to look for a specific named message.
    @param NewMail a list of new mail messages*/
    virtual bool OnNewMail(MOOSMSG_LIST & NewMail);

    /** optionally (see ::EnableCommandMessageFiltering() ) called when a command message (<MOOSNAME>_CMD) is recieved by the application.
    @param a copy of CmdMsg the message purporting to be a "command" - i.e. has the name <MOOSNAME>_CMD */
    virtual bool OnCommandMsg(CMOOSMsg Msg);

    /** make a status string - overload this in a derived class if you want to modify or what the statuts string looks like */
    virtual std::string MakeStatusString();

    /** The MOOSComms node. All communications happens by way of this object. You'll often do things like  m_Comms.Notify("VARIABLE_X","STRING_DATA",dfTime) top send data */
    CMOOSCommClient m_Comms;

    /** Set the time between calls into the DB - can be set using the CommsTick flag in the config file*/
    bool SetCommsFreq(unsigned int nFreq);

    /** Set the time  between calls of ::Iterate (which is where you'll probably do Application work)- can be set using the AppTick flag in the config file*/
    void SetAppFreq(double dfFreq);

    /** return the boot time of the App */
    double GetAppStartTime();

    /**a very useful object that lets us retrieve configuration information from the mission file using calls like ::GetConfigurationParam() */
    CProcessConfigReader m_MissionReader;

    /** A function which Run eventually calls which itself  calls on NewMail and Iterate*/
    bool DoRunWork();

    /** sets the error state of the app and a comment  - this is published as a field in <PROCNAME>_STATUS */
    void SetAppError(bool bFlag, const std::string & sReason);



    /////////////////////////////////////////////////////////////////////////////////////////////
    //                       UTITLITY  METHODS
    /////////////////////////////////////////////////////////////////////////////////////////////


    /** Called to set the MOOS server info used rarely usually this info will be picked up by the MOOSApp
    automatically when it ::Run is called specifying the configuration file (which contains the DB's coordinates)
    @param sServerHost name of the machine hosting the MOOSDB application
    @param lPort port nuymber that MOOSDB listens on*/
    void SetServer(const char * sServerHost="LOCALHOST",long lPort=9000);


    /** By default MOOSDB comms are on - but you may want to use the structuire of MOOSApp as a standalone
    application - if so call this function with a false parameter*/
    bool UseMOOSComms(bool bUse);

    /** call to say if you want mail to be delivered sorted by time*/
    void SortMailByTime(bool bSort=true){m_bSortMailByTime = bSort;};

    /** set to true to make the App sensitive (and quit) to Iterate() returning false*/
    void SetQuitOnFailedIterate(bool bQuit){m_bQuitOnIterateFail = bQuit;};

    /**  Call this to write a debug string to the DB under the name "MOOS_DEBUG"  */
    bool MOOSDebugWrite(const std::string & sTxt);

    /** enable/disable the behind the scenes search for command messages */
    void EnableCommandMessageFiltering(bool bEnable);

    /** Allow ::Iterate to be called without a connection to a DB*/
    void EnableIterateWithoutComms(bool bEnable);

    /** dispatching function for ::OnCommandMsg */
    bool LookForAndHandleAppCommand(MOOSMSG_LIST & NewMail);


    /** return the application name */
    std::string GetAppName();

    /** return the application mission file name */
    std::string GetMissionFileName();


    /////////////////////////////////////////////////////////////////////////////////////////////
    //  DYNAMIC VARIABLES  - AN OPTIONAL GARNISH
    /////////////////////////////////////////////////////////////////////////////////////////////

    /** Add a dynamic (run time) variable
        @param sName name of the variable
        @param sSubscribeName if you call RegisterMOOSVariables() the variable will be updated with mail
        called <sSubscribeName> if and when you call UpdateMOOSVariables()
        @param sPublishName  if you call PublishFreshMOOSVariables() (and you've written to the dynamic varible since the last call) the variable will be published under this name.
        @param CommsTime - if sSubscribeName is not empty this is the minimum time between updates which you are interested in knowing about, so if CommsTime=0.1 then the maximum update rate you will see on the variable from the DB is 10HZ. */
    bool AddMOOSVariable(std::string sName,std::string sSubscribeName,std::string sPublishName,double dfCommsTime);

    /** Sets the value of a previously added dynamic variable to the given CMOOSVariable.
     * @return \c true if the variable's type and value were set, \c false otherwise.   */
    bool SetMOOSVar(const CMOOSVariable& MOOSVar);


    /** return a pointer to a named variable */
    CMOOSVariable * GetMOOSVar(std::string sName);

    /** Register with the DB to be mailed about any changes to any dynamic variables which were created with non-empty sSubscribeName fields */
    bool RegisterMOOSVariables();


    /** Pass mail (usually collected in OnNewMail) to the set of dynamic variables. If they are interested (mail name matches their subscribe name) they will update themselves automatically */
    bool UpdateMOOSVariables(MOOSMSG_LIST & NewMail);

    /** Set  value in a dynamic variable if the variable is of type double (type is set on first write )*/
    bool SetMOOSVar(const std::string & sName,const std::string & sVal,double dfTime);

    /** Set  value in a dynamic variable if the variable is of type string (type is set on first write ) */
    bool SetMOOSVar(const std::string & sVarName,double dfVal,double dfTime);


    /** Send any variables (under their sPublishName see AddMOOSVariable)  which been written too since the last call of PublishFreshMOOSVariables()*/
    bool PublishFreshMOOSVariables();


    /** a map of dynamic/run time moos variables that may be set by comms - avoid messy long
    if else if statements */
    MOOSVARMAP m_MOOSVars;


    /** Returns true if Simulate = true is found in the mission/configuration file (a global flag) - the mission file is not re-read on each call */
    bool IsSimulateMode();

    /** flag saying whether MOOS is running with a simulator
    can be set by registering for SIMULATION_MODE variable*/
    bool m_bSimMode;


    /** called just before the main app loop is entered. Specific initialisation code can be written
    in an overloaded version of this function */
    virtual bool OnStartUp();

    /** start up the comms */
    virtual bool ConfigureComms();

    /** Port on which server application listens for new connection */
    long m_lServerPort;

    /** name of machine on which MOOS Server resides */
    std::string m_sServerHost;

    /** std::string version of m_lServerPort*/
    std::string m_sServerPort;

    /** true if the server has been set */
    bool m_bServerSet;

    /** true if we want to use MOOS comms */
    bool m_bUseMOOSComms;

    /** name of this application */
    std::string m_sAppName;

    /** subscribe name of application usually by default this will be m_sAppName*/
    std::string m_sMOOSName;

    /** frequency at which server will be contacted */
    int m_nCommsFreq;

    /** frequency at which this application will iterate */
    double m_dfFreq;

    /** std::string name of mission file */
    std::string m_sMissionFile;

    /** flag specifying whether command message fitlering is enabled */
    bool m_bCommandMessageFiltering;

    /** flag to say whether or not App should quit after an Iterate returns false*/
    bool m_bQuitOnIterateFail;
    /** The start time of the application */
    double m_dfAppStartTime;

    /** Time at which the Run loop last ran (called Iterate)**/
    double m_dfLastRunTime;


    /**should mail be handed to the user sorted by increasing time*/
    bool m_bSortMailByTime;

    /** string that should be written to the status string if the App Error flag is true */
    std::string m_sAppError;

    /** flag specifying the error state of the App - set via SetAppError()*/
    bool m_bAppError;


    /** Time since last iterate was called*/
    double GetTimeSinceIterate();

    /** Return time at which the Run loop last ran (called Iterate) - this is a local time - you need
     to add GetMOOSSKew to produce a unified system time**/
    double GetLastIterateTime();

    /** return number of times iterate has been called*/
    int GetIterateCount();

    /** returns true if we can iterate without comms*/
    bool CanIterateWithoutComms();




    /** returns the string which constitutes a command string for this application.
    if CommandFiltering is enabled (see EnableCommandMessageFiltering() ) the
    application will filter incoming mail and Call OnCommandMsg() (which can be overiden)
    if a message with this command string as a name is received. Command strings look
    like APPNAME_CMD */
    std::string GetCommandKey();


    bool m_bDebug;

    bool   IsDebug(){return m_bDebug;};


public:
    /**these two functions are used to handle private MOOSApp work that
    need to occur on behalf of derived classes at the same time as the
    overloaded OnConnectToServer and OnDisconnectFromServer methods are
    called. They are public to allow their invokation from a call back. They
    are not interesting to the casual user*/
    void OnDisconnectToServerPrivate();
    void OnConnectToServerPrivate();
    bool OnMailCallBack();

    /* by calling this function Iterate and OnNewMail will be
     called from the thread that is servicing the MOOS Comms client. It
     is provided to let really very specialised MOOSApps have very speedy
     response times. It is not recommended for general use*/
    bool UseMailCallBack();

private:
    /* this function is used to process mail on behalf of the client just before
       the derived OnNewMail is invoked - it has no interest to the casual user*/
    void OnNewMailPrivate(MOOSMSG_LIST & NewMail);
    /* and this is a private iterate - we may need to regularly do things behind the scenes */
    void IteratePrivate();

    /**can we iterate without comms*/
    bool m_bIterateWithoutComms;


private:

    /** Number of times Application has cycled */
    int m_nIterateCount;

    /** Number of  Application has had new mail */
    int m_nMailCount;

    /** last time a status message was sent */
    double m_dfLastStatusTime;

    /** called before starting the Application running. If parameters have not beedn set correctly
    it prints a help statement and returns false */
    bool CheckSetUp();

    /** ::Run continues forever or until this variable is false*/
    bool m_bQuitRequested;


};

#endif


//CMOOSApp.cpp
///////////////////////////////////////////////////////////////////////////
//
//   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 at MIT 2001-2002 and Oxford
//   University 2003-2005. email: pnewman@robots.ox.ac.uk.
//
//   This file is part of a  MOOS Core Component.
//
//   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    //////////////////////////////////

// MOOSApp.cpp: implementation of the CMOOSApp class.
//
//////////////////////////////////////////////////////////////////////
#ifdef _WIN32
#pragma warning(disable : 4786)
#pragma warning(disable : 4503)
#endif

#include "MOOSGlobalHelper.h"
#include "MOOSApp.h"
#include <cmath>
#include <iostream>
#include <sstream>
#include <iterator>

using namespace std;

// predicate for sorting on time
bool MOOSMsgTimeSorter(const CMOOSMsg  &M1, const CMOOSMsg &M2) 
{
    return (M1.GetTime() < M2.GetTime());
}

//////////////////////////////////////////////////
//  these are file-scope methods which allow
//  redirection of a call back into the CMOOSApp Class
//  they don't concern the average user...
///////////////////////////////////////////////////
bool MOOSAPP_OnConnect(void * pParam)
{
    if(pParam!=NULL)
    {
        CMOOSApp* pApp = (CMOOSApp*)pParam;

        //we may have private work to do
        pApp->OnConnectToServerPrivate();

        //client work
        return pApp->OnConnectToServer();

    }
    return false;
}

bool MOOSAPP_OnDisconnect(void * pParam)
{
    if(pParam!=NULL)
    {
        CMOOSApp* pApp = (CMOOSApp*)pParam;

        //private work
        pApp->OnDisconnectToServerPrivate();

        //client work
        return pApp->OnDisconnectFromServer();

    }
    return false;
}


bool MOOSAPP_OnMail(void *pParam)
{
    if(pParam!=NULL)
    {
        CMOOSApp* pApp = (CMOOSApp*)pParam;

        //client mail work
        return pApp->OnMailCallBack();

    }
    return false;

}

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CMOOSApp::CMOOSApp()
{
    m_dfFreq=DEFAULT_MOOS_APP_FREQ;
    m_nCommsFreq=DEFAULT_MOOS_APP_COMMS_FREQ;
    m_nIterateCount = 0;
    m_nMailCount = 0;
    m_bServerSet = false;
    m_dfAppStartTime = -1;
    m_bDebug = false;
    m_bSimMode = false;
    m_bUseMOOSComms = true;
    m_dfLastRunTime = -1;
    m_bCommandMessageFiltering = false;
    m_dfLastStatusTime = -1;
    m_bSortMailByTime = true;
    m_bAppError = false;
    m_bQuitOnIterateFail = false;
    m_bQuitRequested = false;

    SetMOOSTimeWarp(1.0);

    EnableIterateWithoutComms(false);
}

CMOOSApp::~CMOOSApp()
{

}

//this is an overloaded 3 parameter version which allows explicit setting of the registration name
bool CMOOSApp::Run(const char * sName,const char * sMissionFile,const char * sMOOSName)
{
    //fill in specialised MOOSName
    m_sMOOSName = sMOOSName;
    return Run(sName,sMissionFile);
}

//the main MOOSApp Run function
bool CMOOSApp::Run( const char * sName,
                    const char * sMissionFile)
{

    //save absolutely crucial info...
    m_sAppName      = sName;
    m_sMissionFile  = sMissionFile;
    m_MissionReader.SetAppName(m_sAppName);

    //by default we will 
    if(m_sMOOSName.empty())
        m_sMOOSName=m_sAppName;


    //can we see the mission file
    if(sMissionFile!=NULL)
    {
        if(!m_MissionReader.SetFile(m_sMissionFile.c_str()))
        {
            MOOSTrace("Warning Mission File \"%s\" not found...\n",m_sMissionFile.c_str());
        }

        if(1)
        {
            //what is the global time warp
            double dfTimeWarp = 1.0;
            if(m_MissionReader.GetValue("MOOSTimeWarp", dfTimeWarp))
            {
                SetMOOSTimeWarp(dfTimeWarp);
            }


            //are we expected to use MOOS comms?
            m_MissionReader.GetConfigurationParam("UseMOOSComms",m_bUseMOOSComms);

            //are we being asked to sort mail by time..
            m_MissionReader.GetConfigurationParam("SortMailByTime",m_bSortMailByTime);

            //are we in debug mode
            m_MissionReader.GetConfigurationParam("DEBUG",m_bDebug);

            //are we in simulator mode?
            string sSim;
            if(m_MissionReader.GetValue("SIMULATOR",sSim))
            {
                m_bSimMode = MOOSStrCmp(sSim,"TRUE");
            }

            //are we in playback mode
            string sPlayBack;
            if(m_MissionReader.GetValue("PLAYBACK",sPlayBack))
            {
                SetMOOSPlayBack(MOOSStrCmp(sPlayBack,"TRUE"));
            }

            //OK now figure out our tick speeds  above what is set by default
            //in derived class constructors this can be set in the process config block
            //by the mission architect
            m_MissionReader.GetConfigurationParam("APPTICK",m_dfFreq);


            //do we want to enable command filtering (default is set in constructor)
            m_MissionReader.GetConfigurationParam("CatchCommandMessages",m_bCommandMessageFiltering);
        }
    }

    //what time did we start?
    m_dfAppStartTime = MOOSTime();

    //can we start the communications ?
    if(m_bUseMOOSComms)
    {
        if(!ConfigureComms())
        {
            return false;
        }

        ///////////////////////////////////////////////////////////////
        //OK we are going to wait for a conenction to be established
        // this is a little harsh but it saves derived classes having to
        // hold off connecting to the server until ready
        // but we will only hang around for 1 second...
        // so it is possible that notifies will fail...but very unlikely
        // note this is not a hack! just being helpful. Ths success of an
        // application is NOT dependent on this
        int t = 0;
        int dT = 50;
        while(!m_Comms.IsConnected())
        {
            MOOSPause(dT);
            t+=dT;
            if(t>5000)
                break;
        }
        //give iostream time to write comms start details up to screen..this is not really necessary
        //as the code is thread safe...it is aesthetic only
        MOOSPause(500);
    }



    /** let derivatives do stuff before execution*/
    if(!OnStartUp())
    {
        MOOSTrace("Derived OnStartUp() returned false... Quitting\n");
        return false;
    }


    MOOSTrace("%s is Running:\n",GetAppName().c_str());
    MOOSTrace("\t AppTick   @ %.1f Hz\n",m_dfFreq);
    MOOSTrace("\t CommsTick @ %d Hz\n",m_nCommsFreq);
    if(GetMOOSTimeWarp()!=1.0)
        MOOSTrace("\t Time Warp @ %.1f \n",GetMOOSTimeWarp());


    /****************************  THE MAIN MOOS APP LOOP **********************************/

    while(!m_bQuitRequested)
    {
        if(!m_Comms.HasMailCallBack())
        {

            bool bOK = DoRunWork();

            if(m_bQuitOnIterateFail && !bOK)
                return MOOSFail("MOOSApp Exiting as requested");
        }
        else
            MOOSPause(1000);

    }

    /***************************   END OF MOOS APP LOOP ***************************************/

    return true;
}


/*called by a third party to request a MOOS App to quit - only useful for 
 example if a MOOSApp is run in a secondary thread */
bool CMOOSApp::RequestQuit()
{
    m_bQuitRequested = true;
    return true;
}

bool CMOOSApp::DoRunWork()
{
    //look for mail
    double dfT1 = MOOSLocalTime();
    //local vars
    MOOSMSG_LIST MailIn;
    if(m_bUseMOOSComms)
    {
        if( m_Comms.Fetch(MailIn))
        {
            /////////////////////////////
            //   process mail
            if(m_bSortMailByTime)
                MailIn.sort(MOOSMsgTimeSorter);


            //call our own private version
            OnNewMailPrivate(MailIn);

            //classes will have their own personal versions of this
            OnNewMail(MailIn);

            m_nMailCount++;
        }

        if(m_Comms.IsConnected() ||  CanIterateWithoutComms() )
        {
            //do private work
            IteratePrivate();

            //////////////////////////////////////
            //  do application specific processing
            bool bOK = Iterate();

            if(m_bQuitOnIterateFail && !bOK)
                return false;

            m_nIterateCount++;
        }
    }
    else
    {
        //do private work
        IteratePrivate();

        /////////////////////////////////////////
        //  do application specific processing
        bool bOK = Iterate();

        if(m_bQuitOnIterateFail && !bOK)
            return false;


        m_nIterateCount++;
    }

    //store for derived class use the last time iterate was called;
    m_dfLastRunTime = MOOSLocalTime();

    //sleep
    if(m_dfFreq>0)
    {
        int nElapsedTime_ms  = static_cast<int> (1000.0*(m_dfLastRunTime-dfT1));
        int nRequiredWait_ms = static_cast<int> (1000.0/m_dfFreq);

        if (nElapsedTime_ms < 0) nElapsedTime_ms = 0;

        int nSleep = (nRequiredWait_ms - nElapsedTime_ms);

        //a 10 ms sleep is a good as you are likely to get, if we are being told to sleep less than this we may as well
        //tick once more and let the OS schedule us appropriately
        if(nSleep>10 && !m_Comms.HasMailCallBack())
        {
            MOOSPause(nSleep);
        }
    }

    return true;

}



void CMOOSApp::SetServer(const char *sServerHost, long lPort)
{
    m_sServerHost = sServerHost;
    m_lServerPort = lPort;
    m_bServerSet = true;
}

bool CMOOSApp::CheckSetUp()
{
    if(m_sServerHost.empty())
    {
        MOOSTrace("MOOS Server host not specified\n");
        return false;
    }

    if(m_lServerPort==0)
    {
        MOOSTrace("MOOS Server port not specified\n");
        return false;
    }

    return true;
}


void CMOOSApp::SetAppError(bool bErr, const std::string & sErr)
{
    m_bAppError = bErr;
    m_sAppError =  m_bAppError ? sErr : "";
}


/////////////////// EXPERIMENTAL July 2008 ////////////////////
bool CMOOSApp::UseMailCallBack()
{
    /* by calling this function Iterate and OnNewMail will be
     called from the thread that is servicing the MOOS Comms client. It
     is provided to let really very specialised MOOSApps have very speedy
     response times. It is not recommended for general use*/
    m_Comms.SetOnMailCallBack(MOOSAPP_OnMail,this);
    return true;
}

////////////////////// DEFAULT HANDLERS //////////////////////
bool CMOOSApp::OnNewMail(MOOSMSG_LIST &NewMail)
{
    return true;
}

bool CMOOSApp::Iterate()
{
    if(m_nIterateCount==0)
    {
        MOOSTrace("Warning default Iterate handler invoked...doing nothing\n");
    }
    return true;
}

bool CMOOSApp::OnConnectToServer()
{
    MOOSTrace("- default OnConnectToServer called\n");
    return true;
}


bool CMOOSApp::OnDisconnectFromServer()
{
    MOOSTrace("- default OnDisconnectFromServer called\n");
    return true;
}

/** this is a call back from MOOSComms and its use is specialised (not for general consumption)*/
bool CMOOSApp::OnMailCallBack()
{
    return DoRunWork();
}

bool CMOOSApp::OnCommandMsg(CMOOSMsg  CmdMsg)
{
    MOOSTrace("- default OnCommandMsg called\n");
    return true;
}


bool CMOOSApp::ConfigureComms()
{



    if(!m_MissionReader.GetValue("SERVERHOST",m_sServerHost))
    {
        MOOSTrace("Warning Server host not read from mission file: assuming LOCALHOST\n");
        m_sServerHost = "LOCALHOST";
    }


    if(!m_MissionReader.GetValue("SERVERPORT",m_sServerPort))
    {
        MOOSTrace("Warning Server port not read from mission file: assuming 9000\n");
        m_sServerPort = "9000";
    }

    m_lServerPort = atoi(m_sServerPort.c_str());

    if(m_lServerPort==0)
    {
        m_lServerPort = 9000;
        MOOSTrace("Warning Server port not read from mission file: assuming 9000\n");
    }

    if(!CheckSetUp())
        return false;


    //OK now figure out our speeds etc above what is set by default
    //in derived class constructors
    m_MissionReader.GetConfigurationParam("COMMSTICK",m_nCommsFreq);
    m_nCommsFreq = m_nCommsFreq <0 ? 1 : m_nCommsFreq;

    //register a callback for On Connect
    m_Comms.SetOnConnectCallBack(MOOSAPP_OnConnect,this);

    //and one for the disconnect callback
    m_Comms.SetOnDisconnectCallBack(MOOSAPP_OnDisconnect,this);

    //start the comms client....
    if(m_sMOOSName.empty())
        m_sMOOSName = m_sAppName;

    m_Comms.Run(m_sServerHost.c_str(),m_lServerPort,m_sMOOSName.c_str(),m_nCommsFreq);

    return true;
}

/** over load this if you want to do something fancy at statup...*/
bool CMOOSApp::OnStartUp()
{
    return true;
}

int CMOOSApp::GetIterateCount()
{
    return m_nIterateCount;
}

double CMOOSApp::GetAppStartTime()
{
    return m_dfAppStartTime;
}

void CMOOSApp::SetAppFreq(double  dfFreq)
{
    if(m_dfFreq<=MOOS_MAX_APP_FREQ)
    {
        m_dfFreq = dfFreq;
    }
}

bool CMOOSApp::SetCommsFreq(unsigned int nFreq)
{
    if(nFreq<=MOOS_MAX_COMMS_FREQ)
    {
        m_nCommsFreq = nFreq;
        return m_Comms.SetCommsTick(m_nCommsFreq);
    }
    return false;
}

bool CMOOSApp::IsSimulateMode()
{
    return m_bSimMode;
}

bool CMOOSApp::AddMOOSVariable(string sName, string sSubscribeName, string sPublishName,double dfCommsTime)
{
    CMOOSVariable NewVar(sName,sSubscribeName,sPublishName,dfCommsTime);

    //does it already exist?
    if(m_MOOSVars.find(sName)!=m_MOOSVars.end())
        return false;

    m_MOOSVars[sName] = NewVar;

    return true;
}

//this function publishes all the fresh variables belong to the
//apllication. Useful in many sensor applications!
bool CMOOSApp::PublishFreshMOOSVariables()
{
    MOOSVARMAP::iterator p;

    for(p = m_MOOSVars.begin();p!=m_MOOSVars.end();p++)
    {
        CMOOSVariable & Var = p->second;
        if(Var.IsFresh())
        {
            if(Var.IsDouble())
            {
                m_Comms.Notify(Var.GetPublishName(),Var.GetDoubleVal(),Var.GetTime());
            }
            else
            {
                m_Comms.Notify(Var.GetPublishName(),Var.GetStringVal(),Var.GetTime());
            }

            Var.SetFresh(false);
        }
    }

    return true;
}

bool CMOOSApp::SetMOOSVar(const string &sVarName, double dfVal, double dfTime)
{
    MOOSVARMAP::iterator p = m_MOOSVars.find(sVarName);

    if(p==m_MOOSVars.end())
        return false;

    CMOOSVariable & rVar = p->second;

    return rVar.Set(dfVal,dfTime);
}

bool CMOOSApp::SetMOOSVar(const string &sVarName,const  string &sVal, double dfTime)
{
    MOOSVARMAP::iterator p = m_MOOSVars.find(sVarName);

    if(p==m_MOOSVars.end())
        return false;

    CMOOSVariable  & rVar = p->second;

    return rVar.Set(sVal,dfTime);


}

bool CMOOSApp::SetMOOSVar(const CMOOSVariable& Var)
{
    MOOSVARMAP::iterator p = m_MOOSVars.find(Var.GetName ());

    if(p==m_MOOSVars.end())
        return false;

    p->second = Var;
    p->second.SetFresh (true);
    return true;
}

bool CMOOSApp::UpdateMOOSVariables(MOOSMSG_LIST &NewMail)
{
    //we only subscribe to things if we are in simulator mode
    MOOSVARMAP::iterator p;
    double dfTimeNow = MOOSTime();
    for(p = m_MOOSVars.begin();p!=m_MOOSVars.end();p++)
    {
        CMOOSVariable & rVar = p->second;
        CMOOSMsg Msg;
        //MOOSTrace("rVar.GetSubscribeName() is %s\n", rVar.GetSubscribeName().c_str());
        if(m_Comms.PeekMail(NewMail,rVar.GetSubscribeName(),Msg, false, true))
        {
            //MOOSTrace("Peek rVar.GetSubscribeName() is %s********\n", rVar.GetSubscribeName().c_str());
            if(!Msg.IsSkewed(dfTimeNow))
            {
                //MOOSTrace("rVar.GetSubscribeName() is not skewed %s\n", rVar.GetSubscribeName().c_str());
                rVar.Set(Msg);
                rVar.SetFresh(true);
            }
        }
    }

    return true;
}

bool CMOOSApp::RegisterMOOSVariables()
{
    bool bSuccess = true;

    MOOSVARMAP::iterator p;

    for(p = m_MOOSVars.begin();p!=m_MOOSVars.end();p++)
    {
        CMOOSVariable & rVar = p->second;
        if(!rVar.GetSubscribeName().empty())
        {
            double dfCommsTime = rVar.GetCommsTime();
            if(dfCommsTime<0)
            {
                dfCommsTime = 0;
            }

            bSuccess &= m_Comms.Register(rVar.GetSubscribeName(),dfCommsTime);
        }
    }

    return bSuccess;
}

bool CMOOSApp::MOOSDebugWrite(const string &sTxt)
{
    if(m_Comms.IsConnected())
    {
        MOOSTrace(" %s says: %s\n",GetAppName().c_str(),sTxt.c_str());
        return m_Comms.Notify("MOOS_DEBUG",sTxt);
    }
    else
    {
        return false;
    }
}

bool CMOOSApp::CanIterateWithoutComms()
{
    return m_bIterateWithoutComms;
}

void CMOOSApp::EnableIterateWithoutComms(bool bEnable)
{
    m_bIterateWithoutComms = bEnable;
}



double CMOOSApp::GetTimeSinceIterate()
{
    return MOOSLocalTime()-m_dfLastRunTime;
}

double CMOOSApp::GetLastIterateTime()
{
    return m_dfLastRunTime;
}


std::string CMOOSApp::GetMissionFileName()
{
    return m_sMissionFile;
}

string CMOOSApp::GetAppName()
{
    return m_sAppName;
}

bool CMOOSApp::UseMOOSComms(bool bUse)
{
    m_bUseMOOSComms = bUse;

    return true;
}

CMOOSVariable * CMOOSApp::GetMOOSVar(string sName)
{
    MOOSVARMAP::iterator p =  m_MOOSVars.find(sName);
    if(p==m_MOOSVars.end())
    {
        return NULL;
    }
    else
    {
        return &(p->second);
    }
}


std::string CMOOSApp::GetCommandKey()
{
    std::string sCommandKey = GetAppName()+"_CMD";
    MOOSToUpper(sCommandKey);
    return sCommandKey;
}

bool CMOOSApp::LookForAndHandleAppCommand(MOOSMSG_LIST & NewMail)
{
    MOOSMSG_LIST::iterator q;
    bool bResult = true;
    for(q=NewMail.begin();q!=NewMail.end();q++)
    {
        if(MOOSStrCmp(q->GetKey(),GetCommandKey()))
        {
            //give a derived class a chance to respond
            bResult&= OnCommandMsg(*q);
        }
    }
    return bResult;
}


void CMOOSApp::EnableCommandMessageFiltering(bool bEnable)
{
    m_bCommandMessageFiltering = bEnable;
    if(bEnable)
    {
        //we had better register for the message
        m_Comms.Register(GetCommandKey(),0);
    }
    else
    {
        //we are no longer interested
        m_Comms.UnRegister(GetCommandKey());
    }
}


std::string CMOOSApp::MakeStatusString()
{
    std::set<std::string> Published = m_Comms.GetPublished();
    std::set<std::string> Registered = m_Comms.GetRegistered();

    std::stringstream ssStatus;
    ssStatus<<"AppErrorFlag="<<(m_bAppError?"true":"false")<<",";
    if(m_bAppError)
        ssStatus<<"AppErrorReason="<<m_sAppError<<",";

    ssStatus<<"Uptime="<<MOOSTime()-GetAppStartTime()<<",";

    ssStatus<<"MOOSName="<<GetAppName()<<",";

    ssStatus<<"Publishing=\"";
    std::copy(Published.begin(),Published.end(),std::ostream_iterator<string>(ssStatus,","));
    ssStatus<<"\",";

    ssStatus<<"Subscribing=\"";
    std::copy(Registered.begin(),Registered.end(),std::ostream_iterator<string>(ssStatus,","));
    ssStatus<<"\"";

    return ssStatus.str();
}

void CMOOSApp::OnDisconnectToServerPrivate()
{
    return;
}

//called just before calling a derived classes OnConnectToServer()
void CMOOSApp::OnConnectToServerPrivate()
{
    if(m_bCommandMessageFiltering)
    {
        m_Comms.Register(GetCommandKey(),0);
    }
}

/** here we do our private mail processing*/
void CMOOSApp::OnNewMailPrivate(MOOSMSG_LIST & Mail)
{
    //look to handle a command string
    if(m_bCommandMessageFiltering)
        LookForAndHandleAppCommand(Mail);
}

void CMOOSApp::IteratePrivate()
{
    if(fabs(m_dfLastStatusTime-MOOSTime())>STATUS_PERIOD)
    {
        std::string sStatus = GetAppName()+"_STATUS";
        MOOSToUpper(sStatus);
        m_Comms.Notify(sStatus,MakeStatusString());
        m_dfLastStatusTime = MOOSTime();
    }
}

版权声明:本文为博主原创文章,未经博主允许不得转载。

相关文章推荐

机器人操作系统ROS Indigo 入门学习(1)——安装ROS Indigo

Ubuntu14.04安装ROS Indigo   一、安装ROS 1.1配置Ubuntu的软件中心 配置Ubuntu要求允许接受"restricted," "universe," and "mult...

Linux下MOOS编程实例

MOOS简介 CMOOSApp介绍 重要虚函数介绍 1 Iterate 2 OnNewMail 3 OnConnectToServer 4 OnDisconnectFromServer 5 OnSta...

moos-ivp 下载编译运行

旨在帮助研究MOOS-IvP的人员,或一起交流:     首先在MIT官网上下载最新的moos-ivp,下载要用到svn,于是先在ubuntu下下载svn,命令: sudo apt-get in...

linux下设备驱动程序编程

  • 2009-03-19 20:38
  • 813KB
  • 下载

Linux下设备驱动程序编程.

  • 2008-04-04 09:52
  • 280KB
  • 下载

Linux下的C编程实战(五) ――驱动程序设计

1.引言 设备驱动程序是操作系统内核和机器硬件之间的接口,它为应用程序屏蔽硬件的细节,一般来说,Linux的设备驱动程序需要完成如下功能: (1)初始化设备; (2)提供各类设备服务; (3)...

Linux下设备驱动程序编程

  • 2017-10-11 10:11
  • 813KB
  • 下载

linux下串口应用程序编程

这几天,由于多功能温度测量仪项目的需要,涉及到了GSM信息的串口读取,所以在Linux下串口信息的读取有了一点心得体会。1.         打开串口      ...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

(最多只允许输入30个字)