ROS2
TUTORIAL

ROS2 (Robot Operating System 2) adalah framework komunikasi terdistribusi untuk robotika. Dibangun di atas DDS (Data Distribution Service) untuk real-time, reliable communication.

ROS2 vs ROS1

Distribusi ROS2

# Install ROS2 Jazzy di Ubuntu 24.04
# 1. Set locale
locale-gen en_US en_US.UTF-8
update-locale LC_ALL=en_US.UTF-8

# 2. Add ROS2 apt repository
curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list

# 3. Install
apt update && apt install -y ros-jazzy-desktop
pip install colcon-common-extensions

# 4. Source dan setup workspace
source /opt/ros/jazzy/setup.bash
mkdir -p ~/robot_ws/src && cd ~/robot_ws
colcon build --symlink-install
source install/setup.bash

# 5. Test
ros2 run demo_nodes_cpp talker &
ros2 run demo_nodes_py listener

# Paket penting untuk navigasi:
apt install -y ros-jazzy-navigation2 ros-jazzy-nav2-bringup
apt install -y ros-jazzy-slam-toolbox ros-jazzy-robot-localization
apt install -y ros-jazzy-moveit ros-jazzy-ros2-control

Buat Package Baru

cd ~/robot_ws/src

# Python package
ros2 pkg create --build-type ament_python robot_brain \
  --dependencies rclpy std_msgs geometry_msgs nav_msgs sensor_msgs

# C++ package
ros2 pkg create --build-type ament_cmake robot_hal \
  --dependencies rclcpp std_msgs geometry_msgs

colcon build --packages-select robot_brain

#!/usr/bin/env python3
# Robot sensor publisher node
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32MultiArray
import numpy as np

class SonarPublisher(Node):
    def __init__(self):
        super().__init__('sonar_pub')
        self.pub = self.create_publisher(
            Float32MultiArray, '/sonar/distances', 10)
        self.create_timer(0.05, self.timer_cb)  # 20Hz
        self.serial = SerialReader('/dev/ttyUSB0')

    def timer_cb(self):
        dists = self.serial.read_distances()
        msg = Float32MultiArray(data=dists)
        self.pub.publish(msg)

class CmdVelSubscriber(Node):
    def __init__(self):
        super().__init__('cmd_executor')
        self.sub = self.create_subscription(
            Twist, '/cmd_vel', self.cmd_cb, 10)
        self.last_cmd = None
        self.watchdog = self.create_timer(0.5, self.watchdog_cb)

    def cmd_cb(self, msg):
        self.last_cmd = msg
        self.watchdog.reset()
        self.send_to_robot(msg.linear.x, msg.angular.z)

    def watchdog_cb(self):
        # Safety: stop if no cmd_vel for 0.5s
        self.send_to_robot(0, 0)

def main():
    rclpy.init()
    executor = rclpy.executors.MultiThreadedExecutor(num_threads=4)
    sonar = SonarPublisher()
    cmd = CmdVelSubscriber()
    executor.add_node(sonar); executor.add_node(cmd)
    try: executor.spin()
    finally: executor.shutdown()

Custom Service

# srv/SetGoal.srv:
# float32 x
# float32 y
# float32 theta
# ---
# bool success
# string message

from rclpy.node import Node
from robot_interfaces.srv import SetGoal

class GoalServer(Node):
    def __init__(self):
        super().__init__('goal_server')
        self.srv = self.create_service(SetGoal, 'set_goal', self.handle_goal)

    def handle_goal(self, req, res):
        self.get_logger().info(f"Goal: ({req.x:.2f}, {req.y:.2f}, {req.theta:.2f})")
        # Kirim goal ke Nav2...
        res.success = True
        res.message = f"Goal accepted: ({req.x:.2f}, {req.y:.2f})"
        return res

