机器人中的坐标变换

机器人中的坐标变换
    参考：机器人学导论 - 第二章

TF 功能包
    5s之前，机器人头部坐标系相对于全局坐标系的关系是什么样的
    机器人夹取的物体相对于机器人中心坐标系的位置
    机器人中心坐标系相对于全局坐标系的位置

TF坐标变换实现
    广播TF变换
    监听TF变换

坐标变化例程
sudo apt-get install ros-kinetic-turtle-tf
kinetic@vm:~$ rospack find turtle_tf
/opt/ros/kinetic/share/turtle_tf

运行例程
kinetic@vm:~$ roslaunch turtle_tf turtle_tf_demo.launch
kinetic@vm:~$ rosrun turtlesim turtle_teleop_key
kinetic@vm:~$ rosrun tf view_frames
在 home目录下生成 frames.pdf

查找两只海龟坐标变化的关系，执行如下命令
kinetic@vm:~$ rosrun tf tf_echo turtle1 turtle2

rviz 可视环境下运行
kinetic@vm:~$ rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

实现TF广播器
        定义TF广播器(TransformBroadcaster)
        创建坐标变换值

        发布坐标变换值(sendTransform)

~/catkin_ws/src/learning_tf/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;
}

实现TF监听器步骤
        定义TF监听器(TransFormListener)
        查找坐标变换(waitForTransform、lookupTransform)

~/catkin_ws/src/learning_tf/src/turtle_tf_listener.cpp

#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::init(argc, argv, "my_tf_listener");
 
ros::NodeHandle node;
 
	// 通过服务器调用，产生第二只乌龟turtle2
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle = node.serviceClient("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
 
	// 定义 turtle2的速度控制发布器
ros::Publisher turtle_vel = node.advertise("turtle2/cmd_vel", 10);
 
	// tf 监听器
tf::TransformListener listener;
 
ros::Rate rate(10.0);
 
	while( node.ok() ) {
tf::StampedTransform transform;
		try 
{
			// 查找 turtle2 与 turtle1 的坐标变换
listener.waitForTransform( "turtle2", "turtle1", ros::Time(0), ros::Duration(3.0) );
listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform);
} 
		catch (tf::TransformException &ex)
{
ROS_INFO("%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;
}

修改 ~/catkin_ws/src/learning_tf/CMakeLists.txt
find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf
)
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries( turtle_tf_broadcaster  ${catkin_LIBRARIES} )
target_link_libraries( turtle_tf_listener  ${catkin_LIBRARIES} )

编译
kinetic@vm:~/catkin_ws$ catkin_make

创建luanch
kinetic@vm:~/catkin_ws/src/learning_tf$ mkdir launch
~/catkin_ws/src/learning_tf$ cd launch/start_demo_with_listener.launch

运行
kinetic@vm:~$ roslaunch learning_tf start_demo_with_listener.launch