mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
RobotController.cpp
[詳解]
1
5
6#include "RobotController.hpp"
7
8namespace application {
9
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
30void RobotController::setWheelBase(double wheel_base) {
31 wheel_base_ = wheel_base;
32}
33
34void 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
40void 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
67void 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
72void 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
86double 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
100bool 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
プロポとROS 2の操作元を切り替え、左右車輪の目標速度を求める。通信途絶時の指令ゼロも処理する。
void update(const RcControlInput &rc, uint32_t now_ms)
RC入力とモード切替を処理して速度指令を更新
void updateRos2Command(float linear_x, float angular_z, uint32_t stamp_ms)
ROS2からの速度指令を更新
@ MODE_MANUAL
手動制御モード(RC入力)
RobotController(double wheel_base, float max_linear_speed, float max_angular_speed, uint32_t watchdog_timeout)
コンストラクタ
void setWheelBase(double wheel_base)
車輪間距離を設定
正規化済みRC入力と各チャンネルの有効状態。