Lecture5 - ROS 2 Topic and Examples

cd ~/ros2_ws/src/du2023-ros2
./setup_scripts.sh

# CMakeLists.txt 수정 (주석)
colcon build --packages-select src_gazebo
source install/local_setup.bash

# CMakeLists.txt 수정 (주석 해제)
colcon build --packages-select src_gazebo

colcon build --packages-select src_odometry
colcon build --packages-select src_controller
source install/local_setup.bash

ros2 launch src_gazebo empty_world.launch.py

$ ros2 interface show geometry_msgs/msg/Twist
# This expresses velocity in free space broken into its linear and angular parts.

Vector3  linear
Vector3  angular

# Terminal 1
ros2 launch src_gazebo empty_world.launch.py
# Terminal 2
rqt

roslaunch smb_gazebo smb_gazebo.launch

sudo apt install ros-foxy-ros1-bridge -y

# Terminal 1 - ros bridge
ros2 run ros1_bridge dynamic_bridge

# Terminal 2 - ros2 foxy
ros2 run teleop_twist_keyboard teleop_twist_keyboard

├─  wall
   ├── model.config
   └── model.sdf

$ gedit ~/.gazebo/gui.ini

[geometry]
x=0
y=0
[model_paths]
filenames=/home/kimsooyoung/<your-folder-location>

if 'GAZEBO_MODEL_PATH' in os.environ:
    os.environ['GAZEBO_MODEL_PATH'] += ":" + gazebo_model_path
else:
    os.environ['GAZEBO_MODEL_PATH'] = gazebo_model_path

def generate_launch_description():
    ...

    # gazebo
    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
    pkg_path = os.path.join(get_package_share_directory('src_gazebo'))
    world_path = os.path.join(pkg_path, 'worlds', '여러분의-world-file.world')
    ...

    # Start Gazebo server
    start_gazebo_server_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
        launch_arguments={'world': world_path}.items()
    )

colcon build --packages-select src_gazebo
source install/local_setup.bash

ros2 launch src_gazebo empty_world.launch.py

# Terminal 1
ros2 launch src_gazebo empty_world.launch.py

# Terminal 2
cd ~/ros2_ws
colcon build --packages-select py_topic_tutorial
source install/local_setup.bash
ros2 run py_topic_tutorial topic_pub_node

import random

from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node

class TwistPubNode(Node):

    def __init__(self):
        super().__init__('twist_pub_node')

        self.get_logger().info(
            f'TwistPubNode Created at {self.get_clock().now().to_msg().sec}'
        )

        # self.twist_publisher = self.create_publisher(Twist, "twist_topic", 10)
        self.twist_publisher = self.create_publisher(Twist, '/cmd_vel', 10)

        ...
        self.create_timer(0.2, self.timer_callback)

    def timer_callback(self):
        """Timer will run this function periodically."""
        msg = Twist()

        # Fill in msg with compatible values
        msg.linear.x = 0.5
        msg.angular.z = 1.0

        self.twist_publisher.publish(msg)

# Terminal 1
$ ros2 launch src_gazebo empty_world.launch.py

# Terminal 2
$ ros2 run py_topic_tutorial topic_sub_node
[INFO] [1672494602.141200628] [scan_sub_node]:
msg.ranges[0] : inf
msg.ranges[30] : inf
msg.ranges[60] : 0.7975894212722778
msg.ranges[90] : inf
msg.ranges[119] : inf
...

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import LaserScan

