catchrobo2022 / Mbed 2 deprecated catchrobo2022_mbed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

catchrobo_sim/obstacle_avoidance.h@0:803105042c95, 2022-09-26 (annotated)

Committer:
shimizuta
Date:
Mon Sep 26 13:45:05 2022 +0000
Revision:
0:803105042c95
catchrobo2022 finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 0:803105042c95 1 #pragma once
shimizuta 0:803105042c95 2
shimizuta 0:803105042c95 3 #include "catchrobo_sim/transform.h"
shimizuta 0:803105042c95 4 #include "catchrobo_sim/motor_manager.h"
shimizuta 0:803105042c95 5 #include "motor_driver_bridge/motor_driver_struct.h"
shimizuta 0:803105042c95 6 #include "define.h"
shimizuta 0:803105042c95 7
shimizuta 0:803105042c95 8 /////跳ねる実装だったので、没になった。要検証
shimizuta 0:803105042c95 9
shimizuta 0:803105042c95 10 struct Obstacle
shimizuta 0:803105042c95 11 {
shimizuta 0:803105042c95 12 float edge[3][2]; // x(min, max),y(min, rad),z(min, max) の順. edge[0][1]でx軸のmaxが手に入る
shimizuta 0:803105042c95 13 };
shimizuta 0:803105042c95 14
shimizuta 0:803105042c95 15 class ObstacleAvoidance
shimizuta 0:803105042c95 16 {
shimizuta 0:803105042c95 17 private:
shimizuta 0:803105042c95 18 Obstacle obstacles_rad[1];
shimizuta 0:803105042c95 19 void avoidSurface(MotorManager (&motor_manager_)[N_MOTORS])
shimizuta 0:803105042c95 20 {
shimizuta 0:803105042c95 21
shimizuta 0:803105042c95 22 int obstacle_num = 0;
shimizuta 0:803105042c95 23 bool aside_obstacle[] = {false, false, false}; //// 現在値のi軸成分がが障害物のi座標に入っていればtrue. □ * の位置関係にいるときは、y軸がtrueとなる.
shimizuta 0:803105042c95 24 for (size_t i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 25 {
shimizuta 0:803105042c95 26 StateStruct state;
shimizuta 0:803105042c95 27 motor_manager_[i].getState(state);
shimizuta 0:803105042c95 28 float position = state.position;
shimizuta 0:803105042c95 29 float obstacle_min = obstacles_rad[obstacle_num].edge[i][0];
shimizuta 0:803105042c95 30 float obstacle_max = obstacles_rad[obstacle_num].edge[i][1];
shimizuta 0:803105042c95 31 if (obstacle_min < position && position < obstacle_max)
shimizuta 0:803105042c95 32 {
shimizuta 0:803105042c95 33 aside_obstacle[i] = true;
shimizuta 0:803105042c95 34 }
shimizuta 0:803105042c95 35
shimizuta 0:803105042c95 36 //// obstacle　infoの初期化
shimizuta 0:803105042c95 37 motor_manager_[i].setObstacleInfo(false, 0, 0); //// 後ろ２つの引数はdummy.使われない
shimizuta 0:803105042c95 38 }
shimizuta 0:803105042c95 39
shimizuta 0:803105042c95 40 // 障害物に当たりそうならまずyを動かす
shimizuta 0:803105042c95 41 if (aside_obstacle[2] && aside_obstacle[0])
shimizuta 0:803105042c95 42 {
shimizuta 0:803105042c95 43 //// 障害物のy軸方向にいるとき,
shimizuta 0:803105042c95 44 //// y軸の制限を変更
shimizuta 0:803105042c95 45 int change_axis = 1;
shimizuta 0:803105042c95 46 changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
shimizuta 0:803105042c95 47 // ROS_INFO_STREAM("y obstacle");
shimizuta 0:803105042c95 48 }
shimizuta 0:803105042c95 49 else if (aside_obstacle[0] && aside_obstacle[1])
shimizuta 0:803105042c95 50 {
shimizuta 0:803105042c95 51 ////障害物のz軸方向(上空or下)にいるとき.
shimizuta 0:803105042c95 52 //// z軸の制限を変更
shimizuta 0:803105042c95 53 int change_axis = 2;
shimizuta 0:803105042c95 54 changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
shimizuta 0:803105042c95 55 // ROS_INFO_STREAM("z obstacle");
shimizuta 0:803105042c95 56 }
shimizuta 0:803105042c95 57 else if (aside_obstacle[1] && aside_obstacle[2])
shimizuta 0:803105042c95 58 {
shimizuta 0:803105042c95 59 int change_axis = 0;
shimizuta 0:803105042c95 60 changePositionLimitAsideObstacle(change_axis, obstacle_num, motor_manager_);
shimizuta 0:803105042c95 61 // ROS_INFO_STREAM("x obstacle");
shimizuta 0:803105042c95 62 }
shimizuta 0:803105042c95 63 }
shimizuta 0:803105042c95 64
shimizuta 0:803105042c95 65 void changePositionLimitAsideObstacle(int change_axis, int obstacle_num, MotorManager (&motor_manager_)[N_MOTORS])
shimizuta 0:803105042c95 66 {
shimizuta 0:803105042c95 67
shimizuta 0:803105042c95 68 StateStruct state;
shimizuta 0:803105042c95 69 motor_manager_[change_axis].getState(state);
shimizuta 0:803105042c95 70
shimizuta 0:803105042c95 71 float position = state.position;
shimizuta 0:803105042c95 72 float obstacle_min = obstacles_rad[obstacle_num].edge[change_axis][0];
shimizuta 0:803105042c95 73 float obstacle_max = obstacles_rad[obstacle_num].edge[change_axis][1];
shimizuta 0:803105042c95 74 bool near_min = fabs(position - obstacle_min) < fabs(position - obstacle_max);
shimizuta 0:803105042c95 75 if (near_min)
shimizuta 0:803105042c95 76 {
shimizuta 0:803105042c95 77 motor_manager_[change_axis].setObstacleInfo(true, false, obstacle_min);
shimizuta 0:803105042c95 78 }
shimizuta 0:803105042c95 79 else
shimizuta 0:803105042c95 80 {
shimizuta 0:803105042c95 81 motor_manager_[change_axis].setObstacleInfo(true, true, obstacle_max);
shimizuta 0:803105042c95 82 }
shimizuta 0:803105042c95 83 }
shimizuta 0:803105042c95 84
shimizuta 0:803105042c95 85 void noCollisionPlan(MotorManager (&motor_manager_)[N_MOTORS])
shimizuta 0:803105042c95 86 {
shimizuta 0:803105042c95 87 bool temp_mode = false;
shimizuta 0:803105042c95 88 int obstacle_num = 0;
shimizuta 0:803105042c95 89
shimizuta 0:803105042c95 90 catchrobo_msgs::MyRosCmd ros_cmd[N_MOTORS];
shimizuta 0:803105042c95 91 StateStruct state[N_MOTORS];
shimizuta 0:803105042c95 92
shimizuta 0:803105042c95 93 for (size_t i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 94 {
shimizuta 0:803105042c95 95 motor_manager_[i].getState(state[i]);
shimizuta 0:803105042c95 96 motor_manager_[i].getRosCmd(ros_cmd[i]);
shimizuta 0:803105042c95 97 }
shimizuta 0:803105042c95 98
shimizuta 0:803105042c95 99 float z_top = obstacles_rad[obstacle_num].edge[2][1];
shimizuta 0:803105042c95 100 if (ros_cmd[2].position > z_top)
shimizuta 0:803105042c95 101 {
shimizuta 0:803105042c95 102 //// 下から上に向かうときは何も変えなくて大丈夫
shimizuta 0:803105042c95 103 temp_mode = false;
shimizuta 0:803105042c95 104 }
shimizuta 0:803105042c95 105 else
shimizuta 0:803105042c95 106 {
shimizuta 0:803105042c95 107 //// 下が目標値のときは壁より下がりすぎないよう注意
shimizuta 0:803105042c95 108 float now_x = state[0].position;
shimizuta 0:803105042c95 109 float now_y = state[1].position;
shimizuta 0:803105042c95 110 //// 壁をまたぐときは、一時的に目標値変えモード
shimizuta 0:803105042c95 111 temp_mode = isOverHill(ros_cmd[0].position, ros_cmd[1].position, now_x, now_y, obstacles_rad[obstacle_num]);
shimizuta 0:803105042c95 112 }
shimizuta 0:803105042c95 113 ////目標値変換(トグル的な関数なので、何回呼び出しても、temp_modeが変化したタイミングでしか機能しない。
shimizuta 0:803105042c95 114 //// また、temp_mode = falseなら第二変数は機能しない)
shimizuta 0:803105042c95 115 motor_manager_[2].changeTarget(temp_mode, z_top + 0.1); //// 壁ちょい上を目指す
shimizuta 0:803105042c95 116 }
shimizuta 0:803105042c95 117
shimizuta 0:803105042c95 118 bool isOverHill(float target_x, float target_y, float now_x, float now_y, Obstacle &obstacle)
shimizuta 0:803105042c95 119 {
shimizuta 0:803105042c95 120 int target_grid = positionGrid(obstacle, target_x, target_y);
shimizuta 0:803105042c95 121 int now_grid = positionGrid(obstacle, now_x, now_y);
shimizuta 0:803105042c95 122
shimizuta 0:803105042c95 123 // ROS_INFO_STREAM("target : " << target_grid << " now: " << now_grid);
shimizuta 0:803105042c95 124 ////丘をまたぐときreturn true // でなければreturn false
shimizuta 0:803105042c95 125
shimizuta 0:803105042c95 126 switch (now_grid)
shimizuta 0:803105042c95 127 {
shimizuta 0:803105042c95 128 case 0:
shimizuta 0:803105042c95 129 if (target_grid == 11 || target_grid == 12 || target_grid == 21 || target_grid == 22)
shimizuta 0:803105042c95 130 return true;
shimizuta 0:803105042c95 131 break;
shimizuta 0:803105042c95 132 case 1:
shimizuta 0:803105042c95 133 if (10 < target_grid && target_grid < 22)
shimizuta 0:803105042c95 134 return true;
shimizuta 0:803105042c95 135 break;
shimizuta 0:803105042c95 136 case 2:
shimizuta 0:803105042c95 137 if (target_grid == 10 || target_grid == 11 || target_grid == 20 || target_grid == 21)
shimizuta 0:803105042c95 138 return true;
shimizuta 0:803105042c95 139 break;
shimizuta 0:803105042c95 140 case 10:
shimizuta 0:803105042c95 141 if (target_grid == 1 || target_grid == 2 || target_grid == 11 || target_grid == 12 || target_grid == 21 || target_grid == 22)
shimizuta 0:803105042c95 142 return true;
shimizuta 0:803105042c95 143 break;
shimizuta 0:803105042c95 144 case 11:
shimizuta 0:803105042c95 145 return true;
shimizuta 0:803105042c95 146 break;
shimizuta 0:803105042c95 147 case 12:
shimizuta 0:803105042c95 148 if (target_grid == 0 || target_grid == 1 || target_grid == 10 || target_grid == 11 || target_grid == 20 || target_grid == 21)
shimizuta 0:803105042c95 149 return true;
shimizuta 0:803105042c95 150 break;
shimizuta 0:803105042c95 151 case 20:
shimizuta 0:803105042c95 152 if (target_grid == 1 || target_grid == 2 || target_grid == 11 || target_grid == 12)
shimizuta 0:803105042c95 153 return true;
shimizuta 0:803105042c95 154 break;
shimizuta 0:803105042c95 155 case 21:
shimizuta 0:803105042c95 156 if (0 < target_grid && target_grid < 12)
shimizuta 0:803105042c95 157 return true;
shimizuta 0:803105042c95 158 break;
shimizuta 0:803105042c95 159 case 22:
shimizuta 0:803105042c95 160 if (target_grid == 0 || target_grid == 1 || target_grid == 10 || target_grid == 11)
shimizuta 0:803105042c95 161 return true;
shimizuta 0:803105042c95 162 break;
shimizuta 0:803105042c95 163 default:
shimizuta 0:803105042c95 164 return false;
shimizuta 0:803105042c95 165 break;
shimizuta 0:803105042c95 166 }
shimizuta 0:803105042c95 167 return false;
shimizuta 0:803105042c95 168 }
shimizuta 0:803105042c95 169
shimizuta 0:803105042c95 170 //// 9マスに分解 //returnは10の位がx, 1の位がy
shimizuta 0:803105042c95 171 int positionGrid(const Obstacle &obstacle, float pos_x, float pos_y)
shimizuta 0:803105042c95 172 {
shimizuta 0:803105042c95 173 // [0,2]|[1,2]|[2,2]
shimizuta 0:803105042c95 174 // [0,1]|[1,1]|[2,1]
shimizuta 0:803105042c95 175 // [0,0]|[1,0]|[2,0]
shimizuta 0:803105042c95 176
shimizuta 0:803105042c95 177 float ob_x_min = obstacle.edge[0][0];
shimizuta 0:803105042c95 178 float ob_x_max = obstacle.edge[0][1];
shimizuta 0:803105042c95 179 float ob_y_min = obstacle.edge[1][0];
shimizuta 0:803105042c95 180 float ob_y_max = obstacle.edge[1][1];
shimizuta 0:803105042c95 181 int x_num = 0;
shimizuta 0:803105042c95 182 int y_num = 0;
shimizuta 0:803105042c95 183 if (pos_x < ob_x_min)
shimizuta 0:803105042c95 184 x_num = 0;
shimizuta 0:803105042c95 185 else if (ob_x_min <= pos_x && pos_x < ob_x_max)
shimizuta 0:803105042c95 186 x_num = 1;
shimizuta 0:803105042c95 187 else
shimizuta 0:803105042c95 188 x_num = 2;
shimizuta 0:803105042c95 189
shimizuta 0:803105042c95 190 if (pos_y < ob_y_min)
shimizuta 0:803105042c95 191 y_num = 0;
shimizuta 0:803105042c95 192 else if (ob_y_min <= pos_y && pos_y < ob_y_max)
shimizuta 0:803105042c95 193 y_num = 1;
shimizuta 0:803105042c95 194 else
shimizuta 0:803105042c95 195 y_num = 2;
shimizuta 0:803105042c95 196
shimizuta 0:803105042c95 197 return x_num * 10 + y_num;
shimizuta 0:803105042c95 198 }
shimizuta 0:803105042c95 199
shimizuta 0:803105042c95 200 public:
shimizuta 0:803105042c95 201 ObstacleAvoidance(/* args */);
shimizuta 0:803105042c95 202 ~ObstacleAvoidance();
shimizuta 0:803105042c95 203
shimizuta 0:803105042c95 204 void changePositionLimit(MotorManager (&motor_manager_)[N_MOTORS])
shimizuta 0:803105042c95 205 {
shimizuta 0:803105042c95 206
shimizuta 0:803105042c95 207 // avoidSurface(motor_manager_);
shimizuta 0:803105042c95 208 // noCollisionPlan(motor_manager_);
shimizuta 0:803105042c95 209 }
shimizuta 0:803105042c95 210 };
shimizuta 0:803105042c95 211
shimizuta 0:803105042c95 212 ObstacleAvoidance::ObstacleAvoidance(/* args */)
shimizuta 0:803105042c95 213 {
shimizuta 0:803105042c95 214 //// x軸上の配線との干渉を防ぐ
shimizuta 0:803105042c95 215 float edge_m[3][2] = {{-1000, 1.1}, {-0.15, 0.15}, {-1000, 0.12}};
shimizuta 0:803105042c95 216
shimizuta 0:803105042c95 217 // radに変換
shimizuta 0:803105042c95 218 for (int axis = 0; axis < 3; axis++)
shimizuta 0:803105042c95 219 {
shimizuta 0:803105042c95 220 for (int i = 0; i < 2; i++)
shimizuta 0:803105042c95 221 {
shimizuta 0:803105042c95 222 obstacles_rad[0].edge[axis][i] = Transform::m2rad(axis, edge_m[axis][i]);
shimizuta 0:803105042c95 223 }
shimizuta 0:803105042c95 224 }
shimizuta 0:803105042c95 225 }
shimizuta 0:803105042c95 226
shimizuta 0:803105042c95 227 ObstacleAvoidance::~ObstacleAvoidance()
shimizuta 0:803105042c95 228 {
shimizuta 0:803105042c95 229 }