现在的位置: 首页 > 专题 > 机器人 > Robotics Studio > 设计中心 > 文档 > 专题 > 机器人 > 正文

Robotics Studio学习教程:第十五天——继续利用P3DX画地图

2014年05月02日 Robotics Studio, 文档, 机器人 ⁄ 转载:原文链接 ⁄ 共 7875字 ⁄ 字号 评论 9 条 ⁄ 阅读 1,501 次
Robotics Studio学习教程

Robotics Studio学习教程

继之前分享的一篇《Robotics Studio学习教程:第十四天——利用P3DX画地图》, 我们将继续分享下一篇《Robotics Studio学习教程:第十五天——继续利用P3DX画地图》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。

 

[Robotics Studio] P3DX[III] - 继续画地图 -- Day15

 

Day14 的画地图程序只能画当下的 LRF 状态.

其实我们只剩下最后一步了, 就是把自身在地图上的定位找到, 然后藉由移动自己, 重复画地图, 最后你就拥有一张大地图...

这概念其实很简单, 但是实做起来就很头痛.

本来我是没在怕的, 因为看样子人家已经做出来了, 就是那个 ExplorerSim, 结果实际看完 code , 才发现是一点小作弊, 就是把模拟环境 (VSE) 当中的机器人位置直接传回作为定位... (难怪 GenericvDifferentialDrive 提供的大部分属性值通通为 0 , 我还以为是 bug... 但好歹你给我个 CurrentPower, or CurrentSpeed 也好, 都没有, 这个不知道是不是 bug)

现实的机器人世界, 除了户外定位的 GPS 以外, 好像没这么好的传感器啊 XD。大部分的解决办法是透过外部系统的参考点, 算出自己的位置, 比方, 藉由摄影机看到几个地标 (Landmarks) 然后根据那些地标算出自己跟地标之间的相互距离.

恩, 在不是很精准的情况下, 我们可以透过 GenericDifferentialDrive 所提供的 DriveDistance, RotateDegrees 来做其实也可以达到效果.(但是因为 GenericDifferentialDrive 的属性值大都为 0, 所以如果你透过 Dashboard 去操控机器人, 我们的地图就错乱啦)

所以, 基于这个想法, 可以在 LRFStatus Form 上面加上三个 Button, 一个是 Scan Map (from LRF status) , 一个是往前进一个距离 (DriveDistance), 然后 Scan Map, 另外一个是旋转(RotateDegrees) 然后 Scan Map, 做完这样的练习, 我们应该就对 DSS 的讯息传输接收机制有点心得.

为了接收 Button 的指令, 当然我们得要先定义 DataContract (就是讯息的内容 (body)), 如下:

  1.    [DataContract]
  2.     public class ScanMapData
  3.    {
  4.        [DataMember]
  5.        public double ForwardScan;

  6.        [DataMember]
  7.        public double RotateScan;
  8.    }

 

然后, 为了这个需求, 我们还要在收到指令后, 算出下一次 ScanMap 在地图当中的位置, 所以在状态当中新增 NextPosition, 还有, 我们要开始移动了, 地图要大一点才行, 所以状态的定义如下 :

  1.   /// <summary>
  2.   /// LRFDrive state
  3.   /// </summary>
  4.   [DataContract]
  5.   public class LRFDriveState
  6.   {
  7.       [DataMember]
  8.       public DrivePos CurrntPosition;

  9.       [DataMember]
  10.       public DrivePos NextPosition;

  11.       [DataMember]
  12.       public drive.DriveDifferentialTwoWheelState DriveState;

  13.       [DataMember]
  14.       public sicklrf.State LrfState;

  15.       [DataMember]
  16.       public MapBlockInfo Map;

  17.       public LRFDriveState()
  18.       {
  19.           CurrntPosition = new DrivePos();
  20.           Map = LRFMapDrawer.CreateMapBlock(-15, -15, 30, 30, 0.05);
  21.       }
  22.   }

 

