源码如何做网站,网站开发开发,wordpress播放没声音,idc国外服务器目录 一、动态坐标变换#xff08;C实现#xff09;二、动态坐标变换#xff08;Python实现#xff09; 一、动态坐标变换#xff08;C实现#xff09;
所谓动态坐标变换#xff0c;是指两个坐标系之间的相对位置是变化的。比如机械臂末端执行器与 base_link 之间… 目录 一、动态坐标变换C实现二、动态坐标变换Python实现 一、动态坐标变换C实现
所谓动态坐标变换是指两个坐标系之间的相对位置是变化的。比如机械臂末端执行器与 base_link 之间移动机器人base_link与world之间。可以理解动态坐标关系是随时间变化的静态坐标关系即静态是动态对时间的微分。
我们使用ROS的 turtlesim 模拟一个移动机器人通过TF发布它相对世界坐标系的坐标。
在创建的 tf2_learning 包路径下的 src 目录中创建 dynamic_frame_broadcast.cpp 和 dynamic_frame_listen.cpp 修改 CMakeLists.txt 添加如下内容
add_executable(${PROJECT_NAME}_dynamic_broadcast src/dynamic_frame_broadcast.cpp)
add_executable(${PROJECT_NAME}_dynamic_listen src/dynamic_frame_listen.cpp)target_link_libraries(${PROJECT_NAME}_dynamic_broadcast${catkin_LIBRARIES}
)target_link_libraries(${PROJECT_NAME}_dynamic_listen${catkin_LIBRARIES}
)dynamic_frame_broadcast.cpp 实现广播子坐标系相对于父坐标系的动态坐标关系内容如下
/*** file: dynamic_frame_broadcast.cpp* brief: 动态的坐标系相对姿态发布* author: 万俟淋曦(1055311345qq.com)* date: 2023-12-30 22:47:33* modifier:* date: 2023-12-30 22:47:33*/#include ros/ros.h
#include turtlesim/Pose.h
#include tf2_ros/transform_broadcaster.h
#include geometry_msgs/TransformStamped.h
#include tf2/LinearMath/Quaternion.hvoid turtle1PoseCallback(const turtlesim::Pose::ConstPtr pose)
{// 创建 TF 广播器static tf2_ros::TransformBroadcaster broadcaster;// 创建 广播的数据geometry_msgs::TransformStamped tfs;// --头设置tfs.header.frame_id world;tfs.header.stamp ros::Time::now();// --坐标系idtfs.child_frame_id turtle1;// --坐标系相对信息设置tfs.transform.translation.x pose-x;tfs.transform.translation.y pose-y;tfs.transform.translation.z 0.0; // 二维, z为0// --欧拉角转四元数tf2::Quaternion qtn;qtn.setRPY(0, 0, pose-theta); // 二维, 只有偏航角tfs.transform.rotation.x qtn.getX();tfs.transform.rotation.y qtn.getY();tfs.transform.rotation.z qtn.getZ();tfs.transform.rotation.w qtn.getW();// 广播器发布数据broadcaster.sendTransform(tfs);
}int main(int argc, char **argv)
{// 初始化 ROS 节点ros::init(argc, argv, dynamic_frame_broadcast);// 创建 ROS 句柄ros::NodeHandle nh;// 创建订阅对象订阅乌龟的世界位姿ros::Subscriber sub nh.subscribeturtlesim::Pose(/turtle1/pose, 1000, turtle1PoseCallback);ros::spin();return 0;
}dynamic_frame_listen.cpp 订阅动态坐标转换关系并利用该关系将小乌龟坐标系下的坐标转换到 world 坐标系编辑内容如下
/*** file: dynamic_frame_listen.cpp* brief: 订阅动态坐标系并转换相应坐标* author: 万俟淋曦(1055311345qq.com)* date: 2023-12-31 11:55:40* modifier:* date: 2023-12-31 11:55:40*/
#include ros/ros.h
#include tf2_ros/transform_listener.h
#include tf2_ros/buffer.h
#include geometry_msgs/PointStamped.h
#include tf2_geometry_msgs/tf2_geometry_msgs.h // 包含TF坐标转换方法int main(int argc, char **argv)
{// 初始化 ROS 节点ros::init(argc, argv, dynamic_frame_listen);ros::NodeHandle nh;// 创建 TF 订阅节点tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Rate r(1);while (ros::ok()){// 生成一个坐标点, 模拟末端执行器坐标系下的点坐标(小乌龟坐标系下的坐标)geometry_msgs::PointStamped point_turtle1;point_turtle1.header.frame_id turtle1;point_turtle1.header.stamp ros::Time();point_turtle1.point.x 1;point_turtle1.point.y 1;point_turtle1.point.z 0;// 转换坐标点, 计算小乌龟坐标系下的坐标点在 world 下的坐标try{geometry_msgs::PointStamped point_base;point_base buffer.transform(point_turtle1, world);ROS_INFO(point_base: (%.2f, %.2f, %.2f), frame: %s, point_base.point.x, point_base.point.y, point_base.point.z,point_base.header.frame_id.c_str());}catch (const std::exception e){ROS_ERROR(%s, e.what());}r.sleep();ros::spinOnce();}return 0;
}编译后 首先开启小乌龟 rosrun turtlesim turtlesim_node 执行 rosrun tf2_learning tf2_learning_dynamic_broadcast 开始广播坐标此时打开rviz订阅TF看到TF树模型 输入命令rviz 在启动的 rviz 中设置 Fixed Frame 为 world 点击左下的 Add 按钮在弹出的窗口中选择 TF 组件即可显示坐标关系如下 继续执行命令rosrun tf2_learning tf2_learning_listen可以看到转换后的坐标以及所属父坐标系 执行命令 rosrun turtlesim turtle_teleop_key 使用键盘控制小乌龟移动可以看到 rviz以及转换后的坐标都在同步动态变化。 二、动态坐标变换Python实现
在创建的 tf2_learning 包路径下 src 目录的同级创建一个 scripts 目录在这里存储脚本如python脚本我们创建 dynamic_frame_broadcast.py 以实现坐标广播编辑内容如下
#! /usr/bin/env pythonimport rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped# 回调函数处理
def turtle1PoseCallback(pose):# 创建 TF 广播器broadcaster tf2_ros.TransformBroadcaster()# 创建 广播的数据(通过 pose 设置)tfs TransformStamped()tfs.header.frame_id worldtfs.header.stamp rospy.Time.now()tfs.child_frame_id turtle1tfs.transform.translation.x pose.xtfs.transform.translation.y pose.ytfs.transform.translation.z 0.0qtn tf.transformations.quaternion_from_euler(0,0,pose.theta)tfs.transform.rotation.x qtn[0]tfs.transform.rotation.y qtn[1]tfs.transform.rotation.z qtn[2]tfs.transform.rotation.w qtn[3]# 广播器发布数据broadcaster.sendTransform(tfs)if __name__ __main__:# 初始化 ROS 节点rospy.init_node(dynamic_frame_broadcast_py)# 订阅 /turtle1/pose 话题消息sub rospy.Subscriber(/turtle1/pose, Pose, turtle1PoseCallback)rospy.spin()创建 dynamic_frame_listen.py 以订阅静态坐标转换关系并利用该关系将雷达坐标系的点转换到 world 坐标系编辑内容如下
#! /usr/bin/env pythonimport rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ __main__:# 初始化 ROS 节点rospy.init_node(dynamic_frame_listen_py)# 创建 TF 订阅对象buffer tf2_ros.Buffer()listener tf2_ros.TransformListener(buffer)rate rospy.Rate(1)while not rospy.is_shutdown(): # 创建一个 radar 坐标系中的坐标点point_source PointStamped()point_source.header.frame_id turtle1point_source.header.stamp rospy.Time.now()point_source.point.x 10point_source.point.y 2point_source.point.z 3try:# 转换坐标点, 计算小乌龟坐标系下的坐标点在 world 下的坐标point_target buffer.transform(point_source,world,rospy.Duration(1))rospy.loginfo(point_target: (%.2f, %.2f, %.2f), frame: %s,point_target.point.x,point_target.point.y,point_target.point.z,point_target.header.frame_id)except Exception as e:rospy.logerr(%s, e)rate.sleep()
