实现ROS基本功能

实现ROS基本功能（运行自己编写的消息格式）

参考https://zhuanlan.zhihu.com/p/412801837

ROS维基官方教程：http://wiki.ros.org/cn/ROS/Tutorials/#A.2BUh1.2Bp2VZegs-

一、创建Package

• 命令行输入

catkin_create_pkg为创建包的指令，package_name为自定义的包名称，depend为创建的包所依赖的其他包（比如一些ros自带的package)，可写可不写。

catkin_create_pkg [package_name] [depend1] [depend2] [depend3]

• 举例

cd ~/catkin_ws/src
catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

//  控制台返回结果   //
Created file beginner_tutorials/CMakeLists.txt
Created file beginner_tutorials/package.xml
Created folder beginner_tutorials/include/beginner_tutorials
Created folder beginner_tutorials/src
Successfully created files in /home/yinton/catkin_ws/src/beginner_tutorials. Please adjust the values in package.xml.

系统将自动在该目录下生成一个名为beginner_tutorials的文件夹，内容如下

**include——**目前为空，放置头文件（.h）

**src——**目前为空，放置源文件（.cpp）

**CMakeLists.txt——**文件CMakeLists.txt是用于构建软件包的CMake构建系统的输入。任何符合CMake的软件包都包含一个或多个CMakeLists.txt文件，该文件描述如何构建代码以及在何处安装代码。需要使用到某些函数时可去掉注释。内容很长。

package.xml——参考这里 包含了 package 的名称、版本号、内容描述、维护人员、软件许可、编译构建工具、编译依赖、运行依赖等信息。部分内容如下

 beginner_tutorials  //内容描述和包名称
yinton                             // 维护人员
BSD                              // 软件许可
               
http://ros.org/wiki/beginner_tutorials   
                        // 运行依赖项

• 在工作区中编译package

cd ~/catkin_ws
catkin_make

可以看到增加了目录 beginner_tutorials

• 为工作空间添加环境变量到ROS环境

source ~/catkin_ws/devel/setup.bash 

• 注意事项
  1. 需要在ROS_PACKAGEPATH下创建包，一般为 catkin_ws/src ,安装到别的地方会产生警告
  2. 注意**不能用roscreate-pkg**命令，会无法编译package

二、创建ROS消息（message）

• 定义 msg（消息）：msg文件就是文本文件，用于描述ROS消息的字段。它们用于为不同编程语言编写的消息生成源代码
• 命令行指令

cd catkin_ws/src/beginner_tutorials/
mkdir msg                                // 创建新的文件夹存放消息
echo "string user_name 
>（换行）int64 age " > msg/MyFirstMsg.msg // 创建数据类型为string，名称为user_name
                                         // 和数据类型为int64，名称为age的数据类型
                                         // 消息命名为MyFirstMsg.msg，存放在msg文件夹

• 配置修改

1.修改package.xml（新版）

要确保msg文件能被转换为C++、Python和其他语言的源代码。用文本编译器打开xml，在最后添加

message_generation  
// 构建ROS消息
message_runtime       
// 运行ROS消息

2.修改CMakeLists.txt

加入以下内容（其中前三项为创建包时的依赖项）

//   加入message_generation才能产生消息   // 

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)

//   确保导出消息的运行时依赖关系，catkin_package()添加message_runtime  /

catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime)

//   手动添加message.msg文件           ///

add_message_files(
   FILES
   MyFirstMsg.msg
 )

//   确保generate_messages()函数被调用   

generate_messages(
   DEPENDENCIES
   std_msgs
 )

• Msg的使用

查看消息创建是否成功。删除.msg文件后，rosmsg show便不会再显示该消息。

rosmsg show MyFirstMsg.msg

//  显示如下内容  //
[beginner_tutorials/MyFirstMsg]:  // 【定义消息的软件包/消息名称]
string user_name                  //  定义的消息类型
int64 age

三、创建srv信息（service）

