ros1-development

ROS1 開発スキル

このスキルを使う場面

• ROS1 パッケージやノードを構築、保守する
• launch ファイル、メッセージ型、サービスを記述する
• ROS1 の通信（トピック、サービス、アクション）をデバッグする
• catkin ワークスペースとビルドシステムを設定する
• tf/tf2 変換、URDF、ロボットモデルを扱う
• 長時間実行タスクに actionlib を使用する
• ゼロコピー転送のために nodelet を最適化する
• ROS1 から ROS2 への移行を計画する

コアアーキテクチャの原則

1. ノード設計

単一責任のノード：各ノードは、1 つのことを適切に行うようにしてください。何でもこなすモノリシックなノードを作りたくなる誘惑に負けないでください。

# BAD: Monolithic node
class RobotNode:
    def __init__(self):
        self.sub_camera = rospy.Subscriber('/camera/image', Image, self.camera_cb)
        self.sub_lidar = rospy.Subscriber('/lidar/points', PointCloud2, self.lidar_cb)
        self.pub_cmd = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.pub_map = rospy.Publisher('/map', OccupancyGrid, queue_size=1)
        # This node does perception, planning, AND control

# GOOD: Decomposed nodes
class PerceptionNode:    # Fuses sensor data → publishes /obstacles
class PlannerNode:       # Subscribes /obstacles → publishes /path
class ControllerNode:    # Subscribes /path → publishes /cmd_vel

ノード初期化パターン：

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

class MyNode:
    def __init__(self):
        rospy.init_node('my_node', anonymous=False)

        # 1. Load parameters FIRST
        self.rate = rospy.get_param('~rate', 10.0)
        self.frame_id = rospy.get_param('~frame_id', 'base_link')

        # 2. Set up publishers BEFORE subscribers
        #    (prevents callbacks firing before publisher is ready)
        self.pub = rospy.Publisher('~output', String, queue_size=10)

        # 3. Set up subscribers LAST
        self.sub = rospy.Subscriber('~input', String, self.callback)

        rospy.loginfo(f"[{rospy.get_name()}] Initialized with rate={self.rate}")

    def callback(self, msg):
        # Process and republish
        result = String(data=msg.data.upper())
        self.pub.publish(result)

    def run(self):
        rate = rospy.Rate(self.rate)
        while not rospy.is_shutdown():
            # Periodic work here
            rate.sleep()

if __name__ == '__main__':
    try:
        node = MyNode()
        node.run()
    except rospy.ROSInterruptException:
        pass

2. トピック設計

命名規則：

/robot_name/sensor_type/data_type

# Examples:
/ur5/joint_states              # Robot joint states
/realsense/color/image_raw     # Camera color image
/realsense/depth/points        # Depth point cloud
/mobile_base/cmd_vel           # Velocity commands
/gripper/command               # Gripper commands

キューサイズは重要です：

# For sensor data (high frequency, OK to drop old messages):
rospy.Subscriber('/camera/image', Image, self.cb, queue_size=1)

# For commands (don't want to miss any):
rospy.Publisher('/cmd_vel', Twist, queue_size=10)

# For large data (point clouds, images) - use small queues to prevent memory bloat:
rospy.Subscriber('/lidar/points', PointCloud2, self.cb, queue_size=1)

# NEVER use queue_size=0 (infinite) for high-frequency topics
# This WILL cause memory leaks under load

ラッチ付きトピックは、データの変化が少ない場合に使用します：

# Robot description, static maps, calibration data
pub = rospy.Publisher('/robot_description', String, queue_size=1, latch=True)

3. launch ファイルのベストプラクティス

