個人ブログ：http://www.chenjianqu.com/
オリジナルリンク：http://www.chenjianqu.com/show-73.html
座標変換
座標変換のためのコンポーネントの多数が存在するロボット、ロボットにおける基本的な概念であり、各成分は、異なる座標系を有します。座標系変換行列によって達成することができるシステムの並進及び回転、並進及び回転座標別することによって得ることができます。この部分についての知識を見つけることができます：3次元空間剛体運動。
ROSは、座標変換
以下はPR2ロボット座標系のリストです。
基本的に座標系どちらが抽象化されたツリー（TF木）ことができます。
ROSロボットモデルは、リンクシステム、上記フレームに対応する各リンク、すなわちA座標系と呼ばれる成分を多く含んでいます。ロボットの各リンク上に敷設複合l及び多くの参照フレームは、様々な座標系の間の関係を維持し、それがTFツリー処理を頼ることが必要であり、それぞれの座標系との間の通信を維持します。
各円は表すフレームを対応するロボットに、リンクの任意の2つのフレームの間には、ユニコムでなければなりません。両者のそれぞれの間のフレームた放送の詳細は、右側の2つのフレーム間の通信を可能にするために、中間体は、変換座標情報ノードを解放しなければなりません。接続性を維持するためにメッセージを投稿するノードが存在しない場合には、2つのフレーム間の接続が遮断されます。2つのフレーム間の相対運動は、放送事業者は、関連する情報を公開する場合は、放送事業者は、出版社です。
中間座標系次のように、TransformStampde.msgある放送局解放メッセージです。
std_mags/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w上面的消息中：header定义了序号，时间，frame的名称，child_frame的名称。这两个frame之间的变换由geometry_msgs/Transform来定义，Vector3三维向量表示平移，Quaternion四元数表示旋转。
一个机器人系统里有很多不同frame之间的broadcaster在发布坐标变换的tf，他们拼接得到tf tree。TF tree的数据类型是tf/tfMessage.msg（老版）和tf2_msgs/TFMessage.msg（新版）。标准格式如下：
首先header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．Vector3三维向量表示平移，Quaternion四元数表示旋转
geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w如上，tf tree就是TransformStamped数组。
ROS TF常见功能
TF功能包中提供了一些常用功能用来调试和创建TF变换。
1.tf_monitor
该节点可以查看坐标系广播的发布状态。
rosrun tf tf_monitor #查看所有广播器的发布
rosrun tf tf_monitor source_frame target_frame #查看两个坐标系的广播2.tf_echo
该节点可以查看指定坐标系之间的变换关系，如：
rosrun tf tf_echo turtle3 turtle1结果：
At time 1576577139.888
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.952, 0.306]
in RPY (radian) [-0.000, -0.000, 2.519]
in RPY (degree) [-0.000, -0.000, 144.307]3.static_transfrm_publisher
发布两个坐标系之间的静态坐标变换。
4.view_frames
生成pdf显示TF树。如：
rosrun tf view_frames结果：
Turtlesim功能包
turtlesim是ROS自带的一个用于新手教学的功能包，核心就是用键盘控制小乌龟运动。该功能包有两个节点，详细的信息可以参阅官方文档：http://wiki.ros.org/turtlesim 。
TF的turtlesim跟随实例
这里实现的是在turtlesim里面创建多个乌龟，每个乌龟的parent_frame都是世界坐标系，在TF tree上查找某两个乌龟之间的坐标变换，从而实现乌龟的跟随。
1.创建功能包
在工作空间创建功能包，依赖：roscpp tf turtlesim。这里创建的功能包为tf_test
2.节点：生成乌龟。
启动turtlesim turtlesim_node之后，只默认创建一个乌龟，名为/turtle1。这里通过调用turtlesim::Spawn服务，在仿真环境中可以创建新的乌龟。节点tf_test/src/turtle_create.cpp：
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "node_create_turtle");
ros::NodeHandle node;
// 通过服务调用，产生第二只乌龟turtle2
ros::service::waitForService("spawn"); //等待spawn服务可用
ros::ServiceClient add_turtle =node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;//默认参数
add_turtle.call(srv);
return 0;
};
3.节点：发布坐标转换
前面说过，每个frame之间必须要有一个广播节点发布坐标转换，这里的乌龟的坐标系在tf tree上面是世界坐标系的孩子。广播器如下(tf_test/src/turtle_tf_broadcaster.cpp)：
#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::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 发布坐标变换
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化节点
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅乌龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};从官方文档可知，每个乌龟会发布消息turtle_name/pose，其内容turtlesim/Pose如下：
//乌龟在世界坐标系中的坐标
float32 x
float32 y
//乌龟在世界坐标系中，绕z轴旋转的角度
float32 theta
//线速度和角速度
float32 linear_velocity
float32 angular_velocity回调函数里面，首先定义一个广播器br，接着定义一个坐标变换transform。setOrigin函数设置平移变换，设坐标变换A->B，那么该函数就是设置B坐标系原点在A坐标系的坐标。接着定义一个四元数q。setRPY即把B坐标系的相对旋转角度设置为转换四元数。最后发布坐标变换。
4.节点：监听坐标变换，并将坐标转换用于控制乌龟运动。
通过当监听到广播器发出的坐标转换时，查找两个乌龟间的坐标变换关系，并将该坐标变换用于控制乌龟的运动，使一只乌龟能跟上另一只乌龟。因此这里还定义一个topic，叫做turtle_name/cmd_vel用于控制乌龟的运动。代码tf_test/src/turtle_tf_listener.cpp：
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
if (argc != 3){
ROS_ERROR("need turtle name as argument");
return -1;
};
//A跟随B,获取乌龟A和乌龟B的名字
std::string turtleA = argv[1];
std::string turtleB = argv[2];
//定义turtle的速度控制发布器
ros::Publisher turtle_vel =node.advertise<geometry_msgs::Twist>(turtleA+"/cmd_vel", 10);
//tf监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
// 查找turtleA与turtleB的坐标变换
listener.waitForTransform("/"+turtleA, "/"+turtleB, ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/"+turtleA, "/"+turtleB, ros::Time(0), transform);
}
catch (tf::TransformException &ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
//并发布速度控制指令，使turtle2向turtle1移动
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;
};代码的解释可以看注释。
5.启动文件。
tf_test/launch/turtlesim_tf_test.launch
<launch>
<!-- 乌龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!--再另外创建两只乌龟-->
<node pkg="tf_test" type="turtle_create" name="turtle_create2" />
<node pkg="tf_test" type="turtle_create" name="turtle_create3" />
<!-- 三只乌龟的tf广播 -->
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle3" name="turtle3_tf_broadcaster" />
<!-- 监听tf广播，并且控制turtle移动 -->
<node pkg="tf_test" type="turtle_tf_listener" args="turtle2 turtle1" name="listener2" />
<node pkg="tf_test" type="turtle_tf_listener" args="turtle3 turtle2" name="listener3" />
</launch>
6.编译并运行。
tf_test / 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})
add_executable(turtle_create src/turtle_create.cpp)
target_link_libraries(turtle_create ${catkin_LIBRARIES})その後、直接実行するコンパイラの後にコンパイルワークスペースには、結果を確認するには、ファイルを起動します。
結果は以下の通りであります：
あなたは左の亀の動きを制御するためにキーボードを見ることができ、カメは、途中で亀を追跡する権利を、左側の真ん中に従ってください。
リファレンス
[0] CASソフトウェア、重いドイツの諜報会社。MOOCの中国大学は---コースのノートを「アンドロイドOS入門」。Https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/
[1] Huchun徐.ROSロボット開発プラクティス。機械工業プレス
