search

jsk-ros-pkg / jsk_aerial_robot

The platfrom for aerial robot (e.g. general multirotor, hydrus, di, dragon, etc)
http://www.jsk.t.u-tokyo.ac.jp/index-j.html
33 stars 33 forks source link

STM32CubeIDEへの移行 #459

Closed chibi314 closed 2 years ago

chibi314 commented 3 years ago
tongtybj commented 3 years ago

同意

tongtybj commented 3 years ago

@greenpepper123

この件、ちょっと調べてみて、もしかんたんにできそうなら、取り組んでほしいです。 ポイントはCubeMXからのプロジェクト生成で、いままでの状態と競合しないことです。

tongtybj commented 3 years ago

@greenpepper123

で、テストはmater branchではなく、https://github.com/JSKAerialRobot/aerial_robot/pull/417　からスタートしたほうがいい

greenpepper123 commented 3 years ago

以下のようなことを行いました

  1. JSKAerialRobot/aerial_robot:rtos 最新をpullして手元のneuronにTrueSTUDIOで書き込む
  2. CubeIDEをインストール
  3. "Import SW4STM32 or TrueSTUDIO project" で.projectがあるディレクトリ(aerial_robot_nerve/neuron/neuron_f4/TrueSTUDIO/neuron)を選択し、ポチポチする
  4. エラー無しで成功したので、ビルドする。TrueSTUDIOの時と操作は同じ
  5. ビルド成功したので、書き込む。stlink-serverが必要と言われるので、インストールする https://www.st.com/ja/development-tools/st-link-server.html
  6. debや.sh経由でのインストールは謎にエラーで止まってしまい成功しなかった(18.04.5 on ThinkPad X495)が、バイナリを実行可能にして直接/usr/binに置いたら動くようになる
  7. デバッグを行い、これまたTrueSTUDIOの時と同様に一度HAL_Init()のブレークポイントで止まるのでResumeし、終わったらTerminateする
  8. neuronの青点滅が再開する。
chibi314 commented 3 years ago

cubeideで開ける状態をプルリクで送ってもらえますか？

greenpepper123 commented 3 years ago

CubeIDE単体で書き込もうとしたら従来と同じくデバッグを使うことになりますが、STM32CubeProg https://www.st.com/en/development-tools/stm32cubeprog.html を別で入れれば普通に書き込み出来ると思います。リリースビルドじゃないとFlash足りないとかにならない限りは不要そうですが。 その場合、stlink-serverのおかげで、USBデバイスが競合せずにCubeIDEとCubeProgの両方が同時に仮想的なST-Linkに接続したままに出来る(片方を一度disconnectする必要がない)と思われます。

greenpepper123 commented 3 years ago

neuronだけですが一応送ります

461

417 とは違うトピックだと思ったので分けました

greenpepper123 commented 3 years ago

あと書き忘れたこととしてDebugはstlink-serverを入れた後もう一度何かエラーを吐きましたがRun->Debug Configurationsから適当に設定したら出来ました

chibi314 commented 3 years ago

書き込みは別にcubeideのデバッグでOKです．

greenpepper123 commented 3 years ago

spinalもやってみました ros_lib周りの仕組みは一通り読んで理解したつもりなのですが、別にこちらもとりあえず変わらないかなと思ってneuronと同様にconvertしビルドしデバッグしてみました 特にエラーはなくLED的には正しく動くようになったのですが、serial.launchを立ち上げてみたところImuがbad checksumと言われました 移行のせいというより僕の環境またはもともとのコミットが悪い気がしているのでもう少し調べます 一応catkin clean && catkin buildは行いました

$ roslaunch spinal_ros_bridge serial.launch                                                                                          [45/2419]
... logging to /home/greenpepper/.ros/log/48b1871c-9d1e-11eb-922c-287fcf996afb/roslaunch-ubuntu-14928.log                                                                                          
Checking log directory for disk usage. This may take a while.                                                                                                                                      
Press Ctrl-C to interrupt                                                                                                                                                                          
Done checking log file disk usage. Usage is <1GB.                                                                                                                                                  

