ROS 21讲

ROS的官方教程极其详细，不用辅导班：
http://wiki.ros.org/melodic/Installation

命令都以ros开头
#运行小海龟：
roscore #启动：ROS Master（建立talker与listener的连接后，此终端就可退出了）、rosout（日志输出）、parameter server（参数服务器）
rosrun turtlesim turtlesim_node #rosrun <功能包名> <节点名> -运行某个功能包里的某个节点：启动仿真器节点，出现海龟
rosrun turtlesim turtle_teleop_key #键盘控制海龟节点
rosrun rqt_graph tqt_graph #显示系统中所有节点和话题关系图
rqt_：表示基于QT的可视化工具
rqt_graph #导出系统计算图（有助于了解系统结构）
机器人需要许多进程管理各个模块，ROS中每个节点都是一个进程，由ROS Master管理

rosnode +回车 //获取rosnode的帮助信息
rosnode list //把现存所有节点都列出来；其中，rosout 是系统默认本来就有的，不用关心
rosnode info [节点名]  //获取节点的具体信息；这个节点此时正在发布、订阅（如键盘发送给海龟）的话题、一些服务、主机号、pid号、底层通信机制
rosnode kill [节点名] //结束某个node
launch文件一次性启动所有节点
         
rostopic +回车 //获取有关话题的帮助信息
rostopic list //列出当前系统话题列表
rostopic pub [发布的话题名称] [发布的消息] [参数] //发布话题消息，eg.如下
rostopic pub  /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]'  //可用tab键补全；'[]' '[]'：直线行走距离、旋转弧度；ros默认单位：米和弧度 
rostopic info /topic name //显示某个topic的属性信息
rostopic echo /topic name //显示某个topic的内容
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]' //-r 10（rate 10）：以10次/s的频率发布当前指令
rostopic pub topic_name ... 向某个topic发布内容
rostopic bw topic_name  查看某个topic的带宽
rostopic hz topic_name  查看某个topic的频率
rostopic find topic_type    查找某个类型的topic
rostopic type topic_name    查看某个topic的类型(msg)
    
rosmsg list //列出系统中所有 msg
rosmsg show [消息名] //查看某个 msg 内容
rosservice list //输出服务列表
rosservice info service name //显示某个service的属性信息
rosservice call [服务名] //调用某个服务,使用所提供的args调用服务
rosservice call /spawn tab键 //call：调用服务（发布请求）；/spawn：产生新的海龟；x、y：海龟原始坐标；//生成一个新的turtle
rosservice type 打印服务类型
rosservice uri  打印服务ROSRPC uri
rosservice find 按服务类型查找服务
rosservice args 打印服务参数
rossrv md5  显示服务md5sum
rossrv package  列出包中的服务
rossrv packages 列出包含服务的包    
rossrv list //列出系统上所有srv
rossrv show srv name //显示某个srv内容
rosbag record [] //记录系统当前所有话题信息，供后期使用（如日志信息）
rosbag record -a -O cmd_record //record：记录数据；-a：记录所有数据；-o：保存成压缩包；cmd_record：压缩包文件名；之后所有动作都会被记录，直到按键按下ctrl+c（保存压缩包在home下）
rosbag play cmd_record.bag //复现话题；要重新启动roscore、rosrun turtlesim turtlesim_node
rosbag info cmd_record.bag //查看bag文件记录了多少消息，记录的是哪个topic的消息，消息的类型是什么等 
roswtf //运行时检查（在有ROS节点运行时）；对一些系统中看起来异常但可能是正常的运行情况发出警告。也会对确实有问题的情况报告错误。
rosmsg list
rosmsg show /msg_name //显示某个 msg 内容

远程过程调用(Remote Procedure Call，RPC),可以理解为在一个进程里调用另一个进程的函数

workspace：类似于IDE创建的工程，存放工程开发相关文件，所有的源码、编译的结果都在这里
• src：代码空间（放置功能包：为源码的最小单元，所有源码都必须放到功能包里）
• build：编译空间（编译过程中产生的中间文件）
• devel：开发空间（开发过程中编译生成的可执行文件、库、脚本）
• install：安装空间（开放结束后生成的可执行文件、库、脚本，用于后期发布给客户）
• scripts：如果编写的是py文件，则可以右键创建这个文件夹放py文件，以区别cpp所在文件夹（注意设置py文件的文件属性：运行列为可执行文件）

