ROS入门笔记总结

one big bug

已于 2024-03-26 00:25:15 修改

阅读量802

点赞数 20

文章标签：机器人

于 2024-02-11 15:03:56 首次发布

本文链接：https://blog.csdn.net/goodname_you/article/details/134961101

版权

以下将会展示在ros学习过程中的经验和想法，目的是为了理清学习的思路，学习主要参考路径为billbill中虚左&古月居，主要使用的是c++编写，这里默认已经装好了ubuntu和ros1

补充：

如果是想定位到计算机，直接按删除键，就会不断退回上一级

cd .. 返回上一个工作目录

如果是不想重复刷新环境变量，就在终端输入

vi .bashrc

然后滑到最下面一行，输入

source /home/hcx/catkin_ws/devel/setup.bash

获取当前所有的话题：

rostopic list

通过ctrl+"+"可以实现vscode字体的放大或者缩小

一、首先将详细介绍怎么直接在终端运行c++和python的程序

两者在运行之前，都要实现一个共同条件：

首先

配置一个工作环境，两者都是要有工作环境才能运行

mkdir -p 自定义空间名称/src

cd 自定义空间名称

catkin_make

注意：自定义空间可以任意起，比如我起的是demo01，那么直接在“自定义空间名称”输入“demo01”即可，第二个cd就是进入空间的意思，因为第一行代码只是创建了一个空间，创建用的是“mkdir”，对于创建文件夹也是类似，catkin_make可以理解成是装饰好内部环境，就是给房子装修，进去就是以下样子（先cd工作空间才能catkin_make，然后会生成三个文件）

2.装饰好，就要开始配置ros包，进入src目录，输入以下命令，或者可以从终端直接通过cd指令进入，这里cd进入是要遵循一个递进原则，不能直接cd就进入，进入一个小的空间同时就要先进入小的空间所在的大空间。

cd src
catkin_create_pkg 自定义ROS包名 roscpp rospy std_msgs

std_msgs是标准消息库，包含了一些常用的消息类型

注意：ros包名字也是可以任取的，在“自定义ros包名”一栏输入即可，后面3个是依赖包，就是用这些语言来实现编程，一般都是这三个

在基础的配置好后（工作环境+功能包），就可以开始实现编程

1、cpp

首先是源文件问题，可以通过查看ros包的cmaker，其中cpp对应136左右，python对应163左右，是通过这两个来确定文件夹，比如cpp就是src，然后python就是scripts，但是在ros包里面是不存在scripts文件夹的，就需要自己创建，然后这也是py不同于c++的地方。

现在先讲解cpp源文件的编写：

首先进入ros包里面的src（注意，这里不是工作空间里面的src，而是src里面的ros功能包），然后新建文本文件，重命名，当然，重命名的文件后缀要加上cpp，不然写不了cpp文件，加上cpp后，写入以下程序：

#include "ros/ros.h"

int main(int argc, char *argv[])
{
    //执行 ros 节点初始化
    ros::init(argc,argv,"hello");
    //创建 ros 节点句柄(非必须)
    ros::NodeHandle n;
    //控制台输出 hello world
    ROS_INFO("hello world!");

    return 0;
}

写完之后，点保存，回到ros包，就是退一步，编辑 ros 包下的 Cmakelist.txt文件，按照下面方式编辑（实际上是去掉注释，加上源文件名）

add_executable(步骤3的源文件名
  src/步骤3的源文件名.cpp
)
target_link_libraries(步骤3的源文件名
  ${catkin_LIBRARIES}
)

注意：观察上图，其中有3个都是‘步骤3的源文件’，暂且编号为1，2，3。其中1号是名字，这是可以任意起的，3号和1号必须相同，2号要跟ros中src（就是存放源文件的位置）中cpp源文件名字一样，以上准备工作完成后，就可以开始配置文件，并且编译执行了，其实是可以三个都一样的，这样方便一些。

回到工作空间，就是一开始的那个工作空间，输入以下程序（或者通过cd回去），进行编译：

catkin_make

这样子就是开始编译了，等他编译完成，就要重新开一个终端，执行以下程序（启动ros的主节点，保证程序顺利运行）：

