师弟反应镭神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
就可以开始了。