kobuki 红外自动回充源码原理分析

kobuki 核心代码：https://github.com/yujinrobot/kobuki_core

kobuki ros代码：https://github.com/yujinrobot/kobuki

官方回充文档：https://kobuki.readthedocs.io/en/devel/docking.html

翻译回充文档：https://www.ncnynl.com/archives/201611/1109.html

 

0.回充结构图：

0.1 对接站

  • Kobuki的对接站有3个IR（红外）发射。
  • 发射的红外信号灯覆盖了对接站前方的三个区域：左、中、右，各分为两个子场：近和远。
  • 根据每束编码的这些信息，所以机器人知道在任何时刻，他是在哪个区域和子场。
  • 此外，作为区域和子场是独立确定的，它们可以在其边界上的重叠。

0.2 红外接收

  • Kobuki有3个红外接收器。
  • 当机器人被放置停靠站的领域内，和至少一个IR接收器面对它，机器人就会捕获信号和上传数据流到连接控制器上。
  • 信息发布在/mobile_base/sensors/dock_ir话题，带有kobuki_msgs/DockInfraRed格式的消息(ros类型：https://docs.ros.org/en/groovy/api/kobuki_msgs/html/msg/DockInfraRed.html)

 

1.原理

  • 基本的想法很简单。如果机器人被放置在对接站的中心区域，它很容易停靠。只要按照中央区域的信号，机器人可以到达对接站。然而，如果机器人被放置在左或右区域，事情变得更有趣。
  • 如果机器人被放置在左区域上，它将逆时针旋转，直到他的右传感器检测到左区域信号。在这一点上，机器人垂直面向中心，所以他只需要向前移动到中部地区。现在，机器人应该看到中央区域的信号，所以达到了对接站变得微不足道。开始在右区域是相似的，但方向是倒的。

 

2.回充状态机定义：

源码地址：kobuki_core-noetic\kobuki_dock_drive\include\kobuki_dock_drive\state.hpp

源码：

/*
 * Copyright (c) 2013, Yujin Robot.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Yujin Robot nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file /kobuki_dock_drive/include/kobuki_dock_drive/state.hpp
 *
 * @brief States 
 *
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef KOBUKI_DOCK_DRIVE_STATE_HPP_
#define KOBUKI_DOCK_DRIVE_STATE_HPP_

/*****************************************************************************
** States
*****************************************************************************/
#include <iostream>

namespace kobuki {

  // indicates the ir sensor from docking station
  struct DockStationIRState {
    enum State {
      INVISIBLE=0,
      NEAR_LEFT=1,
      NEAR_CENTER=2,
      NEAR_RIGHT=4,
      FAR_CENTER=8,
      FAR_LEFT=16,
      FAR_RIGHT=32,
      NEAR = 7, // NEAR_LEFT + NEAR_CENTER + NEAR_RIGHT
      FAR = 56, // FAR_CENTER + FAR_LEFT + FAR_RIGHT
    };
  };

  // the current robot states
  struct RobotDockingState {
    enum State {
      IDLE,
      DONE,
      DOCKED_IN,
      BUMPED_DOCK,
      BUMPED,
      SCAN,
      FIND_STREAM,
      GET_STREAM,
      ALIGNED,
      ALIGNED_FAR,
      ALIGNED_NEAR,
      UNKNOWN,
      LOST
    };

  };

  /*
  struct RobotDockingState {
      enum State {
        IDLE,
        NEAR_LEFT,
        NEAR_CENTER,
        NEAR_RIGHT,
        FAR_CENTER,
        FAR_LEFT,
        FAR_RIGHT,
        IN_DOCK,
        DONE,
        ERROR,
      };
  };*/
}

#endif // KOBUKI_DOCK_DRIVE_STATE_HPP_
View Code

 

3.状态更新逻辑：/kobuki_driver/src/driver/dock_drive_states.cpp

3.1 DockDrive::scan()
   *  @breif While it rotates ccw, determines the dock location with only middle sensor.
   *         If its middle sensor detects center ir, the robot is aligned with docking station
   *  Shared variable
   *  @dock_detecotr - indicates where the dock is located. Positive means dock is on left side of robot
   *  @rotated       - indicates how much the robot has rotated while scan
该步骤做旋转并标记状态，且只用middle sensor检测。
如果发现mid IR，则进入aligned()模式；发现Left IR,dock_detector--;发现Right IR, dock_detector++;
旋转弧度超过1，进入find_stream()模式；
 
3.2 DockDrive::find_stream()
   * Find stream
   *  @breif based on dock_detector variable, it determines the dock's location and rotates toward the center line of dock
   *  Shared variable
   *  @dock_detector - to determine dock's location
dock_detector > 0，robot is located in right side of dock，turn right, CW until get right signal from left sensor,进入get_stream();
dock_detector < 0，robot is located in left side of dock，turn left , CCW until get left  signal from right sensor,进入get_stream();
该步骤做左右旋转调整过程。充电桩在右侧，右转至左IR sensor接收到右信号。充电桩在左侧，左转至右IR sensor接收到左信号。然后向前直行,进入get_stream()模式。
 
3.3 DockDrive::get_stream()
In this state, robot is heading the center line of dock. When it passes the center, it rotates toward the dock
dock_detector > 0，robot is located in right side of dock，turn right, CW until get right signal from left sensor,进入get_stream();
dock_detector < 0，robot is located in left side of dock，turn left , CCW until get left  signal from right sensor,进入get_stream();
该步骤做向前直行过程。充电桩在右侧，左IR sensor接收到左信号，则开始左转，进入scan()环节。右IR sensor接收到右信号，则开始右转，进入scan()环节。
 
3.4 DockDrive::aligned()：
Robot sees center IR with middle sensor. It is heading dock.It approaches to the dock only using mid sensor。
此时机器人已看见充电桩的mid IR信号，说明充电桩就在正前方，只用底盘的mid IR回充。
仅看到充电桩的mid IR信号，直行；否则，在直行基础上叠加角速度偏转量，修正角度。
 
3.5 DockDrive::bumped()：
碰撞触发后，电机负速度后退，计次延时执行电机停止。
  1 /*
  2  * copyright (c) 2013, Yujin Robot.
  3  * all rights reserved.
  4  *
  5  * redistribution and use in source and binary forms, with or without
  6  * modification, are permitted provided that the following conditions are met:
  7  *
  8  *     * redistributions of source code must retain the above copyright
  9  *       notice, this list of conditions and the following disclaimer.
 10  *     * redistributions in binary form must reproduce the above copyright
 11  *       notice, this list of conditions and the following disclaimer in the
 12  *       documentation and/or other materials provided with the distribution.
 13  *     * neither the name of yujin robot nor the names of its
 14  *       contributors may be used to endorse or promote products derived from
 15  *       this software without specific prior written permission.
 16  *
 17  * this software is provided by the copyright holders and contributors "as is"
 18  * and any express or implied warranties, including, but not limited to, the
 19  * implied warranties of merchantability and fitness for a particular purpose
 20  * are disclaimed. in no event shall the copyright owner or contributors be
 21  * liable for any direct, indirect, incidental, special, exemplary, or
 22  * consequential damages (including, but not limited to, procurement of
 23  * substitute goods or services; loss of use, data, or profits; or business
 24  * interruption) however caused and on any theory of liability, whether in
 25  * contract, strict liability, or tort (including negligence or otherwise)
 26  * arising in any way out of the use of this software, even if advised of the
 27  * possibility of such damage.
 28  */
 29 /**
 30  * @file /kobuki_driver/src/driver/dock_drive_states.cpp
 31  *
 32  **/
 33 
 34 /*****************************************************************************
 35 ** includes
 36 *****************************************************************************/
 37 
 38 #include "kobuki_dock_drive/dock_drive.hpp"
 39 
 40 /*****************************************************************************
 41 ** Namespaces
 42 *****************************************************************************/
 43 
 44 namespace kobuki {
 45   /*********************************************************
 46    * Shared variables among states
 47    * @ dock_detector : records + or - when middle IR sensor detects docking signal
 48    * @ rotated : records how much the robot has rotated in scan state
 49    * @ bump_remainder : from processBumpChargerEvent.
 50    *********************************************************/
 51 
 52 
 53   /*******************************************************
 54    * Idle
 55    *  @breif Entry of auto docking state machine
 56    *
 57    *  Shared variable
 58    *  @dock_detecotr - indicates where the dock is located. Positive means dock is on left side of robot
 59    *  @rotated       - indicates how much the robot has rotated while scan
 60    *******************************************************/
 61   void DockDrive::idle(RobotDockingState::State& nstate,double& nvx, double& nwz) {
 62     dock_detector = 0;
 63     rotated = 0.0;
 64     nstate = RobotDockingState::SCAN;
 65     nvx = 0;
 66     nwz = 0.66;
 67   }
 68 
 69   /********************************************************
 70    * Scan
 71    *  @breif While it rotates ccw, determines the dock location with only middle sensor.
 72    *         If its middle sensor detects center ir, the robot is aligned with docking station
 73    *
 74    *  Shared variable
 75    *  @dock_detecotr - indicates where the dock is located. Positive means dock is on left side of robot
 76    *  @rotated       - indicates how much the robot has rotated while scan
 77    ********************************************************/
 78   void DockDrive::scan(RobotDockingState::State& nstate,double& nvx, double& nwz, const std::vector<unsigned char>& signal_filt, const ecl::LegacyPose2D<double>& pose_update, std::string& debug_str) {
 79     unsigned char mid   = signal_filt[1];
 80 
 81     RobotDockingState::State next_state;
 82     double next_vx;
 83     double next_wz;
 84 
 85     rotated += pose_update.heading() / (2.0 * M_PI);
 86     std::ostringstream oss;
 87     oss << "rotated: " << std::fixed << std::setprecision(2) << std::setw(4) << rotated;
 88     debug_str = oss.str();
 89 
 90 
 91 
 92     if((mid & DockStationIRState::FAR_CENTER) || (mid & DockStationIRState::NEAR_CENTER))
 93     {
 94       next_state = RobotDockingState::ALIGNED;
 95       next_vx = 0.05;
 96       next_wz = 0.0;
 97     }
 98     // robot is located left side of dock
 99     else if(mid & (DockStationIRState::FAR_LEFT + DockStationIRState::NEAR_LEFT))
100     {
101       dock_detector--;
102       next_state = RobotDockingState::SCAN;
103       next_vx = 0.0;
104       next_wz = 0.66;
105     }
106     // robot is located right side of dock
107     else if(mid & (DockStationIRState::FAR_RIGHT + DockStationIRState::NEAR_RIGHT))
108     {
109       dock_detector++;
110       next_state = RobotDockingState::SCAN;
111       next_vx = 0.0;
112       next_wz = 0.66;
113     }
114     // robot is located in front of robot
115     else if(mid) { // if mid sensor sees something, rotate slowly
116       next_state = RobotDockingState::SCAN;
117       next_vx = 0.0;
118       next_wz = 0.10;
119     }
120     else if(std::abs(rotated) > 1.0)
121     {
122       next_state = RobotDockingState::FIND_STREAM;
123       next_vx = 0;
124       next_wz = 0;
125     }
126     else { // if mid sensor does not see anything, rotate fast
127       next_state = RobotDockingState::SCAN;
128       next_vx = 0.0;
129       next_wz = 0.66;
130     }
131 
132     nstate = next_state;
133     nvx = next_vx;
134     nwz = next_wz;
135   }
136 
137   /********************************************************
138    * Find stream
139    *  @breif based on dock_detector variable, it determines the dock's location and rotates toward the center line of dock
140    *
141    *  Shared variable
142    *  @dock_detector - to determine dock's location
143    *
144    ********************************************************/
145   void DockDrive::find_stream(RobotDockingState::State& nstate,double& nvx, double& nwz, const std::vector<unsigned char>& signal_filt) {
146     unsigned char right = signal_filt[0];
147     unsigned char left  = signal_filt[2];
148     RobotDockingState::State next_state = RobotDockingState::UNKNOWN;
149     double next_vx = 0.0;
150     double next_wz = 0.0;
151 
152     if(dock_detector > 0) // robot is located in right side of dock
153     {
154       // turn right, CW until get right signal from left sensor
155       if(left & (DockStationIRState::FAR_RIGHT + DockStationIRState::NEAR_RIGHT)) {
156         next_state = RobotDockingState::GET_STREAM;
157         next_vx = 0.5;
158         next_wz = 0.0;
159       }
160       else {
161         next_state = RobotDockingState::FIND_STREAM;
162         next_vx = 0.0;
163         next_wz = -0.33;
164       }
165     }
166     else if(dock_detector < 0 ) // robot is located in left side of dock
167     {
168       // turn left, CCW until get left signal from right sensor
169       if(right & (DockStationIRState::FAR_LEFT + DockStationIRState::NEAR_LEFT))
170       {
171         next_state = RobotDockingState::GET_STREAM;
172         next_vx = 0.5;
173         next_wz = 0.0;
174       }
175       else {
176         next_state = RobotDockingState::FIND_STREAM;
177         next_vx = 0.0;
178         next_wz = 0.33;
179       }
180     }
181 
182     nstate = next_state;
183     nvx = next_vx;
184     nwz = next_wz;
185   }
186 
187  /********************************************************
188   * Get stream
189   *   @brief In this state, robot is heading the center line of dock. When it passes the center, it rotates toward the dock
190   *
191   *   Shared Variable
192   *   @ dock_detector - reset
193   *   @ rotated       - reset
194   ********************************************************/
195   void DockDrive::get_stream(RobotDockingState::State& nstate,double& nvx, double& nwz, const std::vector<unsigned char>& signal_filt)
196   {
197     unsigned char right = signal_filt[0];
198     unsigned char left  = signal_filt[2];
199     RobotDockingState::State next_state = RobotDockingState::UNKNOWN;
200     double next_vx = 0.0;
201     double next_wz = 0.0;
202 
203     if(dock_detector > 0) { // robot is located in right side of dock
204       if (left & (DockStationIRState::FAR_LEFT + DockStationIRState::NEAR_LEFT)) {
205         dock_detector = 0;
206         rotated = 0;
207         next_state = RobotDockingState::SCAN;
208         next_vx = 0;
209         next_wz = 0.1;
210       }
211       else {
212         next_state = RobotDockingState::GET_STREAM;
213         next_vx = 0.05;
214         next_wz = 0.0;
215       }
216     }
217     else if(dock_detector < 0) { // robot is located left side of dock
218       if(right & (DockStationIRState::FAR_RIGHT + DockStationIRState::NEAR_RIGHT)) {
219         dock_detector = 0;
220         rotated = 0;
221         next_state = RobotDockingState::SCAN;
222         next_vx = 0;
223         next_wz = 0.1;
224       }
225       else {
226         next_state = RobotDockingState::GET_STREAM;
227         next_vx = 0.05;
228         next_wz = 0.0;
229       }
230     }
231 
232     nstate = next_state;
233     nvx = next_vx;
234     nwz = next_wz;
235   }
236 
237 
238  /********************************************************
239   * Aligned
240   *   @breif Robot sees center IR with middle sensor. It is heading dock. It approaches to the dock only using mid sensor
241   *
242   *   Shared Variable
243   *   @ dock_detector - reset
244   *   @ rotated       - reset
245   ********************************************************/
246   void DockDrive::aligned(RobotDockingState::State& nstate,double& nvx, double& nwz, const std::vector<unsigned char>& signal_filt, std::string& debug_str)
247   {
248     unsigned char mid   = signal_filt[1];
249     RobotDockingState::State next_state = nstate;
250     double next_vx = nvx;
251     double next_wz = nwz;
252 
253     if(mid)
254     {
255       if(((mid & DockStationIRState::NEAR) == DockStationIRState::NEAR_CENTER) || ((mid & DockStationIRState::NEAR) == DockStationIRState::NEAR))
256       {
257         debug_str = "AlignedNearCenter";
258         next_state = RobotDockingState::ALIGNED_NEAR;
259         next_vx = 0.05;
260         next_wz = 0.0;
261       }
262       else if(mid & DockStationIRState::NEAR_LEFT) {
263         debug_str = "AlignedNearLeft";
264         next_state = RobotDockingState::ALIGNED_NEAR;
265         next_vx = 0.05;
266         next_wz = 0.1;
267       }
268       else if(mid & DockStationIRState::NEAR_RIGHT) {
269         debug_str = "AlignedNearRight";
270         next_state = RobotDockingState::ALIGNED_NEAR;
271         next_vx = 0.05;
272         next_wz = -0.1;
273       }
274       else if(((mid & DockStationIRState::FAR) == DockStationIRState::FAR_CENTER) || ((mid & DockStationIRState::FAR) == DockStationIRState::FAR)) {
275         debug_str = "AlignedFarCenter";
276         next_state = RobotDockingState::ALIGNED_FAR;
277         next_vx = 0.1;
278         next_wz = 0.0;
279       }
280       else if(mid & DockStationIRState::FAR_LEFT) {
281         debug_str = "AlignedFarLeft";
282         next_state = RobotDockingState::ALIGNED_FAR;
283         next_vx = 0.1;
284         next_wz = 0.3;
285       }
286       else if(mid & DockStationIRState::FAR_RIGHT) {
287         debug_str = "AlignedFarRight";
288         next_state = RobotDockingState::ALIGNED_FAR;
289         next_vx = 0.1;
290         next_wz = -0.3;
291       }
292       else {
293         dock_detector = 0;
294         rotated = 0.0;
295         next_state = RobotDockingState::SCAN;
296         next_vx = 0.0;
297         next_wz = 0.66;
298       }
299     }
300     else {
301         next_state = RobotDockingState::SCAN;
302         next_vx = 0.0;
303         next_wz = 0.66;
304     }
305 
306     nstate = next_state;
307     nvx = next_vx;
308     nwz = next_wz;
309   }
310 
311 
312  /********************************************************
313   * Bumped
314   *  @breif Robot has bumped somewhere. Go backward for 10 iteration
315   *
316   ********************************************************/
317   void DockDrive::bumped(RobotDockingState::State& nstate,double& nvx, double& nwz, int& bump_count)
318   {
319     if(bump_count < 10)
320     {
321       nvx = -0.05;
322       nwz = 0.0;
323       bump_count++;
324     }
325     else {
326       nstate = RobotDockingState::SCAN;
327       nvx = 0.0;
328       nwz = 0.0;
329       bump_count = 0;
330     }
331 
332   }
333 }
View Code

 

4.运行逻辑：/kobuki_driver/src/driver/dock_drive.cpp

  1 /*
  2  * copyright (c) 2013, yujin robot.
  3  * all rights reserved.
  4  *
  5  * redistribution and use in source and binary forms, with or without
  6  * modification, are permitted provided that the following conditions are met:
  7  *
  8  *     * redistributions of source code must retain the above copyright
  9  *       notice, this list of conditions and the following disclaimer.
 10  *     * redistributions in binary form must reproduce the above copyright
 11  *       notice, this list of conditions and the following disclaimer in the
 12  *       documentation and/or other materials provided with the distribution.
 13  *     * neither the name of yujin robot nor the names of its
 14  *       contributors may be used to endorse or promote products derived from
 15  *       this software without specific prior written permission.
 16  *
 17  * this software is provided by the copyright holders and contributors "as is"
 18  * and any express or implied warranties, including, but not limited to, the
 19  * implied warranties of merchantability and fitness for a particular purpose
 20  * are disclaimed. in no event shall the copyright owner or contributors be
 21  * liable for any direct, indirect, incidental, special, exemplary, or
 22  * consequential damages (including, but not limited to, procurement of
 23  * substitute goods or services; loss of use, data, or profits; or business
 24  * interruption) however caused and on any theory of liability, whether in
 25  * contract, strict liability, or tort (including negligence or otherwise)
 26  * arising in any way out of the use of this software, even if advised of the
 27  * possibility of such damage.
 28  */
 29 /**
 30  * @file /kobuki_driver/src/driver/dock_drive.cpp
 31  *
 32  **/
 33 
 34 /*****************************************************************************
 35 ** includes
 36 *****************************************************************************/
 37 
 38 #include "kobuki_dock_drive/dock_drive.hpp"
 39 
 40 /*****************************************************************************
 41 ** defines
 42 *****************************************************************************/
 43 
 44 #define sign(x) (x>0?+1:x<0?-1:0)
 45 #define stringfy(x) #x
 46 #define setState(x) {state=x;}
 47 #define setStateVel(x,v,w) {setState(x);setVel(v,w);}
 48 
 49 /*****************************************************************************
 50 ** Namespaces
 51 *****************************************************************************/
 52 
 53 namespace kobuki {
 54 
 55 /*****************************************************************************
 56 ** Implementation
 57 *****************************************************************************/
 58 DockDrive::DockDrive() :
 59   is_enabled(false)
 60   , can_run(false)
 61   , state(RobotDockingState::IDLE), state_str("IDLE")
 62   , vx(0.0), wz(0.0)
 63   , signal_window(20)
 64   , bump_remainder(0)
 65   , dock_stabilizer(0)
 66   , dock_detector(0)
 67   , rotated(0.0)
 68   , min_abs_v(0.01)
 69   , min_abs_w(0.1)
 70   , ROBOT_STATE_STR(13)
 71 {
 72   // Debug messages
 73   ROBOT_STATE_STR[0] = "IDLE";
 74   ROBOT_STATE_STR[1] = "DONE";
 75   ROBOT_STATE_STR[2] = "DOCKED_IN";
 76   ROBOT_STATE_STR[3] = "BUMPED_DOCK";
 77   ROBOT_STATE_STR[4] = "BUMPED";
 78   ROBOT_STATE_STR[5] = "SCAN";
 79   ROBOT_STATE_STR[6] = "FIND_STREAM";
 80   ROBOT_STATE_STR[7] = "GET_STREAM";
 81   ROBOT_STATE_STR[8] = "ALIGNED";
 82   ROBOT_STATE_STR[9] = "ALIGNED_FAR";
 83   ROBOT_STATE_STR[10] = "ALIGNED_NEAR";
 84   ROBOT_STATE_STR[11] = "UNKNOWN";
 85   ROBOT_STATE_STR[12] = "LOST";
 86 }
 87 
 88 DockDrive::~DockDrive(){;}
 89 
 90 void DockDrive::setVel(double v, double w)
 91 {
 92   vx = sign(v) * std::max(std::abs(v), min_abs_v);
 93   wz = sign(w) * std::max(std::abs(w), min_abs_w);
 94 }
 95 
 96 void DockDrive::modeShift(const std::string& mode)
 97 {
 98   if (mode == "enable")  { is_enabled = true;  can_run = true; state = RobotDockingState::IDLE;}
 99   if (mode == "disable") { is_enabled = false; can_run = false; }
100   if (mode == "run")  can_run = true;
101   if (mode == "stop") can_run = false;
102 }
103 
104 
105 /**
106  * @brief Updates the odometry from firmware stamps and encoders.
107  *
108  * Really horrible - could do with an overhaul.
109  *
110  * @param dock_ir signal
111  * @param bumper sensor
112  * @param charger sensor
113  * @param current pose
114  */
115 void DockDrive::update(const std::vector<unsigned char> &signal
116                 , const unsigned char &bumper
117                 , const unsigned char &charger
118                 , const ecl::LegacyPose2D<double>& pose) {
119 
120   ecl::LegacyPose2D<double> pose_update;
121   std::vector<unsigned char> signal_filt(signal.size(), 0);
122   std::string debug_str;
123 
124   // process bumper and charger event first
125   // docking algorithm terminates here
126   if(bumper || charger) {
127     processBumpChargeEvent(bumper, charger);
128   }
129   else {
130     computePoseUpdate(pose_update, pose);
131     filterIRSensor(signal_filt, signal);
132     updateVelocity(signal_filt, pose_update, debug_str);
133   }
134 
135   velocityCommands(vx, wz);
136 
137   // for easy debugging
138   generateDebugMessage(signal_filt, bumper, charger, pose_update, debug_str);
139 
140   return;
141 }
142 
143 /**
144  * @brief compute pose update from previouse pose and current pose
145  *
146  * @param pose update. this variable get filled after this function
147  * @param pose - current pose
148  **/
149 void DockDrive::computePoseUpdate(ecl::LegacyPose2D<double>& pose_update, const ecl::LegacyPose2D<double>& pose)
150 {
151   double dx = pose.x() - pose_priv.x();
152   double dy = pose.y() - pose_priv.y();
153   pose_update.x( std::sqrt( dx*dx + dy*dy ) );
154   pose_update.heading( pose.heading() - pose_priv.heading() );
155   //std::cout << pose_diff << "=" << pose << "-" << pose_priv << " | " << pose_update << std::endl;
156   pose_priv = pose;
157 
158 }
159 
160 
161 /**
162  * @breif pushing into signal into signal window. and go through the signal window to find what has detected
163  *
164  * @param signal_filt - this get filled out after the function.
165  * @param signal - the raw data from robot
166  **/
167 
168 void DockDrive::filterIRSensor(std::vector<unsigned char>& signal_filt,const std::vector<unsigned char> &signal)
169 {
170   //dock_ir signals filtering
171   past_signals.push_back(signal);
172   while (past_signals.size() > signal_window) {
173     past_signals.erase( past_signals.begin(), past_signals.begin() + past_signals.size() - signal_window);
174   }
175 
176   for ( unsigned int i = 0; i < past_signals.size(); i++) {
177     if (signal_filt.size() != past_signals[i].size())
178       continue;
179     for (unsigned int j = 0; j < signal_filt.size(); j++)
180       signal_filt[j] |= past_signals[i][j];
181   }
182 }
183 
184 
185 void DockDrive::velocityCommands(const double &vx_, const double &wz_) {
186   // vx: in m/s
187   // wz: in rad/s
188   vx = vx_;
189   wz = wz_;
190 }
191 
192 /****************************************************
193  * @brief process bumper and charge event. If robot is charging, terminates auto dokcing process. If it bumps something, Set the next state as bumped and go backward
194  *
195  * @bumper - indicates whether bumper has pressed
196  * @charger - indicates whether robot is charging
197  *
198  ****************************************************/
199 void DockDrive::processBumpChargeEvent(const unsigned char& bumper, const unsigned char& charger) {
200   RobotDockingState::State new_state = RobotDockingState::UNKNOWN;
201   if(charger && bumper) {
202     new_state = RobotDockingState::BUMPED_DOCK;
203     setStateVel(new_state, -0.01, 0.0);
204   }
205   else if(charger) {
206     if(dock_stabilizer++ == 0) {
207       new_state = RobotDockingState::DOCKED_IN;
208       setStateVel(new_state, 0.0, 0.0);
209     }
210     else if(dock_stabilizer > 20) {
211       dock_stabilizer = 0;
212       is_enabled = false;
213       can_run = false;
214       new_state = RobotDockingState::DONE;
215       setStateVel(new_state, 0.0, 0.0);
216     }
217     else {
218       new_state = RobotDockingState::DOCKED_IN;
219       setStateVel(new_state, 0.0, 0.0);
220     }
221   }
222   else if(bumper) {
223     new_state = RobotDockingState::BUMPED;
224     setStateVel(new_state, -0.05, 0.0);
225     bump_remainder = 0;
226   }
227   state_str = ROBOT_STATE_STR[new_state];
228 }
229 
230 /*************************
231  * @breif processing. algorithms; transforma to velocity command
232  *
233  * @param dock_ir signal
234  * @param bumper sensor
235  * @param charger sensor
236  * @param pose_update
237  *
238  *************************/
239 void DockDrive::updateVelocity(const std::vector<unsigned char>& signal_filt, const ecl::LegacyPose2D<double>& pose_update, std::string& debug_str)
240 {
241   std::ostringstream oss;
242   RobotDockingState::State current_state, new_state;
243   double new_vx = 0.0;
244   double new_wz = 0.0;
245 
246   // determine the current state based on ir and the previous state
247   // common transition. idle -> scan -> find_stream -> get_stream -> scan -> aligned_far -> aligned_near -> docked_in -> done
248 
249   current_state = new_state = state;
250   switch((unsigned int)current_state) {
251     case RobotDockingState::IDLE:
252       idle(new_state, new_vx, new_wz);
253       break;
254     case RobotDockingState::SCAN:
255       scan(new_state, new_vx, new_wz, signal_filt, pose_update, debug_str);
256       break;
257     case RobotDockingState::FIND_STREAM:
258       find_stream(new_state, new_vx, new_wz, signal_filt);
259       break;
260     case RobotDockingState::GET_STREAM:
261       get_stream(new_state, new_vx, new_wz, signal_filt);
262       break;
263     case RobotDockingState::ALIGNED:
264     case RobotDockingState::ALIGNED_FAR:
265     case RobotDockingState::ALIGNED_NEAR:
266       aligned(new_state, new_vx, new_wz, signal_filt, debug_str);
267       break;
268     case RobotDockingState::BUMPED:
269       bumped(new_state, new_vx, new_wz, bump_remainder);
270       break;
271     default:
272       oss << "Wrong state : " << current_state;
273       debug_str = oss.str();
274       break;
275   }
276 
277   setStateVel(new_state, new_vx, new_wz);
278   state_str = ROBOT_STATE_STR[new_state];
279 }
280 
281 /*************************
282  * @breif Check if any ir sees the given state signal from dock station
283  *
284  * @param filtered signal
285  * @param dock ir state
286  *
287  * @ret true or false
288  *************************/
289 bool DockDrive::validateSignal(const std::vector<unsigned char>& signal_filt, const unsigned int state)
290 {
291   unsigned int i;
292   for(i = 0; i < signal_filt.size(); i++)
293   {
294     if(signal_filt[i] & state)
295       return true;
296   }
297   return false;
298 }
299 
300 } // kobuki namespace
View Code
 
（1）.状态机默认在IDLE状态初始化，设置旋转速度0.66，然后进入SCAN模式。
（2）.SCAN模式：旋转，发现mid IR，则进入aligned()模式，否则（发现Left IR,dock_detector--;发现Right IR, dock_detector++;），当旋转弧度超过1，进入find_stream()模式
（3）.FIND_STREAM模式：左右旋转，若dock_detector>0,充电桩在右侧,右转至左IR sensor接收到右信号。若dock_detector<0,充电桩在左侧，左转至右IR sensor接收到左信号。调整后，向前直行,进入get_stream()模式。
（4）.GET_STREAM模式： 向前直行,  若dock_detector>0,充电桩在右侧，左IR sensor接收到左信号，则开始左转，进入scan()环节。若dock_detector<0, 右IR sensor接收到右信号，则开始右转，进入scan()环节。
（5）.ALIGNED_NEAR模式：仅看到充电桩的mid IR信号，直行；否则，在直行基础上叠加角速度偏转量，修正角度。
（6）.BUMPED模式：bumper触发，进入该模式，执行后退动作。

5.ros调用代码: /auto_docking/src/auto_docking_ros.cpp/hpp

  1 /**
  2  * @file /auto_docking/src/auto_docking_ros.cpp
  3  *
  4  * @brief File comment
  5  *
  6  * File comment
  7  *
  8  **/
  9 /*****************************************************************************
 10 ** Includes
 11 *****************************************************************************/
 12 
 13 #include "kobuki_auto_docking/auto_docking_ros.hpp"
 14 
 15 namespace kobuki
 16 {
 17 
 18 //AutoDockingROS::AutoDockingROS()
 19 AutoDockingROS::AutoDockingROS(std::string name)
 20 //AutoDockingROS::AutoDockingROS(ros::NodeHandle& nh)
 21 //AutoDockingROS::AutoDockingROS(ros::NodeHandle& nh, std::string name)
 22   : name_(name)
 23   , shutdown_requested_(false)
 24   , as_(nh_, name_+"_action", false)
 25 {
 26   self = this;
 27 
 28   as_.registerGoalCallback(boost::bind(&AutoDockingROS::goalCb, this));
 29   as_.registerPreemptCallback(boost::bind(&AutoDockingROS::preemptCb, this));
 30   as_.start();
 31 }
 32 
 33 AutoDockingROS::~AutoDockingROS()
 34 {
 35   shutdown_requested_ = true;
 36   if (as_.isActive()) {
 37     result_.text = "Aborted: Shutdown requested.";
 38     as_.setAborted( result_, result_.text );
 39   }
 40   dock_.disable();
 41 }
 42 
 43 bool AutoDockingROS::init(ros::NodeHandle& nh)
 44 {
 45   // Configure docking drive
 46   double min_abs_v, min_abs_w;
 47   if (nh.getParam("min_abs_v", min_abs_v) == true)
 48     dock_.setMinAbsV(min_abs_v);
 49 
 50   if (nh.getParam("min_abs_w", min_abs_w) == true)
 51     dock_.setMinAbsW(min_abs_w);
 52 
 53   // Publishers and subscribers
 54   velocity_commander_ = nh.advertise<geometry_msgs::Twist>("velocity", 10);
 55   debug_jabber_ = nh.advertise<std_msgs::String>("debug/feedback", 10);
 56 
 57   debug_ = nh.subscribe("debug/mode_shift", 10, &AutoDockingROS::debugCb, this);
 58 
 59   odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "odom", 10));
 60   core_sub_.reset(new message_filters::Subscriber<kobuki_msgs::SensorState>(nh, "core", 10));
 61   ir_sub_.reset(new message_filters::Subscriber<kobuki_msgs::DockInfraRed>(nh, "dock_ir", 10));
 62   sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), *odom_sub_, *core_sub_, *ir_sub_));
 63   sync_->registerCallback(boost::bind(&AutoDockingROS::syncCb, this, _1, _2, _3));
 64 
 65   return dock_.init();
 66 }
 67 
 68 void AutoDockingROS::spin()
 69 {
 70   return;
 71 
 72   while(!shutdown_requested_){;}
 73 }
 74 
 75 void AutoDockingROS::goalCb()
 76 {
 77   if (dock_.isEnabled()) {
 78     goal_ = *(as_.acceptNewGoal());
 79     result_.text = "Rejected: dock_drive is already enabled.";
 80     as_.setAborted( result_, result_.text );
 81     ROS_INFO_STREAM("[" << name_ << "] New goal received but rejected.");
 82   } else {
 83     dock_.enable();
 84     goal_ = *(as_.acceptNewGoal());
 85     ROS_INFO_STREAM("[" << name_ << "] New goal received and accepted.");
 86   }
 87 }
 88 
 89 void AutoDockingROS::preemptCb()
 90 {
 91   //ROS_DEBUG_STREAM("[" << name_ << "] Preempt requested.");
 92   dock_.disable();
 93   if (as_.isNewGoalAvailable()) {
 94     result_.text = "Preempted: New goal received.";
 95     as_.setPreempted( result_, result_.text );
 96     ROS_INFO_STREAM("[" << name_ << "] " << result_.text );
 97   } else {
 98     result_.text = "Cancelled: Cancel requested.";
 99     as_.setPreempted( result_, result_.text );
100     ROS_INFO_STREAM("[" << name_ << "] " << result_.text );
101     dock_.disable();
102   }
103 }
104 
105 void AutoDockingROS::syncCb(const nav_msgs::OdometryConstPtr& odom,
106                             const kobuki_msgs::SensorStateConstPtr& core,
107                             const kobuki_msgs::DockInfraRedConstPtr& ir)
108 {
109   //process and run
110   if(self->dock_.isEnabled()) {
111     //conversions
112     KDL::Rotation rot;
113     tf::quaternionMsgToKDL( odom->pose.pose.orientation, rot );
114 
115     double r, p, y;
116     rot.GetRPY(r, p, y);
117 
118     ecl::LegacyPose2D<double> pose;
119     pose.x(odom->pose.pose.position.x);
120     pose.y(odom->pose.pose.position.y);
121     pose.heading(y);
122 
123     //update
124     self->dock_.update(ir->data, core->bumper, core->charger, pose);
125 
126     //publish debug stream
127     std_msgs::StringPtr debug_log(new std_msgs::String);
128     debug_log->data = self->dock_.getDebugStream();
129     debug_jabber_.publish(debug_log);
130 
131     //publish command velocity
132     if (self->dock_.canRun()) {
133       geometry_msgs::TwistPtr cmd_vel(new geometry_msgs::Twist);
134       cmd_vel->linear.x = self->dock_.getVX();
135       cmd_vel->angular.z = self->dock_.getWZ();
136       velocity_commander_.publish(cmd_vel);
137     }
138   }
139 
140   //action server execution
141   if( as_.isActive() ) {
142     if ( dock_.getState() == RobotDockingState::DONE ) {
143       result_.text = "Arrived on docking station successfully.";
144       as_.setSucceeded(result_);
145       ROS_INFO_STREAM( "[" << name_ << "]: Arrived on docking station successfully.");
146       ROS_DEBUG_STREAM( "[" << name_ << "]: Result sent.");
147       dock_.disable();
148     } else if ( !dock_.isEnabled() ) { //Action Server is activated, but DockDrive is not enabled, or disabled unexpectedly
149       ROS_ERROR_STREAM("[" << name_ << "] Unintended Case: ActionService is active, but DockDrive is not enabled..");
150       result_.text = "Aborted: dock_drive is disabled unexpectedly.";
151       as_.setAborted( result_, "Aborted: dock_drive is disabled unexpectedly." );
152       ROS_INFO_STREAM("[" << name_ << "] Goal aborted.");
153       dock_.disable();
154     } else {
155       feedback_.state = dock_.getStateStr();
156       feedback_.text = dock_.getDebugStr();
157       as_.publishFeedback(feedback_);
158       ROS_DEBUG_STREAM( "[" << name_ << "]: Feedback sent.");
159     }
160   }
161   return;
162 }
163 
164 void AutoDockingROS::debugCb(const std_msgs::StringConstPtr& msg)
165 {
166   dock_.modeShift(msg->data);
167 }
168 
169 
170 } //namespace kobuki
View Code
 1 /**
 2  * @file /auto_docking/include/auto_docking/auto_docking_ros.hpp
 3  *
 4  * @brief File comment
 5  *
 6  * File comment
 7  *
 8  **/
 9 /*****************************************************************************
10 ** Ifdefs
11 *****************************************************************************/
12 
13 #ifndef AUTO_DOCKING_ROS_HPP_
14 #define AUTO_DOCKING_ROS_HPP_
15 
16 /*****************************************************************************
17 ** Includes
18 *****************************************************************************/
19 
20 #include <ros/ros.h>
21 #include <actionlib/server/simple_action_server.h>
22 #include <kobuki_msgs/AutoDockingAction.h>
23 
24 #include <message_filters/subscriber.h>
25 #include <message_filters/time_synchronizer.h>
26 #include <message_filters/synchronizer.h>
27 #include <message_filters/sync_policies/approximate_time.h>
28 
29 #include <std_msgs/String.h>
30 #include <nav_msgs/Odometry.h>
31 #include <kobuki_msgs/SensorState.h>
32 #include <kobuki_msgs/DockInfraRed.h>
33 
34 #include <sstream>
35 #include <vector>
36 #include <ecl/geometry/legacy_pose2d.hpp>
37 #include <ecl/linear_algebra.hpp>
38 #include <kdl/frames.hpp>
39 #include <kdl_conversions/kdl_msg.h>
40 
41 #include <kobuki_dock_drive/dock_drive.hpp>
42 
43 namespace kobuki
44 {
45 
46 typedef message_filters::sync_policies::ApproximateTime<
47   nav_msgs::Odometry,
48   kobuki_msgs::SensorState,
49   kobuki_msgs::DockInfraRed
50 > SyncPolicy;
51 
52 class AutoDockingROS
53 {
54 public:
55 //  AutoDockingROS();
56   AutoDockingROS(std::string name);
57 //  AutoDockingROS(ros::NodeHandle& nh);
58 //  AutoDockingROS(ros::NodeHandle& nh, std::string name);
59   ~AutoDockingROS();
60 
61   bool init(ros::NodeHandle& nh);
62   void spin();
63 
64 private:
65   AutoDockingROS* self;
66   DockDrive dock_;
67 
68   std::string name_;
69   bool shutdown_requested_;
70 
71   ros::NodeHandle nh_;
72   actionlib::SimpleActionServer<kobuki_msgs::AutoDockingAction> as_;
73 
74   kobuki_msgs::AutoDockingGoal goal_;
75   kobuki_msgs::AutoDockingFeedback feedback_;
76   kobuki_msgs::AutoDockingResult result_;
77 
78   ros::Subscriber debug_;
79   ros::Publisher velocity_commander_, motor_power_enabler_, debug_jabber_;
80 
81   boost::shared_ptr<message_filters::Subscriber<nav_msgs::Odometry> > odom_sub_;
82   boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::DockInfraRed> > ir_sub_;
83   boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::SensorState> > core_sub_;
84   boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
85 
86   void goalCb();
87   void preemptCb();
88 
89   void syncCb(const nav_msgs::OdometryConstPtr& odom,
90               const kobuki_msgs::SensorStateConstPtr& core,
91               const kobuki_msgs::DockInfraRedConstPtr& ir);
92   void debugCb(const std_msgs::StringConstPtr& msg);
93 };
94 
95 } //namespace kobuki
96 #endif /* AUTO_DOCKING_ROS_HPP_ */
View Code

 

posted @ 2021-06-08 11:23  baronluu  阅读(1568)  评论(0)  编辑  收藏  举报