MOOS-ivp 实验九 分布式旅行商问题（1）

MOOS-ivp 实验九 分布式旅行商问题（1）

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
例如：第一章 Python 机器学习入门之pandas的使用

---

提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

在今天的实验中，将继续关注涉及多AUV的自主配置。实验练习将涉及分布在多个车辆上的旅行商问题(TSP)。TSP是寻找到最短路径来访问一组城市的问题，假设任意两个城市之间的距离是已知的，要求访问每个城市至少一次，并最小化总的路程。
今天的实验主要就是如何部署多辆vehicle来处理旅行商问题。给定一组要访问的点，将这些点分配给一组要访问这些点的vehicle。由于优化的旅行商问题算法可能相当复杂，为了将事情简化，要考虑到了非最佳的最短旅行距离，同时专注于让多个合作车辆运行的机制。在这个实验中：
（1）使用uTimerScript脚本工具在海滨生成随机的游览点
（2）编写一个新的MOOS应用程序并在Shoreside上运行，目的是给vehicle发放随机的游览点 。
（3）vehicle需要配置为从shoreside接受访问的游览点
（4）写一个MOOSapp用来生成一个waypoint的旅行规划
（5）自治任务为巡视这些游览点，并定期返回进行加油。
（6）恢复的任务将继续完成未完成的游览，直到完成全部的游览任务

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、分布式TSP——准备shoreside

旅行推销员问题(TSP)是寻找一条路径来访问一组城市的问题，其中任意两个城市之间的距离是已知的，以这种方式来访问每个城市至少一次，并最小化总的旅行距离。在本节中，使用TSP问题来练习我们部署多辆车来处理TSP问题的一个版本——分布式TSP问题。给定一组要访问的点，将这些点分配给一组要访问这些点的车辆。由于优化的旅行商问题算法可能相当复杂，我们将事情简化了一点，并考虑到了非最佳的最短旅行距离，同时专注于让多个合作车辆运行的机制。
在这个部分，任务是：
（1）创建一个uTimerScript脚本，在指定区域生成100个随机点
（2）在岸边创建一个“pPointAssign MOOS”模块，将一半的点发送给一辆车，另一半发送给另一辆车
（3）确保pshare共享通信配置为向车辆共享点
（4）通过验证在一对车辆上接收到访问点来确认其工作

1.创建用于生成访问点的脚本

配置在shoreside社区上运行的uTimerScript脚本，以在操作区域的某个区域内生成随机的点序列。这个区域范围是:
-25,-25
-25,-175
200,-25
200,-175
脚本 需要 发布100个点到 MOOSDB上：

VISIT_POINT = "firstpoint"
VISIT_POINT = "x=8, y=9, id=1"
...
VISIT_POINT = "x=-11, y=-9, id=100"
VISIT_POINT = "lastpoint"

格式类似于上述所示。
根据要求可得配置uTimerScript代码如下：

//--------------------------------------------------------
// uTimerScript Configuration Block


 ProcessConfig = uTimerScript                                    
{
 //  name = random_points_generated
   paused = true
   pause_var = UTS_PAUSE
   
   rand_var    = varname=RND_VAL_X, min=-25, max=200, key=at_post
   rand_var 	 = varname=RND_VAL_Y, min=-175, max=-25, key=at_post
   
   event = var=VISIT_POINT, val="firstpoint", time=0
  event = var=VISIT_POINT, val="x=$[RND_VAL_X], y=$[RND_VAL_Y], id=$[TCOUNT]", time=0, amt=100

event = var=VISIT_POINT, val="lastpoint", time=0
 }

下面对上面的变量及其用法进行解释：
paused = true：设定一开始脚本不启动
pause_var = UTS_PAUSE：设置控制脚本对应的变量名称
rand_var：要在事件值中展开的随机变量宏的声明。

event：定时器脚本中单个事件的描述

event = var=<MOOSVar>, val=<var-value>, time=<time-of-event>

event组件可以包含一个表单为$[macro]的宏，这个宏可以是为数不多的内置宏之一，也可以是一个用户定义的、能够表示随机变量的宏。宏也可以组合在简单的算术表达式中，以提供进一步的表达能力。在每种情况下，宏在事件发布时展开，通常在每次后续发布时都有不同的值。
ps：我在创建脚本建立100个随机点是按照手册上uTimerScript的例程来实现的，实际上amt变量并不能识别，于是我便写了100条重复的event事件来创建100个点，虽然比较麻烦，但是效果是一样的。