接着, 我们还要接收 GenericDifferentialDrive 的两个讯息 (DriveDistance, RotateDegrees) , 这个工作我们之前在 VPL 做过, 现在在 DSS 当作再做一次, 架构上当然是收到通知后, 再转发讯息给我们自己, 这样比较简单. 所以我们又多了两个接收讯息的 DSSP , 现在 LRFDriveOperations 的定义如下:

  1.   /// <summary>
  2.   /// LRFDrive main operations port
  3.   /// </summary>
  4.   [ServicePort]
  5.  public class LRFDriveOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, Subscribe, UpdateDrive, UpdateLRF, ScanMapCommand, UpdateDriveDistance, UpdateDriveRotate>
  6.  {
  7.  }

  8.  public class UpdateDrive : Update<drive.DriveDifferentialTwoWheelState, PortSet<DefaultUpdateResponseType, Fault>> { }

  9.  public class UpdateLRF : Update<sicklrf.State, PortSet<DefaultUpdateResponseType, Fault>> { }   

  10.  public class ScanMapCommand : Update<ScanMapData, PortSet<DefaultUpdateResponseType, Fault>> { }

  11.  public class UpdateDriveDistance : Update<drive.DriveDistanceRequest, PortSet<DefaultUpdateResponseType, Fault>> { }

  12.  public class UpdateDriveRotate : Update<drive.RotateDegreesRequest, PortSet<DefaultUpdateResponseType, Fault>> { }

 

剩下来的就是把通知转为讯息发给我们自己, 当然是透过 Activate 加上 Reciver 来做, 所以我们可以在 Start() 加上 :

  1.  Activate(Arbiter.Receive<drive.DriveDistance>(true, _driveNotify, update => _mainPort.Post(new UpdateDriveDistance() { Body = update.Body })));
  2.  Activate(Arbiter.Receive<drive.RotateDegrees>(true, _driveNotify, update => _mainPort.Post(new UpdateDriveRotate() { Body = update.Body })));

 

再来, 就是把 ScanMapCommand, UpdateDriveDistance, UpdateDriveRotate 的 Handler 写好, 如下:

  1.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  2.   public IEnumerator<ITask> ScanMapCommandHandler(ScanMapCommand smd)
  3.   {
  4.       if ((smd.Body.ForwardScan == 0) && (smd.Body.RotateScan == 0))
  5.       {
  6.           LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
  7.           UpdateMap();
  8.           UpdatePosInfo();
  9.       }
  10.       else if (smd.Body.ForwardScan > 0)
  11.       {
  12.           _state.NextPosition = new DrivePos()
  13.        {
  14.          Direction = _state.CurrntPosition.Direction,
  15.          X = _state.CurrntPosition.X + smd.Body.ForwardScan * Math.Cos(_state.CurrntPosition.Direction),
  16.          Y = _state.CurrntPosition.Y + smd.Body.ForwardScan * Math.Sin(_state.CurrntPosition.Direction)
  17.        };
  18.          _driveDifferentialTwoWheelPort.Post(new drive.DriveDistance(
  19.               new drive.DriveDistanceRequest(smd.Body.ForwardScan, 0.3)));
  20.       }
  21.     else if (smd.Body.RotateScan > 0)
  22.       {
  23.           double currentdegree = (_state.CurrntPosition.Direction / Math.PI) * 180.0;
  24.           double newdegree = currentdegree - smd.Body.RotateScan;
  25.           if (newdegree > 360)
  26.               newdegree -= 360;
  27.           if (newdegree < -360)
  28.               newdegree += 360;

  29.           _state.NextPosition = new DrivePos()
  30.           {
  31.               Direction = newdegree * Math.PI / 180.0,
  32.               X = _state.CurrntPosition.X,
  33.               Y = _state.CurrntPosition.Y
  34.           };

  35.         _driveDifferentialTwoWheelPort.Post(new drive.RotateDegrees(
  36.               new drive.RotateDegreesRequest(smd.Body.RotateScan, 0.2)));
  37.       }

  38.       smd.ResponsePort.Post(DefaultUpdateResponseType.Instance);

  39.       yield break;
  40.   }
  41.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  42.   public IEnumerator<ITask> UpdateDriveDistanceHandler(UpdateDriveDistance update)
  43.   {
  44.  if ((_state.NextPosition != null) && (update.Body.DriveDistanceStage == Microsoft.Robotics.Services.Drive.Proxy.DriveStage.Completed))
  45.      {
  46.          _state.CurrntPosition = new DrivePos()
  47.           {
  48.               Direction = _state.NextPosition.Direction,
  49.               X = _state.NextPosition.X,
  50.               Y = _state.NextPosition.Y
  51.           };
  52.          UpdatePosInfo();        Activate(Arbiter.Receive(false, TimeoutPort(1000), time => _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));
  53.       }
  54.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
  55.       yield break;
  56.   }


  57.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  58.   public IEnumerator<ITask> UpdateDriveRotateHandler(UpdateDriveRotate update)
  59.   {
  60. if ((_state.NextPosition != null) && (update.Body.RotateDegreesStage == Microsoft.Robotics.Services.Drive.Proxy.DriveStage.Completed))
  61.       {
  62.           _state.CurrntPosition = new DrivePos()
  63.           {
  64.               Direction = _state.NextPosition.Direction,
  65.               X = _state.NextPosition.X,
  66.               Y = _state.NextPosition.Y
  67.           };
  68.           UpdatePosInfo();       Activate(Arbiter.Receive(false, TimeoutPort(1000), time => _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));
  69.       }
  70.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
  71.       yield break;
  72.   }

 