started roslaunch server http://ubuntu:42003/                                                                                                                                                      

SUMMARY                                                                                                                                                                                            
========                                                                                                                                                                                           

PARAMETERS                                                                                                                                                                                         
 * /rosdistro: melodic                                                                                                                                                                             
 * /rosserial_server/baud: 921600                                                                                                                                                                  
 * /rosserial_server/port: /dev/ttyUSB0                                                                                                                                                            
 * /rosversion: 1.14.10                                                                                                                                                                            

NODES                                                                                                                                                                                              
  /                                                                                                                                                                                                
    rosserial_message_info (rosserial_python/message_info_service.py)                                                                                                                              
    rosserial_server (spinal_ros_bridge/serial_node)                                                                                                                                               
    spinal_ros_bridge/rosservice_server_manager.py                                                                                                                                                 

auto-starting new master                                                                                                                                                                           
process[master]: started with pid [14963]                                                                                                                                                          
ROS_MASTER_URI=http://localhost:11311                                                                                                                                                              

setting /run_id to 48b1871c-9d1e-11eb-922c-287fcf996afb                                                                                                                                            
process[rosout-1]: started with pid [14991]                                                                                                                                                        
started core service [/rosout]                                                                                                                                                                     
process[rosserial_server-2]: started with pid [14998]                                                                                                                                              
process[rosservice_server_manager_py_ubuntu_14928_3766842799446664388-3]: started with pid [14999]                                                                                                 
[ INFO] [1618403897.097516288]: Opening serial port.                                                                                                                                               
process[rosserial_message_info-4]: started with pid [15010]                                                                                                                                        
[ INFO] [1618403897.121195188]: Starting session.                                                                                                                                                  
[INFO] [1618403897.461505]: rosserial message_info_service node                                                                                                                                    
[ WARN] [1618403898.123569436]: Sync with device lost.                                                                                                                                             
[ WARN] [1618403899.125565511]: Sync with device lost.
[ INFO] [1618403899.133038544]: Attached client is using protocol VER2 (hydro)
[ INFO] [1618403899.134314940]: Creating service server callback for imu_calib
[INFO] [1618403899.137411]: Loading module to return info on spinal/Imu.
[ INFO] [1618403899.144697471]: publisher name: imu, type: spinal/Imu, id: 151
[INFO] [1618403899.146506]: Loading module to return info on geometry_msgs/Vector3Stamped.
[ INFO] [1618403899.178628072]: publisher name: attitude, type: geometry_msgs/Vector3Stamped, id: 152
[ INFO] [1618403899.178790074]: Creating service server callback for mag_declination
[INFO] [1618403899.180436]: Loading module to return info on spinal/Barometer.
[ INFO] [1618403899.183035746]: publisher name: baro, type: spinal/Barometer, id: 154
[INFO] [1618403899.184974]: Loading module to return info on spinal/Gps.
[ INFO] [1618403899.188542276]: publisher name: gps, type: spinal/Gps, id: 155
[INFO] [1618403899.190396]: Loading module to return info on spinal/GpsFull.
[ INFO] [1618403899.193803984]: publisher name: gps_full, type: spinal/GpsFull, id: 156
[INFO] [1618403899.195616]: Loading module to return info on std_msgs/UInt16.
[ INFO] [1618403899.199033293]: publisher name: encoder_angle, type: std_msgs/UInt16, id: 157
[INFO] [1618403899.200913]: Loading module to return info on spinal/ServoStates.
[ INFO] [1618403899.203970253]: publisher name: servo/states, type: spinal/ServoStates, id: 158
[INFO] [1618403899.205744]: Loading module to return info on spinal/ServoTorqueStates.
[ INFO] [1618403899.208748407]: publisher name: servo/torque_states, type: spinal/ServoTorqueStates, id: 159
[ INFO] [1618403899.208902255]: Creating service server callback for get_board_info
[ INFO] [1618403899.209036698]: Creating service server callback for set_board_config
[INFO] [1618403899.210751]: Loading module to return info on std_msgs/Float32.
[ INFO] [1618403899.213810164]: publisher name: battery_voltage_status, type: std_msgs/Float32, id: 162
[INFO] [1618403899.215685]: Loading module to return info on std_msgs/UInt8.
[ INFO] [1618403899.218874685]: publisher name: flight_config_ack, type: std_msgs/UInt8, id: 163
[INFO] [1618403899.220564]: Loading module to return info on spinal/Pwms.
[ INFO] [1618403899.223456374]: publisher name: motor_pwms, type: spinal/Pwms, id: 164
[INFO] [1618403899.225607]: Loading module to return info on spinal/RollPitchYawTerms.
[ INFO] [1618403899.228638848]: publisher name: rpy/pid, type: spinal/RollPitchYawTerms, id: 165
[INFO] [1618403899.230430]: Loading module to return info on spinal/RollPitchYawTerm.
[ INFO] [1618403899.233791138]: publisher name: rpy/feedback_state, type: spinal/RollPitchYawTerm, id: 166
[ INFO] [1618403899.233941580]: Creating service server callback for set_attitude_control
[ INFO] [1618403899.237220405]: subscirber name: baro_config_cmd, type: std_msgs/UInt8, id: 101
[ INFO] [1618403899.240287677]: subscirber name: desire_coordinate, type: spinal/DesireCoord, id: 102
[ INFO] [1618403899.243291164]: subscirber name: extra_servo_cmd, type: spinal/ServoControlCmd, id: 104
[ INFO] [1618403899.246169665]: subscirber name: extra_servo_torque_enable, type: spinal/ServoTorqueCmd, id: 105
[ INFO] [1618403899.249502247]: subscirber name: extra_servo_init_cmd, type: spinal/ServoControlCmd, id: 106
[ INFO] [1618403899.252995169]: subscirber name: servo/target_states, type: spinal/ServoControlCmd, id: 107
[ INFO] [1618403899.256428573]: subscirber name: servo/torque_enable, type: spinal/ServoTorqueCmd, id: 108
[ INFO] [1618403899.259866204]: subscirber name: set_adc_scale, type: std_msgs/Float32, id: 111
[ INFO] [1618403899.261356156]: subscirber name: uav_info, type: spinal/UavInfo, id: 112
[ INFO] [1618403899.262787012]: subscirber name: flight_config_cmd, type: spinal/FlightConfigCmd, id: 113
[ INFO] [1618403899.264506429]: subscirber name: four_axes/command, type: spinal/FourAxisCommand, id: 114
[ INFO] [1618403899.266038638]: subscirber name: motor_info, type: spinal/PwmInfo, id: 115
[ INFO] [1618403899.268844628]: subscirber name: rpy/gain, type: spinal/RollPitchYawTerms, id: 116
[ INFO] [1618403899.271555206]: subscirber name: pwm_test, type: std_msgs/Float32, id: 117
[ INFO] [1618403899.274350476]: subscirber name: p_matrix_pseudo_inverse_inertia, type: spinal/PMatrixPseudoInverseWithInertia, id: 118
[ INFO] [1618403899.277540731]: subscirber name: torque_allocation_matrix_inv, type: spinal/TorqueAllocationMatrixInv, id: 119
[ INFO] [1618403899.282016549]: subscirber name: gps_config_cmd, type: std_msgs/UInt8, id: 121
[ WARN] [1618403899.298168995]: Rejecting message on topicId=151, length=57 with bad checksum.
[ WARN] [1618403899.298470029]: Rejecting message on topicId=151, length=57 with bad checksum.
[INFO] [1618403899.533418]: Setup service server on imu_calib [spinal/ImuCalib]
[INFO] [1618403899.535857]: Setup service server on mag_declination [spinal/MagDeclination]
[INFO] [1618403899.538081]: Setup service server on get_board_info [spinal/GetBoardInfo]
[ERROR] [1618403899.539261]: Creation of service server failed: Checksum does not match: 66fd709be8b736fcb084865c0e818d87,430f7d06257ff67465ab9e333831c45c
[INFO] [1618403899.541519]: Setup service server on set_board_config [spinal/SetBoardConfig]
[INFO] [1618403899.547838]: Setup service server on set_attitude_control [std_srvs/SetBool]
[ERROR] [1618403899.549134]: Creation of service server failed: Checksum does not match: a479adad61b9876d2f6bebef8341cf8b,c02825e9f2b2fd32e312b8e549b32163
greenpepper123 commented 3 years ago

