ROS 실습 (90%)

실행을 위해서는 ROS, PCL-Cpp, PCL-Python이 모두 설치 되어 있어야 합니다. [Docker] 사용 하면 바로 실습이 가능합니다.

ROS에서는 메시지를 토픽(topic)이라고 지칭 합니다. 아래 예제에서는 라이다 센서에서 입력되는 값을 'velodyne_points'이라는 토픽으로 입력 받고, 이를 그대로 다시 /velodyne_points_new이라는 토픽으로 출력하는 예제를 작성해 보겠습니다.

1. PCL-Python & ROS

#!/usr/bin/env python3
# coding: utf-8
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2

import pcl
import pcl_helper


def callback(input_ros_msg):
    cloud = pcl_helper.ros_to_pcl(input_ros_msg)

    # 실행 코드 부분 
    print(cloud)

    cloud_new = pcl_helper.pcl_to_ros(cloud) #PCL을 ROS 메시지로 변경     
    pub.publish(cloud_new)

if __name__ == "__main__":
    rospy.init_node('tutorial', anonymous=True)
    rospy.Subscriber('/velodyne_points', PointCloud2, callback)

    pub = rospy.Publisher("/velodyne_points_new", PointCloud2, queue_size=1)

    rospy.spin()

실행

$ python Part00-Chapter03.py

시각화

$ rviz -d lidar_new_topic.rviz

도커를 활용 하는 경우 roscore, rviz는 Host PC에서 실행하고, python Part00-Chapter03.py는 Docker에서 실행 하면 됩니다. docker run -it --net=host옵션으로 Host PC-Docker간 통신이 가능합니다.

2. PCL-Cpp & ROS

2.1 msg 송수신

$ cd ~/catkin_src/src/
$ catkin_create_pkg pcl_cpp_tutorial pcl pcl_ros roscpp sensor_msgs  #CMakeList.txt 자동 생성

코드 작성

// src/example.cpp

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>       // std::cout
#include <typeinfo>       // operator typeid

ros::Publisher pub;

typedef pcl::PointXYZRGBA              PointXYZRGBA;

void 
cloud_cb (const sensor_msgs::PointCloud2 msg)
{
  std::cout << "msg is: " << typeid(msg).name() << '\n';

  //ros_pcl2 to pcl2
  //http://www.pcl-users.org/ROS-PointCloud2-to-CloudXYZRGBA-td4039503.html
  pcl::PCLPointCloud2 pcl_pc;
  pcl_conversions::toPCL(msg, pcl_pc);  
  std::cout << "cloud is: " << typeid(msg).name() << '\n';

  //pcl2 to pclxyzrgba
  pcl::PointCloud<PointXYZRGBA> input_cloud;
  pcl::fromPCLPointCloud2(pcl_pc, input_cloud);

  // Publish the data
  sensor_msgs::PointCloud2 output;
  output = msg;  
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/velodyne_points", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
  ros::spin ();
}

CMakeLists.txt

  find_package(catkin REQUIRED COMPONENTS
    roscpp
    pcl_conversions
    pcl_ros
  )

  catkin_package(
  INCLUDE_DIRS
  CATKIN_DEPENDS roscpp
                 pcl_conversions
                 pcl_ros
  )

  add_executable(roscpp_pcl_example src/roscpp_pcl_example.cpp)
  target_link_libraries(roscpp_pcl_example ${catkin_LIBRARIES})

"""
target_link_libraries(<executableTargetName>, <lib1>, <lib2>, ... <libN>)


Example:

add_executable(foo src/foo.cpp)
add_library(moo src/moo.cpp)
target_link_libraries(foo moo)  -- This links foo against libmoo.so
"""

package.xml

변경없이 기본 생성 파일 사용

  <build_depend>roscpp</build_depend>
  <build_depend>pcl_conversions</build_depend>
  <build_depend>pcl_ros</build_depend>
  <build_depend>libpcl-all-dev</build_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>pcl_conversions</run_depend>
  <run_depend>pcl_ros</run_depend>
  <run_depend>libpcl-all</run_depend>

RUN

catkin_make --directory ~/catkin_ws --pkg pcl_cpp_tutorial
source ~/devel/setup.sh
rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2

[Tip] Conversion

// Converting pcl::PCLPointCloud2 to pcl::PointCloud and reverse
#include <pcl/conversions.h>
pcl::PCLPointCloud2 point_cloud2;
pcl::PointCloud<pcl::PointXYZ> point_cloud;
pcl::fromPCLPointCloud2( point_cloud2, point_cloud);
pcl::toPCLPointCloud2(point_cloud, point_cloud2);

//Converting sensor_msgs::PCLPointCloud2 to sensor_msgs::PointCloud and reverse

#include <sensor_msgs/point_cloud_conversion.h>
sensor_msgs::PointCloud2 point_cloud2;
sensor_msgs::PointCloud point_cloud;

sensor_msgs::convertPointCloudToPointCloud2(point_cloud, point_cloud2);
sensor_msgs::convertPointCloud2ToPointCloud(point_cloud2, point_cloud);

//Converting a PCL pointcloud to a ROS pcl message/ ROS pcl message to PCL point cloud
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

//converting PCL pointcloud to a ROS PCL message:
sensor_msgs::PointCloud2 object_msg;
pcl::PointCloud::Ptr object_cloud;
object_cloud.reset(new pcl::PointCloud);
pcl::toROSMsg(*object_cloud.get(),object_msg );

//converting ROS PCL message to a PCL pointcloud:
pcl::PointCloud::Ptr received_cloud_ptr;
received_cloud_ptr.reset(new pcl::PointCloud);
sensor_msgs::PointCloud2ConstPtr pointcloud_msg;
pcl::fromROSMsg(*pointcloud_msg.get(), *received_cloud_ptr.get());

• Two ROS to PCL methods

• How to use a PCL tutorial in ROS