roscore

但是有时候是运行不了的，就比如前面已经执行了某个程序（已经启用了ros主节点），可以在roscore那个终端，输入以下命令（强制杀死进程），或者直接将前面的叉掉，或者是ctrl+c强制停止：

killall -9 roscore    //如果提示未找到进程不用管，继续执行下一条
killall -9 rosmaster

跑完roscore后，就可以执行代码了，在工作空间打开终端，输入以下程序（先刷新环境变量，再跑程序），古月提到一种不用刷新环境变量的方法：

source ./devel/setup.bash
rosrun 包名 C++节点

跑程序：rosrun ros包功能包名称（自己起的）程序名称（这个程序名称指的是在cmake里面所命名的），当然，三者间要加入空格，空格十分重要。

然后，就成功执行了，执行的是info，就是源文件里面所写那个。

cpp：

py：

2、python

python要先在ros包里面新建一个文件夹：

mkdir scripts

然后在文件夹里面新建一个文本文档，记住后缀一定要加上：py，然后写下以下程序：

#! /usr/bin/env python

"""
    Python 版 HelloWorld

"""
import rospy

if __name__ == "__main__":
    rospy.init_node("Hello")
    rospy.loginfo("Hello World!!!!")

第一行是一定要加上的！！！

然后python很特殊，要配置权限(在scripits里面)：

chmod +x 自定义文件名.py

然后回到ros包，打开cmaker（大概163的位置），自定义文件名要和scripits里面源文件的文件名一致，或者直接qu'xiao：

catkin_install_python(PROGRAMS scripts/自定义文件名.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

然后返回空间编译：

catkin_make

等待，然后新建终端，执行：

roscore

执行不了看之前cpp配置那里

然后在回到工作空间，执行：

source ./devel/setup.bash
rosrun 包名 自定义文件名.py

二、接着介绍怎么在vs code上运行cpp和python

vs code有个很不错的功能：自动填充，报错，自动填充可以在vs code的终端上用"Tab"键进行填充，因为在记事本上写代码比较麻烦，所以建议在vs code上运行（参考虚左）。

vs code下载版本，可以直接在主机上面下载好再移动到虚拟机上面，在虚拟机上面比较卡。

1.cpp

其实开始的操作都是很类似的，在虚拟机中，直接用快捷键CTRL+ALT+T新建终端

首先，输入以下代码创建工作空间：

mkdir -p xxx_ws/src(必须得有 src)
cd xxx_ws
catkin_make

然后，第二步就是打开vscode（当然是从工作空间里面进入，但是在第一步执行了catkin_make之后，会默认在工作空间，所以可以忽略下面的cd）：

cd xxx_ws
code .

注意：英文句号要空格隔开！！！

进入之后，因为想要方便编译，就不要经常都是输入：catkin_make 进行编译，可以在vscode里面配置，然后每次都是要重新配置的：

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

键盘输入ctrl+shirt+b,会弹出一个东西（因为这里没有是新建工作区，就不会显示，一般会显示两个，然后点击短的（第二个）那个小齿轮(一般写着catkin_built)，把以上代码复制进去就可以了）：

然后就开始新建功能包，鼠标右键点击工作区下的str，下面会有个create catkin package，点击，就是新建工作区，然后如上图那个地方会要你填两个参数：

1.功能包名字

2.依赖（一般是roscpp、rospy、std_msgs）

然后写程序步骤就和之前差不多：

（1）ros包下面str新建文本文档，后缀为.cpp

/*
    控制台输出 HelloVSCode !!!
*/
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");//防止中文报错
    //执行节点初始化
    ros::init(argc,argv,"HelloVSCode");

    //输出日志
    ROS_INFO("Hello VSCode!!!哈哈哈哈哈哈哈哈哈哈");
    return 0;
}

以下代码意思：让最后输出中文不会报错

setlocale(LC_ALL,"");

（2）回到ros包，改cmaker的环境变量（步骤和之前说的一样，136）

add_executable(步骤3的源文件名
  src/步骤3的源文件名.cpp
)
target_link_libraries(步骤3的源文件名
  ${catkin_LIBRARIES}
)

