ROS中动态坐标变换（动态参数调节+动态坐标变换）
目录
坐标变换详解
静态坐标变换与动态坐标变换的区别
项目文件解析
CMakelist文件的配置
Package.xml文件配置
动态参数调节：frame_change.cfg
动态参数调用+坐标系数据发送：dynamic_pub.cpp
坐标系数据的接收以及坐标点新坐标的计算：dynamic_sub.cpp
问题解析
动态坐标转换中的时间戳问题
为何子坐标系不设置时间戳，或者说“发布的坐标变换请求中时间戳无效”？
发布端的时间戳与订阅端的时间戳的含义
为什么每次循环必须更新订阅端的无效时间戳？
最终运行结果
在rviz中查看
在工作台查看
坐标变换详解
四元数：彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美_超级霸霸强的博客-CSDN博客
https://blog.csdn.net/weixin_45590473/article/details/122884112
旋转矩阵：
坐标变换最通俗易懂的解释（推到+图解）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122848202
静态坐标变换与动态坐标变换的区别
动态坐标转换区别于静态坐标转换的唯一一点就是：坐标系的相对位置是可以发生变化的。那如何才可以发生变化呢？这就需要借助外力来改变坐标系间的相对位置：
静态坐标变换详见：
(1条消息) ROS中的静态坐标转换（解析+示例）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122908060
项目文件解析
通过动态参数调节改变参数服务器中的参数值，然后将参数服务器中的参数读出来用于填充坐标系信息，最后，我们将坐标系信息通过static_tf话题传递给订阅者。订阅者接收到坐标系数据后结合坐标点坐标计算出基于参考坐标系的坐标点的新坐标。项目的结构如下所示：
CMakelist文件的配置
// 声明“将ROS项目文件”转换成C++/python项目文件所需的功能包
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
dynamic_reconfigure
message_generation
)
// 生成.cfg配置文件
generate_dynamic_reconfigure_options(
cfg/frame_change.cfg
# cfg/DynReconf2.cfg
)
// 声明编译所需功能包
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES static_tf
CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs tf2 tf2_geometry_msgs tf2_ros dynamic_reconfigure message_runtime
# DEPENDS system_lib
)
// 包含catkin官方库的头文件
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
// 编译为可执行文件
add_executable(dynamic_pub src/dynamic_pub.cpp)
add_executable(dynamic_sub src/dynamic_sub.cpp)
// 声明依赖关系
add_dependencies(dynamic_pub ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
// 链接可执行文件
target_link_libraries(dynamic_sub
${catkin_LIBRARIES}
)
target_link_libraries(dynamic_pub
${catkin_LIBRARIES}
) Package.xml文件配置
catkin
geometry_msgs
roscpp
rospy
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
dynamic_reconfigure
message_generation
geometry_msgs
roscpp
rospy
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
dynamic_reconfigure
geometry_msgs
roscpp
rospy
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
dynamic_reconfigure
message_runtime 动态参数调节：frame_change.cfg
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator();
# translation handler
gen.add("translation_x",double_t,0,"translation along with X axis",0,0,1);
gen.add("translation_y",double_t,1,"translation along with Y axis",0,0,1);
gen.add("translation_z",double_t,2,"translation along with Z axis",0,0,1);
# rotation handler
gen.add("rotation_x",double_t,3,"rotation angle around X axis",0,0,90);
gen.add("rotation_y",double_t,4,"rotation angle around Y axis",0,0,90);
gen.add("rotation_z",double_t,5,"rotation angle around Z axis",0,0,90);
exit(gen.generate("static_tf","dynamic_pub","frame_change")); 运行结果如下所示：
动态参数调节详见：
(1条消息) 详解ROS中动态参数调整与话题通信配合使用（原理+代码+示例）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122673314
动态参数调用+坐标系数据发送：dynamic_pub.cpp
#include "ros/ros.h"
#include "static_tf/frame_changeConfig.h"
#include "dynamic_reconfigure/server.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include
void callback(static_tf::frame_changeConfig &obj, uint32_t level)
{
std::ostringstream ostr;
ostr<<"translation:"< server;
dynamic_reconfigure::Server::CallbackType callbackfunc;
callbackfunc = boost::bind(&callback,_1,_2);
// parameter:boost function object instead of the point of that
server.setCallback(callbackfunc);
tf2_ros::TransformBroadcaster pub;
ros::NodeHandle nh("dynamic_pub");
ros::Rate rate(1);
while(ros::ok()){
geometry_msgs::TransformStamped data;
data.header.frame_id = "base_link";
data.header.stamp = ros::Time::now();
data.child_frame_id = "laser";
data.transform.translation.x = nh.param("translation_x",0);
data.transform.translation.y = nh.param("translation_y",0);
data.transform.translation.z = nh.param("translation_z",0);
tf2::Quaternion qtn;
qtn.setRPY(nh.param("rotation_x",0.0),nh.param("rotation_y",0.0),nh.param("rotation_z",0.0));
data.transform.rotation.w = qtn.getW();
data.transform.rotation.x = qtn.getX();
data.transform.rotation.y = qtn.getY();
data.transform.rotation.z = qtn.getZ();
// publish data
pub.sendTransform(data);
// callback function
ros::spinOnce();
// sleep
rate.sleep();
}
return 0;
} 坐标系数据的接收以及坐标点新坐标的计算：dynamic_sub.cpp
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_sub");
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate rate(1);
while(ros::ok()){
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 0.1;
point_laser.point.y = 0.5;
point_laser.point.z = 0.3;
try
{
geometry_msgs::PointStamped point_base_link = buffer.transform(point_laser,"base_link");
ROS_INFO("point based on %s frame:%f,%f,%f\n\t",point_base_link.header.frame_id.c_str(),
point_base_link.point.x,point_base_link.point.y,point_base_link.point.z);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
rate.sleep();
}
return 0;
} 问题解析
动态坐标转换中的时间戳问题
当子坐标系频繁被创建（频繁变化）可能会导致如下错误提示：
这个C++ exception如下所示：
这个错误的解释为：
An exception class to notify of bad frame number.
This is an exception class to be thrown in the case that a frame not in the graph has been attempted to be accessed. The most common reason for this is that the frame is not being published, or a parent frame was not set correctly causing the tree to be broken.
译文：由于找不到与坐标转换请求发布时间相对应的参考坐标系，导致类似下图所示的坐标转换关系树状图的分支断裂：
这个错误是由于“子坐标系与参考坐标系相对关系频繁变化导致表征坐标系创建时刻的时间戳频繁变化，最终导致在订阅端发布坐标转换请求时，系统无法在buffer订阅者对象的缓冲区内找到与其时间点相匹配的由发布端发送过来的坐标系转换关系。解决这类错误的方法：
1. 实时更新参考坐标系的时间戳
2. 将子坐标系的时间戳置为无效时间戳
我们要清楚以下几点：
1. 我们每更改一次子坐标系和参考坐标系的相对关系就意味着“我们重新创建了一次参考坐标系和子坐标系而且坐标系中的时间戳表示着坐标系转换关系发布的时间，即坐标系转换关系中蕴含的时间戳发生了改变”；
2. 执行坐标转换时，我们需要先发布坐标转换请求，此时也有一个时间戳表示坐标转换请求发布的时间点。如果这个时间戳为具体的时间点（时间戳中时间非零有效），系统会根据这个时间点去查找与相同时刻发布的坐标转换关系；
3. 由于坐标转换请求发布需要时间，因此坐标转换请求发布的时间不可能与坐标转换关系的发布时刻相同，这也就导致了异常的出现；
4. 我们只有屏蔽掉订阅端的坐标转换请求的时间戳，才可以不让系统按苛刻的时间点去buffer中搜寻已经发布的坐标转换关系。当系统根据发布的坐标转换关系的时间顺序查找距离订阅端的坐标转换请求最近的坐标转换关系时，异常就会不复存在。
为何子坐标系不设置时间戳，或者说“发布的坐标变换请求中时间戳无效”？
原因：因为每个监听器（订阅者对象）都有一个（Buffer对象）缓冲区，存储来自不同tf广播的所有坐标转换。当广播者发送转换时，转换进入缓冲区需要一段时间（通常是几毫秒）。所以，当“现在（now）”发布坐标转换请求时，从缓冲区提取不到当前发布的坐标变换信息，于是系统会按照时间顺序提取与执行转换请求发布时间距离最近的那一次坐标变换信息，所以应该使用ros::Time(0)而不是ros::Time::now()。因此，我们在订阅端一般使用ros::Time(0)而不用ros::Time::now()。
发布端的时间戳与订阅端的时间戳的含义
发布端的时间戳：表征着子坐标系和参考坐标系创建的时间；
订阅端的时间戳：坐标点转换请求发出的时间；
为什么每次循环必须更新订阅端的无效时间戳？
我们看如下两段程序：
我们上面提到过“当订阅端的时间戳为ros::Time(0)或者ros::Time()时，坐标转换请求一旦发布系统就会自动去按照时间顺序查找与之发布时间最近的由发布端发布的坐标转换关系”，那么我们想一下“无效时间戳”真的无效吗？不是的，由发布端发布的坐标转换关系与坐标转换请求相距的时间不可以太长，一旦差距时间太长也会报出如下错误：
因此， 将订阅端的时间戳设置为ros::Time(0)或者ros::Time()，可以使得系统在一定误差时间允许的范围之内去查找与坐标转换请求相近的已经被发布端发布的坐标转换关系。就像如下所示，我们将时间戳的更新放在了while循环中实时更新：
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_sub");
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate rate(1);
while(ros::ok()){
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 0.1;
point_laser.point.y = 0.5;
point_laser.point.z = 0.3;
try
{
geometry_msgs::PointStamped point_base_link = buffer.transform(point_laser,"base_link");
ROS_INFO("point based on %s frame:%f,%f,%f\n\t",point_base_link.header.frame_id.c_str(),
point_base_link.point.x,point_base_link.point.y,point_base_link.point.z);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
rate.sleep();
}
return 0;
} 坐标请求的发布时间与坐标转换信息接收的时间之间的时间差在允许的范围内。
添加try-catch块的意义
我们无论通过launch文件还是命令行窗口启动坐标系信息的发布/订阅节点，发布/订阅节点不可能同时启动，但是坐标系转换的实时性不允许“在同一时刻发布/订阅节点仅有一个被启动”。那我们就使用try-catch捕获异常，然后允许异常的存在，我们只在catch程序块中打印出异常并不进行终止程序的处理操作。不久之后，当订阅/发布端已经均处于运行状态，那么异常自然而然地会消失。
为何不使用一个CPP文件去完成坐标变换？
为何不可以使用一个CPP源文件去完成坐标变换？_超级霸霸强的博客-CSDN博客
https://blog.csdn.net/weixin_45590473/article/details/123015364
最终运行结果
在rviz中查看
在工作台查看