# Client call:
class GoalClient(Node):
    def __init__(self):
        super().__init__('goal_client')
        self.cli = self.create_client(SetGoal, 'set_goal')
        self.cli.wait_for_service(timeout_sec=5)

    def send_goal(self, x, y, theta):
        req = SetGoal.Request(); req.x=x; req.y=y; req.theta=theta
        future = self.cli.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return future.result()

Action Server (Long-running Task)

from rclpy.action import ActionServer, ActionClient
from robot_interfaces.action import Navigate

class NavActionServer(Node):
    def __init__(self):
        super().__init__('nav_action_server')
        self._server = ActionServer(self, Navigate, 'navigate', self.execute_cb)

    async def execute_cb(self, goal_handle):
        x, y = goal_handle.request.x, goal_handle.request.y
        feedback = Navigate.Feedback()
        while not self.goal_reached(x, y):
            dist = self.distance_to_goal(x, y)
            feedback.distance_remaining = dist
            goal_handle.publish_feedback(feedback)
            if goal_handle.is_cancel_requested():
                goal_handle.canceled()
                return Navigate.Result(success=False)
            await rclpy.task.sleep(0.1)
        goal_handle.succeed()
        return Navigate.Result(success=True)

# Python Launch File: launch/robot_bringup.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python import get_package_share_directory
import os

def generate_launch_description():
    use_sim = LaunchConfiguration('use_sim')
    nav2_params = os.path.join(get_package_share_directory('robot_nav'),'config','nav2_params.yaml')

    return LaunchDescription([
        DeclareLaunchArgument('use_sim', default_value='false'),

        # Serial Bridge
        Node(package='robot_hal', executable='serial_bridge',
             parameters=[{'port':'/dev/ttyUSB0','baud':115200}]),

        # LiDAR
        Node(package='rplidar_ros', executable='rplidar_node',
             parameters=[{'serial_port':'/dev/ttyUSB1'}]),

        # EKF Localization
        Node(package='robot_localization', executable='ekf_node',
             parameters=[nav2_params]),

        # Nav2 stack
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(get_package_share_directory('nav2_bringup'),
                             'launch','navigation_launch.py')),
            launch_arguments={'params_file':nav2_params}.items()),
    ])

Parameter YAML

# config/nav2_params.yaml
bt_navigator:
  ros__parameters:
    use_sim_time: false
    default_nav_to_pose_bt_xml: "navigate_w_replanning_time.xml"

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.3
      lookahead_dist: 0.6
      max_angular_accel: 3.2

import rclpy, tf2_ros
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class TFPublisher(Node):
    def __init__(self):
        super().__init__('tf_publisher')
        self.static_br = StaticTransformBroadcaster(self)
        self.dyn_br    = tf2_ros.TransformBroadcaster(self)
        self.publish_static_transforms()

    def publish_static_transforms(self):
        # LiDAR mounted 15cm forward, 30cm up
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'
        t.child_frame_id  = 'base_laser'
        t.transform.translation.x = 0.15
        t.transform.translation.z = 0.30
        q = tf_transformations.quaternion_from_euler(0,0,0)
        t.transform.rotation.x=q[0]; t.transform.rotation.y=q[1]
        t.transform.rotation.z=q[2]; t.transform.rotation.w=q[3]
        self.static_br.sendTransform(t)

    def publish_odom_tf(self, x, y, th):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'odom'; t.child_frame_id = 'base_link'
        t.transform.translation.x = x; t.transform.translation.y = y
        q = tf_transformations.quaternion_from_euler(0,0,th)
        t.transform.rotation.z=q[2]; t.transform.rotation.w=q[3]
        self.dyn_br.sendTransform(t)

# Step-by-step: Buat peta baru dengan SLAM Toolbox

# Terminal 1: bringup robot base
ros2 launch robot_bringup bringup.launch.py

# Terminal 2: SLAM Toolbox (online sync mode)
ros2 launch slam_toolbox online_sync_launch.py

# Terminal 3: RViz2 untuk visualisasi
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz

# Teleop untuk mapping
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Setelah peta terbentuk, simpan:
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap "{name: {data: 'my_map'}}"

