android ros 节点编写_ROS学习笔记——初识源代码

ROS系统中的节点功能由CPP源代码或者python脚本决定，也就是说ROS节点中功能需要编程完成。在我看来，学习任何一门编程语言或者编程方法，最重要的是“敲代码”；当然，一些复杂的底层库文件可以直接copy，但主程序务必要亲手敲下来，只有这个过程能够很有效的锻炼阅读代码和理解代码结构的能力；同时：这一“敲代码”过程最大的价值在于：自己敲出来的代码会出现各种各样的bug，如何去独自...

weixin_39782433

138人浏览 · 2020-11-23 11:03:57

weixin_39782433 · 2020-11-23 11:03:57 发布

ROS系统中的节点功能由CPP源代码或者python脚本决定，也就是说ROS节点中功能需要编程完成。在我看来，学习任何一门编程语言或者编程方法，最重要的是“敲代码”；当然，一些复杂的底层库文件可以直接copy，但主程序务必要亲手敲下来，只有这个过程能够很有效的锻炼阅读代码和理解代码结构的能力；同时：这一“敲代码”过程最大的价值在于：自己敲出来的代码会出现各种各样的bug，如何去独自解决这些bug才是初学编程最大的收获。

首次编写代码的功能是：针对ROS中经典的turtlesim_node“小海龟”例子，在“小海龟”运行后，运行velocity_publisher节点以每秒10次的频率发布名为/turtle1/cmd_vel的话题，控制小乌龟x轴和z轴运动的角速度，同时发布当前角度控制信息；再运行pose_subscriber节点，对“小海龟”的位置信息进行订阅并打印。节点结构图如下：

其中，velocity_publisher节点用于发布速度控制话题，该节点源码编写过程如下：

1. 创建功能包 (创建文件夹、添加依赖)
$ cd ~catkin_ws/src
#进入catkin_ws/src文件路径
$ catkin_create_pkg learn_topic roscpp rospy std_msgs geometry_msgs turtlesim
#创建名为learn_topic 的功能包，并添加依赖

2. 在src文件夹中新建velocity_publisher.cpp文件，写入代码：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>//包含头文件
int main(int argc, char **argv)
{
    //节点初始化
    ros::init(argc, argv, "velocity_publisher");
    //创建节点句柄
    ros::NodeHandle n;
    //创建一个发布者，名为/turtle1/cmd_vel的话题
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    //设置循环的频率
    ros::Rate loop_rate(10);
    while (ros::ok())   
 {
        /* code for loop body */
        //初始化geometry_msgs::Twist消息类型
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;

        //发布消息
        turtle_vel_pub.publish(vel_msg);
        ROS_INFO("publish turtle velocity comand[%0.2f m/s,%0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z);//格式化输出当前速度控制信息

        loop_rate.sleep();//循环延迟
    }
        return 0;
}

3. 在Cmakelist.txt文件中，build控制区域下写入如下命令，告诉系统编译时使用CPP代码

add_executable(velocity_publisher src/velocity_publisher.cpp)
#把CPP文件编译成可执行文件
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
#把可执行文件与ROS的库进行链接

4. 执行编译命令：catkin_make 再执行：source devel/setup.bash

5. 若编译通过，即可依次执行（需catkin_ws路径下开启3个终端，每个终端执行一个命令）
$ roscore #启动ROS
$ rosrun tuetlesim turtlesim_node #启动“小乌龟”节点
$ rosrun learn_topic velocity_publisher #启动刚才编写的速度控制程序

以上是CPP代码的执行过程，python程序运行过程如下：
在learn_topic文件夹下建立script文件夹，并新建python脚本文件velocity_publisher.py 。写入以下代码：（笔者并不熟练python代码，故不详写注释）

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
    rospy.init_node('velocity_publisher',anonymous=True) #

    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel',Twist,queue_size=10)

    rate = rospy.Rate(10) #set the frequency of circulation 

    while not rospy.is_shutdown():

        vel_msg = Twist() 
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

        turtle_vel_pub.publish(vel_msg) #publish meseage
        rospy.loginfo("publish turtle velocity comand[%0.2f m/s,%0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z)#print the information of the control-order

        rate.sleep() #delay

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

P:这里注释使用的是英文，因为在这里python默认不支持中文字符，如果使用中文就会出现报错：SyntaxError: Non-ASCII character 'xe5' in file

运行python脚本的方式为：（需catkin_ws路径下开启2个终端，分别执行前两个命令，再于velocity_publisher.py文件所在路径打开终端，运行第三个命令）
$ roscore #启动ROS
$ rosrun tuetlesim turtlesim_node #启动“小乌龟”节点
$ python velocity_publisher.py #运行脚本

两种方式运行结果都是下图所示：

使用CPP文件和python脚本最直观的区别是，python脚本无需编译即可运行。需要注意的是：py文件需要设置属性-权限-允许作为程序执行文件

pose_subscriber节点类似上述步骤，在src中新建CPP文件，写入代码：

#include<ros/ros.h>
#include<turtlesim/Pose.h>//

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    ROS_INFO("Turtle pose: x:%0.6f y:%0.6f",msg->x,msg->y);//格式化输出位置信息
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"pose_subscriber");

    ros::NodeHandle n;

    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose",10,poseCallback);//订阅消息

    ros::spin();//循环等待

    return 0;
}

