ROS（ROS2）- ノード - プログラム

1.　概要

2.　プログラム作成

3.　ビルド

4.　備考

1.　概要

　プログラムを作成します。

2.　プログラム作成

　パブリッシャープログラムを作成します。


vi ~/workspace/src/my_node/my_node/my_server.py


　下記を記述します。


import sys
import threading
import time
import rclpy
from rclpy.node import Node
from my_message.msg import Num

isEnd  =  False

class publisherNode(Node):
  def __init__(self):
    super().__init__('talker')
    self.pub_ = self.create_publisher(Num, 'myNode', 10)
    self.create_timer(0.5, self.callback)
    self.i_ = 0

  def callback(self):
    global isEnd

    msg = Num()

    if isEnd:
      msg.num = -1
      self.pub_.publish(msg)
      time.sleep(0.01)
      raise Exception

    msg.num = self.i_
    self.pub_.publish(msg)
    self.get_logger().info('published: %d' % msg.num)
    self.i_ += 1

def worker_thread():
  global isEnd
  c = sys.stdin.read(1)
  isEnd = True

def main(args=None):
  rclpy.init(args=args)
  publisher_node = publisherNode()

  try:
    thread = threading.Thread(target=worker_thread, daemon=True)
    thread.start()  # スレッドを開始

    rclpy.spin(publisher_node)
  except:
    pass

  publisher_node.destroy_node() 	# ノードを終了
  rclpy.shutdown() 					# ROS2 クライアント ライブラリを終了

if __name__ == '__main__':
  main()


　「create_publisher」の引数は、メッセージ、トピック名、キューサイズだそうです。

　サブスクライバープログラムを作成します。


vi ~/workspace/src/my_node/my_node/my_client.py


　下記を記述します。


import rclpy
from rclpy.node import Node
from my_message.msg import Num

class subscriberNode(Node):
  def __init__(self):
    super().__init__('listener')
    self.create_subscription(Num, 'myNode', self.callback, 10)

  def callback(self, msg):
    self.get_logger().info('subscribed: %d' % msg.num)

    if  msg.num < 0:
      raise Exception

def main(args=None):
  rclpy.init(args=args)
  subscribe_node = subscriberNode()

  try:
    rclpy.spin(subscribe_node)
  except:
    pass

  subscribe_node.destroy_node()   # ノードを終了
  rclpy.shutdown()                # ROS2 クライアント ライブラリを終了

if __name__ == '__main__':
  main()


　「create_subscription」の引数は、メッセージ、トピック名、コールバック関数、キューサイズだそうです。
　「package.xml」「setup.py」を下記のように記述します。


vi ~/workspace/src/my_node/package.xml



<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_node</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="hogehoge@todo.todo">hogehoge</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclcpp</depend>
  <depend>my_message</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>



vi ~/workspace/src/my_node/setup.py



from setuptools import find_packages, setup

package_name = 'my_node'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='hogehoge',
    maintainer_email='hogehoge@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',

    entry_points={
      'console_scripts': [
        'server = my_node.my_server:main',
        'client = my_node.my_client:main',
      ],
    },
)


3.　ビルド

　ビルドして、インストールします。


cd ~/workspace


colcon build --packages-select my_node


　下記のようなメッセージが表示されます。


Starting >>> my_node
Finished <<< my_node [4.75s]

Summary: 1 package finished [5.12s]


　インストール。


. install/setup.bash


　ログイン時に動作するように。


~/.bashrc


　に下記の行を追加しておきます。


. ~/workspace/install/local_setup.bash



4.　備考

　本ページは、下記のサイトを参考にさせていただきました。

「ROS2 でメッセージを自作する #ROS2」