0
点赞
收藏
分享

微信扫一扫

ROS中动态坐标变换(动态参数调节+动态坐标变换)

静态坐标变换与动态坐标变换的区别

动态坐标转换区别于静态坐标转换的唯一一点就是:坐标系的相对位置是可以发生变化的。那如何才可以发生变化呢?这就需要借助外力来改变坐标系间的相对位置:

静态坐标变换详见:

(1条消息) ROS中的静态坐标转换(解析+示例)_超级霸霸强的博客-CSDN博客icon-default.png?t=M0H8https://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文件配置

<buildtool_depend>catkin</buildtool_depend>  
<build_depend>geometry_msgs</build_depend>  
<build_depend>roscpp</build_depend>  
<build_depend>rospy</build_depend>  
<build_depend>std_msgs</build_depend>  
<build_depend>tf2</build_depend>  
<build_depend>tf2_geometry_msgs</build_depend>  
<build_depend>tf2_ros</build_depend>  
<build_depend>dynamic_reconfigure</build_depend>  
<build_depend>message_generation</build_depend>  
<build_export_depend>geometry_msgs</build_export_depend>  
<build_export_depend>roscpp</build_export_depend>  
<build_export_depend>rospy</build_export_depend>  
<build_export_depend>std_msgs</build_export_depend>  
<build_export_depend>tf2</build_export_depend>  
<build_export_depend>tf2_geometry_msgs</build_export_depend>  
<build_export_depend>tf2_ros</build_export_depend>  
<build_export_depend>dynamic_reconfigure</build_export_depend>  
<exec_depend>geometry_msgs</exec_depend>  
<exec_depend>roscpp</exec_depend>  
<exec_depend>rospy</exec_depend>  
<exec_depend>std_msgs</exec_depend>  
<exec_depend>tf2</exec_depend>  
<exec_depend>tf2_geometry_msgs</exec_depend>  
<exec_depend>tf2_ros</exec_depend>  
<exec_depend>dynamic_reconfigure</exec_depend>  
<exec_depend>message_runtime</exec_depend> 

动态参数调节: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博客icon-default.png?t=M0H8https://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 <sstream>  
  
void callback(static_tf::frame_changeConfig &obj, uint32_t level)  
{  
    std::ostringstream ostr;  
    ostr<<"translation:"<<obj.translation_x<<","<<obj.translation_y<<","<<obj.translation_z<<"\n\t";  
    ostr<<"rotation:"<<obj.rotation_x<<","<<obj.rotation_y<<","<<obj.rotation_z<<"\n\t";  
    ROS_INFO("frame info:%s",ostr.str().c_str());  
}  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    ros::init(argc,argv,"dynamic_pub");  
    dynamic_reconfigure::Server<static_tf::frame_changeConfig> server;  
    dynamic_reconfigure::Server<static_tf::frame_changeConfig>::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;  
} 

问题解析

动态坐标转换中的时间戳问题

当子坐标系频繁被创建(频繁变化)可能会导致如下错误提示:

这个错误是由于“子坐标系频繁变化导致表征坐标系创建时刻的时间戳频繁变化,最终导致子坐标系和参考坐标系的时间戳相差较大无法进行转换”而导致的。解决这类错误的方法:  

1. 实时更新参考坐标系的时间戳

2. 将子坐标系的时间戳置为无效时间戳

首先,我们要清楚子坐标系的变化是动态的,也就是说子坐标系变化可以发生在任何一个时刻,这就导致子坐标系的时间戳和参考坐标系的时间戳不可能一致。当我们赋予动态坐标系(这里子坐标系不断变化,因此子坐标系为动态坐标系)无效时间戳,那么参考坐标系的时间戳与子坐标系的时间戳不再进行比对,这就保证了不在抛异常。

基于上一个问题,为何参考坐标系的时间戳需要实时更新而子坐标系却不用?

首先,我们要明确这两个时间戳的含义不同: 

参考坐标系的时间戳:必须实时更新发布坐标系的信息及坐标系间的相对位置;

子坐标系的时间戳:表征着将“子坐标系中坐标点的坐标转换至参考坐标系中坐标点的坐标”的时刻,由于坐标转换并不是时刻发生,因此这个时间戳有无均可。

最终运行结果

在rviz中查看

在工作台查看

 

举报

相关推荐

0 条评论