לחבר בקר מנוע משלכם ל-ROS 2: כותבים hardware_interface
מדריך מעשי · תוכנה לרובוטיקה · זמן קריאה 14 דקות
כל מדריכי ROS 2 נעצרים בדיוק בנקודה שבה הרובוט מתחיל: הסימולציה עובדת, RViz נראה מצוין — ואז אתם מול בקר המנוע שלכם על השולחן, בלי שום דרך ברורה שבה /cmd_vel אמור להגיע אליו. החלק החסר הוא hardware interface של ros2_control — כ-200 שורות C++ שהופכות כרטיס מחובר בחיבור טורי לצירים שכל בקר סטנדרטי של ROS יודע להניע. כתבנו ממשקים כאלה לבקרי ה-BLDC שלנו ולמכונות של לקוחות; זו גרסת המאמר: הארכיטקטורה, פרוטוקול התקשורת, מימוש מלא עם הערות — ופרטי התזמון, היחידות וה-watchdog שמכריעים אם זה באמת יעבוד.
זה גם ההמשך הטבעי של מאמר ה-FOC שלנו: שם בנינו את הקושחה בתוך הדרייבר. כאן אנחנו שמים את הדרייבר הזה על רובוט.
1 · איפה ros2_control יושב — ולמה זה לא Topics
הדבר הראשון שצריך לשכוח: בתוך ros2_control, שום דבר אינו Topic. המסגרת מריצה לולאת זמן-אמת אחת, וכל מה שבתוכה מתקשר דרך זיכרון משותף — משתני double גולמיים, לא הודעות DDS.
שלוש שכבות חשובות:
- בקרים (
diff_drive_controller, joint_trajectory_controller, joint_state_broadcasterועוד) הם פלאגינים שצורכים ממשקי מצב ומייצרים ממשקי פקודה. אתם מגדירים אותם בקובץ קונפיגורציה; כמעט אף פעם לא כותבים אותם. - ה-controller_manager מחזיק את הלולאה. בקצב קבוע (
update_rate) הוא קורא ל-read() על החומרה שלכם, ל-update() על כל בקר פעיל, ואז ל-write() על החומרה. - רכיב החומרה — הקוד שלכם — מממש את
read() ו-write() וחושף את הצירים. מחלקת פלאגין אחת, שנטענת מתוך ה-URDF.
החוזה צר ומדויק: הבקרים רואים פקודות velocity ומצבי position/velocity ביחידות SI (רדיאנים, רדיאנים לשנייה). התפקיד שלכם הוא להיות המתרגם האמין בין ה-double-ים האלה לבין מה שהאלקטרוניקה שלכם מדברת.
2 · החוזה: SystemInterface ומחזור החיים שלו
רכיב חומרה יורש מ-hardware_interface::SystemInterface ומממש קומץ פונקציות מחזור-חיים ושתי פונקציות במסלול החם:
| פונקציה | מתי נקראת | מה עושים בה |
| --- | --- | --- |
| on_init | פעם אחת, בטעינה | קוראים פרמטרים מה-URDF, מקצים את וקטורי המצב והפקודה |
| on_configure | בשלב הקונפיגורציה | פותחים את הפורט הטורי / סוקט ה-CAN |
| on_activate | לפני שהלולאה מתחילה | מפעילים את הדרייבר, מאפסים אנקודרים, מנקים פקודות ישנות |
| read() | כל מחזור | שואלים את הכרטיס, ממירים ספירות ליחידות SI, ממלאים את וקטורי המצב |
| write() | כל מחזור | ממירים מ-SI לספירות, אורזים בפריים, שולחים |
| on_deactivate | בעצירה | שולחים פקודת מהירות אפס, מכבים את הדרייבר |
ערכי ההחזרה הם חלק מהחוזה: return_type::ERROR מ-read()/write() אומר ל-controller_manager שהחומרה אבדה — והוא עוצר את הבקרים במקום להמשיך לאינטגרל על זבל. משתמשים בו לכשל מתמשך, לא לפריים בודד שאבד (עוד על זה בטבלת התקלות).
3 · הגדרת החומרה ב-URDF
תגית <ros2_control> היא המקום שבו מוצהרים הפלאגין, הפרמטרים שלו והממשקים החשופים:
<ros2_control name="DiffBotSystem" type="system">
<hardware>
<plugin>segev_drivers/DiffBotSystem</plugin>
<param name="device">/dev/ttyUSB0</param>
<param name="baud_rate">115200</param>
<param name="counts_per_rev">4096</param>
<param name="cmd_watchdog_ms">100</param>
</hardware>
<joint name="left_wheel_joint">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
כל מה שכאן מגיע לקוד שלכם דרך info_: הצירים עם הממשקים שלהם, ו-info_.hardware_parameters כמחרוזות. זה מכוון — אותו פלאגין מקומפל מניע מכונה אחרת על ידי עריכת XML, לא C++.
4 · קודם אלקטרוניקה: מתכננים את פרוטוקול התקשורת לפני ה-C++
זה השלב שמדריכי התוכנה מדלגים עליו — ובדיוק שם רובוטים נכשלים בפועל. הכללים שלנו לקו שבין מחשב הרובוט לדרייבר:
פריימים בינאריים עם CRC, לא printf. פריים קבוע — בית סנכרון, אורך, פקודה, נתונים, CRC-16 — מפוענח במספר דטרמיניסטי של מחזורים על המיקרו-בקר ושורד כבל רועש. שני פריימים מספיקים לרובוט דיפרנציאלי:
Host → Board SET_VEL : [0xAA][len][0x01][int32 vL][int32 vR][crc16] (counts/s)
Board → Host STATE : [0xAA][len][0x81][int32 pL][int32 pR]
[int32 vL][int32 vR][u8 status][crc16] (counts, counts/s)
הכרטיס מדווח את המהירות שלו בעצמו. גזירה של ספירות אנקודר בצד המחשב ב-100Hz נותנת רעש, לא מהירות. הדרייבר ממילא מריץ לולאת מהירות (מכווננת כמו שתיארנו כאן) עם שערוך נקי בקצבי קילו-הרץ — שולחים את השערוך הזה בפריים ה-STATE.
ה-watchdog לפקודות חי בקושחה. אם לא הגיעה פקודת SET_VEL תקינה בתוך כ-100 מילי-שניות, הכרטיס עוצר את המנועים בעצמו. ה-cmd_vel_timeout של diff_drive_controller עוזר רק כל עוד התוכנה חיה; כבל USB שנשלף הוא בדיוק המקרה שהוא לא מכסה. הקו חייב להיכשל למצב בטוח.
עושים את חשבון ה-Baud לפני שבוחרים קצב לולאה. ב-115200 baud, בית אחד ≈ 87 מיקרו-שניות. פריים STATE של 21 בתים ועוד SET_VEL של 13 בתים הם כ-3 מילי-שניות של זמן קו בכל מחזור — נוח ב-100Hz, חסר סיכוי ב-1kHz. זו הסיבה האמיתית שלולאת ה-ros2_control של רובוטים קטנים רצה ב-50–100Hz, ולולאות הקילו-הרץ נשארות בתוך הדרייבר, במקום שבו הן צריכות להיות. צריכים הרבה צירים או סנכרון ברמת מיקרו-שניות? זה שטח של EtherCAT — מאמר אחר, אותה משמעת הנדסית.
והמרות היחידות, שנכתבות פעם אחת ונבדקות — כי כל שגיאת סימן ברובוט מובילה בסוף לכאן:
5 · המימוש — עם הערות, ומקוצר
המחלקה המלאה, בלי לוגים ו-include-ים, לטובת הקריאוּת. המבנה עוקב אחרי הדוגמאות הרשמיות (ros2_control_demos, דוגמה 2) ומכוון להפצת ROS 2 עדכנית — ה-API עדיין משתנה בין גרסאות, אז השוו מול הדוגמה של ההפצה שלכם:
class DiffBotSystem : public hardware_interface::SystemInterface {
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override {
if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
return CallbackReturn::ERROR;
// URDF params arrive as strings — parse once, here.
device_ = info_.hardware_parameters["device"];
baud_ = std::stoi(info_.hardware_parameters["baud_rate"]);
cpr_ = std::stod(info_.hardware_parameters["counts_per_rev"]);
pos_.assign(info_.joints.size(), 0.0);
vel_.assign(info_.joints.size(), 0.0);
cmd_.assign(info_.joints.size(), 0.0);
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces() override {
std::vector<hardware_interface::StateInterface> s;
for (size_t i = 0; i < info_.joints.size(); i++) {
s.emplace_back(info_.joints[i].name, "position", &pos_[i]);
s.emplace_back(info_.joints[i].name, "velocity", &vel_[i]);
}
return s; // controllers will read these doubles directly
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override {
std::vector<hardware_interface::CommandInterface> c;
for (size_t i = 0; i < info_.joints.size(); i++)
c.emplace_back(info_.joints[i].name, "velocity", &cmd_[i]);
return c;
}
CallbackReturn on_activate(const rclcpp_lifecycle::State &) override {
if (!port_.open(device_, baud_)) return CallbackReturn::ERROR;
port_.send_enable(); // energize the drive
port_.request_zero(); // encoder zero = pose zero
std::fill(cmd_.begin(), cmd_.end(), 0.0); // never replay a stale command
return CallbackReturn::SUCCESS;
}
return_type read(const rclcpp::Time &, const rclcpp::Duration &) override {
StateFrame f;
if (!port_.poll_state(f, /*timeout_ms=*/5)) {
if (++miss_ > 5) return return_type::ERROR; // persistent loss → stop
return return_type::OK; // single miss → hold last
}
miss_ = 0;
for (size_t i = 0; i < pos_.size(); i++) {
pos_[i] = 2.0 * M_PI * f.pos_counts[i] / cpr_; // counts → rad
vel_[i] = 2.0 * M_PI * f.vel_counts[i] / cpr_; // board's estimate
}
return return_type::OK;
}
return_type write(const rclcpp::Time &, const rclcpp::Duration &) override {
int32_t v[2];
for (size_t i = 0; i < cmd_.size(); i++) {
double clamped = std::clamp(cmd_[i], -max_w_, max_w_); // respect limits
v[i] = static_cast<int32_t>(clamped * cpr_ / (2.0 * M_PI));
}
port_.send_velocity(v[0], v[1]); // one frame, CRC'd, feeds the watchdog
return return_type::OK;
}
// on_configure / on_deactivate / on_cleanup: open/quiet/close — symmetric.
};
PLUGINLIB_EXPORT_CLASS(segev_drivers::DiffBotSystem,
hardware_interface::SystemInterface)
שלושה פרטים ששווה לשים לב אליהם, כי הם ההבדל בין קוד הדגמה לקוד שטח: איפוס הפקודות הישנות ב-on_activate (הגורם מספר 1 לרובוטים שמזנקים בהפעלה), מונה ההחמצות ב-read() (פריים פגום אחד אסור שיעצור את הבקרים), וההגבלה לפני ההמרה ב-write() (ה-URDF הבטיח רדיאנים לשנייה; לכרטיס יש דעה משלו על המקסימום).
6 · הבקרים: קונפיגורציה, לא קוד
ברגע שממשק החומרה חושף צירים נקיים, כל השאר הוא YAML:
controller_manager:
ros__parameters:
update_rate: 100 # Hz — must clear the serial budget from §4
diff_drive_controller:
type: diff_drive_controller/DiffDriveController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
diff_drive_controller:
ros__parameters:
left_wheel_names: ["left_wheel_joint"]
right_wheel_names: ["right_wheel_joint"]
wheel_separation: 0.31 # measure, don't trust the CAD
wheel_radius: 0.0525
cmd_vel_timeout: 0.5
publish_rate: 50.0
מריצים, ואז בודקים לפני שנוסעים:
ros2 run controller_manager spawner joint_state_broadcaster
ros2 run controller_manager spawner diff_drive_controller
ros2 control list_hardware_interfaces # both wheels: claimed velocity cmd,
ros2 topic echo /joint_states # positions moving when you spin by hand
ros2 run teleop_twist_keyboard teleop_twist_keyboard # now drive
סיבוב גלגל ביד תוך צפייה ב-/joint_states הוא הבדיקה בעלת הערך הגבוה ביותר בכל התהליך: היא מאמתת את הפורט, את הפרוטוקול, את ה-CRC, את ההמרות ואת הסימנים — לפני שמנוע כלשהו מקבל מתח.
7 · התקלות שגוזלות את השבוע הראשון
| תסמין | כמעט תמיד | תיקון |
| --- | --- | --- |
| חלק ב-10Hz, נכשל ב-100Hz | טיימר ההשהיה של USB-Serial (FTDI: 16ms כברירת מחדל) | setserial /dev/ttyUSB0 low_latency, או CDC-ACM/CAN |
| הרובוט מזנק ברגע שהבקרים עולים | פקודה ישנה או אנקודרים לא מאופסים בהפעלה | לאפס את שניהם ב-on_activate — סעיף 5 |
| גלגל אחד נוסע הפוך | מוסכמת סימנים: מנוע מותקן במראה | פרמטר כיוון לכל ציר; מתקנים במקום אחד |
| /joint_states קפוא אבל הרובוט נוסע | read() מחזיר OK בלי למלא את הווקטורים | למלא pos_/vel_ בכל מחזור, גם בהחמצה |
| גרף המהירות נראה כמו רעש | גזירת ספירות בצד המחשב | לדווח את שערוך המהירות של הדרייבר עצמו (סעיף 4) |
| כבל נשלף ← הרובוט ממשיך לנסוע | אין watchdog בקושחה | watchdog של 100ms על הכרטיס, לא רק ב-ROS |
| הבקרים נעצרים באקראי | return ERROR על פריים בודד שאבד | מונה החמצות; ERROR רק על אובדן מתמשך |
| מושלם על מחשב המעבדה, רועד על מחשב הרובוט | מושל צריכת חשמל + אין עדיפות זמן-אמת | מושל performance, SCHED_FIFO ללולאה, נעילת זיכרון |
| עובד עד שהזרוע נעה מהר | התנגשות בין update_rate לתקציב הטורי | להוריד קצב, לאחד לטרנזקציה אחת — או EtherCAT |
8 · לאן זה גדל
קו טורי ו-200 שורות C++ סוחבים כמות מפתיעה של רובוט — אבל לארכיטקטורה הזו יש תקרה: הרבה צירים, סנכרון ברמת מיקרו-שניות, תנועה בדרגת בטיחות. שם רכיב החומרה מפסיק לעטוף UART ומתחיל לעטוף EtherCAT Master עם דרייברים בתקן DS402 — הטריטוריה של מערכת הבמות עם 26 הצירים שלנו, ושל מה שבאמת נדרש כדי לגרום לרובוט לנוע במדויק. המשמעת לא משתנה: יחידות מפורשות, תקציבים מפורשים, כשל למצב בטוח כבר ברמת החיווט.
מקושחת הדרייבר ועד הרובוט
אנחנו בונים את שני הצדדים של הכבל הזה — קושחת ה-FOC שבתוך הדרייבר ואת אינטגרציית ה-ROS 2 שמעליה, במכונות מאוטומציית מעבדה ועד פלטפורמות על גלגלים. אם הרובוט שלכם צריך שהמנועים שלו ידברו ROS — דברו איתנו.
מבוסס על: התיעוד הרשמי של ros2_control ("Writing a Hardware Component", control.ros.org); מאגר ros-controls/ros2_control_demos (דוגמה 2, DiffBot); תיעוד ros2_controllers (diff_drive_controller); ו-S. Chitta et al., "ros_control: A generic and simple control framework for ROS", Journal of Open Source Software, 2017. ה-API משתנה בין גרסאות ROS 2 — בדקו את הדוגמאות של ההפצה שלכם.