（3）改完之后，编译的话，不用跑回终端，直接用快捷键ctrl+shirt+b就可以实现编译

（4）新建终端：在右下角，很容易看见，他会直接跳到工作空间，输入roscore

(5) 再新建终端：

source ./devel/setup.bash
rosrun ROS包名 ros包含的str里面的源文件名字

2.python

python在很大程度上和cpp差不多，以下为一些不同点

1.需要在ros包新建文件夹：scripts

2.python写程序：

#! /usr/bin/env python
"""
    Python 版本的 HelloVScode，执行在控制台输出 HelloVScode
    实现:
    1.导包
    2.初始化 ROS 节点
    3.日志输出 HelloWorld


"""

import rospy # 1.导包

if __name__ == "__main__":

    rospy.init_node("Hello_Vscode_p")  # 2.初始化 ROS 节点
    rospy.loginfo("Hello VScode, 我是 Python ....")  #3.日志输出 HelloWorld

注意：以下一行不能漏，是配置可执行权限的！！！

然后写完程序，还要在终端添加可执行权限,要鼠标右键点击scripts然后在终端打开：

chmod +x 自定义文件名.py

或者可以添加全部py文件的可执行权限：

chmod +x *.py

修改配置文件，163

catkin_install_python(PROGRAMS scripts/自定义文件名.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

编译：ctrl+shirt+b

新建窗口输入：roscore

这是如果是打开了其他终端，想要回去工作空间的话，要输入：

cd ~/工作空间名称/

注意：是~这个波浪线

然后source......../devel/setup.bash

rosrun......功能包........文件名.....

三、话题通信的实现

基本的模型图：

可以简单理解成发布者是发布消息，然后订阅者是接受消息，本节程序主要功能是：创建小海龟（官方已经给了库，直接调用），让小海龟动起来，其中发布者主要功能就是：实时发布速度信息。动起来后，小海龟位置不断变化，其中订阅者功能主要功能就是：实时反馈小海龟的位置坐标信息（x轴和y轴）

更具体的模型图：

rosmaster相当于一个管理者的角色，即roscore，发布者发布信息，包括话题，地址，如果发布者发布的信息是订阅者需要的，那么两者可以构成通信，实现交互，但是有时候订阅者接受不到前几条信息，这是因为发布者发布信息的时候速度快，管理者还没配置好，此时可以在发布者中输入：

ros::Duration(3).sleep();//这一句是延时3秒的意思，延时之后，先让发布者进行休眠，让订阅者先启动，就能不丢失信息

如果想要直观的看到发布者和订阅者之间的关系，可以调用计算图（在终端输入）

rqt_graph

1、发布者

缺乏依赖，要改两个地方：

一/package

二/cmake

补充：终端输入rostopic echo 话题名称

可以直接打印出发布者的发布信息

字符串的拼接：引入sstream头文件,比如我是拼接hello和count，就采用以下方式

#include <sstream>

ros::stringstream ss;

ss<<"hello -->"<<count;

一些基本的步骤是不变的：1.创建工作空间；2.在src目录创建功能包（功能包的依赖有所改变，如下图所示，主要在建立功能包的时候新增了geometry_msgs,turtlesim(这是要调用官方小海龟要添加的)）；3，在功能包里面编写代码（cpp用到src，python需要新建scripts）；4.编译执行（这次观看古月视频，发现可以通过一个步骤删去重新配置环境的步骤（source devel/setup.bash），正常来说是要重新配置环境才能进行rosrun（跑程序）），（还发现一个技巧：CTRL+H（隐藏或者显示文件，特别好用）），编译cpp和py也是不同的，py需要给权限

cpp

（在ros包的src里面新建后缀为c的文件）

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc,char **argv)
{
    //节点初始化
    ros::init(argc,argv,"publisher");
    //创建节点句柄
    ros::NodeHandle n;
    //创建发布者，发布名为turtle1/cmd_vel的话题，消息类型为<geometry_msgs::Twist>，队列长度为10
    ros::Publisher turtle_vel_pub=n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    //设置循环频率
    ros::Rate loop_rate(10);
    int count=0;
    while(ros::ok())
    {
        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 command[%0.2f m/s,%0.2f rad/s]",
                            vel_msg.linear.x,vel_msg.angular.z);
        loop_rate.sleep();                                
    }
    return 0;
}

