一步一步复现loam<一>、rviz可视化rosbag发布的数据

一、在空的工作空间创建功能包

catkin_create_pkg aloam roscpp rospy rosbag nav_msgs  geometry_msgs  image_transport tf  std_msgs  sensor_msgs

　　后续发现依赖别的包，可以在package.xml中修改。

 1 <?xml version="1.0"?>
 2 <package format="2">
 3   <name>aloam</name>
 4   <version>0.0.0</version>
 5   <description>The aloam package</description>
 6 
 7   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8   <!-- Example:  -->
 9   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10   <maintainer email="sry@todo.todo">sry</maintainer>
11 
12 
13   <!-- One license tag required, multiple allowed, one license per tag -->
14   <!-- Commonly used license strings: -->
15   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16   <license>TODO</license>
17 
18 
19   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21   <!-- Example: -->
22   <!-- <url type="website">http://wiki.ros.org/aloam</url> -->
23 
24 
25   <!-- Author tags are optional, multiple are allowed, one per tag -->
26   <!-- Authors do not have to be maintainers, but could be -->
27   <!-- Example: -->
28   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 
30 
31   <!-- The *depend tags are used to specify dependencies -->
32   <!-- Dependencies can be catkin packages or system dependencies -->
33   <!-- Examples: -->
34   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35   <!--   <depend>roscpp</depend> -->
36   <!--   Note that this is equivalent to the following: -->
37   <!--   <build_depend>roscpp</build_depend> -->
38   <!--   <exec_depend>roscpp</exec_depend> -->
39   <!-- Use build_depend for packages you need at compile time: -->
40   <!--   <build_depend>message_generation</build_depend> -->
41   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42   <!--   <build_export_depend>message_generation</build_export_depend> -->
43   <!-- Use buildtool_depend for build tool packages: -->
44   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45   <!-- Use exec_depend for packages you need at runtime: -->
46   <!--   <exec_depend>message_runtime</exec_depend> -->
47   <!-- Use test_depend for packages you need only for testing: -->
48   <!--   <test_depend>gtest</test_depend> -->
49   <!-- Use doc_depend for packages you need only for building documentation: -->
50   <!--   <doc_depend>doxygen</doc_depend> -->
51   <buildtool_depend>catkin</buildtool_depend>
52   <build_depend>geometry_msgs</build_depend>
53   <build_depend>image_transport</build_depend>
54   <build_depend>nav_msgs</build_depend>
55   <build_depend>rosbag</build_depend>
56   <build_depend>roscpp</build_depend>
57   <build_depend>rospy</build_depend>
58   <build_depend>sensor_msgs</build_depend>
59   <build_depend>std_msgs</build_depend>
60   <build_depend>tf</build_depend>
61   <build_export_depend>geometry_msgs</build_export_depend>
62   <build_export_depend>image_transport</build_export_depend>
63   <build_export_depend>nav_msgs</build_export_depend>
64   <build_export_depend>rosbag</build_export_depend>
65   <build_export_depend>roscpp</build_export_depend>
66   <build_export_depend>rospy</build_export_depend>
67   <build_export_depend>sensor_msgs</build_export_depend>
68   <build_export_depend>std_msgs</build_export_depend>
69   <build_export_depend>tf</build_export_depend>
70   <exec_depend>geometry_msgs</exec_depend>
71   <exec_depend>image_transport</exec_depend>
72   <exec_depend>nav_msgs</exec_depend>
73   <exec_depend>rosbag</exec_depend>
74   <exec_depend>roscpp</exec_depend>
75   <exec_depend>rospy</exec_depend>
76   <exec_depend>sensor_msgs</exec_depend>
77   <exec_depend>std_msgs</exec_depend>
78   <exec_depend>tf</exec_depend>
79 
80 
81   <!-- The export tag contains other, unspecified, tags -->
82   <export>
83     <!-- Other tools can request additional information be placed here -->
84 
85   </export>
86 </package>
View Code

二、添加依赖库

　　package.xml中，不用修改，在CMakelist中添加如下内容：

