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-noetickobuki_dock_driveincludekobuki_dock_drivestate.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_
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()模式。
该步骤做左右旋转调整过程。充电桩在右侧，右转至左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 }
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
（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
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_ */