配置好后，改cmake：

add_executable(publisher_c src/publisher_c.cpp)
target_link_libraries(publisher_c ${catkin_LIBRARIES})

然后CTRL+shirt+b(注意：前提是配置好一键编译)

一键编译配置路径：

编译完后，开终端：rosrun

然后配置环境，先输入海龟的生成：

rosrun turtlesim turtlesim_node

如果想要海龟自己跑(可以自己用上下左右键控制海龟运动，但是那个光标要在键盘控制命令的终端里)：

rosrun turtlesim turtle_teleop_key

然后再新开终端（重新用source配环境）

rosrun learning_topic publisher_c

注意，这里有个很奇怪的点，就是跑cpp程序时不用加后缀，但是如果是跑py程序时一定要后缀，比如：

rosrun learning_topic publisher_py.py

不然py的运行不了

py

然后对于py的，和cpp差不多，这里将一下代码，在ros包新建scripts，然后输入：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
import rospy
from geometry_msgs.msg import Twist
def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('publisher', anonymous=True)

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

		# 发布消息
        turtle_vel_pub.publish(vel_msg)
        rospy.loginfo("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
                vel_msg.linear.x, vel_msg.angular.z)
		# 按照循环频率延时
        rate.sleep()
if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

然后千万记得给权限，不然不给你干活：

chmod +x 自定义文件名.py

然后按照上面步骤走就可以，cmakes不要忘记，py的cmakes也是要后缀，就是rosrun记得加后缀，为什么呢，可能和devel文件有关，因为不加后缀他找不到py这个文件了，然后在devel里面就这样子显示（就py文件比较奇怪）：

2、订阅者

补充：有时候发布者比订阅者先启动，就会使得订阅者错失消息，那么可以在发布者的循环之前，加上休眠：

ros::Duration().sleep();
//催眠多少直接在duration里面填入相应的参数即可，注意单位是s

总体来说，和发布者差不多：

1.cpp

cpp文件代码：

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 
#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，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

配置cmake：

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

2.py

文件代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

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():
	# ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)

	# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

配置cmake：

catkin_install_python(PROGRAMS
  scripts/pose_subscriber.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

四、launch文件封装

如果节点数量很多，一个一个执行起来很麻烦，这时候就要用到launch文件，一键执行全部节点

launch文件的建立和c++，py的可编译文件建立也是很类似的

由上图可以看出，先新建一个功能包，添加依赖，常用依赖就是roscpp rospy std_msgs,然后在str目录下面，右击新建文件，文件的后缀一定要是.launch，建好后，就可以输入指令：

<launch>
    <node pkg="" type="" name="" output="screen" />
</launch>

以上是一个最基础的一个框架，pkg是功能包的名称，想封装哪些文件，就要先找到相应的功能包，type是可执行节点，如果是封装cpp的话，将相应的文件去掉后缀即可，name是自己起的名字，根据自己喜好决定，主要就是起到一个区分的作用，output就是输出到哪里的意思，以上是“screen”，就是输出到屏幕上。

然后还能在内部添加很多指令，是添加到

里面，用空格隔开即可，然后每次都要重编译（好像保存也是可以的）

常用的launch指令集：

pkg="包名"

节点所属的包

type="nodeType"

节点类型(与之相同名称的可执行文件)

name="nodeName"

节点名称(在 ROS 网络拓扑中节点的名称)

args="xxx xxx xxx" (可选)

将参数传递给节点

machine="机器名"

在指定机器上启动节点

respawn="true | false" (可选)

如果节点退出，是否自动重启

respawn_delay=" N" (可选)

如果 respawn 为 true, 那么延迟 N 秒后启动节点

required="true | false" (可选)

该节点是否必须，如果为 true,那么如果该节点退出，将杀死整个 roslaunch

ns="xxx" (可选)

在指定命名空间 xxx 中启动节点

clear_params="true | false" (可选)

在启动前，删除节点的私有空间的所有参数

output="log | screen" (可选)

日志发送目标，可以设置为 log 日志文件，或 screen 屏幕,默认是 log

编写好launch之后，CTRL+SHIRT+B编译，打开终端，不用roscore，但是要更新环境变量：

source devel/setup.bash

然后，直接跑程序：

roslaunch 功能包名 launch文件名

五、服务通信的实现

概念图：

注意也是通过相同话题将client和server链接在一起

client（客户端）和server（服务端），客户端需要完成某种需求，将需求发送给ros master，master为其找到合适的服务端,服务端处理好之后将结果反映给客户端。一个节点向另一个节点发数据，然后另一个节点响应。

主要实现流程：

1.srv的实现

定义srv，类似msg消息的创建

1.新建一个ros包，给好依赖

2.在ros包新建一个文件夹（可命名为srv），然后在该文件夹下新建后缀为srv的文件，输入文本（就是想发什么，然后想得到什么），需求和响应用”---“隔开，如下图

新建srv文件夹以及srv文件

在srv文件内定义想得到什么消息

3.编辑配置文件

message_generation：这是一个编译时需要的软件包。在开发过程中，当需要定义新的ROS消息时，message_generation包会被用来生成必要的代码，这些代码允许ROS系统理解和处理这些新定义的消息。
message_runtime：这是一个运行时需要的软件包。当ROS系统运行并且需要处理消息时，message_runtime包就会被使用。它提供了运行时支持，使得ROS节点能够发布和订阅消息。

（1）package.xml

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

(2) CmakeLists

开头

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)
# 需要加入 message_generation,必须有 std_msgs