add_executable(ascanRegistration src/scanRegistration.cpp)
target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})

　　在功能包src目录下新建 scanRegistration.cpp，准备编码。

 　　在CMakelist中添加第三方依赖库：OpenCV、PCL、Eigen等等。

 1 cmake_minimum_required(VERSION 3.0.2)
 2 project(aloam)
 3 
 4 ## Compile as C++11, supported in ROS Kinetic and newer
 5 # add_compile_options(-std=c++11)
 6 set(CMAKE_BUILD_TYPE "Release")
 7 set(CMAKE_CXX_FLAGS "-std=c++14")
 8 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 9 
10 ## Find catkin macros and libraries
11 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
12 ## is used, also find other catkin packages
13 find_package(catkin REQUIRED COMPONENTS
14         geometry_msgs
15         image_transport
16         nav_msgs
17         rosbag
18         roscpp
19         rospy
20         sensor_msgs
21         std_msgs
22         cv_bridge
23         tf
24         )
25 
26 find_package(PCL REQUIRED)
27 find_package(OpenCV REQUIRED)
28 
29 
30 include_directories(
31         include
32         ${catkin_INCLUDE_DIRS}
33         ${PCL_INCLUDE_DIRS}
34         ${OpenCV_INCLUDE_DIRS}
35 )
36 
37 catkin_package(
38         #  INCLUDE_DIRS include
39         #  LIBRARIES aloam
40         CATKIN_DEPENDS geometry_msgs image_transport nav_msgs rosbag roscpp rospy sensor_msgs std_msgs tf
41         DEPENDS EIGEN3 PCL
42         INCLUDE_DIRS include
43         #  DEPENDS system_lib
44 )
45 
46 
47 
48 
49 add_executable(ascanRegistration src/scanRegistration.cpp)
50 target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
View Code

三、开始编码

　　我们可以采用rosbag播放数据，直接拿rviz作可视化，参考我的博客：https://www.cnblogs.com/winslam/p/15064478.html

　　现在咱们顺序是：rosbag播放数据 -> 算法处理 -> rviz可视化，如下图：

 

　　知道框架以后，就可以编码了，已知：

rosbag发布节点：/velodyne_points  #默认
自定义帧ID：/camera_init
自定义发布节点：velodyne_cloud_2
 1 //
 2 // Created by sry on 2021/7/26.
 3 //
 4 #include <iostream>
 5 #include<opencv2/opencv.hpp>
 6 
 7 #include<ros/ros.h>
 8 #include <pcl/point_cloud.h>
 9 #include <pcl_conversions/pcl_conversions.h>
10 #include<sensor_msgs/PointCloud2.h>
11 #include<tf/transform_datatypes.h>
12 
13 typedef pcl::PointXYZI PointType;
14 //pcl::PointCloud<PointType> g_Points;
15 ros::Publisher pubLaserCloud;
16 
17 // 激光点云接收函数
18 void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
19 {
20     // 接受 topic：velodyne_points 发布的点云
21     pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
22     pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
23     std::cout << laserCloudIn.points.size() << "; w = " << laserCloudIn.width << "; h = " << laserCloudIn.height << std::endl;
24 
25     // pcl -> ros
26     sensor_msgs::PointCloud2 laserCloudOutMsg;
27     pcl::toROSMsg(laserCloudIn, laserCloudOutMsg);
28     laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
29     laserCloudOutMsg.header.frame_id = "/camera_init";
30     // 发布
31     pubLaserCloud.publish(laserCloudOutMsg);
32 
33 
34 }
35 
36 int main(int argc, char** argv)
37 {
38     ros::init(argc, argv, "scanRegistration");
39     ros::NodeHandle nh;
40     // parameter
41     // 订阅
42     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
43     // 发送到插件
44     pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("velodyne_cloud_2", 100);
45     ros::spin(); // 循环等待
46     return 1;
47 }

四、可视化

　　点击add -> PointCloud2，修改如下图两个位置：

 

posted @ 2021-07-27 17:42  佚名12  阅读(63)  评论(0)  编辑  收藏  举报