class ScanSubNode(Node):

    def __init__(self):

        super().__init__('scan_sub_node')

        queue_size = 10  # Queue Size

        self.pose_subscriber = self.create_subscription(
            LaserScan, 'scan', self.sub_callback, queue_size
        )

    def sub_callback(self, msg):
        """Timer will run this function periodically."""
        self.get_logger().info(f' \
            \nmsg.ranges[0] : {msg.ranges[0]} \
            \nmsg.ranges[30] : {msg.ranges[30]} \
            \nmsg.ranges[60] : {msg.ranges[60]} \
            \nmsg.ranges[90] : {msg.ranges[90]} \
            \nmsg.ranges[119] : {msg.ranges[119]} \
        ')

    # gazebo
    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
    pkg_path = os.path.join(get_package_share_directory('src_gazebo'))
    # world_path = os.path.join(pkg_path, 'worlds', 'empty_world.world')
    world_path = os.path.join(pkg_path, 'worlds', 'wall_world.world')

# Terminal 1
$ ros2 launch src_gazebo empty_world.launch.py

# Terminal 2
$ ros2 run py_topic_tutorial parking_node
...
[INFO] [1672563255.067421128] [parking_node]: Distance from Front Object : 0.6280616521835327
[INFO] [1672563255.173734128] [parking_node]: Distance from Front Object : 0.5546504259109497
[INFO] [1672563255.281276729] [parking_node]: Distance from Front Object : 0.5124648809432983
[INFO] [1672563255.389067729] [parking_node]: ==== Parking Done!!! ====
...

class ParkingNode(Node):

    def __init__(self):

        super().__init__('parking_node')
        queue_size = 10  # Queue Size

        self.twist_publisher = self.create_publisher(Twist, 'cmd_vel', queue_size)
        self.scan_subscriber = self.create_subscription(
            LaserScan, 'scan', self.sub_callback, queue_size
        )

    def sub_callback(self, msg):

        twist_msg = Twist()
        distance_forward = msg.ranges[60]

        if distance_forward > 0.5:
            self.get_logger().info(f'Distance from Front Object : {distance_forward}')
            twist_msg.linear.x = 0.5
            self.twist_publisher.publish(twist_msg)
        else:
            self.get_logger().info('==== Parking Done!!! ====\n')
            twist_msg.linear.x = 0.0
            self.twist_publisher.publish(twist_msg)

# 예제 실행 준비
sudo apt install ros-foxy-rosbag2
cd ~/ros2_ws
colcon build --packages-select py_cv_tutorial
source install/local_setup.bash

# 터미널 1 – 데이터 위치로 이동하여 ros2 bag 실행
cd <데이터를 저장한 위치>
ros2 bag play quadrupped_train.bag_0.db3

# 터미널 2 – image_view 실행
ros2 run rqt_image_view rqt_image_view

# 터미널 1
$ cd <bag 파일 위치>
$ ros2 bag play quadruped_train.bag_0.db3
[INFO] [1667077909.362223100] [rosbag2_storage]: Opened database 'quadrupped_train/quadrupped_train.bag_0.db3' for READ_ONLY.

# 터미널 2
$ ros2 run py_cv_tutorial img_sub
[INFO] [1667077909.613586300] [image_subscriber]: Receiving video frame
[INFO] [1667077910.934147000] [image_subscriber]: Receiving video frame
...

from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library

class ImageSubscriber(Node):

    def __init__(self):

        # Used to convert between ROS and OpenCV images
        self.br = CvBridge()
        ...

    def listener_callback(self, data):
        ...
        # Convert ROS Image message to OpenCV image
        current_frame = self.br.imgmsg_to_cv2(data, "bgr8")
        edge_frame = self.hough_transform(current_frame)

$ ros2 run py_cv_tutorial rosbag2_to_timedimg
/home/kimsooyoung/djhrd_ws/quadrupped_train/quadrupped_train.bag_0.db3
saved: color_1666796292992515592.png
saved: color_1666796293035428479.png
saved: color_1666796293079139778.png
saved: color_1666796293120768031.png

def main(args=None):

    # Change below roots to your ros2 bag locations
    ROOT_DIR = "/home/kimsooyoung/djhrd_ws/quadrupped_train"
    FILENAME = "/quadrupped_train.bag_0.db3"
    DESCRIPTION = "color_"