Navigation을 위해 Intel의 Realsense D435와 T265를 사용한다.
D435는 3차원 데이터를 취득하기 위함이고 T265는 로봇의 현재 위치와 방향을 상대적으로 얻기 위함이다.
Octomap생성 시 Odometry를 활성화시키면 위의 그림처럼 자주색 공이 나타나고 카메라의 방향을 따라 붉은 화살표가 생기는 것을 확인할 수 있다.
즉 Odometry로 넘어가는 메시지의 정보만 파악하면 현재 카메라의 위치와 방향을 알 수 있게 된다.
메시지 이름은 '/t265/odom/sample' 이며 메시지 타입은 'nav_msgs/Odometry'에 해당한다.
메시지로 전달되는 데이터는 아래와 같다.
pose정보를 보면 position와 orientation이 나눠져 있으며 위치와 방향에 대한 파악이 가능한 것을 확인할 수 있다.
나중에는 Tracking Camera를 사용하지 않고 Visual SLAM에 사용되는 Visual Inertial Odometry를 적용하여
오직 RGB-D Camera로만 위치인식을 진행해봐야겠다.
그럼 본격적으로 해당 메시지를 통해 작업을 수행할 수 있도록 x, y, yaw 정보를 받아오는 노드를 작성해보겠다.
우선 nav_msgs/Odometry 메시지가 어떻게 구성되있는지 살펴보자.
opt/ros/kinetic/include에 해당 정보가 존재하니 이를 include하여 작업을 진행하면 될 것으로 보인다.
해당 메시지의 데이터는 pose와 Twist로 구성되는데 pose는 x, y, z로 구성된 position 정보와 x, y, z, w로 구성된 orientation 정보로 이루어져 있다.
위치인식에 필요한 Particle Filter에서는 로봇의 x, y, yaw정보가 요구되므로 이 정보들을 출력해줄 수 있는 노드를 작성해보겠다.
우선 소스코드는 아래와 같이 작성하였다.
#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
std::cout<<"position x: "<<msg->pose.pose.position.x<<" y: "<<msg->pose.pose.position.y<<" z: "<<msg->pose.pose.position.z<<std::endl;
std::cout<<"orientation x: "<<msg->pose.pose.orientation.x<<" y: "<<msg->pose.pose.orientation.y<<" z: "<<msg->pose.pose.orientation.z<<" w: "<<msg->pose.pose.orientation.w<<std::endl;
std::cout<<"twist linear x: "<<msg->twist.twist.linear.x <<" y: "<<msg->twist.twist.linear.y<<" z: "<<msg->twist.twist.linear.z<<std::endl;
std::cout<<"twist angular x: "<<msg->twist.twist.angular.x<<" y: "<<msg->twist.twist.angular.y<<" z: "<<msg->twist.twist.angular.z<<std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_listener");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/t265/odom/sample", 1000, poseCallback);
ros::spin();
}
/t265/odom/sample 메시지를 받아 와 poseCallback 함수를 실행하는 노드로
position, orientation, twist_linear, twist_angular 값들을 모두 확인할 수 있도록 작성하였다.
하지만 위의 코드와 같이 작성하면 /t265/odom/sample 메시지가 너무 빠르게 전달되므로
값을 확인하기에 무리가 있어 이를 아래와 같이 일부 수정하여 딜레이를 갖도록 구성하였다.
위의 코드를 통해 확인해보니 출력되는 position의 x, y, z는 아래와 같은 구성을 갖고있다.
그리고 Orientation은 Quaternion 좌표계로 3차원 공간상의 이동을 나타내는 것으로 보이나 Quaternion에 대한 이해가 부족하기에 자세히 설명하긴 어렵다.
결론적으로 보면 Quaternion 좌표계는 3차원의 벡터로 표시될 수 있는 방향성을 나타내어주고
ROS의 TF 라이브러리에서는 이를 오일러각(Roll, Pitch, Yaw)으로 변환할 수 있는 함수를 제공해주므로
우리는 위치인식에 필요한 x, y, yaw정보를 취득할 수 있다.
최종적인 코드는 아래와 같다.
#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include <tf/tf.h>
#include <geometry_msgs/Pose2D.h>
ros::Publisher pub_pose_;
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
geometry_msgs::Pose2D pose2d;
pose2d.x = msg->pose.pose.position.x;
pose2d.y = msg->pose.pose.position.y;
tf::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w
);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
pose2d.theta = yaw;
std::cout<<"yaw: "<<yaw<<" pitch: "<<pitch<<" roll: "<<roll<<std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_listener");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/t265/odom/sample", 1000, poseCallback);
ros::Rate loop_rate(0.5);
while(nh.ok()){
ros::spinOnce();
loop_rate.sleep();
}
}