<launch>
  
  <arg name="robot_name" default="ur5"/>
  <arg name="sim" default="false"/>
  <arg name="debug" default="false"/>

  
  <group ns="$(arg robot_name)">

    
    <group if="$(arg sim)">
      <include file="$(find my_pkg)/launch/sim_drivers.launch"/>
    </group>
    <group unless="$(arg sim)">
      <include file="$(find my_pkg)/launch/real_drivers.launch"/>
    </group>

    
    <node pkg="my_pkg" type="perception_node.py" name="perception"
          output="screen" respawn="true" respawn_delay="5">
      <param name="rate" value="30.0"/>
      <param name="frame_id" value="$(arg robot_name)_base_link"/>
      <remap from="~input_image" to="/$(arg robot_name)/camera/image_raw"/>
      <remap from="~output_detections" to="detections"/>
      
      <rosparam file="$(find my_pkg)/config/perception.yaml" command="load"/>
    </node>

  </group>

  
  <group if="$(arg debug)">
    <node pkg="rviz" type="rviz" name="rviz"
          args="-d $(find my_pkg)/rviz/debug.rviz"/>
    <node pkg="rqt_graph" type="rqt_graph" name="rqt_graph"/>
  </group>
</launch>

4. TF 変換ツリー

ルール：

• 各フレームの親は必ず 1 つです（グラフではなくツリー）。
• 静的変換には static_transform_publisher を使用します。
• 動的変換は一貫したレートでパブリッシュします。
• タイムスタンプは必ず正しく設定してください。

import tf2_ros

# Publishing transforms
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()  # CRITICAL: Use current time
t.header.frame_id = "odom"
t.child_frame_id = "base_link"
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.rotation = quaternion_from_euler(0, 0, theta)
br.sendTransform(t)

# Listening for transforms (with timeout and exception handling)
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)

try:
    trans = tf_buffer.lookup_transform(
        'map', 'base_link',
        rospy.Time(0),          # Get latest available
        rospy.Duration(1.0)     # Wait up to 1 second
    )
except (tf2_ros.LookupException,
        tf2_ros.ConnectivityException,
        tf2_ros.ExtrapolationException) as e:
    rospy.logwarn(f"TF lookup failed: {e}")

5. 長時間実行タスク向けの actionlib

import actionlib
from my_msgs.msg import PickPlaceAction, PickPlaceGoal, PickPlaceResult

# Server
class PickPlaceServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer(
            'pick_place',
            PickPlaceAction,
            execute_cb=self.execute,
            auto_start=False  # ALWAYS set auto_start=False
        )
        self.server.start()

    def execute(self, goal):
        feedback = PickPlaceFeedback()

        # Check for preemption INSIDE your loop
        for step in self.plan_steps(goal):
            if self.server.is_preempt_requested():
                self.server.set_preempted()
                return
            self.execute_step(step)
            feedback.progress = step.progress
            self.server.publish_feedback(feedback)

        result = PickPlaceResult(success=True)
        self.server.set_succeeded(result)

よくある落とし穴と障害モード

時刻の同期

# BAD: Comparing timestamps from different clocks
if camera_msg.header.stamp == lidar_msg.header.stamp:  # Almost never true

# GOOD: Use message_filters for approximate time sync
import message_filters
sub_cam = message_filters.Subscriber('/camera/image', Image)
sub_lidar = message_filters.Subscriber('/lidar/points', PointCloud2)
sync = message_filters.ApproximateTimeSynchronizer(
    [sub_cam, sub_lidar], queue_size=10, slop=0.05  # 50ms tolerance
)
sync.registerCallback(self.synced_callback)

コールバックのスレッド処理

# ROS1 uses a single-threaded spinner by default.
# Long-running callbacks BLOCK all other callbacks.

# BAD:
def callback(self, msg):
    result = self.expensive_computation(msg)  # Blocks for 2 seconds!
    self.pub.publish(result)

# GOOD: Use a MultiThreadedSpinner or process in a separate thread
rospy.init_node('my_node')
# ... setup ...
spinner = rospy.MultiThreadedSpinner(num_threads=4)
spinner.spin()

# Or use a processing thread:
import threading, queue
class MyNode:
    def __init__(self):
        self.work_queue = queue.Queue(maxsize=1)
        self.worker = threading.Thread(target=self._process_loop, daemon=True)
        self.worker.start()

    def callback(self, msg):
        try:
            self.work_queue.put_nowait(msg)  # Non-blocking
        except queue.Full:
            pass  # Drop old data

    def _process_loop(self):
        while not rospy.is_shutdown():
            msg = self.work_queue.get()
            result = self.expensive_computation(msg)
            self.pub.publish(result)

パラメーターサーバーのアンチパターン