从 code 你可以知道, 我们收到 ForwardScan == 0, RotateScan == 0 时就马上 ScanMap, 否则就丢给 GenericDifferentialDrive, 之后收到通知以后, 再等一秒 (因为你知道的, DriveDistance 都是暴力停止, 在这个情况下, LRF 会传回地面数据给我们) 然后再要求 ScanMap. 当然, 之前的 UpdateLRFHandler 要改为:

  1.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  2.   public IEnumerator<ITask> UpdateLRFHandler(UpdateLRF update)
  3.   {
  4.       _state.LrfState = update.Body;
  5.       UpdateLRFImage();
  6.       //LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
  7.       //UpdateMap();
  8.       //UpdatePosInfo();
  9.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
  10.       yield break;
  11.   }

 

最后, 要怎么从 LRFStatus Form 去呼叫我们的 LRFDrive DSS Service ? 当然是把自己这个 MainPort 丢给它最简单, 所以在 LRFStatus 当中加上这个属性:

  1.   public LRFDriveOperations LRFPort;

 

然后在 start() 生出 Form 的时候把值设定好, 像是这样:

  1.    WinFormsServicePort.Post(new RunForm(() =>
  2.    {
  3.        LRFForm = new LRFStatus();
  4.        LRFForm.LRFPort = _mainPort;
  5.        return LRFForm;
  6.    }));

 

最后在 Form 弄三个 Button, 按下去的时候分别做以下的动作:

  1.   private void button1_Click(object sender, EventArgs e)
  2.   {
  3.       LRFPort.Post(new ScanMapCommand()
  4.       {
  5.           Body = new ScanMapData() { RotateScan = 0, ForwardScan = 1 }
  6.       });
  7.   }

  8.   private void button2_Click(object sender, EventArgs e)
  9.   {
  10.       LRFPort.Post(new ScanMapCommand()
  11.       {
  12.           Body = new ScanMapData() { RotateScan = 0, ForwardScan = 0 }
  13.       });
  14.   }

  15.   private void button3_Click(object sender, EventArgs e)
  16.   {
  17.       LRFPort.Post(new ScanMapCommand()
  18.       {
  19.           Body = new ScanMapData() { RotateScan = 90, ForwardScan = 0 }
  20.       });
  21.   }

 

好! 去 VPL 玩玩看:

clip_image002

假设定位精准 (现实上 DriveDistance, RotateDegrees 有误差, LRF 也会有误差), 我们应该可以画出整个地图...

 

 

让我们继续一下章教程:

Robotics Studio学习教程:第十六天——利用P3DX自动画地图

fgx


分享到:

Wopus问答

  1. 虽然原博文已经写了很多年了,楼主在这里将繁体转换成了简体,赞一个,

×