作業の備忘録です。
C++のパブリッシャとサブスクライバの関数呼び出しの理解に時間をかけすぎて、全然話が進んでいませんが、流石にそろそろJOYスティックを動かします。
C++に寄り道しましたが、Python2.7で行います。ROSはPython2.7しか正式サポートしてないが故。

#やりたかったことはなに？
ROSでJOYスティックの入力をとり、おもちゃのサーボを動かします。
ラジコンの自動運転をやるための基礎をおさえるのが目的です。
JOYスティックを動かしたら、サーボモータがぐりぐり動くようにします。

#機材はなに？
制御基板はJetson-nanoでUbuntu18.04、JetPack4.2.2が入っています。
ROSとREALSENSEのドライバはセットアップ済みです。
PlayStationのJOYスティック、サーボコントローラPCA9685、サーボモータ２個を用意しています。

#JOYスティックの入力をPython2.7で取る

自分用のros_rc_packageパッケージを作成します。

$ cd ~/catkin_ws/
$ catkin_create_pkg ros_rc_package std_msgs rospy roscpp
$ catkin build
$ . ~/catkin_ws/devel/setup.bash
$ rospack list

リストを確認して、登録されているかどうかを確認します。
OKなら作ったプログラムをパッケージのcmakelists.txtに前回作成したプログラムを追加します。仮にTeleopRCCar.cppを作った想定で行います。
ros_rc_package/src の下にTeleopRCCar.cppを作ります。
cmakefiles.txtデフォルトのファイルの適当なところにadd_executable,target_link_librariesを追加します。

CMakeLists.txt

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/ros_rc_package_node.cpp)

add_executable(TeleopRCCar src/TeleopRCCar.cpp)

target_link_libraries(TeleopRCCar
  ${catkin_LIBRARIES}
)

PythonでJOYスティックの入力とサーボへの出力を記述したrc_servo.pyファイルをパッケージのディレクトリros_rc_package/script/の下に置きます。
ローンチファイルを用意し、JOYスティックのノードと作成したros_rc_packageのrc_servoのノードを定義します。

ros_rc_car.launch

<launch>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <node name="rc_servo" pkg="ros_rc_package" type="rc_servo.py" />
</launch>

結局Pythonだけで記述してC++で書いていませんね（苦笑）。
GitHubにパッケージを載せておきます。

kumaxxp/ros_rc_package

ターミナル3つで実行する

roscore
rosrun joy joy_node
rosrun ros_rc_package rc_servo

と書いて実行するか、launchファイルを呼び出して実行するとrc_servo.pyを動かすことができます。

roslaunch ros_rc_car.launch

ファイルを実行することができただけで、問題が続出しましたので、それを順番に解決していきました。

#発生したトラブルを順番に解決
###JOYスティックが本当に動いているのか？
JOYスティックを動かすとサーボが動くようにするつもりですが、まったく動きません。
JOYスティックは本当に動いているのでしょうか？

ターミナルを３つ開いて、それぞれに以下のコマンドを入れて実行します。
アナログJOYSTICKを操作すると画面に反応が出力されます。
これで入力を確認しながら実行するとわかりやすいです。

JOYSTICK

$ roscore
$ rosrun joy joy_node
$ rostopic echo /joy

実行するとアナログスティックの動きに合わせて、入力の情報が流れます。
どうやら正常に動いているようです。

###I2Cが本当に動いているのか？
一般的なI2Cの接続に沿ってPCA9685とJetson-nanoをつないでいますが、つながっているか確認します。

$ sudo i2cdetect -y -r 1

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
70: 70 -- -- -- -- -- -- --

アドレス0x40に反応があるので、PCA9685との接続は問題なさそうです。
この後、何をやってもまったくPCA965につないだサーボが動かずかなり難儀します。
結論から言うと、サーボドライバの配線が4本では動かなかった 。
Donkey Car では4本で接続していたのでそのままだったのですが、V+にもつないで電圧を供給しないと個別のサーボが動かなかったです。
(「Jetson Nano 超入門」には写真入りで5本配線が懇切丁寧に説明されていました。慣れでやってはだめですね)

さらにトラブルが続きます。

###ドライバをインストール。ただし、Python2.7へ
実行すると ImportError: No module named Adafruit_GPIO.I2C が発生しました。
importしたドライバが入っていないようです。
Python3.x用にはすでにインストールしていましたが、改めてPython2.7側にインストールする必要があります。
pipで入れようとすると警告がでます。

WARNING: pip is being invoked by an old script wrapper. This will fail in a future version of pip.
Please see https://github.com/pypa/pip/issues/5599 for advice on fixing the underlying issue.
To avoid this problem you can invoke Python with '-m pip' instead of running pip directly.
DEPRECATION: Python 2.7 reached the end of its life on January 1st, 2020. Please upgrade your Python as Python 2.7 is no longer maintained. A future version of pip will drop support for Python 2.7. More details about Python 2 support in pip, can be found at https://pip.pypa.io/en/latest/development/release-process/#python-2-support
ERROR: Could not find a version that satisfies the requirement Adafruit_GPIO.I2C (from versions: none)
ERROR: No matching distribution found for Adafruit_GPIO.I2C

もういい加減古いから、pipを使うなということですかね？
そう、もうpython2.7は収束していくのでした。
関連ドライバを落として、手動で入れるしかありません。
以下のリンクを参考にドライバファイルを落としてきました。
Raspberry Pi 3でPCA9685を使う

