仅做一下记录，因为这个我折腾了好久，防止忘记
参考
http://docs.ros.org/jade/api/tf/html/python/tf_python.html
http://docs.ros.org/kinetic/api/tf/html/python/tf_python.html#transformerros
http://docs.ros.org/api/geometry_msgs/html/index-msg.html
http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html

C++版本

监听tf的关系

try{this->tf_camera_to_robot.waitForTransform("/base_link", "/camera_link", ros::Time(0), ros::Duration(50.0));}catch (tf::TransformException &ex){ROS_ERROR("[adventure_tf]: (wait) %s", ex.what());ros::Duration(1.0).sleep();}try{this->tf_camera_to_robot.lookupTransform("/base_link", "/camera_link", ros::Time(0), (this->camera_to_robot_));}catch (tf::TransformException &ex){ROS_ERROR("[adventure_tf]: (lookup) %s", ex.what());}

其中


#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>/*** @brief camera_to_robot takes care of the transformation from camera to robot frame*/tf::StampedTransform camera_to_robot_;/*** @brief tf_camera_to_robot is an instance of tf_listener*/tf::TransformListener tf_camera_to_robot;/*** @brief obj_camera_frame, obj_robot_frame are instance of tf::Vector*/tf::Vector3 obj_camera_frame, obj_robot_frame;

使用

    // ROS_INFO("Image Message Received");float obj_x, obj_y;vMng_.get2DLocation(cv_ptr->image, obj_x, obj_y); #图像处理得到的2d坐标// Temporary Debuggingstd::cout<< " X-Co-ordinate in Camera Frame :" << obj_x << std::endl;std::cout<< " Y-Co-ordinate in Camera Frame :" << obj_y << std::endl;// 坐标变换，模型决定obj_camera_frame.setZ(-obj_y);obj_camera_frame.setY(-obj_x);obj_camera_frame.setX(0.45);利用前边得到的tf转换关系求得一个点在另一坐标系中的坐标obj_robot_frame = camera_to_robot_ * obj_camera_frame;

Python 版本

读取tf关系

        self.tf_camera_to_robot = tf.TransformListener()got_tf = Falsewhile not got_tf:try: # 如果查询得到的话，就将结果保存到camera_to_robot_，保存x,y,z和四元数一共7个值(self.trans,self.rot) = self.tf_camera_to_robot.lookupTransform("/base_link", "/camera_link", rospy.Time(0))rospy.loginfo("[adventure_tf]: got")got_tf = Trueexcept (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):rospy.loginfo("[adventure_tf]: (wait)")rospy.sleep(1.0)

import tf
from geometry_msgs.msg import PoseStamped, Pose, PointStamped, Point

使用

            rospy.loginfo("Image Message Received")# 调用vision_manager中的函数获取目标的位置，位置坐标是以摄像头中心点的位置作为0坐标obj_x, obj_y = self.vMng_.get2DLocation(cv_image)print(" X-Co-ordinate in Camera Frame : ", obj_x)print(" Y-Co-ordinate in Camera Frame : ", obj_y)# 通过坐标变换，将二维坐标变换为相机坐标系下的三维坐标，在本程序中与URDF建模有关系obj_camera_frame  = PointStamped(header=rospy.Header(stamp = rospy.get_rostime(),frame_id="/camera_link"), point=Point(x=0.45,y=-obj_x,z=-obj_y))print("obj_camera_frame",obj_camera_frame)# 关键的一行代码，将相机坐标系下的位置转化为base_link坐标系下的坐标obj_robot_frame = self.tf_camera_to_robot.transformPoint(target_frame="/base_link", ps=obj_camera_frame)print("obj_robot_frame_tf",obj_robot_frame)

ros 中对一个点利用tf工具旋转，python or C++相关推荐

平面中，一个点绕任意点旋转θ度后的坐标（C++实现）
平面中,一个点(x,y)绕任意点(dx,dy)顺时针旋转a度后的坐标 xx= (x - dx)*cos(-a) - (y - dy)*sin(-a) + dx ; yy= (x - dx)*sin(- ...
Heartrate：一个牛逼的工具，Python执行实时可视化
实时堆栈追踪(stacktrace) Heartate--如监测心率般追踪程序运行项目地址:https://github.com/alexmojaki/heartrate (点击阅读原文即可访问) ...
用 Python 的GUI 库 PySimpleGUI，写一个肥胖指数计算工具，Python开发就是这么简单
Python 的 GUI 库 PySimpleGUI,写一个简单的有界面的小工具很简单!这里我们来写一个计算肥胖指数(BMI)的小工具.首先,通过百度得到 BMI 指数的计算公式为: 体重 / (身高 ...
python代码覆盖率测试_利用coverage工具进行Python代码覆盖率测试
Coverage是一种用于统计Python代码覆盖率的工具,通过它可以检测测试代码对被测代码的覆盖率情况. Coverage安装 1.安装命令:pip install coverage 2.查看cov ...
ros中有关欧拉角和四元数互转的python写法
1.欧拉角转四元数如:pos = Pose() q = tf.transformations.quaternion_from_euler(0, 0, point.z) pos.orientation ...
python制作安装包_利用setupTool工具制作python安装包
编写完成的python库源代码,需要进行制作python安装包,方便更新版本.方法记录如下: 1.新建一个目录结构如下的目录: --src ----TestLibrary ----TestLibrar ...
求在平面直角坐标系中，一个点绕坐标原点旋转一定角度后点的坐标
如图,在平面直角坐标系中(忽略坐标轴上的刻度值),求坐标点P0(x0, y0)绕坐标原点旋转角度B后得到新的点的坐标P1(x1, y1).这是最基本的坐标点绕坐标原点旋转问题,通过这样的思想我们还可以 ...
ROS中的TF坐标变换
坐标变换是机器人系统中常用的基础功能,ROS中的坐标变换系统由TF功能包维护.一个机器人系统通常有很多三维坐标系,而且会随时间的推移发生变化,如世界坐标系(World Frame).基坐标系(Base ...
ROS学习-创建一个ROS msg和一个srv
目录 ROS msg和srv的介绍使用msg 创建一个msg 使用rosmsg命令使用srv 创建一个srv 使用rossrv命令 msg和srv的通用步骤使用Help 简要总结回顾 ROS m ...

ros 中对一个点利用tf工具旋转，python or C++

C++版本

Python 版本

ros 中对一个点利用tf工具旋转，python or C++相关推荐

最新文章

热门文章