前回の記事のoutputノードを動かしても、動作するトリガーがなにも設定されていないので動作しません。
そこでコマンドラインから擬似メッセージをパブリッシュしてみます。（このフォーマットがわからんかった！）
まず
ros2 topic listとすると、/output/servo/left, /output/servo/rightなどが存在しているはずです。
次に
ros2 topic info /output/servo/leftと入力すると、/output/servo/leftがどういうメッセージの型なのか、パブリッシュされた回数などが出ます。
ここではTypeはstd_msgs/msg/Int8となっているはずです。
そこでデータをパブリッシュしてみます。
ros2 topic pub --once /output/servo/left std_msgs/msg/Int8 "data: 45"と入力すると左用モーターが45度に回ります。–onceを入れていないと、同じメッセージがどんどんパブリッシュされてしまいます。
角度を変えてメッセージを送ってみてください。
これで、プログラムの動作が確認できました。
最後に人感センサーデータを受け取って、サーボモーターを動かすコントローラーノードを作ります。
import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from std_msgs.msg import Int8
import random
ANGLE_MIN = -70
ANGLE_MAX = 70
class MyController(Node):
def __init__(self):
super().__init__('controller_node')
self.prev_sensor_data = False
self.pub_servo_left = self.create_publisher(Int8, '/output/servo/left', 10)
self.pub_servo_right = self.create_publisher(Int8, '/output/servo/right', 10)
self.sub_sensor = self.create_subscription(Bool, '/input/human_sensor', self.sensor_callback, 10)
def sensor_callback(self, sensor_msg):
if self.prev_sensor_data != sensor_msg.data:
self.prev_sensor_data = sensor_msg.data
if sensor_msg.data == True:
self.pub_servo_left.publish( Int8(data=random.randint(ANGLE_MIN, ANGLE_MAX)) )
else:
self.pub_servo_right.publish( Int8(data=random.randint(ANGLE_MIN, ANGLE_MAX)) )
def main(args=None):
rclpy.init(args=args)
node = MyController()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()パッケージ作成時に自動生成された setup.py に、今回作成した3つのプログラムの情報を定義します。
加えて、launch/ に後で作成する *.launch.py ファイルを読み込めるようにします。
from glob import glob
from setuptools import setup
package_name = 'rpi_robot_py'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
## 後で作成する 'launch/*.launch.py'用に追記
('share/' + package_name, glob('launch/*.launch.py'))
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='ns',
maintainer_email='ns@hoge.com',
description='sample robot program',
license='',
tests_require=['pytest'],
## 追記
entry_points={
'console_scripts': [
'human_sensor = rpi_robot_py.in_sr501:main',
'servo = rpi_robot_py.out_servo:main',
'controller = rpi_robot_py.controller:main',
],
},
)
Launchファイルを作って、３つのプログラムを自動的に起動できるようにします。
cd ~/ros2_1/src/rpi_robot_py
mkdir launch
cd launch
touch rpi_robot.launch.pyrpi_robot.lunch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='rpi_robot_py',
node_executable='human_sensor',
),
Node(
package='rpi_robot_py',
node_executable='servo',
output='screen'
),
Node(
package='rpi_robot_py',
node_executable='controller',
)
])現在、ソースファイルは以下のように置いてあります。(赤字は編集、追加したファイル）
src
└── rpi_robot_py
├── launch
│ └── rpi_robot.launch.py
├── package.xml
├── resource
│ └── rpi_robot_py
├── rpi_robot_py
│ ├── controller.py
│ ├── __init__.py
│ ├── in_sr501.py
│ └── out_servo.py
├── setup.cfg
├── setup.py
└── test
├── test_copyright.py
├── test_flake8.py
└── test_pep257.py
次の手順でbuildします。
cd ~/ros2_1
colcon build
bash ./install/setup.bashcolconの結果は以下のとおり。
ros2 launch rpi_robot_py rpi_robot.launch.pyで、動きだしました。角度がランダムなので動きは若干、不気味です。笑
定義などを間違えている時は、colcon buildはやりなおさねばなりません。
半分くらい意味がわかっていませんが、ROS2で基本的な動きを知ることができました。記事を制作してくれた@nasu_onigiri氏には感謝です。