远程过程调用(Remote Procedure Call，RPC),可以理解为在一个进程里调用另一个进程的函数

#创建工作空间
    mkdir -p ~/catkin_ws/src #src名不可变；-p：递归创建所有层级目录
    cd ~/catkin_ws/src
    catkin_init_workspace #把当前文件夹src初始化成ros工作空间（属性变化）
#编译工作空间
    cd ~/catkin_ws/
    catkin_make #编译src下所有功能包的源码；生成build、devel
    catkin_make install #生成install，非必须的
#创建功能包；同一工作空间下，功能包不可同名；工作空间中的功能包文件夹可直接delete删除，并创建其他功能包
cd ~/catkin_ws/src #功能包要放在src中
#配置功能包程序依赖：catkin_create_pkg <功能包名字> [依赖1] [依赖2]；一个功能包要依赖其他功能包工作；rospy：要编译python；roscpp：编译c++
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim  #src中生成功能包learning_topic（文件夹）， 其中包含package.xml、CMakeLists.txt以来
#设置环境变量：告知系统上编译的可执行文件位置，因为可执行文件都放在工作空间里；修改完环境变量之后重启终端窗口才可以生效
    source ~/catkin_ws/devel/setup.bash #要运行功能包中的程序，需要设置工作空间的环境变量，才能让系统找到工作空间和其中的功能包（只在当前终端有效）
    在 ~/.bashrc文件最后一行写入（永久有效）：
    	source /home/fei/catkin_ws/devel/setup.bash
    	或在终端中输入：echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc；使修改生效：source ~/.bashrc
    #此时，系统就可以在 devel/lib/learning_topic/ 中找到可执行文件
#检查是否成功设置环境变量
    echo $ROS_PACKAGE_PATH #ROS_PACKAGE_PATH：代表ros中所有功能包的路径（包括上面自己写的功能包）
#此时成功建立ROS工作空间 

/* 在文件夹 “learning_topic/src” 中创建发布者代码
    代码实现发布者的步骤：
    1. 初始化ROS节点
    2. 向ROS Master注册节点信息，包括发布的话题名和话题的消息类型
    3. 创建消息数据
    4. 循环发布消息
创建发布者，发布话题：turtle1/cmd_vel，消息类型：geometry_msgs::Twist
*/ 
#include <ros/ros.h> //包含了大部分ROS中常用的头文件
#include <geometry_msgs/Twist.h>
int main(int argc, char **argv)
{
    // 1. ROS节点初始化
    ros::init(argc, argv, "velocity_publisher"); //定义节点的名字（不能重名）：velocity_publisher
        // 创建节点句柄：管理节点相关的API资源
    ros::NodeHandle n;
    // 2. 创建一个Publisher，系统Publisher节点发布类型为geometry_msgs::Twist、名为/turtle1/cmd_vel（订阅者就是通过与这个同名调用这里发布的消息）的topic，队列长度10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10); //小海龟订阅的就是 /turtle1/cmd_vel，所以当小海龟启动后和这个节点都启动后，海龟就会照着运动
        /*向话题turtle1/cmd_vel中发布数据，10：队列长度，当底层（包括以太网等）来不及响应发布的命令，而此时100ms已到，要再次发布数据了，没响应的就先存放在这个队列中，当还不够，则队列中只存放最新的10个数据）*/
        // 设置循环的频率，10Hz
    ros::Rate loop_rate(10); //发送的频率
    int count = 0;
    while (ros::ok()) //当节点发生异常，ros::ok() 返回 false
    {
    // 3. 初始化geometry_msgs::Twist类型消息的内容
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5; //m/s
        vel_msg.angular.z = 0.2; //绕z轴旋转0.2弧度/s
        //其他未赋值的参数默认为0
    // 4. 发布消息 .publish()
        turtle_vel_pub.publish(vel_msg); //ros底层通过一定的通信机制将数据压入队列，再发送
        
        // 打印信息,类似printf()
        ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);
        
        //处理节点订阅话题的所有回调函数
        ros::spinOnce(); //虽然此段代码没有订阅话题，但是为了保证功能的无误，建议所有节点都默认写上
 
            
        // 按照ros::Rate 的循环频率延时100ms，之后再次进入节点信息发布工作
        loop_rate.sleep();
    }
    return 0;
}
#修改leaning_topic中的CMakeLists.txt文件：（添加在 ## Install ## 上面）
add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
#编译功能包
cd ~/catkin_ws
catkin_make
运行C++可执行文件：
    #终端运行：
    roscore、rosrun turtlesim turtlesim_node、rosrun learning_topic velocity_publisher（这是 cpp 文件名字，而不是节点名）
        #learning_topic：catkin_create_pkg定义的功能包；velocity_publisher：在ros::init()中定义的节点