別件ですが、rosserial_arduinoのように一発でcatkinから書き込みまで出来るようにならないかなあと思ったのですが、CubeIDEはその点に関してはTrueSTUDIOと特に変わらなそうです https://ioloa.com/?p=296 この記事のようにMakefileは自前で書いてCubeIDEでデバッグだけ行うという手法もあるようですがやらないでしょう ただ、TrueSTUDIOと違うのは一応CubeIDEはビルドするとDebugディレクトリにmakefileを生成しています https://github.com/greenpepper123/aerial_robot/blob/rtos/aerial_robot_nerve/spinal/mcu_project/TrueSTUDIO/spinal/Debug/makefile 実際ビルドする際もこれのmake allを行っているだけのようなので、少なくとも依存関係が変わらない限りはmake allでビルドしてopenocdまたはst-flashで書き込めばコマンドラインから一発は実現できなくもなさそうです

tongtybj commented 3 years ago

@greenpepper123

serial.launchを立ち上げてみたところImuがbad checksumと言われました

これはros_lib(spinal用)とros側(host PC)のmsg(ここではSpinal/Imu)のバージョンが違うからおきる問題と思う。 以下のコマンドで解決するはず

$ rosrun spinal make_libraries.py

これは、現在ros側の最新のmsg情報からros_libを生成するコマンドです。本当はCmakeListにadd_custom_targetとしていれたほうがいいかもしれない。

