ROS基础学习（实现一个激光雷达避障）（二）

实现一个激光雷达避障

创建功能包

cd catkin_ws/src
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs

创建节点

lidar_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
ros::Publisher vel_pub;
static int nCount =0;

void LidarCallback(sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    int nMid = nNum/2;

    float fMidDist = msg.ranges[nMid];
    ROS_INFO("front distent ranges[180] = %f meter",fMidDist);
    geometry_msgs::Twist vel_msg;
    if(nCount>0){
        nCount --;
        return ;
    }
    
    if(fMidDist<1.2f){
        vel_msg.angular.z = 0.3;
        nCount = 50;
    }
    else{
        vel_msg.linear.x = 0.05;
    }
    vel_pub.publish(vel_msg);
}
int main(int argc, char  *argv[])
{
    //setlocale(LC_ALL,"zh_CN.UTF-8");
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan",10,&LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
    return 0;
}

创建编译规则

CMakeLists.txt
${PROJECT_NAME}_node 改为节点名字
cpp文件也改为对应文件

add_executable(lidar_node src/lidar_node.cpp)
add_dependencies(lidar_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(lidar_node
  ${catkin_LIBRARIES}
)

运行仿真环境并运行节点

roslaunch wpr_simulation wpb_simple.launch
//另开一个终端
rosrun lidar_pkg lidar_node 
//折叠数组查看雷达消息格式
rostopic echo /scan --noarr