• 定义 srv（服务）：一个srv文件描述一个服务。它由两部分组成：请求（request）和响应（response），新建srv文件夹存放

cd catkin_ws/src/beginner_tutorials/
mkdir srv

• srv文件的创建

从另一个包复制现有的srv定义，而不是手动创建新的srv。roscp是一个实用的命令行工具，用于将文件从一个ros已有的srv包复制到另一个包

这里是从srv/addTwoInts.src 复制到当前目录的AddTwoInts.srv

roscp [package_name] [file_to_copy_path] [copy_path]

• 举例

roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv  //  复制了ros已有服务

• 配置修改

1.修改package.xml（新版）

要确保srv文件能被转换为C++、Python和其他语言的源代码。打开旧版xml，在最后添加

message_generation  // 构建ROS消息
message_runtime       // 运行ROS消息

2.修改CMakeLists.txt

在旧版中加入以下内容（其中前三项为创建包时的依赖项）

//   加入message_generation才能产生消息   // 

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)

//   手动添加AddTwoInts.srv文件           ///

add_service_files(
  FILES
  AddTwoInts.srv
)

• Srv的使用

查看服务创建是否成功

rossrv show AddTwoInts

//  显示如下内容  //
[beginner_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

[rospy_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

这里显示了两个服务。第一个是刚刚在beginner_tutorials包中创建的，第二个是之前rospy_tutorials包中已经存在的。

四、编写发布（Publisher）和订阅（Subscriber）节点

• 定义 节点：连接到ROS网络的可执行文件
• 创建发布节点

发布节点文件名为talker.cpp，内含publisher；在src文件夹下创建talker.cpp，内容如下

1.初始化ROS系统

2.向主节点宣告我们将要在chatter话题上发布std_msgs/String类型的消息

3.以每秒10次的速率向chatter循环发布消息

#include "ros/ros.h"                // 包括了使用ROS系统中最常见的公共部分所需的全部头文件
#include "std_msgs/String.h"        // 引用了位于std_msgs包里的std_msgs/String消息
#include 

int main(int argc, char **argv)
{
   ros::init(argc, argv, "talker");   // 初始化ros节点，从命令行接受参数argc,argv，设置节点名称
   ros::NodeHandle n;                 // 句柄是与ROS系统通信的主要访问点. 第一个被构造的NodeHandle将完全初始化此节点
                                      // 最后一个被析构的NodeHandle将解构此节点
   ros::Publisher chatter_pub = n.advertise("chatter", 1000);
   // advertise函数告诉主线master：将发布一个类型为std_msgs::String的消息到topic "chatter"
   // 发布的队列为1000（防止发布太快，建立一个大小为1000的缓冲区，表示缓存多少信息后开始舍弃多余的信息
   // 当所有的这个对象ros::Publisher都被销毁时，topic会自动的被回收。

   ros::Rate loop_rate(10);           // 循环运行的频率为10Hz，与loop_rate.sleep()结合使用，在后面
                                      // 根据Rate::sleep()出现的位置，去消耗时间来使程序运行总时间满足设定的频率
   
   int count = 0;
   while (ros::ok())
   {
    // ros::ok()返回false，代表可能发生了以下事件  
	   // 1.SIGINT被触发(Ctrl-C)调用了ros::shutdown()
	   // 2.被另一同名节点踢出 ROS 网络
	   // 3.ros::shutdown()被程序的另一部分调用
	   // 4.节点中的所有ros::NodeHandles 都已经被销毁 
      std_msgs::String msg;
      std::stringstream ss;
      ss << "hello world " << count;   // 输出 hello world
      msg.data = ss.str();             // 由于msg数据类型是string 可设置其内容为子服从ss

      ROS_INFO("%s", msg.data.c_str());// 在终端显示

      chatter_pub.publish(msg);        // 将消息发布出去,广播给了任何已连接的节点
      ros::spinOnce();
                                       // ros::spin() 在调用后不会再返回，也就是你的主程序到这儿就不往下执行了
                                       //  而 ros::spinOnce() 后者在调用后还可以继续执行之后的程序
                                       //  注意ros::spinOnce()/ros::spin()不可缺乏，否则消息发不出去

      loop_rate.sleep();               //  用ros::Rate在剩下的时间内睡眠，以让我们达到10Hz的发布速率
      ++count;
   }

   return 0;
}

可能第一次看到这些数据类型会很不习惯，但它们的名称和使用都是固定的，多看就能接受了

• 创建订阅节点

发布节点文件名为listener.cpp，内含Subscriber；在src文件夹下创建listener.cpp，内容如下

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg) // 回调函数，当有新消息到达chatter话题时它就会被调用
{ // n.advertise("chatter", 1000) 数据类型要一致
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");  // 初始化ros节点，从命令行接受参数argc,argv，设置节点名称
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); // 订阅名为"chatter"的主题，并调用回调函数chatterCallback
                                      // 第二个参数是队列大小，以防我们处理消息的速度不够快。在本例中，如果队列达到1000条，再有新消息到达时旧消息会被丢弃
  ros::spin();                        // 启动了一个自循环，它会尽可能快地调用消息回调函数
  
  return 0;
}