tongtybj commented 3 years ago

実際ビルドする際もこれのmake allを行っているだけのようなので、少なくとも依存関係が変わらない限りはmake allでビルドしてopenocdまたはst-flashで書き込めばコマンドラインから一発は実現できなくもなさそうです

お、これはいい！ 実際Deubgディレクトリでmake allしてみて、おなじ生成物が出たわけだね。 正直、ライトユーザはコマンドラインから使えるようにしたほうがいいかもね

greenpepper123 commented 3 years ago

rosrun spinal make_libraries.py

もちろん一度したのですが、catkin buildしなおした後にもう一度するのを忘れていました、すみません。土曜辺りになってしまいそうですがやります。

greenpepper123 commented 3 years ago

やりなおしました。 とりあえず赤い方のCreation of service server failed: Checksum does not matchは出なくなって、実際各topicも読めました(例えば/imu) [ WARN] [1618643202.456159431]: Rejecting message on topicId=2, length=87 with bad checksum.は出ていて、1,2,3や151(spinal/Imu),158(servo/states)についても出たり出なかったりします。ただこれは1パケット失敗して出ているだけかなと勝手に思っています。

ただ、rqtが最初起動しなくて、Ctrl+Cを押すと

^C[ERROR] [1618643568.984192]: /imu_calib service call failed: transport error completing service call: receive_once[/imu_calib]: unexpected error [Errno 4] システムコール割り込み
^C[ERROR] [1618643570.895363]: /mag_declination service for get decalination call failed: transport error completing service call: receive_once[/mag_declination]: unexpected error [Errno 4] システムコール割り込み

これらがコンソールに出て、一応起動します でIMU calibやBoard Configuratorなどで情報が確認できるのですが、試しにacc calibを押すとrqtがフリーズします(コンソールにはメッセージなし) serial.launchの方には

[INFO] [1618643659.983523]: Setup service server on imu_calib [spinal/ImuCalib]
[INFO] [1618643659.986193]: Setup service server on mag_declination [spinal/MagDeclination]

がちゃんと出ていますし、rqtがフリーズしてもLEDの点滅も止まっていないしデバッグで動かしても別に何のハンドラも踏まないので、何の問題なのかわからないところです

このrtosブランチってどれくらい動作確認されていますか？

greenpepper123 commented 3 years ago