二、创建一个MOOSapp来分发点

实验要求创建一个MOOSapp命名为pPointAssign ，以此app来分发点，一半的点分发给一艘vehicle，另一半的点分发给另外一艘vehicle。
（1）首先这个app应该订阅uTimerScript发布的变量值，并产生如下的输出

VISIT_POINT_HENRY = "firstpoint"
VISIT_POINT_HENRY = "x=8, y=9, id=1"
...
VISIT_POINT_HENRY = "x=33, y=29, id=50"
VISIT_POINT_HENRY = "lastpoint"
VISIT_POINT_GILDA = "firstpoint"
VISIT_POINT_GILDA = "x=19, y=111, id=51"
...
VISIT_POINT_GILDA = "x=-11, y=-9, id=100"
VISIT_POINT_GILDA = "lastpoint"

（2）pointassign发布的第一个和最后一个帖子，共享给每辆vehicle，应该是变量VNAME=“firstpoint”和访问点VNAME=“lastpoint”。这些帖子，可以确认已经发送和接收了完整的访问点。
（3）应用程序应该有一个配置参数vname，它将已知的车辆添加到将要分发的车辆列表中
（4）应用程序应该支持两种方式之一的分数分配。在第一种方式中，分数是交替分配的。第一点指向第一辆车，第二点指向第二辆车，第三点指向第一辆车，以此类推。第二种方式是根据地区来分配分数，比如东西方向。应用程序应该通过assign_by_region参数分配的地区设置为true或false来配置为任何一种模式。
（5）在按东/西区域分配点数时，不需要给每辆车分配一个精确的平均数量。要把这个区域分成两半，然后相应地分配。考虑到随机点的均匀分布，它们应该在每一边接近50-50，但不能完全保证。

1.订阅uTimerScript脚本中发布的变量值

首先在.h文件中对常用变量进行相关的定义

class PointAssign : public CMOOSApp
{
 public:
   PointAssign();
   ~PointAssign();

 protected: // Standard MOOSApp functions to overload  
   bool OnNewMail(MOOSMSG_LIST &NewMail);
   bool Iterate();
   bool OnConnectToServer();
   bool OnStartUp();

 protected:
   void RegisterVariables();
   bool PointRegionIsEast(double x_val);
   void postViewPoint(double x, double y, std::string label, std::string color);


 private: // Configuration variables
   std::vector<std::string> m_vname_list;
   std::vector<std::string> m_visit_points;
   bool m_assign_by_region;
   std::string m_vname_str;
   bool m_reached_first_point;
   bool m_reached_last_point;
   bool m_notified_all;
 private: // State variables
};

接下来首先来订阅脚本发布的变量

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

  for(p=NewMail.begin(); p!=NewMail.end(); p++) {
    CMOOSMsg &msg = *p;
    string key   = msg.GetKey();
    string sval  = msg.GetString();


    if (key=="VISIT_POINT"){
      if (sval=="firstpoint"){
	m_reached_first_point = true;
	std::cout<<"reached first point"<<std::endl;
      }
      else if (sval == "lastpoint"){
	m_reached_last_point = true;
	std::cout<<"reached last point"<<std::endl;
      }
      else if (m_reached_first_point==true && m_reached_last_point ==false){
	m_visit_points.push_back(sval);
	std::cout<<"added point to visit points"<<std::endl;
      }
    }

#if 0 // Keep these around just for template
    string key   = msg.GetKey();
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
    string sval  = msg.GetString();
    string msrc  = msg.GetSource();
    double mtime = msg.GetTime();
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();
#endif
  }

   return(true);
}

一般订阅脚本中的变量这一步骤都在函数PointAssign::OnNewMail(MOOSMSG_LIST &NewMail)中来进行实现， string key = msg.GetKey()与string sval = msg.GetString()函数分别用来获取MOOSDB上发布的变量的名称以及变量值，如果变量 if (key==“VISIT_POINT”)，那么就将其值进行存储m_visit_points.push_back(sval);如果发布的变量是第一个以及最后一个，那么改变相应的标志为，存储的变量都是按照发布顺序，除了第一个与最后一个发布的变量来进行存储的。
这里再讲一下pushback函数的用法：
函数将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素
push_back() 在Vector最后添加一个元素（参数为要插入的值）