约58行

add_service_files(
  FILES
  AddInts.srv（创建的srv文件的名称）
)

约72行

generate_messages(
  DEPENDENCIES
  std_msgs
)

约105行

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES plumbing_server_client
 CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

然后ctrl+shirt+b一键编译

2.具体路径配置

为了防止跑异常，需要在（vscode）c_cpp_properies.json上的path中输入include的路径，路径可以通过右击打开include的集成终端，输入pwd，然后直接将路径复制，把路径粘贴到include_path中，记得复制上去之后，前一条路径后面要加上"," ,不然会跑异常

"/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径

3.在ros包的src目录新建文件

1.服务端（server）

1.新建后缀为cpp的文件

输入以下代码，这个是实现两个整数相加减的，但是大概流程是这样

第二个include,包含的是<功能包名字/srv的文件名.h>

节点名称保持唯一，但是可以自己按照喜好起

创建对象部分，输入的参数是<话题+回调函数>，回调函数返回的是布尔类型

回调函数返回布尔类型的数，因此在回调函数前面加上bool

回调函数的引用参数（ros功能包名::srv文件去掉后缀，然后其他就是按模板来，就是请求对象和响应对象）

/*
    需求: 
        编写两个节点实现服务通信，客户端节点需要提交两个整数到服务器
        服务器需要解析客户端提交的数据，相加后，将结果响应回客户端，
        客户端再解析

    服务器实现:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建 服务 对象
        5.回调函数处理请求并产生响应
        6.由于请求有多个，需要调用 ros::spin()
*/
#include "ros/ros.h"
#include "ros package/srv.AddInts.h"
// bool 返回值由于标志是否处理成功
bool doReq(demo03_server_client::AddInts::Request& req,
          demo03_server_client::AddInts::Response& resp){
    int num1 = req.num1;
    int num2 = req.num2;
    ROS_INFO("服务器接收到的请求数据为:num1 = %d, num2 = %d",num1, num2);
    //逻辑处理
    if (num1 < 0 || num2 < 0)
    {
        ROS_ERROR("提交的数据异常:数据不可以为负数");
        return false;
    }
    //如果没有异常，那么相加并将结果赋值给 resp
    resp.sum = num1 + num2;
    return true;
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"AddInts_Server");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建 服务 对象
    ros::ServiceServer server = nh.advertiseService("AddInts",doReq);
    ROS_INFO("服务已经启动....");
    //     5.回调函数处理请求并产生响应
    //     6.由于请求有多个，需要调用 ros::spin()
    ros::spin();
    return 0;
}