@chibi314 に見て頂いたんですが、 ・CubeIDE上でspinal.iocがプロジェクトに含まれてすらおらず、その時点で理想的な移行が行われていない ・そのせいで、HALのバージョンが違うとか何かうまく行っていないのでは？ という話でした。 それから、origin/rtosでTrueSTUDIOで書き込み直してみたら上記のservice関連は問題なしだったので、やはり移行がうまく言っていないことがほぼ確実です。

それで、「spinal.iocだけCubeIDEに食わせてコード生成して、残りのユーザーコードをコピーしてきてインクルードパスとか設定し直せば確実ではないか」という話だったのですが、どう思いますか？ @tongtybj

greenpepper123 commented 3 years ago

あと、 cubeversion iocを使おうとするとこのように出ますが、continueでいいですよね？

chibi314 commented 3 years ago

見てみるとバージョンが最新が1.16.1のところ，V1.16.0を使っていると言われているので，Migrateで最新にしてもいいかもしれません． 多分バグフィックスがあるだけだと思うので．

tongtybj commented 3 years ago

やはり移行がうまく言っていないことがほぼ確実です。 それで、「spinal.iocだけCubeIDEに食わせてコード生成して、残りのユーザーコードをコピーしてきてインクルードパスとか設定し直せば確実ではないか」という話だったのですが、どう思いますか？

議論の場にいなかったので、完全に理解していないのですが・・・ 移行がうまく行っていないというのは、以下のどれかな？ 1) HAL周りのコードの生成がioc->CubeIDEの過程でうまく行ってない？ となると、「残りのユーザーコードをコピー、・・・」は意味ないのでは？ここちゃんと理解していない 2) ユーザーコードの部分の移行がうまく行っていない? なら、「残りのユーザーコードをコピー、・・・」は意味あると思っているけど

見てみるとバージョンが最新が1.16.1のところ，V1.16.0を使っていると言われているので，Migrateで最新にしてもいいかもしれません．

はい、パッチバージョンが変わっただけで、APIは変わっていないとおもう。

greenpepper123 commented 3 years ago

どちらかというと2でしょうか そもそも公式の移行順序だとiocを使ってくれすらせず、TrueSTUDIOの.projectをCubeIDE用に少し変換してくれるだけで、そのせいでその変換がうまくいっていないのではないかということですね

chibi314 commented 3 years ago

@greenpepper123

rtosブランチのプログラムをTrueSTUDIOで書き込んで動かしてみましたが，特に問題なさそうです． やはり移行したことによって何か問題が起きているということになります．

tongtybj commented 3 years ago

そもそも公式の移行順序だとiocを使ってくれすらせず、TrueSTUDIOの.projectをCubeIDE用に少し変換してくれるだけで、そのせいでその変換がうまくいっていないのではないかということですね

公式の移行チュートリアルはioc (STM32CubeMX)を使っていないということ？

いずれにせよ、少し不可解な問題だと思うね。

試しにacc calibを押すとrqtがフリーズします(コンソールにはメッセージなし)

これは、 1) rosserialからデータが来なくなったときのフリーズなのか -> spinal, rosserial周りがおかしい 2) rqtが変な値を受け取ったからフリーズしたのか -> ホストPC側に問題 に切り分けられるけど、 @greenpepper123 はrqtがフリーズしたとき、他のトピックはちゃんとechoできたかな(/imuとか)。

あと、今使っているspinalって机の上に置いてあるもの？

greenpepper123 commented 3 years ago

公式の移行チュートリアルはioc (STM32CubeMX)を使っていないということ？

そうですね。ほぼ情報量ないですが https://www.st.com/resource/en/user_manual/dm00613834-migration-guide-from-truestudio-to-stm32cubeide-stmicroelectronics.pdf これが公式です 実際、行われていることを見てもCubeMXのことは全然気にせずただTrueSTUDIOの.projectと.cprojectをCubeIDE用に変換するだけの機能に見えます。