int num = 10;
vector<int> vec;
vec.push_back(num);

或者再string中最后插入一个字符

string str;
str.push_back('d');

类似函数用法有：

pop_back() //移除最后一个元素

clear() //清空所有元素

empty() //判断vector是否为空，如果返回true为空

erase() // 删除指定元素

2.配置参数vname以及参数assign_by_region

通过配置文件给进程进行参数设置，相关代码如下，一般读取配置文件的参数这一步骤在PointAssign::OnStartUp()函数中完成：

bool PointAssign::OnStartUp()
{
  Notify("UTS_PAUSE","false");
  list<string> sParams;
  m_MissionReader.EnableVerbatimQuoting(false);
  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
    list<string>::iterator p;
    for(p=sParams.begin(); p!=sParams.end(); p++) {
      string original_line = *p;
      string param = stripBlankEnds(toupper(biteString(*p, '=')));
      string value = stripBlankEnds(*p);
         std::cout<<"param: "<<param<<std::endl;
      if(param == "VNAME") {
        //add to vehicle list of names
		m_vname_list.push_back(value);
      }
      else if(param == "ASSIGN_BY_REGION") {
	if (value=="true"){
	  m_assign_by_region = true;
	}
	else{
	  m_assign_by_region = false;
	}
      }
    }
  }
  m_timewarp = GetMOOSTimeWarp();
  RegisterVariables();
  return(true);
}

