点群の並進移動および回転移動の実装をご紹介します。
【ROS＋PCL】点群の並進移動＆回転
点群を扱うときに、形を崩さずに並進／回転移動させたいことはよくあります。
この記事では、ROSとPCLを使った実装例をご紹介します。
目次
仕様
Subscribeした点群に対して、並進／回転移動を適用してPublishする仕様とします。
ただし、回転の考え方は複数あり、本記事では、以下の2種類を実装します。
- ロール・ピッチ・ヨーをもとにする回転：
回転前の座標に固定されたx、y、z軸回りに回転させる - オイラー角をもとにする回転：
ロボット座標に固定されたx、y、z軸回りに回転させる（i.e. 各軸の回転ごとに回転軸は回転する）
以下に、実装における、入出力とパラメータを記載します。
入力（Subscribe）
- sensor_msgs::PointCloud2：入力点群
出力（Publish）
- sensor_msgs::PointCloud2：変換された点群
パラメータ
- double：x軸方向の移動距離 [m]
- double：y軸方向の移動距離 [m]
- double：z軸方向の移動距離 [m]
- double：ロール（またはx軸回りの）回転角度 [deg]
- double：ピッチ（またはy軸回りの）回転角度 [deg]
- double：ヨー（またはz軸回りの）回転角度 [deg]
環境構築
今回のプログラムを動かすためには、以下をインストールする必要があります。
- ROS
- PCL
（Dockerを使う場合）
Dockerを使う場合は、以下のDockerfileを例として参考にしてください。
########## Pull ########## FROM ros:noetic ########## ROS setup ########## RUN mkdir -p ~/catkin_ws/src && \ cd ~/catkin_ws && \ /bin/bash -c "source /opt/ros/noetic/setup.bash; catkin_make" && \ echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc && \ echo "export ROS_WORKSPACE=~/catkin_ws" >> ~/.bashrc ########## Requirements ########## RUN apt-get update && \ apt-get install -y \ libpcl-dev \ ros-noetic-pcl-conversions \ ros-noetic-rviz
実装
ディレクトリ構造
~/catkin_ws/src/pc_transform/
┣ src/
┃ ┣ rpy_transform.cpp
┃ ┗ eular_transform.cpp
┣ launch/
┃ ┗ transform.launch
┗ CMakeLists.txt
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(pc_transform)
add_compile_options(-std=c++14 -O2 -g -Wall)
find_package(catkin REQUIRED COMPONENTS
roscpp
tf
)
find_package(PCL)
catkin_package(
)
include_directories(
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
add_executable(tf_transform src/tf_transform.cpp)
target_link_libraries(tf_transform
${catkin_LIBRARIES}
)
add_executable(eular_transform src/eular_transform.cpp)
target_link_libraries(eular_transform
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)
add_executable(rpy_transform src/rpy_transform.cpp)
target_link_libraries(rpy_transform
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)
rpy_transform.cpp
ロール・ピッチ・ヨーをもとに回転させる実装です。
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
class PcRpyTransform{
private:
/*node handle*/
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
/*subscriber*/
ros::Subscriber sub_;
/*publisher*/
ros::Publisher pub_;
/*parameter*/
std::string publish_frame_;
double x_m_, y_m_, z_m_;
double r_deg_, p_deg_, y_deg_;
public:
PcRpyTransform();
void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg);
void transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
void publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
double degToRad(double deg);
};
PcRpyTransform::PcRpyTransform()
: nh_private_("~")
{
std::cout << "--- pc_rpy_transform ---" << std::endl;
/*parameter*/
nh_private_.param("publish_frame", publish_frame_, std::string(""));
std::cout << "publish_frame_ = " << publish_frame_ << std::endl;
nh_private_.param("x_m", x_m_, 0.0);
std::cout << "x_m_ = " << x_m_ << std::endl;
nh_private_.param("y_m", y_m_, 0.0);
std::cout << "y_m_ = " << y_m_ << std::endl;
nh_private_.param("z_m", z_m_, 0.0);
std::cout << "z_m_ = " << z_m_ << std::endl;
nh_private_.param("r_deg", r_deg_, 0.0);
std::cout << "r_deg_ = " << r_deg_ << std::endl;
nh_private_.param("p_deg", p_deg_, 0.0);
std::cout << "p_deg_ = " << p_deg_ << std::endl;
nh_private_.param("y_deg", y_deg_, 0.0);
std::cout << "y_deg_ = " << y_deg_ << std::endl;
/*subscriber*/
sub_ = nh_.subscribe("/point_cloud", 1, &PcRpyTransform::callbackPC, this);
/*publisher*/
pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_cloud/transformed", 1);
}
void PcRpyTransform::callbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr pc (new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*msg, *pc);
transformPC(pc);
publication(pc);
}
void PcRpyTransform::transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
Eigen::Affine3f transformatoin = pcl::getTransformation(x_m_, y_m_, z_m_, degToRad(r_deg_), degToRad(p_deg_), degToRad(y_deg_));
pcl::transformPointCloud(*pc, *pc, transformatoin);
}
void PcRpyTransform::publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
sensor_msgs::PointCloud2 ros_pc;
pcl::toROSMsg(*pc, ros_pc);
if(publish_frame_ != "") ros_pc.header.frame_id = publish_frame_;
pub_.publish(ros_pc);
}
double PcRpyTransform::degToRad(double deg)
{
double rad = deg / 180.0 * M_PI;
rad = atan2(sin(rad), cos(rad));
return rad;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "pc_rpy_transform");
PcRpyTransform pc_rpy_transform;
ros::spin();
}
eular_transform.cpp
オイラー角をもとに回転させる実装です。
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
class PcEularTransform{
private:
/*node handle*/
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
/*subscriber*/
ros::Subscriber sub_;
/*publisher*/
ros::Publisher pub_;
/*parameter*/
std::string publish_frame_;
double x_m_, y_m_, z_m_;
double x_deg_, y_deg_, z_deg_;
public:
PcEularTransform();
void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg);
void transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
void publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc);
double degToRad(double deg);
};
PcEularTransform::PcEularTransform()
: nh_private_("~")
{
// rpy_transform.cppと同じ
}
void PcEularTransform::callbackPC(const sensor_msgs::PointCloud2ConstPtr &msg)
{
// rpy_transform.cppと同じ
}
void PcEularTransform::transformPC(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
Eigen::Quaternionf rotation =
Eigen::AngleAxisf(degToRad(x_deg_), Eigen::Vector3f::UnitX())
* Eigen::AngleAxisf(degToRad(y_deg_), Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(degToRad(z_deg_), Eigen::Vector3f::UnitZ());
Eigen::Vector3f offset(x_m_, y_m_, z_m_);
pcl::transformPointCloud(*pc, *pc, offset, rotation);
}
void PcEularTransform::publication(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
// rpy_transform.cppと同じ
}
double PcEularTransform::degToRad(double deg)
{
// rpy_transform.cppと同じ
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "pc_eular_transform");
PcEularTransform pc_eular_transform;
ros::spin();
}
transform.launch
<launch> <!-- global rosparam --> <param name="use_sim_time" value="true"/> <!-- rosbag --> <node pkg="rosbag" type="play" name="player" args="--clock $(env HOME)/rosbag/demo.bag"/> <!-- rviz --> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find pc_transform)/rviz_config/demo.rviz"/> <!-- main --> <node pkg="pc_transform" type="rpy_transform" name="rpy_transform" output="screen"> <!-- <node pkg="pc_transform" type="eular_transform" name="eular_transform" output="screen"> --> <remap from="/point_cloud" to="/demo_points"/> <remap from="/point_cloud/transformed" to="/demo_points/transformed"/> <param name="x_m" type="double" value="1.0"/> <param name="y_m" type="double" value="2.0"/> <param name="z_m" type="double" value="3.0"/> <param name="x_deg" type="double" value="10.0"/> <param name="y_deg" type="double" value="20.0"/> <param name="z_deg" type="double" value="30.0"/> </node> </launch>
使い方
ビルド
cd ~/catkin_ws catkin_make
実行
roslaunch pc_transform transform.launch
さいごに
ROSとPCLを使って、3次元点群の並進／回転移動を実装してみました。
参考になれば幸いです。
関連記事
本記事では指定した値で点群を並進移動／回転させました。
一方で、tfが設定されており、点群を座標変換したい場合は、tf::TransformListener::transformPointCloudを使うと便利です。
Ad.
コメント