# BAD: Hardcoded values
self.threshold = 0.5

# BAD: Global params without namespace
self.threshold = rospy.get_param('threshold', 0.5)  # Collides across nodes

# GOOD: Private params with defaults
self.threshold = rospy.get_param('~threshold', 0.5)

# GOOD: Dynamic reconfigure for runtime tuning
from dynamic_reconfigure.server import Server
from my_pkg.cfg import MyNodeConfig
self.dyn_server = Server(MyNodeConfig, self.dyn_callback)

ゼロコピー転送のための nodelet

同じプロセス内でノードが大量のデータ（画像、点群）を交換する場合、nodelet を使うとシリアライズのオーバーヘッドをなくせます。

// my_nodelet.h
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

class MyNodelet : public nodelet::Nodelet {
  virtual void onInit() {
    ros::NodeHandle& nh = getNodeHandle();
    ros::NodeHandle& pnh = getPrivateNodeHandle();
    // Use shared_ptr for zero-copy: pass pointers, not copies
    pub_ = nh.advertise<sensor_msgs::Image>("output", 1);
    sub_ = nh.subscribe("input", 1, &MyNodelet::callback, this);
  }
};
PLUGINLIB_EXPORT_CLASS(MyNodelet, nodelet::Nodelet)

パッケージ構成

my_robot_pkg/
├── CMakeLists.txt
├── package.xml
├── setup.py                    # For Python packages
├── config/
│   ├── robot_params.yaml       # Default parameters
│   └── dynamic_reconfigure/    # .cfg files
├── launch/
│   ├── robot.launch            # Top-level launcher
│   ├── drivers.launch          # Hardware drivers
│   └── perception.launch       # Perception pipeline
├── msg/                        # Custom message definitions
│   └── Detection.msg
├── srv/                        # Service definitions
│   └── GetPose.srv
├── action/                     # Action definitions
│   └── PickPlace.action
├── src/                        # C++ source
│   └── my_node.cpp
├── scripts/                    # Python nodes (executable)
│   └── perception_node.py
├── include/my_robot_pkg/       # C++ headers
│   └── my_node.h
├── rviz/                       # RViz configs
│   └── debug.rviz
├── urdf/                       # Robot model
│   └── robot.urdf.xacro
└── test/                       # Unit and integration tests
    ├── test_perception.py
    └── test_perception.test    # rostest launch file

デバッグツールキット

# Essential diagnostic commands
rostopic list                     # See all active topics
rostopic hz /camera/image_raw     # Check publish rate
rostopic bw /lidar/points         # Check bandwidth
rostopic echo /joint_states -n 1  # Inspect one message

rosnode list                      # Active nodes
rosnode info /perception          # Connections and subscriptions

roswtf                            # Automated diagnostics

rqt_graph                         # Visual node/topic graph
rqt_console                       # Log viewer with filtering

# TF debugging
rosrun tf tf_monitor              # Monitor TF tree health
rosrun tf view_frames             # Generate TF tree PDF
rosrun tf tf_echo map base_link   # Print transform continuously

# Bag file operations
rosbag record -a                  # Record everything (careful with disk!)
rosbag record /camera/image /tf   # Record specific topics
rosbag info recording.bag         # Inspect bag contents
rosbag play recording.bag --clock # Playback with simulated time

ROS1 から ROS2 への移行チェックリスト

移行を計画する際は、次の主な違いに注意してください。

• rospy → rclpy、roscpp → rclcpp
• catkin_make → colcon build
• roslaunch XML → ROS2 の Python launch ファイル
• グローバルパラメーターサーバー → ノードごとのパラメーター
• rospy.Rate → node.create_timer()
• 単一の roscore → DDS によるディスカバリー（中央のマスターなし）
• message_filters は両方で動作しますが、API は異なります。
• カスタムメッセージ：同じ .msg 形式ですが、ビルドシステムは異なります。
• Nodelet → ROS2 Components（プロセス内通信）
• dynamic_reconfigure → コールバック付きの ROS2 パラメーター

リーフノード（センサー、アクチュエーター）から移行を始め、内側へ進めてください。 移行中は ros1_bridge パッケージを使用して、両方のスタックを同時に実行してください。