Nav2 dengan Peta yang Sudah Ada

# Launch navigation dengan peta:
ros2 launch nav2_bringup bringup_launch.py \
  map:=/home/robot/maps/my_map.yaml \
  use_sim_time:=false

# Set initial pose via command:
ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{ header: {frame_id: map}, pose: { pose: {position: {x: 0.0, y: 0.0}, orientation: {w: 1.0} } } }" --once

import rclpy
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState
import numpy as np

rclpy.init()
robot = MoveItPy(node_name='arm_controller')
arm   = robot.get_planning_component('arm')
gripper = robot.get_planning_component('gripper')

def move_to_pose(x, y, z, qx=0, qy=0, qz=0, qw=1):
    from geometry_msgs.msg import Pose
    arm.set_start_state_to_current_state()
    target = Pose()
    target.position.x=x; target.position.y=y; target.position.z=z
    target.orientation.x=qx; target.orientation.y=qy
    target.orientation.z=qz; target.orientation.w=qw
    arm.set_goal_state(pose_stamped_msg=target, pose_link='end_effector')
    plan = arm.plan()
    if plan:
        robot.execute(plan.trajectory, controllers=[])
        return True
    return False

def pick_and_place(pick_xyz, place_xyz):
    move_to_pose(*pick_xyz)
    gripper.set_named_target('open'); gripper.plan(); robot.execute()
    # lower to pick height
    move_to_pose(pick_xyz[0], pick_xyz[1], pick_xyz[2]-0.05)
    gripper.set_named_target('close'); gripper.plan(); robot.execute()
    move_to_pose(*place_xyz)

# ros2_control hardware interface (C++):
// robot_hardware.hpp
#include <hardware_interface/system_interface.hpp>

class RobotHardware : public hardware_interface::SystemInterface {
public:
  CallbackReturn on_init(const HardwareInfo& info) override;
  CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
  std::vector<StateInterface> export_state_interfaces() override;
  std::vector<CommandInterface> export_command_interfaces() override;
  hardware_interface::return_type write(const rclcpp::Time&, const rclcpp::Duration&) override;
  hardware_interface::return_type read(const rclcpp::Time&, const rclcpp::Duration&) override;

private:
  serial::Serial serial_;
  std::vector<double> hw_commands_, hw_states_;
};

URDF/xacro Robot Description

<!-- robot_base.urdf.xacro -->
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="materials.xacro"/>

  <link name="base_link">
    <visual><geometry><cylinder radius="0.15" length="0.1"/></geometry></visual>
    <collision><geometry><cylinder radius="0.15" length="0.1"/></geometry></collision>
    <inertial><mass value="5.0"/><inertia ixx="0.1" iyy="0.1" izz="0.1" ixy="0" ixz="0" iyz="0"/></inertial>
  </link>

  <joint name="base_laser_joint" type="fixed">
    <parent link="base_link"/><child link="base_laser"/>
    <origin xyz="0.15 0 0.3" rpy="0 0 0"/>
  </joint>
</robot>

# ROS2 command line tools:

# Lihat semua topics
ros2 topic list
ros2 topic echo /cmd_vel
ros2 topic hz /scan  # check publish rate

# Lihat nodes
ros2 node list
ros2 node info /nav2_controller

# Services
ros2 service list
ros2 service call /set_goal robot_interfaces/srv/SetGoal "{x: 1.0, y: 0.5, theta: 0.0}"

# Bag recording (all topics)
ros2 bag record -a -o /bags/test_run
ros2 bag play /bags/test_run

# RViz2
ros2 run rviz2 rviz2

# rqt_graph - lihat node/topic graph
rqt_graph

# rqt_plot - plot numeric topics
rqt_plot /odom/pose/pose/position/x /odom/pose/pose/position/y

# TF tree visualize
ros2 run tf2_tools view_frames
ros2 run rqt_tf_tree rqt_tf_tree