4.1tf坐标系统
4.1.1ROS中的坐标系管理系统
-
坐标变换
-
TF（transform）功能包作用
- 管理所有坐标系，封装底层的坐标变换运算
- 有时间属性，默认记录10秒内机器人坐标系的位置关系
-
TF坐标变换实现方法
-
广播和监听
-
坐标系间的数据变换例子
-
以小海龟为例
#安装tf工具包 yxq@yxq-ThinkPad-S2:~$ sudo apt-get install ros-kinetic-turtle-tf #启动tf的example launch文件 yxq@yxq-ThinkPad-S2:~$ roslaunch turtle_tf turtle_tf_demo.launch #打开键盘控制 yxq@yxq-ThinkPad-S2:~$ rosrun turtlesim turtle_teleop_key #记录坐标系及关系到PDF文件中，存储在terminal所处目录下 yxq@yxq-ThinkPad-S2:~$ rosrun tf view_frames Listening to /tf for 5.000000 seconds Done Listening dot - graphviz version 2.38.0 (20140413.2041) Detected dot version 2.38 frames.pdf generated #查看坐标关系 yxq@yxq-ThinkPad-S2:~$ rosrun tf tf_echo turtle1 turtle2 At time 1610867201.206 - Translation: [0.000, 0.000, 0.000] - Rotation: in Quaternion [0.000, 0.000, -0.498, 0.867] in RPY (radian) [0.000, 0.000, -1.043] in RPY (degree) [0.000, 0.000, -59.782] #可视化（然后在rviz中fixed frame选择world，再add TF） yxq@yxq-ThinkPad-S2:~$ rosrun rviz rviz `rospack find turtle_tf`/rviz/turtle_rviz.rviz [ INFO] [1610867382.945435080]: rviz version 1.12.17 [ INFO] [1610867382.945489749]: compiled against Qt version 5.5.1 [ INFO] [1610867382.945500077]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1610867383.250226732]: Stereo is NOT SUPPORTED [ INFO] [1610867383.250339375]: OpenGl version: 3 (GLSL 1.3).
4.1.2 tf坐标系广播与监听的编程实现
-
创建功能包
yxq@yxq-ThinkPad-S2:~/catkin_ws/src$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim -
编写TF广播器代码
- 定义TF广播器（TransformBroadcaster）
- 创建坐标变换值
- 发布坐标变换（sendTransform）
/*********************************************************************** Copyright 2020 GuYueHome (www.guyuehome.com). ***********************************************************************/ /** * 该例程产生tf数据，并计算、发布turtle2的速度指令 */ #include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg) { // 创建tf的广播器 static tf::TransformBroadcaster br; // 初始化tf数据 tf::Transform transform; //平移参数 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); //旋转参数 tf::Quaternion q; q.setRPY(0, 0, msg->theta); //以x，y，z轴旋转 transform.setRotation(q); //旋转参数 // 广播world与海龟坐标系之间的tf数据（插入到树中） br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); } int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, "my_tf_broadcaster"); // 输入参数作为海龟的名字 if (argc != 2) { ROS_ERROR("need turtle name as argument"); return -1; } turtle_name = argv[1]; // 订阅海龟的位姿话题 ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); // 循环等待回调函数 ros::spin(); return 0; }; -
编写TF监听器代码
- 定义TF监听器（TransformListener）
- 查找坐标变换（waitForTransform、lookupTransform）
/*********************************************************************** Copyright 2020 GuYueHome (www.guyuehome.com). ***********************************************************************/ /** * 该例程监听tf数据，并计算、发布turtle2的速度指令 */ #include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h> int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, "my_tf_listener"); // 创建节点句柄 ros::NodeHandle node; // 请求产生turtle2 ros::service::waitForService("/spawn"); ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn"); turtlesim::Spawn srv; add_turtle.call(srv); // 创建发布turtle2速度控制指令的发布者 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10); // 创建tf的监听器 tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()) { // 获取turtle1与turtle2坐标系之间的tf数据 tf::StampedTransform transform; try { //关键 //ros::Time(0)：当前时间 ros::Duration(3.0)等待时间（超时提示错误） transform：结果保留在该变量中 listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令 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)); turtle_vel.publish(vel_msg); rate.sleep(); } return 0; }; -
编写CMakeList.txt
-
编译运行
$ catkin_make $ roscore $ rosrun turtlesim turtlesim_node $ sss #__name:=turtle1_tf_broadcaster 重映射（注意空格） /turtle1 main函数参数 $ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1 $ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2 $ rosrun learning_tf turtle_tf_listener $ rosrun turtlesim turtle_teleop_key
4.2launch启动文件的使用方法
-
Launch文件语法(参考http://wiki.ros.org/roslaunch/XML)
*lanuch文件可以自动运行roscore
<launch> <!-- 作用：启动某个节点 --> <!-- pkg:节点所在功能报名称 type：节点的可执行文件名称 name：节点运行时的名称（会取代掉程序中初始化的节点名 --> <!-- ouput：控制某个节点是否要把它的日志信息打印到终端里 respwn：挂掉是否重启 required：是否一定要某个节点启动 ns：命名空间 args：给每个节点输入参数--> <node pkg="package-name" type="executable-name" name="node-name"/> <!-- 作用：设置ROS系统运行中的参数，存储在参数服务器中 --> <!-- name:参数名 value：参数值 --> <param name="output_frame" value="odom"/> <!-- 作用：加载参数文件中的多个参数 --> <rosparam file="params.yaml" command="load" ns="params"/> <!-- 作用：launch文件内部的局部变量，仅限于launch文件使用 --> <!-- name:参数名 default：参数值 --> <arg name="arg-name" default="arg-value"/> <!-- 调用 --> <param name="foo" value="$(arg arg-name)"/> <node name="node" pkg="package" type="type" args="$(arg arg-name)"/> <!-- 作用：重映射，重映射ROS计算图资源的命名 --> <!-- from:原命名 to：映射之后的命名 --> <remap from="/turtlebot/cmd_vel" to="cmd_vel"/> <!-- 作用：嵌套，包含其他launch文件，类似C语言中的头文件包含 --> <!-- file：包含的其他launch文件路径 --> <include file="$(dirname)/other.launch"/> </launch> -
常用标签的使用方法
-
新建功能包
-
编写xml
-
simple.launch
<!-- simple.launch --> <launch> <node pkg="learning_topic" type="person_subscriber" name="listener" output="screen"/> <node pkg="learning_topic" type="person_publisher" name="talker" output="screen"/> </launch>- turtlesim_parameter_config.launch
<!-- turtlesim_parameter_config.launch --> <launch> <param name="/turtle_number" value="2"/> <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"> <param name="turtle_name1" value="Tom"/> <param name="turtle_name2" value="Jerry"/> <rosparam file="$(find learning_launch)/config/param.yaml" command="load"/> </node> <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/> </launch>roslaunch之后再利用rosparam
$ rosparam list /background_b /background_g /background_r /rosdistro /roslaunch/uris/host_yxq_thinkpad_s2__35475 /rosversion /run_id /turtle_number /turtlesim_node/A /turtlesim_node/B /turtlesim_node/group/C /turtlesim_node/group/D /turtlesim_node/turtle_name1 /turtlesim_node/turtle_name2 $ rosparam get turtle_number 2- start_tf_demo_c++.launch
<!-- start_tf_demo_c++.launch --> <launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_listener" name="listener" /> </launch>- turtlesim_remap.launch
<!-- turtlesim_remap.launch --> <launch> <include file="$(find learning_launch)/launch/simple.launch" /> <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"> <remap from="/turtle1/cmd_vel" to="/cmd_vel"/> </node> </launch>roslaunch之后再查看topic
$ rostopic list /cmd_vel /person_info /rosout /rosout_agg /turtle1/color_sensor /turtle1/pose -
4.3常用可视化工具的使用
-
Qt工具箱
yxq@yxq-ThinkPad-S2:~$ rqt_ rqt_bag rqt_dep rqt_image_view rqt_plot rqt_console rqt_graph rqt_logger_level rqt_shell-
rqt_graph
见2.4 ros命令行工具的使用
-
rqt_console
-
显示日志信息
-
-
rqt_plot
-
绘制数据曲线
报错：
yxq@yxq-ThinkPad-S2:~$ rqt_plot /usr/lib/python2.7/dist-packages/matplotlib/axis.py:1015: UserWarning: Unable to find pixel distance along axis for interval padding of ticks; assuming no interval padding needed. warnings.warn("Unable to find pixel distance along axis " /usr/lib/python2.7/dist-packages/matplotlib/axis.py:1025: UserWarning: Unable to find pixel distance along axis for interval padding of ticks; assuming no interval padding needed. warnings.warn("Unable to find pixel distance along axis " Traceback (most recent call last): File "/opt/ros/kinetic/lib/python2.7/dist-packages/rqt_plot/data_plot/mat_data_plot.py", line 107, in resizeEvent self.figure.tight_layout() File "/usr/lib/python2.7/dist-packages/matplotlib/figure.py", line 1754, in tight_layout rect=rect) File "/usr/lib/python2.7/dist-packages/matplotlib/tight_layout.py", line 349, in get_tight_layout_figure pad=pad, h_pad=h_pad, w_pad=w_pad) File "/usr/lib/python2.7/dist-packages/matplotlib/tight_layout.py", line 128, in auto_adjust_subplotpars fig.transFigure.inverted()) File "/usr/lib/python2.7/dist-packages/matplotlib/transforms.py", line 1775, in inverted self._inverted = Affine2D(inv(mtx), shorthand_name=shorthand_name) File "/usr/lib/python2.7/dist-packages/numpy/linalg/linalg.py", line 526, in inv ainv = _umath_linalg.inv(a, signature=signature, extobj=extobj) File "/usr/lib/python2.7/dist-packages/numpy/linalg/linalg.py", line 90, in _raise_linalgerror_singular raise LinAlgError("Singular matrix") numpy.linalg.linalg.LinAlgError: Singular matrix报错原因：在Matplotlib官网可以发现，kinetic中的python2.7和Matplotlib是不兼容的，并且不再支持Python2。
解决办法：我们可以安装 另一个可视化工具，PyQtGraph。
$ sudo apt install python-pip $ pip install --upgrade pip $ pip install pyqtgraph
-
-
rqt_image_view
-
显示摄像头图像
-
-
rqt
-
综合工具
-
-
-
rviz
-
rviz作用
-
rviz界面
-
-
gazebo
-
使用方法（第一次运行要很久）
yxq@yxq-ThinkPad-S2:~$ roslaunch gazebo_ gazebo_dev gazebo_plugins gazebo_ros_control gazebo_msgs gazebo_ros yxq@yxq-ThinkPad-S2:~$ roslaunch gazebo_ros elevator_world.launch rubble_world.launch empty_world.launch shapes_world.launch mud_world.launch willowgarage_world.launch range_world.launch yxq@yxq-ThinkPad-S2:~$ roslaunch gazebo_ros willowgarage_world.launch
-
5 进阶展望
5.1课程总结与进阶功略
这个部分我就没有记笔记啦，可以看看最后一讲
1560
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