运行python文件：（打开 velocity_publisher.py 的可执行权限，且无需catkin_make编译）
    1、将文件设置为可执行文件
    2、roscore、rosrun turtlesim turtlesim_node、rosrun learning_topic velocity_publisher.py   

/** 创建Subscriber，该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
    代码实现订阅者的步骤：
    1、初始化ROS节点
    2、订阅需要的话题
    3、循环等待话题消息，接收到消息后进入回调函数
    4、在回调函数中完成消息处理
*/
#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节点
   ros::init(argc, argv, "pose_subscriber");
   // 创建节点句柄
   ros::NodeHandle n;
   // 创建一个Subscriber；订阅名为/turtle1/pose的topic（由Publisher定义）；10：订阅的话题容许的最大队列长度，超过10则舍弃时间最早的话题数据；一旦订阅到数据就进入回调函数poseCallback；订阅段不用定义消息类型，程序之查找系统中是否有同名的消息名
   ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback); // 接收到Publisher定义的话题 /turtle1/pose 后（这里订阅的是小海龟的位姿，小海龟启动后就开始发布 /turtle1/pose ），会进入回调函数poseCallback()
   ros::spin(); // 这里内部永远循环，不断查看队列中是否有上面寻求的消息"/turtle1/pose"进来，有的话就调用上面回调函数；没有的话就一直死循环等待；直到ros::ok()返回false时退出
   return 0; //一般永远不会运行到这里
}
/*
    add_executable(pose_subscriber src/pose_subscriber.cpp)
    target_link_libraries(pose_subscriber ${catkin_LIBRARIES})
    编译、设置环境变量
    执行：roscore、rosrun turtlesim turtlesim_node、rosrun learning_topic pose_subscriber
    移动海龟可发现输出海龟位置数据开始变化
*/

message：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter3/3.5.html

msg类型：bool、int8、int16、int32、int64(以及uint)、float、float64、string、time、duration、header、可变长数组array[]、固定长度数组array[C]
每个 message 就像一个类，包含不同类型的参数，每次发布的内容为对象
自定义话题消息：（如果想要发布、订阅非系统自带的消息类型，就要自定义消息类型）
1、//在功能包learning_topic中创建msg/Person.msg文件，内容如下：
    string name
    uint8 sex
    uint8 age
    uint8 unknown=0
    uint8 male =1
    uint8 female=2
2、修改package.xml、CMakeList.txt，添加动态生成程序的功能包依赖：
    -- package.xml
    在 <!-- The export tag contains other ...> 上面：添加一个动态生成程序的功能包的依赖
    <build_depend> message_generation </build_depend> #编译依赖；依赖于会动态产生message的依赖的功能包
    <exec_depend> message_runtime </exec_depend> #执行依赖；依赖于动态message的runtime的运行时依赖
    -- CmakeLists.txt
    find_package(... roscpp rospy message_generation)
    catkin_package(... CATKIN_DEPENDS message_runtime ...) //创建message运行的依赖，对应<exec_depend>message_runtime</exec_depend>
    //把 msg、srv 编译成对应的不同的程序文件的配置项
    add_message_files(FILES Person.msg Person.srv) #把Person.msg作为我的交际接口，在编译时就会发现这个接口并针对Person.msg做编译
    generate_messages(DEPENDENCIES std_msgs) #编译Person.msg时需要依赖哪些已有的库、包   
3、//编译工作空间（可在devel/include中找到对 .msg编译出的 .h文件）
    catkin_make