試しにacc calibを押すとrqtがフリーズします(コンソールにはメッセージなし)これは、 rosserialからデータが来なくなったときのフリーズなのか -> spinal, rosserial周りがおかしい rqtが変な値を受け取ったからフリーズしたのか -> ホストPC側に問題 に切り分けられるけど、 @greenpepper123 はrqtがフリーズしたとき、他のトピックはちゃんとechoできたかな(/imuとか)。

できます。 実際何が起きているのかログを吐いてくれないので仕方なくデバッガで追跡したのですが、 rqtを開いた時Jsk_Lib/sensors/imu/imu_ros_cmd.cppのimuCalibCallback()までは呼ばれているのですがその後一生imuCalibCallback()が呼ばれ続けているように見えました rosserialのserviceの仕様で繰り返し呼ばれるのかと思ったのですが、そもそも https://github.com/greenpepper123/aerial_robot/blob/b740d12729bb6dc373c117add60f358f9436d9cf/aerial_robot_nerve/spinal/mcu_project/Jsk_Lib/sensors/imu/imu_ros_cmd.cpp#L62 ここのgetIMUCalibData()の後res = imu_calib_res_;にたどり着かず何故か急にimuCalibCallback()の先頭に戻るという挙動でした。 なので、フリーズするのは無限ループになっているからかなと思います。

あと、今使っているspinalって机の上に置いてあるもの？

そうです。最初に頂いたspinalを全て使っています。 ただorigin/rtosで正常動作しているので個体の問題を疑っているのだとしたらそうではないと思います。

greenpepper123 commented 3 years ago

Project->Properties->C/C++ Build->Setting->MCU GCC Compiler->Optimization->Optimization levelを-O2から-O0へ Project->Properties->C/C++ Build->Setting->MCU G++ Compiler->Optimization->Optimization levelを-O1から-O0へ 変更したら動きました。 https://github.com/greenpepper123/aerial_robot/commit/5150f43d5ced611346cade777c30261772ea0450 ちなみに両方-O1とか、gccがO0でg++がO2とかもダメで、両方O0に戻したらやはり動くことを確認しました。なので気のせいではないと思います。

なので、 ・実機に載せてみてO0でも性能が足りるか確認 ・結局spinal.iocが関連付けされていない状態のままなので、ちゃんとCubeMXでregenerateしていけるように再構成 をしていこうと思います。

tongtybj commented 3 years ago

デバッグおつかれさまです。

思い返しみるとKeil ARM-MDKからTrueStudioに移行したときも、-O周りで躓いていました。いにしえのコードを引っ張りだして、grepしてみたら、-O3になっていました。なので、TrueStudioでは-O3は動かず、やむを得ず-O2にした記憶があります。

今回の似たようなシチュエーションでしょうね。こう考えると、Keilのコンパイラーが一番優秀なのかもしれないですね。

tongtybj commented 3 years ago

@greenpepper123

あと、-O0にしてから、rostopic echo /imuがたまに途切れるように見えたけど、それは治ったの？

greenpepper123 commented 3 years ago

それは一時的なもの(おそらくデバッガ繋いでいたせい)で、再現していません。

greenpepper123 commented 3 years ago

spinalは-O0, neuronは-O2のままで、書き込んで試してみました https://github.com/greenpepper123/aerial_robot/tree/hydrus_xi_under_actuated_model_planning2

普通に飛行することができました。spinalの負荷が高くなる条件というのが何か人為的に生み出せればまた試したいと思いますが、とりあえず ・spinalは-O0でとりあえずパフォーマンスに問題がなさそうなこと ・neuronは従来通り-O2で問題なく動きそうなこと が検証できました

tongtybj commented 3 years ago

僕も実験に付き合って、ちゃんと飛んでいるところを確認みました。 全機体のCubuIDEへの移行はこれでいけると考えられるが、

問題は, #461 は #416 をベースにしていて、

以上の手間とバージョン更新を許容できるのなら、近々 #416, #461 の順でマージしようと思うます。 @chibi314 @Takuzumi240 どうでしょうか

ちなみに、PrimeXを設置するためのMotive更新に伴い, mocap_optitrack (ros package)の変更も必要なので、 この件もちょうどv1.3.0への移行に合わせて一緒にやったほうがいいと思うね。