2.配置cmake

名称和cpp文件一致（约136行）

add_executable(AddInts_Server src/AddInts_Server.cpp)

add_dependencies(AddInts_Server ${PROJECT_NAME}_gencpp)

target_link_libraries(AddInts_Server
  ${catkin_LIBRARIES}
)

3.跑程序

一般来说，编写完server是不会直接运行的，要有客户端发出请求，但是可以用内部程序测试：

先通过source更新环境，然后输入：（记得在话题名称后面，先输入一个空格隔开，再通过tab补齐，不然tab不会生效）

rosservice call 话题名称 +Tab

此时可以通过键盘的左、右键去更改此时的输入参数，然后回车得到反映结果

2、客户端

在原来的ros包的src目录，新建后缀为cpp的文件

不用加回调函数了

第二个include也是<功能包名字/srv的文件名.h>

/*
    需求: 
        编写两个节点实现服务通信，客户端节点需要提交两个整数到服务器
        服务器需要解析客户端提交的数据，相加后，将结果响应回客户端，
        客户端再解析
    服务器实现:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建 客户端 对象
        5.请求服务，接收响应
*/
// 1.包含头文件
#include "ros/ros.h"
#include "demo03_server_client/AddInts.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 调用时动态传值,如果通过 launch 的 args 传参，需要传递的参数个数 +3
    if (argc != 3)
    // if (argc != 5)//launch 传参(0-文件路径 1传入的参数 2传入的参数 3节点名称 4日志路径)
    {
        ROS_ERROR("请提交两个整数");
        return 1;
    }
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"AddInts_Client");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建 客户端 对象
    ros::ServiceClient client = nh.serviceClient<ros_package::AddInts>("topic");
    //等待服务启动成功
    //方式1
    ros::service::waitForService("AddInts");
    //方式2
    // client.waitForExistence();
    // 5.组织请求数据
    demo03_server_client::AddInts ai;
    ai.request.num1 = atoi(argv[1]);
    ai.request.num2 = atoi(argv[2]);
    // 6.发送请求,返回 bool 值，标记是否成功
    bool flag = client.call(ai);
    // 7.处理响应
    if (flag)
    {
        ROS_INFO("请求正常处理,响应结果:%d",ai.response.sum);
    }
    else
    {
        ROS_ERROR("请求处理失败....");
        return 1;
    }
    return 0;
}

然后配置相关文件，用cpp文件来配置


add_executable(AddInts_Client src/AddInts_Client.cpp)
add_dependencies(AddInts_Client ${PROJECT_NAME}_gencpp)
target_link_libraries(AddInts_Client
  ${catkin_LIBRARIES}
)

1.argv数组灵活应用

atoi（）->强制转整型

.c_str()->转字符型

argc 是一个整数，表示命令行参数的数量。
argv 是一个指针数组，每个元素指向一个命令行参数。
比如运行节点 '' rosrun 功能包 my_program 10 20 '',那么argc等于3，因为有3个命令行参数，argv也是3，但是是以数组的形式存在，具体指向某个数，比如argv[ 0 ]=my_program,argv[ 1 ]=10,argv[ 2 ]=20

(假设我们需要输入两个数字)跑程序用到的命令行是

rosrun xxxxx(ros包) xxxxx(具体的程序名) 参数1 参数2

而这个跑程序的命令行中，节点程序名+参数1+参数2 是我们需要输入的3个变量，那么argc=3，argv=3，然后可以用argv具体指代输入的参数，这样就可以不直接定义数字大小，实现自己手动输入参数

想实现自己输入数字，就比如想输入两个数字，可以用argv[ 1 ],argv[ 2 ]表示，但是这个获取的是字符串类型，需要用atoi（）转为整形

2.如果想先启用客户端

一般只有启动服务器端，才能启动客户端

有两种方法：

1.简单一点

client.waitForExitence();

2.难一点

topic是话题

ros::service::waitForService("topic");

六、action通信的实现

action通信和服务通信的实现差不多，但是action通信应用性更广

1.实现场景