/** 
    创建发布者代码：
    步骤同之前方法，只不过创建消息数据是Person
    将 person_publisher.cpp、person_subscriber.cpp放在功能包的src下，
     * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#include "learning_topic/Person.h" //调用 .msg生成的 .h文件 (/devel/include/learning_topic)
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_publisher");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Publisher，发布名为 “/person_info” 的topic（自己创建的消息）；消息类型为learning_topic::Person，而消息名可以任意起；队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);
    // 设置循环的频率
    ros::Rate loop_rate(1);
    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_topic::Person类型的消息
        learning_topic::Person person_msg;
        person_msg.name = "Tom";
        person_msg.age  = 18;
        person_msg.sex  = learning_topic::Person::male;
        // 发布消息
        person_info_pub.publish(person_msg);
        ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", person_msg.name.c_str(), person_msg.age, person_msg.sex);
        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;
}
/**创建订阅者代码：
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */
#include <ros/ros.h>
#include "learning_topic/Person.h"
// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", msg->name.c_str(), msg->age, msg->sex);
}
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
}
配置代码编译规则(在CMakeLists.txt中的 ## Install ## 上面）：
add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp) #因为有些代码是动态生成的，所以需要可执行文件和动态生成的程序也要产生依赖的关系
add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp) 
//上面的两个add_dependencies()：动态的和 .msg生成的 .h文件做连接    
         
编译、加入环境变量
执行：roscore、rosrun learning_topic person_subscriber、rosrun learning_topic person_publisher
运行后关闭roscore发现并不影响发布者和订阅者传输消息，这是因为roscore只负责建立联系，而和传输消息无关

Service实现如下（参考pdf）：Client端发布request并阻塞，Server端收到request并向Client发送reply反馈，Client继续执行；
Topic：异步，所以Publish端会一直执行，而Subscribe端接收评率比Publish低，导致Publish浪费很多计算资源

客户端Client请求服务的编程实现：
创建功能包：catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim
/**
初始化ROS节点
创建一个Client实例
发布服务请求
等待Server处理之后的应答结果
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn；实现请求创建海龟的服务
 */
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "turtle_spawn");
    // 创建节点句柄
    ros::NodeHandle node;
    // 查看系统中是否有/spawn服务（就像想使用服务器，服务器要开机），没有就阻塞等待
    ros::service::waitForService("/spawn");
    // 创建客户端Client，给spawn服务发送请求，请求的数据类型为turtlesim::Spawn；"/spawn"：服务的名字
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    // 初始化turtlesim::Spawn的请求数据；没添加角度则=0
    turtlesim::Spawn srv;
    srv.request.x = 2.0;
    srv.request.y = 2.0;
    srv.request.name = "turtle2";
    // 请求服务调用
    ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
             srv.request.x, srv.request.y, srv.request.name.c_str());
    // 将request发送出去，并一直等待服务器给反馈，没有反馈则阻塞
    add_turtle.call(srv);
    // 显示服务调用结果
    ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());
    return 0;
};
修改CMakeLists.txt的 ## Install ## 上面：
add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})
编译、设置环境变量
执行 roscore、rosrun turtlesim turtlesim_node、rosrun learning_service turtle_spawn

