lego loam 跑镭神32线激光雷达

师弟反应镭神32线激光雷达（32C）录制的数据包不能跑lego loam，这里就总结一下。

首先lego loam默认的接受的topic name是velodyne_points，点云的frame_id是velodyne，镭神驱动发布的topic name为point_raw，frame_id为world这里需要写一个程序转一下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/PointCloud2.h"
#include <string>

#include <sstream>

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */

static ros::Publisher g_scan_pub;

static void main_topic_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
    sensor_msgs::PointCloud2 msg = *input;
    msg.header.frame_id = "velodyne";
    g_scan_pub.publish(msg);
}


int main(int argc, char *argv[])
{
    /**
     * The ros::init() function needs to see argc and argv so that it can perform
     * any ROS arguments and name remapping that were provided at the command line.
     * For programmatic remappings you can use a different version of init() which takes
     * remappings directly, but for most command-line programs, passing argc and argv is
     * the easiest way to do it.  The third argument to init() is the name of the node.
     *
     * You must call one of the versions of ros::init() before using any other
     * part of the ROS system.
     */
    ros::init(argc, argv, "trans_leishen_velodyne");

    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle private_nh("~");
    ros::NodeHandle n;

    /**
     * The advertise() function is how you tell ROS that you want to
     * publish on a given topic name. This invokes a call to the ROS
     * master node, which keeps a registry of who is publishing and who
     * is subscribing. After this advertise() call is made, the master
     * node will notify anyone who is trying to subscribe to this topic name,
     * and they will in turn negotiate a peer-to-peer connection with this
     * node.  advertise() returns a Publisher object which allows you to
     * publish messages on that topic through a call to publish().  Once
     * all copies of the returned Publisher object are destroyed, the topic
     * will be automatically unadvertised.
     *
     * The second parameter to advertise() is the size of the message queue
     * used for publishing messages.  If messages are published more quickly
     * than we can send them, the number here specifies how many messages to
     * buffer up before throwing some away.
     */

    std::string main_topic;
    std::string scan_topic;

    if (!private_nh.getParam("main_topic", main_topic))
    {
        ROS_ERROR("can not get main_topic!");
        exit(0);
    }

    if (!private_nh.getParam("scan_topic", scan_topic))
    {
        ROS_ERROR("can not get scan_topic!");
        exit(0);
    }

    g_scan_pub = n.advertise<sensor_msgs::PointCloud2>(scan_topic, 10);
    ros::Subscriber main_topic_sub = n.subscribe<sensor_msgs::PointCloud2>(main_topic, 10000, main_topic_callback);

    /**
     * A count of how many messages we have sent. This is used to create
     * a unique string for each message.
     */
    ros::spin();

    return 0;
}

在把我的launch文件(leishen_dispatcher.launch)也贴出来：

<launch>
  <arg name="scan_topic_name" default="velodyne_points" />
  <arg name="main_topic_name" default="point_raw" />

  <node pkg="record_gnss_pc" name="trans_leishen_velodyne" type="trans_leishen_velodyne" output="screen">  
    <param name="scan_topic" value="$(arg scan_topic_name)" />
    <param name="main_topic" value="$(arg main_topic_name)" />
  </node>
</launch>

在将代码lego loam中的utility.h文件中添加的镭神32C的配置文件（激光雷达扫描频率分别为5Hz与10Hz）也贴一下：

// LeiShen-32C-5Hz
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN = 4000;
// extern const float ang_res_x = 360.0 / float(Horizon_SCAN);
// extern const float ang_res_y = 26 / float(N_SCAN-1);
// extern const float ang_bottom = 16.5;
// extern const int groundScanInd = 10; //地面的线扫条数

// LeiShen-32C-10Hz
extern const int N_SCAN = 32;
extern const int Horizon_SCAN = 2000;
extern const float ang_res_x = 360.0 / float(Horizon_SCAN);
extern const float ang_res_y = 26.0 / float(N_SCAN-1);
extern const float ang_bottom = 16.5;
extern const int groundScanInd = 10; //地面的线扫条数

最后把imageProjection.cpp中159行的注释删除，程序部分调整完毕。

编译。

运行lego loam：

$ roslaunch lego_loam run.launch

运行上面的launch文件

$ roslaunch record_data leishen_dispatcher.launch

最后运行你的bag

$ rosbag play --clock 2019-06-21-10-27-25.bag

不要忘记--clock

就可以开始了。

 

标签：main,const,lego,loam,float,topic,32,extern,ros
来源： https://www.cnblogs.com/hgl0417/p/11067660.html