适用于操作耗时，需要得到连续反馈（就是在一段时间就会反馈一次，时间可以自己定，每次都能收到一个小结果，可以用百分比显示任务条）

2.通信接口图

红色为发送请求，蓝色为接受请求

1.定义action文件：

1.先创建一个ros功能包，依赖要包括以下内容（使用到actionlib）：

roscpp rospy std_msgs actionlib actionlib_msgs

创建好之后，建立一个文件夹来存放action的文件，文件夹名字可以自己取，但是创建的文件一定要后缀名为action ,action文件中，要包含以下3个内容（意思是得到的结果是什么，就启用什么类型），用点划线分割开：

#目标值
int32 num
---
#最终结果
int32 result
---
#连续反馈
float64 progress_bar

2.配置cmake

一共要改3个地方：

1.66行

add_action_files(
  FILES
  所创建的action文件名
)

2.72行(取消actionlib和std的注释)

generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs
)

3.107行(取消catkin那一行的注释)

catkin_package(

#  INCLUDE_DIRS include
#  LIBRARIES demo04_action

 CATKIN_DEPENDS roscpp rospy std_msgs actionlib actionlib_msgs

#  DEPENDS system_lib

)

4.配置完之后，就可以ctrl+shirt+b进行一键编译（当然，进行这一步需要先配置一键编译，上文已经提及，最好是创建好文件，进入vs code就设置好一键配置）

5.在进行服务端客户端配置时，可以先将devel中include在集成终端中打开,输入pwd,将路径复制下来配置到c_cpp_properies.json

2.服务端

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "ros文件/action文件Action.h"

//经常性使用，可以定义为Server
typedef actionlib::SimpleActionServer<demo01_action::AddIntsAction> Server;


void cb(const demo01_action::AddIntsGoalConstPtr &goal,Server* server){
    //获取目标值
    int num = goal->num;
    ROS_INFO("目标值:%d",num);
    //累加并响应连续反馈
    int result = 0;
    demo01_action::AddIntsFeedback feedback;//连续反馈
    ros::Rate rate(10);//通过频率设置休眠时间,这里为1/10秒，就是0.1毫秒
    for (int i = 1; i <= num; i++)//这是这里需要实现的功能函数，类似的，可以每次执行完，休眠一次
    {
        result += i;
        //组织连续数据并发布
        feedback.progress_bar = i / (double)num;//执行过程中需要反馈的结果
        server->publishFeedback(feedback);
        rate.sleep();
    }
    //设置最终结果
    demo01_action::AddIntsResult r;
    r.result = result;
    server->setSucceeded(r);
    ROS_INFO("最终结果:%d",r.result);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("action服务端实现");
    // 2.初始化ROS节点;
    ros::init(argc,argv,"AddInts_server");
    // 3.创建NodeHandle;
    ros::NodeHandle nh;
    // 4.创建action服务对象;然后以下是具体一些参数
    /*SimpleActionServer(ros::NodeHandle n, 
                        std::string name, 
                        boost::function<void (const demo01_action::AddIntsGoalConstPtr &)> execute_callback, 
                        bool auto_start)
    */
    // actionlib::SimpleActionServer<ros包名::AddIntsAction> server(....);
    Server server(nh,"主题",boost::bind(&cb,_1,&server),false);
    server.start();
    // 5.处理请求,产生反馈与响应;

    // 6.spin().   
    ros::spin();
    return 0;
}

写完之后，还需要配置cmake(3个地方！！！)

验证服务端是否可以正常运行：

//roscore

//source...

1.rosnode list

2.rostopic list

3.使用rostopic中的话题来验证

(1)rostopic pub /主题/goal +tab补齐
(2)直接改数据,就是自身提交输入

4.rostopic echo /主题/feedback

5.rostopic echo /主题/result ->得到结果,打印出结果

3.客户端

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "ROS包名/action文件Action.h"

typedef actionlib::SimpleActionClient<ros包名::AddIntsAction> Client;


