ここからはhttps://qiita.com/nasu_onigiri/items/dc10c3b0bfcd4754357fを試してみました。
- 人感センサ… HC-SR501
- サーボモータ*2つ… MG90S
- サーボドライバ… PCA9685
によるシンプルな構成です。
ROS2パッケージの作成
mkdir -p ~/ros2_1/srcでディレクトリーを作成し、cdコマンドでros2_1に移動します。
colcon buildファイルがいっぱいできます。
次にソースコードのためのパッケージを作ります。
cd ~/ros2__1/src
ros2 pkg create --build-type ament_python rpi_robot_py pythonでプログラムを書くのでament_pythonです。rpi_robot_pyはプロジェクト名です。
できたファイル構造です。
ros2_1
├── build
│ └── COLCON_IGNORE
├── install
│ ├── COLCON_IGNORE
│ ├── local_setup.bash
│ ├── local_setup.ps1
│ ├── local_setup.sh
│ ├── _local_setup_util_ps1.py
│ ├── _local_setup_util_sh.py
│ ├── local_setup.zsh
│ ├── setup.bash
│ ├── setup.ps1
│ ├── setup.sh
│ └── setup.zsh
├── log
│ ├── build_2022-06-04_11-11-31
│ │ ├── events.log
│ │ └── logger_all.log
│ ├── COLCON_IGNORE
│ ├── latest -> latest_build
│ └── latest_build -> build_2022-06-04_11-11-31
└── src
└── rpi_robot_py
├── package.xml
├── resource
│ └── rpi_robot_py
├── rpi_robot_py
│ └── __init__.py
├── setup.cfg
├── setup.py
└── test
├── test_copyright.py
├── test_flake8.py
└── test_pep257.py
11 directories, 23 files
src/rpi_robot_py/package.xmlを編集します。nameとかmaintenarとかを修正します。
シンプルなロボットプログラム
HC-SR501の接続
あちこちに乗っています。例えばここ
入力ノードの作成
ここでは人感センサー(HC-SR501)からの入力を書きます。
場所はsrc/rpi_robot_piの下です。ファイル名はhc_sr501.pyとしました。
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
import pigpio
import time
SENSOR_GPIO_PIN = 17
TIMER_INTERVAL = 0.5
class HumanSensor(Node):
def __init__(self):
super().__init__('human_sensor_node')
self.init_sensor()
self.pub_sensor = self.create_publisher(Bool, '/input/human_sensor', 10)
self.timer = self.create_timer(TIMER_INTERVAL, self.sensor_timer_callback)
self.prev_sensor_data = False
def sensor_timer_callback(self):
sensor_msg = Bool(data=self.get_sensor_data())
self.pub_sensor.publish(sensor_msg)
def init_sensor(self):
self.pi = pigpio.pi()
self.pi.set_mode(SENSOR_GPIO_PIN, pigpio.INPUT)
self.pi.set_pull_up_down(SENSOR_GPIO_PIN, pigpio.PUD_UP)
def get_sensor_data(self):
if self.pi.read(SENSOR_GPIO_PIN) == 1:
return True
else:
return False
def main(args=None):
rclpy.init(args=args)
node = HumanSensor()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
これを単体で動かします。
もうひとつターミナルを立ち上げ、
ros2 topic listと入力すると、
/input/human_sensor (上のプログラムで定義した）
/parameter_events
/rosout
の3つがリストされます。
トピックとはROS2内でメッセージのやりとりの場ですから、/input/human_sensorの中を見てみましょう。
ros2 topic echo /input/human_sensor
と次々に表示されます。trueのときはセンサーの前で手を振った時です。
これでセンサーが正しく動いていることと、メッセージ内容を確認できます。
ROS2 便利！
PCA9685とサーボモーターMG90Sの接続
モーターを接続するため、別の電源を用意します。（メンドクサ）
モータードライブはI2C接続で16チャンネルをもつ、PCA9685です。
制御線のところを拡大すると、
- GND：電源のマイナスとラズパイのGND共通接続
- OE：未接続（Hiで全ての動作を停止）
- SCL：GPIO3番(SCL)ピンへ接続
- SDA：GPIO2番(SDA)ピンへ接続
- VCC：ラズパイから電源（3.3V）
- V+：サーボモーター用電源
I2Cで動作するので、I2CライブラリーとPCA9685のドライバー(Adafruits謹製）をインストールします。
sudo apt install i2c-toolsi2Cが動いているか確認します。
ls -l /dev/i2c-1
このように存在していればOK
crw-rw—- 1 root i2c 89, 1 May 31 21:32 /dev/i2c-1
ハードウェアを繋いだら以下のコマンドでI2Cの状況を見てみましょう。
なお、私のもっているサーボモーターMG90Sはオレンジ色の線がPWMでした。接続を間違えると動きませんからね。
sudo i2cdetect -y 1
動作確認
こちらのサイトのお世話になります。(このプログラムはROS2と関係なく、動作テストプログラムです。私はDocumentsに保管してます）
# import RPi.GPIO as GPIO
import Adafruit_PCA9685
from time import sleep
# 設定周波数（Hz）
set_freq = 50
# 動作角度からPCA9685に渡す値を変換
def convert_deg(deg,freq):
# 分解能（ステップ数）
step = 4096
# 接続サーボモーターの最大最小角度時のパルス間隔（ms）
# この数値はSG90の仕様に基づくものですが、必要に応じて調整してください。
max_pulse = 2.4 # 90°時
min_pulse = 0.5 # -90°時
# サーボモーターの0°時のパルス間隔（ms）
center_pulse = (max_pulse - min_pulse) / 2 + min_pulse
#サーボモーター1°あたりのパルス間隔（ms）
one_pulse = round((max_pulse - min_pulse) / 180, 2)
# 要求角度のパルス間隔（ms）を算出しPCA9685に渡す値を算出
deg_pulse = center_pulse + deg * one_pulse
deg_num = int(deg_pulse / (1.0 / freq * 1000 / step))
# デバッグ
print('deg:' + str(deg) + '(' + str(deg_num) + ')')
return deg_num
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(set_freq)
try:
while True:
pwm.set_pwm(0, 0, convert_deg(0,set_freq))
sleep(1)
pwm.set_pwm(0, 0, convert_deg(45,set_freq))
sleep(1)
pwm.set_pwm(0, 0, convert_deg(0,set_freq))
sleep(1)
pwm.set_pwm(0, 0, convert_deg(-45,set_freq))
sleep(1)
except KeyboardInterrupt:
pwm.set_pwm(0, 0, 0)
print("KeyboardInterrupt")
pass
テストプログラムを動かすと、/etc/i2c-1へpermission deniedをくらいました。
調べると /etc/group にユーザーを追加し、権限をもつ必要があるようです。
sudo nano /etc/groupi2cの項目へ、他の項目のまねをして最後にubuntuを追加して、リブートしましょう。
うまく動作しました。
出力ノードの作成
サーボモーターのノード用プログラムを動かしてみます。
プログラムの注釈にあるゆにサーボモーターをPCA9685のチャネル0, 3に接続します。
import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
# from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import Int8
import Adafruit_PCA9685
ANGLE_MIN = -70
ANGLE_MAX = 70
# PCA9685 の 0 と 3 に、2つのサーボを接続
SERVO_LEFT_ID = 0
SERVO_RIGHT_ID = 3
class Servo(Node):
def __init__(self, servo_name, servo_id):
super().__init__('servo_node_' + servo_name)
self.init_pca9685()
self.servo_name = servo_name
self.servo_id = servo_id
self.sub_topic_name = '/output/servo/' + servo_name
self.sub_servo = self.create_subscription(Int8, self.sub_topic_name, self.servo_callback, 10)
def init_pca9685(self):
self.pwm = Adafruit_PCA9685.PCA9685(address=0x40)
self.pwm.set_pwm_freq(60)
def servo_callback(self, servo_msg):
self.get_logger().info('subscribe servo angle: {}'.format(servo_msg.data))
self.set_angle(servo_msg.data)
def set_angle(self, angle):
angle = max(ANGLE_MIN, angle)
angle = min(ANGLE_MAX, angle)
pulse = (600-150) / 180 * (angle + 90) + 150
self.pwm.set_pwm(self.servo_id, 0, int(pulse))
def main(args=None):
rclpy.init(args=args)
executor = SingleThreadedExecutor()
## mutli thread の場合はこっち
# executor = MultiThreadedExecutor(num_threads=2)
## ノード（クラス）を2つインスタンス化し、executor に登録
node_left = Servo(servo_name='left', servo_id=SERVO_LEFT_ID)
node_right = Servo(servo_name='right', servo_id=SERVO_RIGHT_ID)
executor.add_node(node_left)
executor.add_node(node_right)
try:
executor.spin()
except KeyboardInterrupt:
pass
executor.shutdown()
node_left.destroy_node()
node_right.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
ExecutorというものがNodeの実行管理をします。ここではnode_left, node_rightを一組としてExecutorに登録します。
Executor.spinで動き出します。Executor.shutdownで実行終了。
長くなったので次に続く。。。