服务端Server接收与回馈的编程实现：
/**
初始化ROS节点
创建Server实例
循环等待服务请求，进入回调函数
在回调函数中完成服务功能的处理，并反馈给应答数据
 * 该例程执行数据类型为std_srvs/Trigger的/turtle_command服务；服务器在接受到请求时之后发出回应，然后发布者发布消息令海龟动起来
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>
ros::Publisher turtle_vel_pub;
bool pubCommand = false;
// service回调函数，是真正实现服务功能的部分；参数是服务数据类型描述文件中声明的请求与应答数据结构：输入参数req，输出参数res；结束后，结果会放到应答数据中，反馈给Client
bool commandCallback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res)
{
    pubCommand = !pubCommand;
    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");
    // 设置反馈应答数据：指示数据是否执行成功
    res.success = true;
    res.message = "Change turtle command state!"
    return true;
}
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "turtle_command_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建名为 "/turtle_command" 的server；接收到服务数据后进入回调函数commandCallback；因为不知道request什么时候进来，通过回调函数机制处理请求；收到服务request后，跳入commandCallback中
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);
    // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ROS_INFO("Ready to receive turtle command.");
    // 设置循环的频率
    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        // 查看回调函数队列中是否有服务数据消息进来了，一旦有服务数据进来，就到回调函数中来
        ros::spinOnce();
        
        // 如果标志为true，则发布速度指令
        if(pubCommand)
        { //发布速度指令
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5;
            vel_msg.angular.z = 0.2;
            turtle_vel_pub.publish(vel_msg);
        }
        loop_rate.sleep();//按照循环频率延时
    }
    return 0;
}
//查看Trigger：rossrv show std_srvs/Trigger；  ---上方为request、下方为response   
CMakeLists.txt：
add_executable(turtle_command_server src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})
编译、设置环境变量
执行：roscore、rosrun turtlesim turtlesim_node、rosrun learning_service turtle_command_server、rosservice call /turtle_command tab键 //发送请求

服务来调用自定义的服务数据：

自定义的服务数据：在工作空间learning_service中创建srv/Person.srv文件：srv可以嵌套msg在其中，但它不能嵌套srv
    #Client发送给Server的内容
    string name
    uint8 age
    uint8 sex
    uint8 unknown=0
    uint8 male=1
    uint8 female=2
    ---
    #Server反馈给Client的内容
    string result
修改package.xml、CMakeList.txt，添加动态生成程序的功能包依赖：
    -- package.xml
    在 <!-- The export tag contains other ...> 上面：添加一个动态生成程序的功能包的依赖
    <build_depend> message_generation </build_depend> #编译依赖；依赖于会动态产生message的依赖的功能包
    <exec_depend> message_runtime </exec_depend> #执行依赖；依赖于动态message的runtime的运行时依赖
    -- CmakeLists.txt
    find_package(... roscpp rospy message_generation)
    catkin_package(... CATKIN_DEPENDS message_runtime ...) //创建message运行的依赖，对应<exec_depend>message_runtime</exec_depend>
    //把 msg、srv 编译成对应的不同的程序文件的配置项
    add_message_files(FILES Person.msg Person.srv) #把Person.msg作为我的交际接口，在编译时就会发现这个接口并针对Person.msg做编译
    generate_messages(DEPENDENCIES std_msgs) #编译Person.msg时需要依赖哪些已有的库、包    
编译：devel/include/learning_topic 中生成三个 .h 文件
    Person.srv中的 --- 上面部分放在devel/include/learning_service/PersonRequest.h中、下面部分放在PersonResponse.h中、包含上两个头文件的一个总头文件Person.h（使用时只包含这一个就可以）
客户端和服务端处理自定义服务数据：
/**服务端
 * 该例程将执行/show_person服务，服务数据类型learning_service::Person
 */
#include <ros/ros.h>
#include "learning_service/Person.h" 
// service回调函数，输入参数req，输出参数res
bool personCallback(learning_service::Person::Request  &req, //Request：这是ROS强制定义的，必须要加上
                    learning_service::Person::Response &res) 
{
    // 显示请求数据
    ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);
    // 设置反馈数据
    res.result = "OK";
    return true;
}
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个名为/show_person的server，注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
    // 循环等待回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();
    return 0;
}
/** 客户端
 * 该例程请求/show_person服务，服务数据类型为learning_service::Person
 */
#include <ros/ros.h>
#include "learning_service/Person.h" //Person.h中含盖了 PersonRequest.h、PersonResponse.h，所以包含 Person.h就可以了
int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_client");
    // 创建节点句柄
    ros::NodeHandle node;
    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    ros::service::waitForService("/show_person"); // 查看系统中是否有/show_person（就像想使用服务器，服务器要开机），没有就阻塞直到 show_person 服务的提供者运行起来
        
    // 创建 /show_person 的Client实例，指定服务类型为learning_service::Person；client会发布Person类型请求数据，请求的对象是"/show_person"
    ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");
    // 初始化learning_service::Person的请求数据
    learning_service::Person srv;
    srv.request.name = "Tom";
    srv.request.age  = 20;
    srv.request.sex  = learning_service::Person::Request::male;
    ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
             srv.request.name.c_str(), srv.request.age, srv.request.sex);
    person_client.call(srv); //cal：将请求数据发送出去；阻塞等待server的反馈结果
    // 显示服务调用结果
    ROS_INFO("Show person result : %s", srv.response.result.c_str());
    return 0;
};
add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp)
add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)
    
