古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望

4.1tf坐标系统

4.1.1ROS中的坐标系管理系统

  • 坐标变换

    古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第1张图片

  • TF(transform)功能包作用

    • 管理所有坐标系,封装底层的坐标变换运算
    • 有时间属性,默认记录10秒内机器人坐标系的位置关系
  • TF坐标变换实现方法

  • 广播和监听

  • 坐标系间的数据变换例子

    古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第2张图片

  • 以小海龟为例

    #安装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 
    #include 
    #include 
    
    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 
    #include 
    #include 
    #include 
    
    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启动文件的使用方法

古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第3张图片

  • Launch文件语法(参考http://wiki.ros.org/roslaunch/XML)

    *lanuch文件可以自动运行roscore

    <launch>
        
        
        
    	<node pkg="package-name" type="executable-name" name="node-name"/>
        
        
        
        <param name="output_frame" value="odom"/>
        
        
        <rosparam file="params.yaml" command="load" ns="params"/>
        
        
        
        <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)"/>
        
        
        
        <remap from="/turtlebot/cmd_vel" to="cmd_vel"/>
        
        
        
        <include file="$(dirname)/other.launch"/>
    launch>
    
  • 常用标签的使用方法

    • 新建功能包

    • 编写xml

    • 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
    
    <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
    
    <launch>
    
        
        <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
    
    <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

      • 显示日志信息

        古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第4张图片

    • 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
        

        古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第5张图片

    • rqt_image_view

      • 显示摄像头图像

        古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第6张图片

    • rqt

      • 综合工具

        古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第7张图片

  • rviz

    古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第8张图片

    • rviz作用
      古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第9张图片

    • rviz界面

      古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第10张图片

  • gazebo

    古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第11张图片

    古月居ROS入门21讲学习笔记——四 常用组件、五进阶展望_第12张图片

    • 使用方法(第一次运行要很久)

      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课程总结与进阶功略

这个部分我就没有记笔记啦,可以看看最后一讲

你可能感兴趣的:(ROS学习笔记,linux,ubuntu,c++)