Notify(“UTS_PAUSE”,“false”);设置脚本启动输出point数据
m_MissionReader.GetConfiguration(GetAppName()函数就是用来对配置文件中的相关数据进行读取和使用的， if(param == “VNAME”)
如果读取到的配置参数为"VNAME"，那么记录下当前读取到的值，并将其存入姓名列表里
m_vname_list.push_back(value)。另外一个参数是用来对两种分配方式进行确定的，
if(param == “ASSIGN_BY_REGION”)那么就对相关变量 m_assign_by_region进行相应的赋值，来确定后续的point分配方式。

3.按两种方式分配点数

其中涉及到到关于stringstream常见用法介绍详情可以参考这篇博文。
设计到按两种方式分配点数的代码如下，通过判断m_assign_by_region 的值来进行不同类型的点数分配工作：

bool PointAssign::Iterate()
{
  // NVM: Only iterate if first and last points
  std::cout<<"notified all: "<<m_notified_all<<std::endl;
  //  if (m_reached_first_point == true && m_reached_last_point == true && m_notified_all == false){
  //   if (m_reached_first_point == true && m_reached_last_point == true && m_notified_all == false){
  //  if (m_reached_first_point == true && m_reached_last_point == true){

  //Loop through list of points and alternate assignment
  if (m_assign_by_region ==false){

    //  std::vector<std::string>::const_iterator i = m_vname_list.begin();
    int i = 0;
    for (std::vector<std::string>::const_iterator k = m_visit_points.begin(); k != m_visit_points.end(); ++k){

      std::string x_str = tokStringParse(*k, "x", ',', '=');//进行数据筛选
      std::string y_str = tokStringParse(*k, "y", ',', '=');
      std::string id_str = tokStringParse(*k,"id",',','=');
      std::cout<<"ID = "<<id_str<<std::endl;
       double x_double = 0.0;
       double y_double = 0.0;
       stringstream rr;
       stringstream ww;

       rr<<x_str;
       ww<<y_str;
       rr>>x_double;
       ww>>y_double;
       std::string color_label_str;

      if (i==0){
	m_vname_str="HENRY";
	i=1;
	color_label_str = "red";
      }
      else{
	m_vname_str="GILDA";
	i=0;
	color_label_str= "yellow";
      }
	stringstream ss;
	ss<<"VISIT_POINT_"<<m_vname_str;
	Notify(ss.str(),*k);
	postViewPoint(x_double, y_double,id_str, color_label_str);
    }
    std::cout<<"finished looping through all points"<<std::endl;
    m_notified_all = true;

  }
  else{ //Assign by region
    std::cout<<"Assigning by region"<<"size of visit points: "<<m_visit_points.size()<<std::endl;
    for (std::vector<std::string>::const_iterator k = m_visit_points.begin(); k != m_visit_points.end(); ++k){
      std::string x_str = tokStringParse(*k, "x", ',', '=');
      std::string y_str = tokStringParse(*k, "y", ',', '=');
      std::string id_str = tokStringParse(*k,"id",',','=');
      std::cout<<"ID = "<<id_str<<std::endl;
       double x_double = 0.0;
       double y_double = 0.0;
       stringstream rr;
       stringstream ww;
       rr<<x_str;
       ww<<y_str;
       rr>>x_double;
       ww>>y_double;

       bool is_east = PointRegionIsEast(x_double);
       std::cout<<"east: "<<is_east<<std::endl;
       if (is_east){
	 	stringstream vv;
		vv<<"VISIT_POINT_"<<m_vname_list[0];
		std::cout<<vv.str()<<std::endl;
		Notify(vv.str(),*k);
		std::cout<<"calling post view point"<<std::endl;
		postViewPoint(x_double, y_double, id_str, "yellow");

       }
       else{
	 	stringstream vv;
		vv<<"VISIT_POINT_"<<m_vname_list[1];
		std::cout<<vv.str()<<std::endl;
		Notify(vv.str(),*k);
		std::cout<<"calling post view point"<<std::endl;
		postViewPoint(x_double, y_double, id_str, "red");


       }
    }
    std::cout<<"finished looping through all points"<<std::endl;
    m_notified_all = true;
  }

  // }
  return(true);

}

第一种分配方式是按照点数顺序随机给两台AUV分配point， std::string x_str = tokStringParse(*k, “x”, ‘,’, ‘=’);是在对传入的字符串进行数据筛选，选出x、y、id的值， 定义stringstream rr类是来进行数据的输入输出操作，主要目的是实现数据类型的转换，可以将str类型的数据转换为double型。分配完成数据点之后使用Notify(ss.str(),*k)将分配后的结果上传至MOOSDB中去，并使用postViewPoint函数将结果绘制在pMarineViewer显示的图中。
其主要代码是：

void PointAssign::postViewPoint(double x, double y, string label, string color){
 {
   XYPoint point(x, y);
   point.set_label(label);
   point.set_label_color(color);
   point.set_color("vertex", color);
   point.set_param("vertex_size", "5");

   string spec = point.get_spec();
   std::cout<<"notifying view point"<<std::endl;
   Notify("VIEW_POINT", spec);
 }

XYPoint类的主要作用就是给pMarineViewer应用上的图进行绘制，与之类似的类还有XYPolygon, XYSegList, XYPoint, XYSeglr 和 XYVector等等。
第二种方式就是按照区域进行分配，通过判断x坐标的大小，来对点数进行分配，判断的函数是 is_east = PointRegionIsEast(x_double)，其代码非常简单：

bool PointAssign::PointRegionIsEast(double x_val){
  return (x_val<100.0);
}

4.从MOOSDB上订阅变量

这行代码非常简单，只需要对"VISIT_POINT"变量进行订阅即可，其调用在onstartup函数上进行，也就是在程序的最开始初始化时进行订阅。

void PointAssign::RegisterVariables()
{
  // Register("FOOBAR", 0);
  Register("VISIT_POINT",0);
}

三、编译 PointAssign查看效果

此处总结一下如何使用cede::block对文件进行编译：
新建工程项目选择empty project

选择C\C++header

生成新工程之后，选中Project，右键，选择“Properties”，“Project settings”选项卡中的“Makefile”填入Makefile 文件名，并勾选“This is a custom Makefile”。

注意execuytion directory选择目录moos-ivp-extend下的build文件夹：

选中 Project，右键，选择“Build options”，删除““Make” command”选项卡中所有的“$target”。更改后如下图所示

记得在该目录下添加要编译的工程名：

如果新建的工程需要一些MOOS自带的库函数，在工程目录下的Cmakelist里进行添加：

本项目就需要另外添加geometry库，否则编译时XYpoint类会报错。
点击进入相关目录下，执行

./launch

可以看到如图的显示，100个随机点分布在图形中，说明程序编写的没有问题

总结

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