基于STM32F103的树莓派ROS小车——创建配置新程序包

序言

为了实现小车自动返回充电，需要将雷达扫描的点云数据提取出来，并新建一个功能包，编程实现小车的近距对接。

创建ROS程序包（package）

首先，将目录切换到工作区（wheeltec_robot）

$ cd wheeltec_robot/src

使用catkin_create_pkg命令创建一个叫autorc的程序包，这个包依靠std_msgs、roscpp、rospy

$ catkin_create_pkg autorc std_msgs rospy roscpp

在工作区编译新建的程序包

$ cd wheeltec_robot/

$ catkin_make

编写发布、订阅程序

首先，将目录切换到新建的程序包中

$ cd wheeltec_robot/src/autorc

上一步完成程序包的编译之后，会在autorc文件夹下看到CmakeList.txt、package.xml文件和include、src这两个目录。
接下来进入程序包的src子目录

$ cd src

在src子目录下创建cpp文件
创建发布话题cpp文件

$ touch lidarpub.cpp

其内容如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "lidarpub");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Rate loop_rate(10);
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

创建订阅话题cpp文件，接收发布的“hello word ”，并接收雷达扫描的点云数据

$ touch lidarsub.cpp

其内容如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/LaserScan.h"
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int count = scan->scan_time / scan->time_increment;
    //count为激光雷达旋转一圈所对应获得激光反射点的数量
    //scan->scan_time为旋转一圈所需要的时间
    //scan->time_increment为每读取一个点所需要的时间
    ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
    ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
    //scan->angle_min和scan->angle_max分别对应旋转一周中最小的角度和最大的角度,分别为-pi和pi,通过RAD2DEG转化成角度
    for(int i = 0; i < count; i++) 
    {
        float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
        ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
    //输出旋转一周里所有点云的角度和距离,相当于对应极坐标
    //scan->angle_increment为两个点之间的角度差值,接近于pi/180,也就是1度
    //scan->range[i]为每个点离原点的距离
    }
}
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "lidarsub");
  ros::NodeHandle n;
  ros::Subscriber chatter_sub = n.subscribe("chatter", 1000, chatterCallback)；
  ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
  ros::spin();
  return 0;
}

编辑Cmakelists.txt文件

在编译创建的新节点之前，需要编辑Cmakelist.txt文件（autorc程序包目录下的Cmakelist.txt文件）
找到文件所在位置双击打开Cmakelist.txt文件或者输入指令打开

$ gedit CMakeLists.txt

在文件末尾添加

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(lidarpub src/lidarpub.cpp)
target_link_libraries(lidarpub ${catkin_LIBRARIES})
add_dependencies(lidarpub autorc_generate_messages_cpp)##出错则屏蔽该条程序

add_executable(lidarsub src/lidarsub.cpp)
target_link_libraries(lidarsub ${catkin_LIBRARIES})
add_dependencies(lidarsub autorc_generate_messages_cpp)##出错则屏蔽该条程序

将目录切换到wheeltec_robot工作区，并执行catkin_make运行编译命令

$ gcd wheeltec_robot

单独编译新建的程序包autorc，提高编译效率

$ catkin_make -DCATKIN_WHITELIST_PACKAGES="autorc"

运行程序打印数据

打开四个新的终端窗口，分别执行ssh登录，然后运行命令
第一个终端运行开启节点管理器

$ roscore

第二个终端开启发布雷达点云数据的节点

$ rosrun rplidar_ros rplidarNode

第三个终端开启新建的发布hello word节点

$ rosrun autorc lidarpub

第四个终端开启新建的订阅hello word和雷达点云数据的节点

$ rosrun autorc lidarsub

打开一个新的终端，直接运行，显示消息的流向

$ rqt_graph

后续会继续跟进小车自动返回充电的内容，下一步是订阅到雷达扫描点云数据之后的数据处理部分。