编译：catkin_make //生成devel/person_client_ 、person_server可执行文件
添加环境变量
执行：roscore、rosrun learning_service person_server、rosrun learning_service person_client

ROS Master中的参数服务器Parameter Server（相当于全局字典）用来保存各个节点间的配置参数，被所有节点共享
参数服务器维护着一个数据字典，字典里存储着各种参数和配置（每一个key不重复，且每一个key对应着一个value）；使用互联网传输，在节点管理器中运行；
因为字典的这种静态的映射特点，项目中往往将一些不常用到的参数和配置放入参数服务器的字典里，这样对这些数据进行读写都将方便高效
参数服务器三种护方式：
    命令行维护
        rosparam set param_key param_value  设置参数
        rosparam get param_key  显示参数
        rosparam load file_name.yaml    从文件加载参数
        rosparam dump file_name.yaml    保存参数到文件
        //load、dump文件需要遵守YAML格式，可以把YAML文件的内容理解为字典，因为它也是键值对的形式(Key: Value)
        rosparam delete 删除参数
        rosparam list   列出参数名称
        
    launch文件内读写：标签 <param>、<rosparam>
        # param只给出了key，没有直接给出value，这里的value是由后没的脚本运行结果作为value进行定义的
        <param name= "robot description" command= "$(find xacro)/xacro.py $(find robot _sim_ demo)/urdf/robot.xacro" />
        # <param>一般只设置一个参数
        <param name= "publish frequency" value="100.0"/> 
        # rosparam的典型用法，先指定一个YAML文件，然后施加command,其效果等于rosparam load file_name
        <rosparam file="$(find robot sim_ _demo)/config/xbot2_ control,yaml" command= "load"/>
    node源码：利用API修改ROS的源码，也就是对参数服务器操作

创建功能包：catkin_create_pkg learning_parameter roscpp rospy std_srvs
首先 roscore、rosrun turtlesim turtlesim_node 后rosparam才能起作用：
    列出所有的参数：rosparam list
    获取rosparam list出的某个参数的值：rosparam get [param_key]
    设置参数值 param_key = param_value：rosparam set [param_key] [param_value]
    上面修改值后虽然值已经变了，但是需要发送请求后才能起作用：rosservice call clear tab键
    保存ROS系统中的参数到文件中：rosparam dump [file_name.yaml] //保存在终端当前路径下
    加载文件中的参数到系统中：rosparam load [路径/file_name] （刷新后才能起作用）
    删除系统中的参数：rosparam delete [param_key]
    常用YAML存储参数，并加载
/**
实现上面命令行的操作：
    1、初始化ROS节点
    2、get函数获取参数
    3、set函数设置参数
 */
 
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
int main(int argc, char **argv)
{
    int red, green, blue;
    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");
    // 创建节点句柄
    ros::NodeHandle node;
    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);
    ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);
    // 设置背景颜色参数
    ros::param::set("/background_r", 0);
    ros::param::set("/background_g", 255);
    ros::param::set("/background_b", 255);
    ROS_INFO("Set Backgroud Color[255, 255, 255]");
    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);
    ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);
    // 调用服务，刷新背景颜色
    ros::service::waitForService("/clear");
    ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
    std_srvs::Empty srv;
    clear_background.call(srv);
    
    sleep(1);
    return 0;
}
add_executable(parameter_config src/parameter_config.cpp)
target_link_libraries(parameter_config ${catkin_LIBRARIES})
编译、加入环境变量    
执行：roscore、rosrun turtlesim turtlesim_node、rosrun learning_parameter parameter_config    

ROS中的坐标系管理系统：（坐标变换：旋转、平移）
TF坐标变换功能包（sudo apt-get install -y ros-melodic-turtle-tf）：记录着10s内机器各个部位间的坐标关系；坐标变换通过广播TF变换和监听TF变换实现
roslaunch：launch 脚本文件，可以将很多节点的启动都写进去，一次性启动这些节点
roslaunch turtle_tf turtle_tf_demo.launch //启动实例：两个海龟自动跟踪
rosrun tf view_frames //查看并保存当前ROS系统中所有tf之间的关系，保存为 ~/frames.pdf（有world（全局坐标系）+turtle1+turtle2三个坐标系）
rosrun tf tf_echo turtle1 turtle2 //tf_echo工具：实时查询tf树中任意两个坐标系间的关系
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz //启动可视化工具rviz

