坐标转换tf _坐标

在src文件下新建一个.cpp写入以下代码
// 头文件包含#include #include #include #include int main(int argc, char** argv){ros::init(argc, argv, "my_tf_listener");ros::NodeHandle node;// 调用服务生成第二只乌龟ros::service::waitForService("spawn");ros::ServiceClient add_turtle =node.serviceClient("spawn");turtlesim::Spawn srv;add_turtle.call(srv);// 创建一个话题发布器，发布第二只乌龟运行的速度ros::Publisher turtle_vel =node.advertise("turtle2/cmd_vel", 10);// 创建tf侦听器listenertf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){tf::StampedTransform transform;try{// 最多等待10秒，如果提前得到了坐标的转换信息，直接结束等待listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0));// 从tf上侦听turtle2在turtle1坐标系中的位置,将该位置存储在transform中listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0),transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}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));// 计算turtle2跑的速度和方向turtle_vel.publish(vel_msg);rate.sleep();}return 0;};
修改.txt文件
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES} )
编译：在.中添加以下代码：
运行
.
效果
一般一步实现了前面例子的乌龟跟随
运行
分析是跟着跑还是跟着跑？
添加一个坐标系为什么要添加坐标系？操作小记
在src//src下创建文件.cpp，写入以下代码：
#include #include int main(int argc, char** argv){ros::init(argc, argv, "my_tf_broadcaster");ros::NodeHandle node;tf::TransformBroadcaster br;tf::Transform transform;ros::Rate rate(10.0);while (node.ok()){transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );// 子坐标系在父坐标系中的初始姿态transform.setRotation( tf::Quaternion(0, 0, 0, 1) );// 发布一个以turtle1为父级命名为carrot1的坐标系br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}return 0;};
在.txt中添加如下代码：
add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
编译：
在.中添加如下代码：
运行.
运行命令查看tf树：
此时，操作键盘控制乌龟移动，发现跟之前的行为并没有什么区别
在.cpp文件中做以下修改:
try{listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10.0));listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0),transform);}
重新编译运行，查看效果跟之前的区别
之前，我们创建了一个固定关系的坐标系，它相对于父级是不会变化的。现在创建一个动态的坐标系
修改.cpp中对应的代码:
while (node.ok()){transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );transform.setRotation( tf::Quaternion(0, 0, 0, 1) );br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}
编译运行，观察现象
tf和时间操作小记
打开.cpp，原来代码为:
ros::Time(0)参数指的就是获取当前最新的转化关系，将代码做如下修改: