Coche Raspberry Pi ROS basado en STM32F103: cree y configure un nuevo paquete

Prefacio

Para lograr que el automóvil vuelva automáticamente a la carga, es necesario extraer los datos de la nube de puntos escaneados por el radar y crear un nuevo paquete de funciones para programar el acoplamiento cercano del automóvil.

Crear un paquete ROS

Primero, cambie el directorio al espacio de trabajo (wheeltec_robot)

$ cd wheeltec_robot/src

Utilice el comando catkin_create_pkg para crear un paquete de programa llamado autorc, que se basa en std_msgs, roscpp, rospy

$ catkin_create_pkg autorc std_msgs rospy roscpp

Compile el paquete recién creado en el espacio de trabajo.

$ cd wheeltec_robot/

$ catkin_make

Escribir programas de publicación y suscripción.

Primero, cambie los directorios al paquete recién creado.

$ cd wheeltec_robot/src/autorc

Después de completar la compilación del paquete en el paso anterior, verá los archivos CmakeList.txt, package.xml y los directorios include y src en la carpeta autorc.
Luego vaya al subdirectorio src del paquete.

$ cd src

Cree un archivo cpp en el subdirectorio src.
Cree un archivo cpp de tema de publicación.

$ touch lidarpub.cpp

Su contenido es el siguiente:

#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;
}

Cree un archivo cpp de tema de suscripción, reciba la "palabra de saludo" publicada y reciba los datos de la nube de puntos escaneados por el radar

$ touch lidarsub.cpp

Su contenido es el siguiente:

#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;
}

Edite el archivo Cmakelists.txt

Antes de compilar el nuevo nodo creado, debe editar el archivo Cmakelist.txt (archivo Cmakelist.txt en el directorio del paquete autorc),
buscar la ubicación del archivo, hacer doble clic para abrir el archivo Cmakelist.txt o ingresar el comando para abrelo.

$ gedit CMakeLists.txt

Agregar al final del archivo

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)##出错则屏蔽该条程序

Cambie el directorio al espacio de trabajo wheeltec_robot y ejecute catkin_make para ejecutar el comando de compilación

$ gcd wheeltec_robot

Compile el paquete autorc recién creado por separado para mejorar la eficiencia de la compilación.

$ catkin_make -DCATKIN_WHITELIST_PACKAGES="autorc"

Ejecute el programa para imprimir datos.

Abra cuatro nuevas ventanas de terminal, inicie sesión ssh respectivamente y luego ejecute el comando
Ejecute la primera terminal para abrir el administrador de nodos.

$ roscore

La segunda terminal abre el nodo que publica datos de nubes de puntos de radar.

$ rosrun rplidar_ros rplidarNode

La tercera terminal abre el nuevo nodo de publicación de palabras de saludo.

$ rosrun autorc lidarpub

La cuarta terminal abre el nuevo nodo que se suscribe a datos de nube de puntos de radar y palabras de saludo.

$ rosrun autorc lidarsub

Abra una nueva terminal, ejecútela directamente y muestre el flujo de mensajes

$ rqt_graph

Continuaremos haciendo un seguimiento del contenido del automóvil que regresa automáticamente a la carga. El siguiente paso es la parte de procesamiento de datos después de suscribirse a los datos de la nube de puntos de escaneo del radar.