mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
RobotController.cpp
[詳解]
1
5
6
#include "
RobotController.hpp
"
7
8
namespace
application
{
9
10
RobotController::RobotController
(
11
double
wheel_base,
12
float
max_linear_speed,
13
float
max_angular_speed,
14
uint32_t watchdog_timeout
15
)
16
: wheel_base_(wheel_base)
17
, max_linear_speed_(max_linear_speed)
18
, max_angular_speed_(max_angular_speed)
19
, watchdog_timeout_(watchdog_timeout)
20
, control_mode_(
MODE_MANUAL
)
21
, linear_x_(0.0f)
22
, angular_z_(0.0f)
23
, l_vel_cmd_(0.0)
24
, r_vel_cmd_(0.0)
25
, last_ros2_cmd_time_(0)
26
, prev_sw_high_(false)
27
{
28
}
29
30
void
RobotController::setWheelBase
(
double
wheel_base) {
31
wheel_base_ = wheel_base;
32
}
33
34
void
RobotController::updateRos2Command
(
float
linear_x,
float
angular_z, uint32_t stamp_ms) {
35
linear_x_ = linear_x;
36
angular_z_ = angular_z;
37
last_ros2_cmd_time_ = stamp_ms;
38
}
39
40
void
RobotController::update
(
const
RcControlInput
&rc, uint32_t now_ms) {
41
// 有効なSWの立ち上がりエッジで制御モードを切り替える。
42
// ROS2モードからは必ずMANUALへ移り、RC信号喪失中は状態を維持する。
43
if
(rc.
mode_switch_valid
) {
44
const
bool
current_sw_high = rc.
mode_switch
> 0.2f;
45
if
(current_sw_high && !prev_sw_high_) {
46
control_mode_ = (control_mode_ ==
MODE_MANUAL
) ?
MODE_ROS2
:
MODE_MANUAL
;
47
}
48
prev_sw_high_ = current_sw_high;
49
}
50
51
if
(control_mode_ ==
MODE_ROS2
) {
52
if
(!checkWatchdog(now_ms)) {
53
updateRos2WheelCommands();
54
}
55
return
;
56
}
57
58
// MANUALでは3チャンネルすべてが有効な場合だけRC指令を使う。
59
if
(!rc.
left_valid
|| !rc.
mode_switch_valid
|| !rc.
right_valid
) {
60
r_vel_cmd_ = 0.0;
61
l_vel_cmd_ = 0.0;
62
return
;
63
}
64
updateManualCommand(rc);
65
}
66
67
void
RobotController::updateManualCommand(
const
RcControlInput
&rc) {
68
l_vel_cmd_ = rc.
left
* max_linear_speed_;
69
r_vel_cmd_ = rc.
right
* max_linear_speed_;
70
}
71
72
void
RobotController::updateRos2WheelCommands() {
73
// 差動駆動の逆運動学。入力上限を適用してから左右速度を制限する。
74
const
double
linear = clampWheelCmd(linear_x_);
75
const
double
angular_limit =
static_cast<
double
>
(max_angular_speed_);
76
const
double
angular = (angular_limit > 0.0)
77
? (angular_z_ > angular_limit ? angular_limit
78
: (angular_z_ < -angular_limit ? -angular_limit : angular_z_))
79
: 0.0;
80
const
double
r_raw = linear + (wheel_base_ / 2.0) * angular;
81
const
double
l_raw = linear - (wheel_base_ / 2.0) * angular;
82
r_vel_cmd_ = clampWheelCmd(r_raw);
83
l_vel_cmd_ = clampWheelCmd(l_raw);
84
}
85
86
double
RobotController::clampWheelCmd(
double
vel_cmd)
const
{
87
if
(!(max_linear_speed_ > 0.0f)) {
88
return
0.0;
89
}
90
const
double
limit =
static_cast<
double
>
(max_linear_speed_);
91
if
(vel_cmd > limit) {
92
return
limit;
93
}
94
if
(vel_cmd < -limit) {
95
return
-limit;
96
}
97
return
vel_cmd;
98
}
99
100
bool
RobotController::checkWatchdog(uint32_t now_ms) {
101
if
((now_ms - last_ros2_cmd_time_) > watchdog_timeout_) {
102
r_vel_cmd_ = 0.0;
103
l_vel_cmd_ = 0.0;
104
return
true
;
105
}
106
return
false
;
107
}
108
109
}
// namespace application
RobotController.hpp
プロポとROS 2の操作元を切り替え、左右車輪の目標速度を求める。通信途絶時の指令ゼロも処理する。
application::RobotController::update
void update(const RcControlInput &rc, uint32_t now_ms)
RC入力とモード切替を処理して速度指令を更新
定義
RobotController.cpp:40
application::RobotController::updateRos2Command
void updateRos2Command(float linear_x, float angular_z, uint32_t stamp_ms)
ROS2からの速度指令を更新
定義
RobotController.cpp:34
application::RobotController::MODE_MANUAL
@ MODE_MANUAL
手動制御モード(RC入力)
定義
RobotController.hpp:28
application::RobotController::MODE_ROS2
@ MODE_ROS2
ROS2制御モード
定義
RobotController.hpp:29
application::RobotController::RobotController
RobotController(double wheel_base, float max_linear_speed, float max_angular_speed, uint32_t watchdog_timeout)
コンストラクタ
定義
RobotController.cpp:10
application::RobotController::setWheelBase
void setWheelBase(double wheel_base)
車輪間距離を設定
定義
RobotController.cpp:30
application
定義
RobotController.cpp:8
application::RcControlInput
正規化済みRC入力と各チャンネルの有効状態。
定義
RobotController.hpp:14
application::RcControlInput::mode_switch_valid
bool mode_switch_valid
定義
RobotController.hpp:19
application::RcControlInput::right_valid
bool right_valid
定義
RobotController.hpp:20
application::RcControlInput::left
float left
定義
RobotController.hpp:15
application::RcControlInput::mode_switch
float mode_switch
定義
RobotController.hpp:16
application::RcControlInput::left_valid
bool left_valid
定義
RobotController.hpp:18
application::RcControlInput::right
float right
定義
RobotController.hpp:17
src
application
RobotController.cpp
構築:
1.18.0