创建tf广播器与监听器：将上一节中一个海龟相对于world系的pose广播出来，监听器获取turtle1和turtle2的坐标关系，使turtle2跟随着turtle1运动
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
/**
实现一个tf广播器步骤：
    定义TF广播器
    创建坐标变换值
    发布坐标变换
 * 该例程产生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) ); //setOrigin：设置在（x, y, z）上的平移值
      /*旋转变换*/
    tf::Quaternion q; //四元素描述旋转
    q.setRPY(0, 0, msg->theta); //旋转
    transform.setRotation(q); //加载平移、旋转数据
    // sendTransform()：广播器将坐标变换关系插入TF树并发布
        //发布的TF消息类型为StampedTransform：此处包含 坐标变换关系、时间戳、要变换的源坐标系、目标坐标系
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); //默认保存10s内的数据
}
int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "my_tf_broadcaster"); //本程序需要运行两次分别计算turtle1、turtle2与world的位置关系，但会导致节点重名；对节点名重映射: __name:=
    // 输入参数作为海龟的名字
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    turtle_name = argv[1]; //argv[]：输入的海龟名（turtle1, turtle2）
    ros::NodeHandle node;
    
    // 订阅海龟仿真器中不断发布的海龟位姿消息
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
};
/**
创建tf监听器步骤：
    定义tf监听器
    查找坐标变换
 * 该例程监听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速度控制指令的发布者，发布消息类型为Twist的话题 "/turtle2/cmd_vel" 
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
    // 创建tf监听器（监听tf中任意两个坐标系之间的关系），创建成功后监听器会自动接收TF树的消息，并且缓存10秒
    tf::TransformListener listener;
    ros::Rate rate(10.0);
    while (node.ok())
    {
        // 获取turtle1与turtle2坐标系之间的tf数据
        tf::StampedTransform transform; //transform：用来保存tf关系
        
        try
        {
            //阻塞程序，直到系统中存在 源坐标系"/turtle2" 和目标坐标系"/turtle1" 之间在指定时间的变换关系， 所以要设置超时时间：等待超过3s则提示错误
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); //Time(0)：在保存的10s数据中查询当前最新的数据
            
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); //查询两坐标在指定时间的坐标变换关系，并保存在transform中
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令（turtle2跟随turtle1）
        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)); //2s完成跟随
        turtle_vel.publish(vel_msg);
        rate.sleep();
    }
    return 0;
};
CMakeLists.txt：
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
编译、加入环境变量
roscore、rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1 //节点名重映射为：turtle1_tf_broadcaster；argv[]=turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener //查询两个海龟间的速度关系，并给turtle2发布速度指令
rosrun turtlesim turtle_teleop_key

快速实现多个节点的配置、启动；实现后搭建相关系统多是搭建积木式的调用launch，很少修改原程序；教程：http://wiki.ros.org/roslaunch/XML

Launch文件启动时，如果ROS Master没有启动，则自动启动 ROS Master
Launch文件语法：
    运行Launch文件：roslaunch+功能包名+launch文件名
    
    启动：         <launch>
    节点配置与启动： <node pkg=[“节点所在功能包”] name=["启动的节点名，覆盖ros::init()中赋予的节点名"] type=["节点的可执行文件名"]/>
    参数设置：       <param name="参数名" value="参数值" output="screen"/> #对参数赋值value，保存在参数服务器中（ROS Master）;output=“screen”则会将日志信息打印到当前终端
        当参数很多，<param>会非常麻烦: <rosparam file=".../params.yaml" command="load" ns="params"/> #将YAML中参数加载到ROS参数服务器中，command属性为“load”，命名空间为“ns”
    结束：         </launch>
    
    pkg、type：相当于使用rosrun命令后跟着的两个参数；还有其他参数属性见pdf
eg：
    <launch>
        <node pkg="turtlesim" name="sim1" type="turtlesim_node"/>  //<node pkg="package-name" type="executable-name" name="node-name"/>
        <node pkg="turtlesim" name="sim2" type="turtlesim_node"/>
    </launch>
    
其他可选属性：
    output：将某个节点日志信息打印到终端中
    respawn：重启
    required：要求节点是否必须要启动
    args：类似 rosrun 后卖弄跟随的指令，给节点输入参数供使用
<arg ...> ：存在launch文件内部使用局部参数；<arg name="参数名" default="默认参数名"/>
    调用本参数的方式：<param name="foo" value="$(arg 参数名)"/> ； <node name="node" pkg="package" type="type" args="$arg arg-name)"/>
<remap ...> ：重映射，对ROS计算图中的资源重新命名（原名称销毁）；<remap from="/turtlebot/cmd_vel" to="/cmd_vel"/>
<include ...> ：类似 .h文件，launch文件内容较多时，将其拆分模块化，但是各个模块之间互相包含，则用 <include >调用其他模块；<include file="$(dirname)/other.launch"/>
实例：
catkin_create_pkg learning_launch （launch是一个启动文件，基本都是链接，不需要任何的依赖）
在文件夹learning_launch中创建放置 .launch 文件的文件夹名launch，便于管理功能包中各种资源
<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
</launch>
编译、加入环境变量
roslaunch learning_launch simple.launch （此时可发现，两个节点并行运行）
<launch>
    <param name="/turtle_number"   value="2"/> // 参数为 /turtle_number
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"> //“turtlesim_node”：下面节点名字的命名空间，解决同名冲突
        <param name="turtle_name1" value="Tom"/> // 参数为 /turtlesim_node/turtle_name1
        <param name="turtle_name2" value="Jerry"/>
        <rosparam file="$(find learning_launch)/config/param.yaml" command="load"/> //(find learning_launch)：搜索功能包并输出路径；要在learning_launch下创建 config/param.yaml
    </node>
    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen" /> // output="screen"：日志输出
</launch>
编译、 roslaunch learning_launch turtlesim_parameter_config.launch
新终端下：rosparam list，可发现参数都被加载进去了
海龟跟随：
 <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" /> // rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
    <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>
编译、roslaunch learning_launch start_tf_demo_c++.launch 
<launch>
    <include file="$(find learning_launch)/launch/simple.launch" /> //载入simple.launch文件，并启动simple.launch中所有内容
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"> //启动一个仿真器节点
        <remap from="/turtle1/cmd_vel" to="/cmd_vel"/> //重映射
    </node>
</launch>
编译、 roslaunch learning_launch turtlesim_remap.launch
打开新终端：rostopic list （可发现 "/turtle1/cmd_vel" 消失了）；rostopic pub /cmd_vel tab键 tab键 （可发现，海龟动了，说明重命名其作用了）
<launch>    <!--根标签-->
<node>    <!--需要启动的node及其参数-->
<include>    <!--包含其他launch-->
<machine>    <!--指定运行的机器-->
<env-loader>    <!--设置环境变量-->
<param>    <!--定义参数到参数服务器-->
<rosparam>    <!--启动yaml文件参数到参数服务器-->
<arg>    <!--定义变量-->
<remap>    <!--设定参数映射-->
<group>    <!--设定命名空间-->
</launch>    <!--根标签-->

常用可视化工具的使用
rqt_ tab健：列出ROS提供的Qt工具箱里的工具
rqt_console：日志输出工具:输出错误，警告等
rqt_graph：计算图可视化工具
rqt_plot：数据绘图工具
rqt_image_view：图像渲染工具
rqt 整合了上面的系学习工具，可以自定义整合多种工具
Rviz
机器人开发过程中的数据可视化界面，是一款三维可视化工具
可以使用可扩展标记语言XML对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并在界面中呈现出来
同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等信息
Gazebo（服务器在国外，所以可能加载不出来）（如下报错不用管：[Err] [REST.cc:205] Error in REST request）
一款功能强大的三维物理仿真平台
有强大的物理引擎，高质量的图形渲染，方便的编程与图形接口，开源免费
应用场景：测试机器人算法，机器人设计，现实情况下的回溯测试

最后一节值得再看