机器人中的坐标变换
位置描述: B P = [ p x p y p z ] ^BP=\begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} BP=⎣⎡pxpypz⎦⎤表示在坐标系{ B }中的 P P P点的位置, p x , p y , p z p_x,p_y,p_z px,py,pz分别表示沿 x , y , z x,y,z x,y,z轴的坐标
旋转矩阵: B A R = [ A X ^ A Y ^ A Z ^ ] = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] ^A_BR=\begin{bmatrix} ^A\hat{X} & ^A\hat{Y} & ^A\hat{Z} \end{bmatrix}=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} &r_{33} \end{bmatrix} BAR=[AX^AY^AZ^]=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤ 表示从坐标系{ B }旋转到坐标系{ A }的旋转矩阵,其中 X ^ , Y ^ , Z ^ \hat{X},\hat{Y},\hat{Z} X^,Y^,Z^表示坐标系 B { B } B主轴方向的单位矢量
平移向量: A P B O R G = [ p x b − p x a p y b − p y a p z b − p z a ] ^AP_{BORG}=\begin{bmatrix} p_{xb}-p_{xa} \\ p_{yb}-p_{ya} \\ p_{zb}-p_{za} \end{bmatrix} APBORG=⎣⎡pxb−pxapyb−pyapzb−pza⎦⎤ 表示将从坐标系{ A }的原点到坐标系{ B }的原点的平移向量
变换矩阵: B A P = [ B A R A P B O R G 0 0 0 1 ] ^A_BP= \bigg [ \begin{array}{c|c} ^A_BR&^AP_{BORG}\\ \hline 0\space\space0\space\space0&1 \\ \end{array} \bigg] BAP=[BAR0 0 0APBORG1]
A P = B A T B P ^AP=^A_BT^BP AP=BATBP
tf 功能包
1、能确定某个坐标在全局坐标中的位置
2、能确定物体坐标相对于机器人中心坐标系的位置
3、能确定机器人中心坐标系相对于全局坐标系的位置
tf 坐标变换可以通过广播TF变换、监听TF变换实现
小海龟跟随实验
$ sudo apt-get install ros-melodic-turtle-tf
$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key
$ rosrun tf view_frames
// 命令行工具
$ rosrun tf tf_echo turtle1 turtle2
// 可视化工具
$ rosrun rviz rviz -d`rospack find turtle_tf`/rviz/turtle_rviz.rv
$ cd ~/catkin_new_ws/src/
$ catkin_create_pkg my_tf roscpp rospy turtlesim
// 定义 TF 广播器
// 创建坐标变换值
// 发布坐标变换
// 该例程产生tf数据,并计算、发布turtle2的速度指令
#include
#include
#include
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster my_broadcaster;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); // 只有平面x,y 没有z
tf::Quaternion quat;
quat.setRPY(0, 0, msg->theta); //R:翻滚角 与 P:俯仰角都为0 只有偏航角
transform.setRotation(quat);
// 广播world与海龟坐标系之间的tf数据
my_broadcaster.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;
};
// 定义 TF 监听器
// 查找坐标变换
// 该例程监听tf数据,并计算、发布turtle2的速度指令
#include
#include
#include
#include
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("/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())
{
// 获取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 = 2.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 2.0 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
// 添加包
find_package(...tf...)
// 设置需要编译的代码和生成的可执行文件
add_executable(my_turtle_tf_broadcaster src/my_turtle_tf_broadcaster.cpp)
add_executable(my_turtle_tf_listener src/my_turtle_tf_listener.cpp)
// 设置链接库
target_link_libraries(my_turtle_tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(my_turtle_tf_listener ${catkin_LIBRARIES})
// 打开第一个终端
$ cd ~/catkin_new_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
// 打开第二个终端
rosrun turtlesim turtlesim_node
// 打开第三个终端
$ cd ~/catkin_new_ws
$ source devel/setup.bash
$ rosrun my_tf my_turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
// 打开第四个终端
$ cd ~/catkin_new_ws
$ source devel/setup.bash
$ rosrun my_tf my_turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
// 打开第五个终端
$ cd ~/catkin_new_ws
$ source devel/setup.bash
$ rosrun my_tf my_turtle_tf_listener
// 打开第六个终端
$ rosrun turtlesim turtle_teleop_key
pkg:节点所在的功能包名称
type:节点的可执行文件名称
name:节点运行时的名称
output、respawn、required、ns、args
/
// 设置 ROS 系统运行中的参数,存储在参数服务器中。
// name:参数名 value:参数值
// 加载参数文件中的多个参数:
// launch 文件内部的局部变量,仅用于 launch 文件使用
// name:参数名 value:参数值
// 调用
// 重映射 ROS 计算图资源的命名
// from:原命名 to:映射之后的命名
// 包含其他 launch 文件,类似 C 语言中的头文件包含
// file:包含的其他 launch 文件路径
// 打开方式
$ roscore
$ rqt_console
// 打开方式
$ roscore
$ rqt_graph
// 打开方式
$ roscore
$ rqt_plot
// 打开方式
$ roscore
$ rqt_image_view
打开方式
$ roscore
$ rviz
// 打开方式
$ roscore
$ gazebo