同样需要修改Cmakelist.txt文件，方法参照上述velocity_publisher例子，编译后先启动“小海龟”，在键入rosrun learn_topic pose_subscriber即可订阅小海龟位置信息。

同一功能的Python脚本代码如下：

import rospy
from turtlesim.msg import Pose

def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f y:%0.6f",msg.x,msg.y)

def pose_subscriber():
    rospy.init_node('pos_subscriber',anonymous=True)

    rospy.Subscriber("/turtle1/pose",Pose,poseCallback)

    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

两种源码运行结果均为：

其中：右上方为velocity_publisher节点输出信息，左下方为pose_subscriber输出信息。

launch文件：

由于需要启动3个节点，整个操作过程相当繁琐，这里可以使用编写launch文件来同时启动turtlesim_node、velocity_publisher、pose_subscriber节点。

launch文件创建：在learntopic 文件夹下创建launch文件夹，在该文件夹内新建turtle_c.launch文件，文件内写入代码：

<launch>   
<node pkg="turtlesim" name="turtlesim_node" type="turtlesim_node"/>    
<node pkg="learn_topic" name="velocity_publisher" type="velocity_publisher" output="screen"/>  
<node pkg="learn_topic" name="pose_subscriber" type="pose_subscriber" output="screen"/>   
</launch>

launch文件运行：
进入turtle_c.launch所在文件路径打开终端，输入命令：
$ roslaunch turtle_c.launch即可一次运行三个节点。

其中：pkg：是节点所在的 package 名称 type ：是 package 中的可执行文件，如果是 python 编写的，就可能是 .py 文件，如果是 c++ 编写的，就是源文件编译之后的可执行文件的名字。name ：是节点启动之后的名字, 每一个节点都要有自己独一无二的名字。output ：默认情况下，launch 启动 node 的信息会存入下面的 log 文件中/.ros/log/run_id/nodename-number-stdout.log，可以使用output="screen"把输出信息显示出来，即如果不加这个命令，终端就不会输出ROS_INFO信息。

除了上述的命令，还有下列加黑的命令

<launch>
    <node
        pkg=""
        type=""
        name=""  
        output="screen"
        respawn="true" 
       required="true"
       launch-prefix="xterm -e"
       ns="some_namespace"
    />
</launch>

respawn：若该节点关闭，是否自动重新启动required：若该节点关闭，是否关闭其他所有节点launch-prefix：是否新开一个窗口执行。例如，需要通过窗口进行机器人移动控制的时候，应该为控制 node 新开一个窗口；或者当 node 有些信息输出，不希望与其他 node 信息混杂在一起的时候。ns：将 node 归入不同的 namespace，即在 node name 前边加 ns 指定的前缀。为了实现这类操作，在 node 源文件中定义 node name 和 topic name 时要采用 relative name，即不加slash符号 /.

注意： roslaunch 不能保证 node 的启动顺序。

笔者水平有限，如有错误，欢迎批评指正。