ダウンロード


$ git clone https://github.com/adafruit/Adafruit_Python_PCA9685.git
$ git clone https://github.com/adafruit/Adafruit_Python_GPIO.git
$ git clone https://github.com/adafruit/Adafruit_Python_PureIO.git

Adafruit_Python_GPIO をダウンロードして以下のようにインストール。
sudo python2 setup.py install
これでImportError: No module named Adafruit_GPIO.I2Cエラーは消えました。
が、次はRuntimeError: Could not determine default I2C bus for platform.
が発生します。

ドライバファイルを書き換えて正しく動かす必要のあるところです。
「Jetson Nano 超入門」にも詳細な記述がありました。

python2のドライバのインストールされたディレクトリの中からI2C.pyを探して、
下のリンクの内容を参考にして書き換える必要があります。
Jetson nanoとPCA9685でサーボを動かそうとするときのI2Cエラー対処法！
I2C.pyプログラムの修正（workaround） を参考にして書き換えます。

が・・・python2の関連ディレクトリにドライバファイルがありません。

##ドライバファイルが無い。謎の.eggファイル

Adafruit_PCA9685 をimportして、どこのパスにインストールされているか調べます。

$ python2
Python 2.7.17 (default, Nov  7 2019, 10:07:09) 
[GCC 7.4.0] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import Adafruit_PCA9685
>>> Adafruit_PCA9685.__file__
'/usr/local/lib/python2.7/dist-packages/Adafruit_PCA9685-1.0.1-py2.7.egg/Adafruit_PCA9685/__init__.pyc'
>>>

そのディレクトリを見に行ったのですが、ディレクトリがありません。
dist-packages/に変な拡張子https://kokensha.xyz/jetson/jetson-nano-pca9685-i2c-error-resolution/
より深い階層のディレクトリがありません。
代わりに*.eggというファイルがあるだけです。

Adafruit_GPIO-1.0.4-py2.7.egg     easy-install.pth
Adafruit_PCA9685-1.0.1-py2.7.egg  spidev-3.4-py2.7-linux-aarch64.egg
Adafruit_PureIO-1.0.4-py2.7.egg

この.eggファイルはどうやらインストールしたドライバを圧縮したzipファイルで、unzipで解凍できるとのこと。

$ sudo unzip Adafruit_GPIO-1.0.4-py2.7.egg
[sudo]  のパスワード: 
Archive:  Adafruit_GPIO-1.0.4-py2.7.egg
  inflating: Adafruit_GPIO/FT232H.py  
  inflating: Adafruit_GPIO/FT232H.pyc  
  inflating: Adafruit_GPIO/GPIO.py   
  inflating: Adafruit_GPIO/GPIO.pyc  
  inflating: Adafruit_GPIO/I2C.py    
  inflating: Adafruit_GPIO/I2C.pyc   
  inflating: Adafruit_GPIO/MCP230xx.py  
  inflating: Adafruit_GPIO/MCP230xx.pyc  
  inflating: Adafruit_GPIO/PCA95xx.py  
  inflating: Adafruit_GPIO/PCA95xx.pyc  
  inflating: Adafruit_GPIO/PCF8574.py  
  inflating: Adafruit_GPIO/PCF8574.pyc  
  inflating: Adafruit_GPIO/PWM.py    
  inflating: Adafruit_GPIO/PWM.pyc   
  inflating: Adafruit_GPIO/Platform.py  
  inflating: Adafruit_GPIO/Platform.pyc  
  inflating: Adafruit_GPIO/SPI.py    
  inflating: Adafruit_GPIO/SPI.pyc   
  inflating: Adafruit_GPIO/__init__.py  
  inflating: Adafruit_GPIO/__init__.pyc  
  inflating: EGG-INFO/PKG-INFO       
  inflating: EGG-INFO/SOURCES.txt    
  inflating: EGG-INFO/dependency_links.txt  
  inflating: EGG-INFO/requires.txt   
  inflating: EGG-INFO/top_level.txt  
  inflating: EGG-INFO/zip-safe       
  inflating: tests/MockGPIO.py       
  inflating: tests/MockGPIO.pyc      
  inflating: tests/__init__.py       
  inflating: tests/__init__.pyc      
  inflating: tests/test_GPIO.py      
  inflating: tests/test_GPIO.pyc     
  inflating: tests/test_I2C.py       
  inflating: tests/test_I2C.pyc      
  inflating: tests/test_PWM.py       
  inflating: tests/test_PWM.pyc      
  inflating: tests/test_Platform.py  
  inflating: tests/test_Platform.pyc  
  inflating: tests/test_SPI.py       
  inflating: tests/test_SPI.pyc

解凍して展開できました。
改めて以下のサイトを参考にして、ドライバの不備を修正します。
Jetson nanoとPCA9685でサーボを動かそうとするときのI2Cエラー対処法！
I2C.pyプログラムの修正（workaround） を参考にして書き換えます。

書き換えの後、ドライバを解凍したままにしておけば問題なく動きます。

###JOYスティックでサーボを動かすプログラムを実行する

では動かします。
個別のターミナルで動かす場合は、こちら。

$ roscore
$ rosrun joy joy_node
$ rosrun ros_rc_package rc_servo.py

もしくは、ros_rc_car.launch を起動する。

$ roslaunch ros_rc_package ros_rc_car.launch

はあ、大変だった。

（多分続きます）