obstacle_avoidance.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/transform.h"
00004 #include "catchrobo_sim/motor_manager.h"
00005 #include "motor_driver_bridge/motor_driver_struct.h"
00006 #include "define.h"
00007 
00008 /////跳ねる実装だったので、没になった。要検証
00009 
00010 struct Obstacle
00011 {
00012     float edge[3][2]; // x(min, max),y(min, rad),z(min, max) の順. edge[0][1]でx軸のmaxが手に入る
00013 };
00014 
00015 class ObstacleAvoidance
00016 {
00017 private:
00018     Obstacle obstacles_rad[1];
00019     void avoidSurface(MotorManager (&motor_manager_)[N_MOTORS])
00020     {
00021 
00022         int obstacle_num = 0;
00023         bool aside_obstacle[] = {false, false, false}; //// 現在値のi軸成分がが障害物のi座標に入っていればtrue. □ * の位置関係にいるときは、y軸がtrueとなる.
00024         for (size_t i = 0; i < N_MOTORS; i++)
00025         {
00026             StateStruct state;
00027             motor_manager_[i].getState(state);
00028             float position = state.position;
00029             float obstacle_min = obstacles_rad[obstacle_num].edge[i][0];
00030             float obstacle_max = obstacles_rad[obstacle_num].edge[i][1];
00031             if (obstacle_min < position && position < obstacle_max)
00032             {
00033                 aside_obstacle[i] = true;
00034             }
00035 
00036             //// obstacle　infoの初期化
00037             motor_manager_[i].setObstacleInfo(false, 0, 0); //// 後ろ２つの引数はdummy.使われない
00038         }
00039 
00040         // 障害物に当たりそうならまずyを動かす
00041         if (aside_obstacle[2] && aside_obstacle[0])
00042         {
00043             //// 障害物のy軸方向にいるとき,
00044             //// y軸の制限を変更
00045             int change_axis = 1;
00046             changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
00047             // ROS_INFO_STREAM("y obstacle");
00048         }
00049         else if (aside_obstacle[0] && aside_obstacle[1])
00050         {
00051             ////障害物のz軸方向(上空or下)にいるとき.
00052             //// z軸の制限を変更
00053             int change_axis = 2;
00054             changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
00055             // ROS_INFO_STREAM("z obstacle");
00056         }
00057         else if (aside_obstacle[1] && aside_obstacle[2])
00058         {
00059             int change_axis = 0;
00060             changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
00061             // ROS_INFO_STREAM("x obstacle");
00062         }
00063     }
00064 
00065     void changePositionLimitAsideObstacle(int change_axis, int obstacle_num, MotorManager (&motor_manager_)[N_MOTORS])
00066     {
00067 
00068         StateStruct state;
00069         motor_manager_[change_axis].getState(state);
00070 
00071         float position = state.position;
00072         float obstacle_min = obstacles_rad[obstacle_num].edge[change_axis][0];
00073         float obstacle_max = obstacles_rad[obstacle_num].edge[change_axis][1];
00074         bool near_min = fabs(position - obstacle_min) < fabs(position - obstacle_max);
00075         if (near_min)
00076         {
00077             motor_manager_[change_axis].setObstacleInfo(true, false, obstacle_min);
00078         }
00079         else
00080         {
00081             motor_manager_[change_axis].setObstacleInfo(true, true, obstacle_max);
00082         }
00083     }
00084 
00085     void noCollisionPlan(MotorManager (&motor_manager_)[N_MOTORS])
00086     {
00087         bool temp_mode = false;
00088         int obstacle_num = 0;
00089 
00090         catchrobo_msgs::MyRosCmd ros_cmd[N_MOTORS];
00091         StateStruct state[N_MOTORS];
00092 
00093         for (size_t i = 0; i < N_MOTORS; i++)
00094         {
00095             motor_manager_[i].getState(state[i]);
00096             motor_manager_[i].getRosCmd(ros_cmd[i]);
00097         }
00098 
00099         float z_top = obstacles_rad[obstacle_num].edge[2][1];
00100         if (ros_cmd[2].position > z_top)
00101         {
00102             //// 下から上に向かうときは何も変えなくて大丈夫
00103             temp_mode = false;
00104         }
00105         else
00106         {
00107             //// 下が目標値のときは壁より下がりすぎないよう注意
00108             float now_x = state[0].position;
00109             float now_y = state[1].position;
00110             //// 壁をまたぐときは、一時的に目標値変えモード
00111             temp_mode = isOverHill(ros_cmd[0].position, ros_cmd[1].position, now_x, now_y, obstacles_rad[obstacle_num]);
00112         }
00113         ////目標値変換(トグル的な関数なので、何回呼び出しても、temp_modeが変化したタイミングでしか機能しない。
00114         ////         また、temp_mode = falseなら第二変数は機能しない)
00115         motor_manager_[2].changeTarget(temp_mode, z_top + 0.1); //// 壁ちょい上を目指す
00116     }
00117 
00118     bool isOverHill(float target_x, float target_y, float now_x, float now_y, Obstacle &obstacle)
00119     {
00120         int target_grid = positionGrid(obstacle, target_x, target_y);
00121         int now_grid = positionGrid(obstacle, now_x, now_y);
00122 
00123         // ROS_INFO_STREAM("target : " << target_grid << " now: " << now_grid);
00124         ////丘をまたぐときreturn true // でなければreturn false
00125 
00126         switch (now_grid)
00127         {
00128         case 0:
00129             if (target_grid == 11 || target_grid == 12 || target_grid == 21 || target_grid == 22)
00130                 return true;
00131             break;
00132         case 1:
00133             if (10 < target_grid && target_grid < 22)
00134                 return true;
00135             break;
00136         case 2:
00137             if (target_grid == 10 || target_grid == 11 || target_grid == 20 || target_grid == 21)
00138                 return true;
00139             break;
00140         case 10:
00141             if (target_grid == 1 || target_grid == 2 || target_grid == 11 || target_grid == 12 || target_grid == 21 || target_grid == 22)
00142                 return true;
00143             break;
00144         case 11:
00145             return true;
00146             break;
00147         case 12:
00148             if (target_grid == 0 || target_grid == 1 || target_grid == 10 || target_grid == 11 || target_grid == 20 || target_grid == 21)
00149                 return true;
00150             break;
00151         case 20:
00152             if (target_grid == 1 || target_grid == 2 || target_grid == 11 || target_grid == 12)
00153                 return true;
00154             break;
00155         case 21:
00156             if (0 < target_grid && target_grid < 12)
00157                 return true;
00158             break;
00159         case 22:
00160             if (target_grid == 0 || target_grid == 1 || target_grid == 10 || target_grid == 11)
00161                 return true;
00162             break;
00163         default:
00164             return false;
00165             break;
00166         }
00167         return false;
00168     }
00169 
00170     //// 9マスに分解 //returnは10の位がx, 1の位がy
00171     int positionGrid(const Obstacle &obstacle, float pos_x, float pos_y)
00172     {
00173         // [0,2]|[1,2]|[2,2]
00174         // [0,1]|[1,1]|[2,1]
00175         // [0,0]|[1,0]|[2,0]
00176 
00177         float ob_x_min = obstacle.edge[0][0];
00178         float ob_x_max = obstacle.edge[0][1];
00179         float ob_y_min = obstacle.edge[1][0];
00180         float ob_y_max = obstacle.edge[1][1];
00181         int x_num = 0;
00182         int y_num = 0;
00183         if (pos_x < ob_x_min)
00184             x_num = 0;
00185         else if (ob_x_min <= pos_x && pos_x < ob_x_max)
00186             x_num = 1;
00187         else
00188             x_num = 2;
00189 
00190         if (pos_y < ob_y_min)
00191             y_num = 0;
00192         else if (ob_y_min <= pos_y && pos_y < ob_y_max)
00193             y_num = 1;
00194         else
00195             y_num = 2;
00196 
00197         return x_num * 10 + y_num;
00198     }
00199 
00200 public:
00201     ObstacleAvoidance(/* args */);
00202     ~ObstacleAvoidance();
00203 
00204     void changePositionLimit(MotorManager (&motor_manager_)[N_MOTORS])
00205     {
00206 
00207         // avoidSurface(motor_manager_);
00208         // noCollisionPlan(motor_manager_);
00209     }
00210 };
00211 
00212 ObstacleAvoidance::ObstacleAvoidance(/* args */)
00213 {
00214     //// x軸上の配線との干渉を防ぐ
00215     float edge_m[3][2] = {{-1000, 1.1}, {-0.15, 0.15}, {-1000, 0.12}};
00216 
00217     // radに変換
00218     for (int axis = 0; axis < 3; axis++)
00219     {
00220         for (int i = 0; i < 2; i++)
00221         {
00222             obstacles_rad[0].edge[axis][i] = Transform::m2rad(axis, edge_m[axis][i]);
00223         }
00224     }
00225 }
00226 
00227 ObstacleAvoidance::~ObstacleAvoidance()
00228 {
00229 }