[ROS 연구 노트] (10) ROS의 좌표계 관리 시스템

1. 로봇의 좌표 변환

TF 기능 패키지는 모든 좌표계를 관리하는 데 사용됩니다. 10 초 이내에 모든 좌표계 간의 관계를 기록 할 수 있으며 로봇의 중심 좌표계를 기준으로 파지 된 물체의 위치를 ​​표시 할 수 있습니다.

2. 예 : 실험 후 작은 거북이

1. 작은 거북이가 따라옵니다

두 거북이 나타난 후 한 거북은 중심점에, 다른 거북은 아래에 나타납니다. 중앙에있는 거북은 움직일 수 있도록 제어 할 수 있으며, 아래의 거북은 우리가 제어하는 ​​거북을 자동으로 따라 이동합니다.

sudo apt-get install ros-noetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
#rosrun turtlesim turtle_teleop_key

그중 roslaunch는 스크립트 파일을 시작하고 많은 파일을 시작하는 데 사용되며
Noetic은 ROS 버전 번호입니다.

따라가는 거북이를 제어하려면 터미널의 화살표 키를 누르십시오.

ubuntu20.04의 noetic 버전에 오류가있는 경우 다음 방법을 참조하여 해결할 수 있습니다.

cd /usr/bin
sudo rm -r python		# 有的可能没有这个文件，就省略这一步
sudo cp python3 python

2. tf 관계보기

rosrun tf view_frames

pdf 파일이 생성 될 때까지 5 초 동안 기다렸다가 열어서 현재 시스템에서 tf 좌표의 위치 관계를 확인합니다.

세계는 전역 좌표계이고 다른 turtle1과 turtle2는 두 거북이의 좌표계입니다. 루틴의 목적은 두 좌표계가 좌표에서 겹치도록하는 것입니다.

이 단계에서 오류가있는 경우 오류를보고 한 파일을 수정해야합니다.
sudo gedit /opt/ros/noetic/lib/tf/view_frames
88 행 print(vstr)위에 문장을 추가하기 vstr=str(vstr)만하면됩니다.

3. tf_echo 좌표 관계

rosrun tf tf_echo turtle1 turtle2

turtle2 좌표계가 turtle1 좌표계로 변환되는 방식을 설명하는 두 좌표계 간의 관계를 출력합니다. 평행 이동 및 회전 회전 (쿼터니언, 라디안, 회전을 설명하는 세 가지 각도)이 포함됩니다.

4. rviz 3D 시각화 디스플레이 플랫폼

rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz

먼저 왼쪽의 고정 프레임을 세계로 변경하십시오.

왼쪽 하단의 추가를 클릭하여 TF를 추가하여 TF의 위치 관계를 표시합니다.

거북이의 움직임을 제어하면 그림의 두 좌표계가 움직이는 것을 볼 수 있습니다.

3. TF 좌표계 방송 및 모니터링 프로그래밍 구현

1. 기능 패키지 만들기

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

2. tf 브로드 캐스터 코드 생성

learning_tf/src/카탈로그를 열고turtle_tf_broadcaster.cpp

내용은 다음과 같습니다.

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 * REFERENCE:www.guyuehome.com
 */

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器
	static tf::TransformBroadcaster br;

	// 初始化tf数据
	tf::Transform transform;
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "my_tf_broadcaster");

	// 输入参数作为海龟的名字
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}

	turtle_name = argv[1];

	// 订阅海龟的位姿话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数
	ros::spin();

	return 0;
};

3. 리스너 리스너 코드 생성

마찬가지로 turtle_tf_listener.cpp콘텐츠로 다른 하나 를 만듭니다.

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 * REFERENCE:www.guyuehome.com
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	// 创建发布turtle2速度控制指令的发布者
	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
				                        transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
				                      pow(transform.getOrigin().y(), 2));
		turtle_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};

4. tf 브로드 캐스터 및 리스너 코드 컴파일 규칙 구성

구성에서 learning_tf, CMakeLists.txt그림의 위치에 다음 코드를 추가합니다

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

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

즉, 두 개의 cpp 파일이 두 개의 실행 파일로 컴파일 된 다음 라이브러리가 연결됩니다.

5. 컴파일

cd ~/catkin_ws
catkin_make

6. 프로그램 실행

다음 프로그램의 각 줄에는 별도의 터미널이 필요합니다.

roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key