• 配置修改

1.修改CMakeLists.txt

///  在文件末尾添加以下几行  //
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})      # 使用以下变量来依赖所有必需的目标
add_dependencies(listener beginner_tutorials_generate_messages_cpp) # 为可执行目标添加依赖项到消息生成目标 

生成两个可执行文件talker和listener并注明其源码路径，最终可执行文件存储在devel文件夹中,devel文件夹存放开发文件

注意！！

1.使用roscreate-pkg创建的CMakeLists.txt有点问题，如果按roscreate-pkg的CMakeLists.txt填写，执行catkin_make之后无法生成可执行文件/节点talker和listener，需要重新操作

2.启动roscore需要正常联网！否则ros的任何命令都没反应。

五、测试订阅和发布节点、测试自定义消息格式

• 测试订阅和发布节点

新开3个终端，分别输入

roscore 
rosrun beginner_tutorials talker
rosrun beginner_tutorials listener

发布者发布的消息：

订阅者收到的消息

• 测试自定义消息格式

下面使用自定义的消息格式

//  beginner_tutorials/msg/MyFirstMsg.msg  //
string user_name
int64 age

需要修改

1.src/talker.cpp

①加入消息头文件 ②修改发布类型为beginner_tutorials::MyFirstMsg.msg ③修改终端输出

#include  // 虽然.msg不算头文件，但这样写就能包括进来
                                           // 包名/消息名.h
ros::Publisher chatter_pub = n.advertise("chatter", 1000);


在while函数中修改（赋值+显示+发布）
beginner_tutorials::MyFirstMsg msg1;
msg1.user_name = "yinton";
msg1.age = 12;
      
ROS_INFO("user_name:%s,age:%i", msg1.user_name.c_str(),msg1.age);// 在终端显示
      
chatter_pub.publish(msg1);        // 将消息发布出去,广播给了任何已连接的节点      

2.src/listener

①加入消息头文件 ②修改发布类型为beginner_tutorials::MyFirstMsg.msg ③修改终端输出

#include 

void chatterCallback(const beginner_tutorials::MyFirstMsg::ConstPtr& msg)
{
  ROS_INFO("I heard: user_name:[%s],age:[%i]", msg->user_name.c_str(), msg->age);
}

3.package.xml

加入依赖项msg文件夹，注意是exec_depend，写成run depend无法编译

msg
msg

最后执行编译

cd catkin_ws
catkin_make

再新开三个终端即可运行

#include

void chatterCallback(const beginner_tutorials::MyFirstMsg::ConstPtr& msg)
{
ROS_INFO(“I heard: user_name:[%s],age:[%i]”, msg->user_name.c_str(), msg->age);
}

3.package.xml

加入依赖项msg文件夹，注意是exec_depend，写成run depend无法编译

```text
msg
msg

最后执行编译

cd catkin_ws
catkin_make

再新开三个终端即可运行