//处理最终结果
void done_cb(const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result){
    if (state.state_ == state.SUCCEEDED)
    {
        ROS_INFO("最终结果:%d",result->result);
    } else {
        ROS_INFO("任务失败！");
    }

}
//服务已经激活
void active_cb(){
    ROS_INFO("服务已经被激活....");
}
//处理连续反馈
void  feedback_cb(const demo01_action::AddIntsFeedbackConstPtr &feedback){
    ROS_INFO("当前进度:%.2f",feedback->progress_bar);
}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化ROS节点;
    ros::init(argc,argv,"AddInts_client");
    // 3.创建NodeHandle;
    ros::NodeHandle nh;
    // 4.创建action客户端对象;
    // SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
    // actionlib::SimpleActionClient<demo01_action::AddIntsAction> client(nh,"addInts");
    Client client(nh,"addInts",true);
    //等待服务启动
    client.waitForServer();
    // 5.发送目标，处理反馈以及最终结果;
    /*  
        void sendGoal(const demo01_action::AddIntsGoal &goal, 
            boost::function<void (const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result)> done_cb, 
            boost::function<void ()> active_cb, 
            boost::function<void (const demo01_action::AddIntsFeedbackConstPtr &feedback)> feedback_cb)

    */
    demo01_action::AddIntsGoal goal;
    goal.num = 10;

    client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
    // 6.spin().
    ros::spin();
    return 0;
}

同样的也需要配置cmake文件

配置好之后，就可以直接运行了，运行的步骤也是类似的。

七、TF坐标变换

概念图：

1.雷达和小车（相对位置之间的转换，将物体与雷达转换为物体与小车）

2.带机械臂的机器人（具有多个坐标系）

1.静态坐标变换

常用的信息载体：

发布者和订阅者

geometry_msgs/TransformStamped
{
std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

}

geometry_msgs/PointStamped
{
std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

}

2.一般来说，功能包中的依赖有：

 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

1.发布方

时间戳：

now是当前时间，0.0是取最近时间

ros::Time::now（）&ros::Time（0.0）

介绍一下例子的要求：

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，
坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对
于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一
障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，
该障碍物相对于主体的坐标是多少？

// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"//欧拉角和四元数的转换

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();//时间为现在
    ts.header.frame_id = "base_link";//父级坐标系，也就是基坐标系
    //----设置子级坐标系
    ts.child_frame_id = "laser";
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);//翻滚角，俯仰角，偏航角
    //转化为四元数
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

2.订阅方

//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

八.参数服务的实现

1.参数的设置/增添

参数服务主要使用两套api（通过其中一个api就可以实现）：

ros::NodeHandle

ros::param

对于nodehandle：
创建一个节点句柄，比如是nh

就可以调用nh.setparam去设置参数

或者是调用ros内部的param

比如ros::param::set去设置参数

在命令行输入rosparam list可以验证是否增加参数成功

想获得具体参数值，可以继续在命令行输入rosparam get 路径（这里路径参考通过list列举出来的路径）

如果是需要修改参数，可以在后面重复以上操作，注意名字要一致，这里可以理解成重置

示例代码：

#include "ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"can");
    ros::NodeHandle nh;
    nh.setParam("type","xioahuang");
    nh.setParam("rad",0.15);

    ros::param::set("ty_pa","dalao");
    ros::param::set("ra_pa",0.15);

    nh.setParam("rad",88);
    ros::param::set("ra_pa",66);
    /* code */
    return 0;
}

2.参数的查阅

常用查阅函数：

 参数服务器操作之查询_C++实现:
 在 roscpp 中提供了两套 API 实现参数操作

    ros::NodeHandle

        param(键,默认值) 
            存在，返回对应结果，否则返回默认值

        getParam(键,存储结果的变量)
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值

        getParamCached键,存储结果的变量)--性能上有提升，更快
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值

        getParamNames(std::vector<std::string>)
            获取所有的键,并存储在参数 vector 中 

        hasParam(键)
            是否包含某个键，存在返回 true，否则返回 false

        searchParam(参数1，参数2)
            搜索键，参数1是被搜索的键，参数2存储搜索结果的变量

    ros::param ----- 与 NodeHandle 类似

删除键：

/* 
    参数服务器操作之删除_C++实现:

    ros::NodeHandle
        deleteParam("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false

    ros::param
        del("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false


*/