├── .gitignore
├── maps
    └── .gitkeep
├── src
    ├── lidar_mapping_node.cpp
    └── lidar_mapping.cpp
├── package.xml
├── launch
    ├── lidar_mapping.launch
    └── carla_ros_bridge_with_example_ego_vehicle.launch
├── CMakeLists.txt
├── README.md
├── config
    └── objects.json
├── include
    └── lidar_mapping.h
└── rviz
    └── mapping.rviz


/.gitignore:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/maps/.gitkeep:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/src/lidar_mapping_node.cpp:
--------------------------------------------------------------------------------
1 | #include "lidar_mapping.h"
2 | 
3 | int main(int argc, char** argv) {
4 |   ros::init(argc, argv, "lidar_mapping");
5 |   LidarMapping node;
6 |   ros::spin();
7 |   return 0;
8 | }
9 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>lidar_mapping</name>
 4 |   <version>1.0.0</version>
 5 |   <description>The lidar_mapping package</description>
 6 |   <maintainer email="robotpengxu@gmail.com">Casper-Auto</maintainer>
 7 |   <license>MIT</license>
 8 | 
 9 |   <buildtool_depend>catkin</buildtool_depend>
10 | 
11 |   <depend>roscpp</depend>
12 |   <depend>rospy</depend>
13 |   <depend>geometry_msgs</depend>
14 |   <depend>nav_msgs</depend>
15 |   <depend>sensor_msgs</depend>
16 |   <depend>tf</depend>
17 | </package>
18 | 


--------------------------------------------------------------------------------
/launch/lidar_mapping.launch:
--------------------------------------------------------------------------------
 1 | <!-- Launch file for naive_motion_predict -->
 2 | <launch>
 3 |   <arg name="new_scan" default="true"/>
 4 |   <arg name="vehicle_radius" default="8.0"/>
 5 |   <arg name="cloud_resolution" default="5000"/>
 6 | 
 7 |   <node pkg="lidar_mapping" type="lidar_mapping" name="lidar_mapping" output="screen">
 8 |     <param name="new_scan" value="$(arg new_scan)"/>
 9 |     <param name="vehicle_radius" value="$(arg vehicle_radius)"/>
10 |     <param name="cloud_resolution" value="$(arg cloud_resolution)"/>
11 |   </node>
12 | 
13 |   <node pkg="rviz" type="rviz" name="rviz" output="screen" args="-d $(find lidar_mapping)/rviz/mapping.rviz"/>
14 | 
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(lidar_mapping)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
 6 | 
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   rospy
 9 |   roscpp
10 |   std_msgs
11 |   geometry_msgs
12 |   nav_msgs
13 |   sensor_msgs
14 | )
15 | 
16 | find_package(Eigen3 REQUIRED)
17 | find_package(PCL REQUIRED)
18 | 
19 | catkin_package(
20 |   CATKIN_DEPENDS
21 | )
22 | 
23 | install(DIRECTORY launch/
24 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
25 | )
26 | 
27 | include_directories(
28 |   include
29 |   ${catkin_INCLUDE_DIRS}
30 | )
31 | 
32 | include_directories(${EIGEN3_INCLUDE_DIR})
33 | include_directories(${PCL_INCLUDE_DIRS})
34 | 
35 | link_directories(${PCL_LIBRARY_DIRS})
36 | add_definitions(${PCL_DEFINITIONS})
37 | 
38 | add_executable(lidar_mapping
39 |   src/lidar_mapping_node.cpp
40 |   src/lidar_mapping.cpp
41 | )
42 | 
43 | target_link_libraries(lidar_mapping
44 |   ${catkin_LIBRARIES}
45 |   ${PCL_LIBRARIES}
46 | )
47 | 
48 | add_dependencies(lidar_mapping
49 |   ${${PROJECT_NAME}_EXPORTED_TARGETS}
50 |   ${catkin_EXPORTED_TARGETS}
51 | )
52 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Carla Lidar Mapping
 2 | 
 3 | Mapping with Ground-truth localization.
 4 | 
 5 | ## Carla version
 6 | 
 7 | The demo was tested with Carla 0.9.11, though the version does not matter for the mapping itself. Feel free to test it with other Carla versions.
 8 | 
 9 | ## Motivation
10 | 
11 | **Be short:** To purely observe how mapping works, without coupling with localization problems.
12 | 
13 | Simultaneous Localization and Mapping (SLAM) is treated a complicated problem partially because we have to deal with two problems together and they are mutually dependent, which is like a "chicken and egg" problem.
14 | 
15 | In fact, the two problems are not in the same scale considering the complexity. Why not start from the easier one, which is mapping? In reality, when we say mapping, it is always a different wording of SLAM, or LAM if we don't care about the real-time processing. With a simulator, it becomes possible to do mapping with perfect localization. In that case, we can find mapping is more or less just a problem of coordinate transformation.
16 | 
17 | ## PCL Viewer
18 | 
19 | Install:
20 | 
21 | ```
22 | sudo apt install pcl-tools
23 | ```
24 | 
25 | Use:
26 | 
27 | ```
28 | pcl_viewer my_map.pcd
29 | ```
30 | 


--------------------------------------------------------------------------------
/launch/carla_ros_bridge_with_example_ego_vehicle.launch:
--------------------------------------------------------------------------------
 1 | <!-- -->
 2 | <launch>
 3 |   <!-- CARLA connection -->
 4 |   <arg name='host' default='localhost'/>
 5 |   <arg name='port' default='2000'/>
 6 |   <arg name='timeout' default='10'/>
 7 | 
 8 |   <!-- Ego vehicle -->
 9 |   <arg name='role_name' default='ego_vehicle'/>
10 |   <arg name="vehicle_filter" default='vehicle.*'/>
11 |   <arg name="spawn_point" default="150,-193.2,3,0,0,180"/><!-- use comma separated format "x,y,z,roll,pitch,yaw" -->
12 | 
13 |   <!-- Map to load on startup (either a predefined CARLA town (e.g. 'Town01'), or a OpenDRIVE map file) -->
14 |   <arg name="town" default=''/>
15 | 
16 |   <!-- Enable/disable passive mode -->
17 |   <arg name='passive' default=''/>
18 | 
19 |   <!-- Synchronous mode-->
20 |   <arg name='synchronous_mode_wait_for_vehicle_control_command' default='False'/>
21 |   <arg name='fixed_delta_seconds' default='0.05'/>
22 | 
23 | 
24 |   <include file="$(find carla_ros_bridge)/launch/carla_ros_bridge.launch">
25 |     <arg name='host' value='$(arg host)'/>
26 |     <arg name='port' value='$(arg port)'/>
27 |     <arg name='town' value='$(arg town)'/>
28 |     <arg name='timeout' value='$(arg timeout)'/>
29 |     <arg name='passive' value='$(arg passive)'/>
30 |     <arg name='synchronous_mode_wait_for_vehicle_control_command' value='$(arg synchronous_mode_wait_for_vehicle_control_command)'/>
31 |     <arg name='fixed_delta_seconds' value='$(arg fixed_delta_seconds)'/>
32 |   </include>
33 | 
34 |   <!-- the ego vehicle, that will be controlled by an agent (e.g. carla_ad_agent) -->
35 |   <include file="$(find carla_spawn_objects)/launch/carla_example_ego_vehicle.launch">
36 |     <arg name="objects_definition_file" value='$(find lidar_mapping)/config/objects.json'/>
37 |     <arg name='role_name' value='$(arg role_name)'/>
38 |     <arg name="spawn_point_ego_vehicle" value="$(arg spawn_point)"/>
39 |     <arg name="spawn_sensors_only" value="false"/>
40 |   </include>
41 | 
42 |   <include file="$(find carla_manual_control)/launch/carla_manual_control.launch">
43 |     <arg name='role_name' value='$(arg role_name)'/>
44 |   </include>
45 | 
46 | </launch>
47 | 


--------------------------------------------------------------------------------
/config/objects.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "objects":
 3 |     [
 4 |         {
 5 |             "type": "sensor.pseudo.objects",
 6 |             "id": "objects"
 7 |         },
 8 |         {
 9 |             "type": "sensor.pseudo.actor_list",
10 |             "id": "actor_list"
11 |         },
12 |         {
13 |             "type": "sensor.pseudo.markers",
14 |             "id": "markers"
15 |         },
16 |         {
17 |             "type": "vehicle.tesla.model3",
18 |             "id": "ego_vehicle",
19 |             "sensors":
20 |             [
21 |                 {
22 |                     "type": "sensor.camera.rgb",
23 |                     "id": "rgb_front",
24 |                     "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
25 |                     "image_size_x": 800,
26 |                     "image_size_y": 600,
27 |                     "fov": 90.0
28 |                 },
29 |                 {
30 |                     "type": "sensor.camera.rgb",
31 |                     "id": "rgb_view",
32 |                     "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0},
33 |                     "image_size_x": 800,
34 |                     "image_size_y": 600,
35 |                     "fov": 90.0,
36 |                     "attached_objects":
37 |                     [
38 |                         {
39 |                             "type": "actor.pseudo.control",
40 |                             "id": "control"
41 |                         }
42 |                     ]
43 |                 },
44 |                 {
45 |                     "type": "sensor.lidar.ray_cast",
46 |                     "id": "lidar",
47 |                     "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
48 |                     "range": 50,
49 |                     "channels": 64,
50 |                     "points_per_second": 320000,
51 |                     "upper_fov": 25.0,
52 |                     "lower_fov": -26.8,
53 |                     "rotation_frequency": 20,
54 |                     "noise_stddev": 0.0
55 |                 },
56 |                 {
57 |                     "type": "sensor.pseudo.tf",
58 |                     "id": "tf"
59 |                 },
60 |                 {
61 |                     "type": "sensor.pseudo.objects",
62 |                     "id": "objects"
63 |                 },
64 |                 {
65 |                     "type": "sensor.pseudo.odom",
66 |                     "id": "odometry"
67 |                 },
68 |                 {
69 |                     "type": "sensor.pseudo.speedometer",
70 |                     "id": "speedometer"
71 |                 }
72 |             ]
73 |         }
74 |     ]
75 | }
76 | 


--------------------------------------------------------------------------------
/include/lidar_mapping.h:
--------------------------------------------------------------------------------
  1 | // system
  2 | #include <iostream>
  3 | #include <chrono>
  4 | #include <ctime>
  5 | #include <sstream>
  6 | #include <string>
  7 | #include <thread>
  8 | 
  9 | // eigen
 10 | #include <Eigen/Geometry>
 11 | 
 12 | // pcl
 13 | #include <pcl_conversions/pcl_conversions.h>
 14 | #include <pcl/filters/voxel_grid.h>
 15 | #include <pcl/io/pcd_io.h>
 16 | 
 17 | // ros
 18 | #include <ros/ros.h>
 19 | 
 20 | #include <geometry_msgs/Quaternion.h>
 21 | #include <geometry_msgs/Vector3.h>
 22 | #include <nav_msgs/Odometry.h>
 23 | #include <sensor_msgs/PointCloud2.h>
 24 | 
 25 | #include <tf/LinearMath/Matrix3x3.h>
 26 | #include <tf/transform_datatypes.h>
 27 | 
 28 | typedef pcl::PointXYZ PointT;
 29 | typedef pcl::PointCloud<PointT> PointCloudT;
 30 | 
 31 | const double pi = M_PI;
 32 | 
 33 | 
 34 | struct Point {
 35 |   double x, y, z;
 36 | 
 37 |   Point() : x(0), y(0), z(0) {}
 38 | 
 39 |   Point(double setX, double setY, double setZ) : x(setX), y(setY), z(setZ) {}
 40 | 
 41 |   void Print() {
 42 |     std::cout << "x: " << x << " y: " << y << " z: " << z << std::endl;
 43 |   }
 44 | };
 45 | 
 46 | struct Rotate {
 47 |   double roll, pitch, yaw;
 48 | 
 49 |   Rotate() : roll(0), yaw(0), pitch(0) {}
 50 | 
 51 |   Rotate(double setRoll, double setPitch, double setYaw)
 52 |       : roll(setRoll), pitch(setPitch), yaw(setYaw) {}
 53 | 
 54 |   void Print() {
 55 |     std::cout << "roll: " << roll << " pitch: " << pitch << " yaw: " << yaw << std::endl;
 56 |   }
 57 | };
 58 | 
 59 | struct Pose {
 60 | 
 61 |   Point position;
 62 |   Rotate rotation;
 63 | 
 64 |   Pose() : position(Point(0, 0, 0)), rotation(Rotate(0, 0, 0)) {}
 65 | 
 66 |   Pose(Point setPos, Rotate setRotation)
 67 |       : position(setPos), rotation(setRotation) {}
 68 | 
 69 |   Pose operator-(const Pose &p) {
 70 |     Pose result(Point(position.x - p.position.x, position.y - p.position.y,
 71 |                       position.z - p.position.z),
 72 |                 Rotate(rotation.yaw - p.rotation.yaw,
 73 |                        rotation.pitch - p.rotation.pitch,
 74 |                        rotation.roll - p.rotation.roll));
 75 |     return result;
 76 |   }
 77 | };
 78 | 
 79 | class LidarMapping {
 80 | public:
 81 |   LidarMapping();
 82 |   ~LidarMapping();
 83 | 
 84 | private:
 85 |   // ros nodehandler
 86 |   ros::NodeHandle nh_;
 87 |   ros::NodeHandle private_nh_;
 88 | 
 89 |   // ros subscribers
 90 |   ros::Subscriber point_cloud_sub_;
 91 |   ros::Subscriber localization_sub_;
 92 | 
 93 |   // ros publishers
 94 |   ros::Publisher point_cloud_map_pub_;
 95 | 
 96 |   // ros timer
 97 |   ros::Timer timer_;
 98 | 
 99 |   // vars
100 |   bool new_scan_;
101 |   double vehicle_radius_;
102 |   int cloud_resolution_;
103 |   // ros::Time prev_scan_time_;
104 |   std::chrono::time_point<std::chrono::system_clock> prev_scan_time_;
105 |   Pose current_pose_;
106 |   std::vector<Point> scan_poses_;
107 |   PointCloudT::Ptr scaned_cloud_ptr_;
108 |   PointCloudT pcl_cloud_; // point cloud map before filtering
109 |   sensor_msgs::PointCloud2 saved_map_;
110 | 
111 | 
112 |   // callback functions
113 |   void pointCloudCallback(const sensor_msgs::PointCloud2::Ptr msg);
114 |   void localizationCallback(const nav_msgs::Odometry::ConstPtr& msg);
115 |   void timerCallback(const ros::TimerEvent &e);
116 | 
117 |   // core method
118 |   void scanTransformation(const PointCloudT::Ptr scan_cloud);
119 | 
120 |   // methods
121 |   Eigen::Matrix4d transform3D(double yaw, double pitch, double roll, double xt, double yt, double zt);
122 |   Eigen::Quaternionf getQuaternion(float theta);
123 |   Pose getPose(Eigen::Matrix4d matrix);
124 |   double getDistance(Point p1, Point p2);
125 |   double minDistance(Point p1, std::vector<Point> points);
126 |   void print4x4Matrix(const Eigen::Matrix4d &matrix);
127 |   void print4x4Matrixf(const Eigen::Matrix4f &matrix);
128 |   PointCloudT::Ptr downsamplePointCloud(const PointCloudT& pcl_cloud);
129 |   void savePointCloudMap(const PointCloudT::Ptr cloud_filtered);
130 | };
131 | 


--------------------------------------------------------------------------------
/rviz/mapping.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |       Splitter Ratio: 0.5
 10 |     Tree Height: 719
 11 |   - Class: rviz/Selection
 12 |     Name: Selection
 13 |   - Class: rviz/Tool Properties
 14 |     Expanded:
 15 |       - /2D Pose Estimate1
 16 |       - /2D Nav Goal1
 17 |       - /Publish Point1
 18 |     Name: Tool Properties
 19 |     Splitter Ratio: 0.5886790156364441
 20 |   - Class: rviz/Views
 21 |     Expanded:
 22 |       - /Current View1
 23 |     Name: Views
 24 |     Splitter Ratio: 0.5
 25 |   - Class: rviz/Time
 26 |     Experimental: false
 27 |     Name: Time
 28 |     SyncMode: 0
 29 |     SyncSource: PointCloudMap
 30 | Preferences:
 31 |   PromptSaveOnExit: true
 32 | Toolbars:
 33 |   toolButtonStyle: 2
 34 | Visualization Manager:
 35 |   Class: ""
 36 |   Displays:
 37 |     - Alpha: 0.5
 38 |       Cell Size: 1
 39 |       Class: rviz/Grid
 40 |       Color: 160; 160; 164
 41 |       Enabled: true
 42 |       Line Style:
 43 |         Line Width: 0.029999999329447746
 44 |         Value: Lines
 45 |       Name: Grid
 46 |       Normal Cell Count: 0
 47 |       Offset:
 48 |         X: 0
 49 |         Y: 0
 50 |         Z: 0
 51 |       Plane: XY
 52 |       Plane Cell Count: 10
 53 |       Reference Frame: <Fixed Frame>
 54 |       Value: true
 55 |     - Class: rviz/MarkerArray
 56 |       Enabled: true
 57 |       Marker Topic: /carla/markers
 58 |       Name: MarkerArray
 59 |       Namespaces:
 60 |         "": true
 61 |       Queue Size: 100
 62 |       Value: true
 63 |     - Angle Tolerance: 0.10000000149011612
 64 |       Class: rviz/Odometry
 65 |       Covariance:
 66 |         Orientation:
 67 |           Alpha: 0.5
 68 |           Color: 255; 255; 127
 69 |           Color Style: Unique
 70 |           Frame: Local
 71 |           Offset: 1
 72 |           Scale: 1
 73 |           Value: true
 74 |         Position:
 75 |           Alpha: 0.30000001192092896
 76 |           Color: 204; 51; 204
 77 |           Scale: 1
 78 |           Value: true
 79 |         Value: true
 80 |       Enabled: true
 81 |       Keep: 100
 82 |       Name: Odometry
 83 |       Position Tolerance: 0.10000000149011612
 84 |       Queue Size: 10
 85 |       Shape:
 86 |         Alpha: 1
 87 |         Axes Length: 1
 88 |         Axes Radius: 0.10000000149011612
 89 |         Color: 255; 25; 0
 90 |         Head Length: 0.30000001192092896
 91 |         Head Radius: 0.10000000149011612
 92 |         Shaft Length: 1
 93 |         Shaft Radius: 0.05000000074505806
 94 |         Value: Arrow
 95 |       Topic: /carla/ego_vehicle/odometry
 96 |       Unreliable: false
 97 |       Value: true
 98 |     - Alpha: 1
 99 |       Autocompute Intensity Bounds: true
100 |       Autocompute Value Bounds:
101 |         Max Value: 10
102 |         Min Value: -10
103 |         Value: true
104 |       Axis: Z
105 |       Channel Name: intensity
106 |       Class: rviz/PointCloud2
107 |       Color: 255; 255; 255
108 |       Color Transformer: Intensity
109 |       Decay Time: 0
110 |       Enabled: true
111 |       Invert Rainbow: false
112 |       Max Color: 255; 255; 255
113 |       Min Color: 0; 0; 0
114 |       Name: LidarPointCLoud
115 |       Position Transformer: XYZ
116 |       Queue Size: 10
117 |       Selectable: true
118 |       Size (Pixels): 3
119 |       Size (m): 0.20000000298023224
120 |       Style: Flat Squares
121 |       Topic: /carla/ego_vehicle/lidar
122 |       Unreliable: false
123 |       Use Fixed Frame: true
124 |       Use rainbow: true
125 |       Value: true
126 |     - Alpha: 1
127 |       Autocompute Intensity Bounds: true
128 |       Autocompute Value Bounds:
129 |         Max Value: 10
130 |         Min Value: -10
131 |         Value: true
132 |       Axis: Z
133 |       Channel Name: intensity
134 |       Class: rviz/PointCloud2
135 |       Color: 255; 255; 255
136 |       Color Transformer: Intensity
137 |       Decay Time: 0
138 |       Enabled: true
139 |       Invert Rainbow: false
140 |       Max Color: 255; 255; 255
141 |       Min Color: 0; 0; 0
142 |       Name: PointCloudMap
143 |       Position Transformer: XYZ
144 |       Queue Size: 10
145 |       Selectable: true
146 |       Size (Pixels): 3
147 |       Size (m): 0.10000000149011612
148 |       Style: Flat Squares
149 |       Topic: /point_cloud_map
150 |       Unreliable: false
151 |       Use Fixed Frame: true
152 |       Use rainbow: true
153 |       Value: true
154 |   Enabled: true
155 |   Global Options:
156 |     Background Color: 48; 48; 48
157 |     Default Light: true
158 |     Fixed Frame: ego_vehicle
159 |     Frame Rate: 30
160 |   Name: root
161 |   Tools:
162 |     - Class: rviz/Interact
163 |       Hide Inactive Objects: true
164 |     - Class: rviz/MoveCamera
165 |     - Class: rviz/Select
166 |     - Class: rviz/FocusCamera
167 |     - Class: rviz/Measure
168 |     - Class: rviz/SetInitialPose
169 |       Theta std deviation: 0.2617993950843811
170 |       Topic: /initialpose
171 |       X std deviation: 0.5
172 |       Y std deviation: 0.5
173 |     - Class: rviz/SetGoal
174 |       Topic: /move_base_simple/goal
175 |     - Class: rviz/PublishPoint
176 |       Single click: true
177 |       Topic: /clicked_point
178 |   Value: true
179 |   Views:
180 |     Current:
181 |       Class: rviz/Orbit
182 |       Distance: 77.182373046875
183 |       Enable Stereo Rendering:
184 |         Stereo Eye Separation: 0.05999999865889549
185 |         Stereo Focal Distance: 1
186 |         Swap Stereo Eyes: false
187 |         Value: false
188 |       Field of View: 0.7853981852531433
189 |       Focal Point:
190 |         X: 0
191 |         Y: 0
192 |         Z: 0
193 |       Focal Shape Fixed Size: true
194 |       Focal Shape Size: 0.05000000074505806
195 |       Invert Z Axis: false
196 |       Name: Current View
197 |       Near Clip Distance: 0.009999999776482582
198 |       Pitch: 0.8003979921340942
199 |       Target Frame: <Fixed Frame>
200 |       Yaw: 3.1253983974456787
201 |     Saved: ~
202 | Window Geometry:
203 |   Displays:
204 |     collapsed: true
205 |   Height: 1016
206 |   Hide Left Dock: true
207 |   Hide Right Dock: true
208 |   QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
209 |   Selection:
210 |     collapsed: false
211 |   Time:
212 |     collapsed: false
213 |   Tool Properties:
214 |     collapsed: false
215 |   Views:
216 |     collapsed: true
217 |   Width: 1848
218 |   X: 72
219 |   Y: 297
220 | 


--------------------------------------------------------------------------------
/src/lidar_mapping.cpp:
--------------------------------------------------------------------------------
  1 | #include "lidar_mapping.h"
  2 | 
  3 | LidarMapping::LidarMapping() : nh_(), private_nh_("~") {
  4 |   // param server
  5 |   private_nh_.param<bool>("new_scan", new_scan_, true);
  6 |   private_nh_.param<double>("vehicle_radius", vehicle_radius_, 8.0);
  7 |   private_nh_.param<int>("cloud_resolution", cloud_resolution_, 5000);
  8 | 
  9 |   // initialize subscribers
 10 |   point_cloud_sub_ = nh_.subscribe("/carla/ego_vehicle/lidar", 1, &LidarMapping::pointCloudCallback, this);
 11 |   localization_sub_ = nh_.subscribe("/carla/ego_vehicle/odometry", 1, &LidarMapping::localizationCallback, this);
 12 | 
 13 |   // initialize publishers
 14 |   point_cloud_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_cloud_map", 1);
 15 | 
 16 |   // createTimer
 17 |   timer_ = nh_.createTimer(ros::Duration(0.1), &LidarMapping::timerCallback, this);
 18 | 
 19 |   // smart pointers
 20 |   scaned_cloud_ptr_ = boost::make_shared<PointCloudT>();
 21 | }
 22 | 
 23 | LidarMapping::~LidarMapping() {}
 24 | 
 25 | void LidarMapping::pointCloudCallback(const sensor_msgs::PointCloud2::Ptr msg) {
 26 |   ROS_INFO("Point cloud received ...");
 27 |   pcl::fromROSMsg(*msg, *scaned_cloud_ptr_);
 28 | }
 29 | 
 30 | void LidarMapping::localizationCallback(const nav_msgs::Odometry::ConstPtr& msg) {
 31 |   ROS_INFO("Odometry received ...");
 32 | 
 33 |   // update current_pose_.position
 34 |   current_pose_.position.x = msg->pose.pose.position.x;
 35 |   current_pose_.position.y = msg->pose.pose.position.y;
 36 |   current_pose_.position.z = msg->pose.pose.position.z;
 37 | 
 38 |   // quaternion to rpy
 39 |   geometry_msgs::Quaternion geo_quat = msg->pose.pose.orientation;
 40 | 
 41 |   // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion
 42 |   tf::Quaternion quat;
 43 |   tf::quaternionMsgToTF(geo_quat, quat);
 44 | 
 45 |   // the tf::Quaternion has a method to acess roll pitch and yaw
 46 |   double roll, pitch, yaw;
 47 |   tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 48 | 
 49 |   // update current_pose_.rotation
 50 |   current_pose_.rotation.roll = roll;
 51 |   current_pose_.rotation.pitch = pitch;
 52 |   current_pose_.rotation.yaw = yaw;
 53 | }
 54 | 
 55 | void LidarMapping::timerCallback(const ros::TimerEvent &e) {
 56 | 
 57 |   scanTransformation(scaned_cloud_ptr_);
 58 | 
 59 |   //////////////////////////////////////////////////////////////////////////
 60 |   // Mapping Visualization
 61 |   //////////////////////////////////////////////////////////////////////////
 62 |   saved_map_.header.frame_id = "map";
 63 |   point_cloud_map_pub_.publish(saved_map_);
 64 | }
 65 | 
 66 | ///////////////////////////////////////////////////////////
 67 | // Magic happens here: Transformation
 68 | ///////////////////////////////////////////////////////////
 69 | 
 70 | void LidarMapping::scanTransformation(const PointCloudT::Ptr scan_cloud) {
 71 |   // Set transform to pose using transform3D()
 72 |   Eigen::Matrix4d transform = transform3D(
 73 |       current_pose_.rotation.yaw, current_pose_.rotation.pitch, current_pose_.rotation.roll,
 74 |       current_pose_.position.x, current_pose_.position.y, current_pose_.position.z);
 75 | 
 76 |   for (auto point : *scan_cloud) {
 77 |     // Don't include points touching ego
 78 |     if ((point.x * point.x +
 79 |          point.y * point.y +
 80 |          point.z * point.z) >
 81 |         vehicle_radius_) {
 82 |       Eigen::Vector4d local_point(point.x, point.y, point.z, 1);
 83 |       // Multiply local_point by transform
 84 |       Eigen::Vector4d transform_point = transform * local_point;
 85 |       pcl_cloud_.points.push_back(PointT(transform_point[0], transform_point[1], transform_point[2]));
 86 |     }
 87 |   }
 88 | 
 89 |   PointCloudT::Ptr cloud_filtered = downsamplePointCloud(pcl_cloud_);
 90 |   pcl::toROSMsg(*cloud_filtered.get(), saved_map_);
 91 | 
 92 |   // scan resolution
 93 |   if (pcl_cloud_.points.size() > cloud_resolution_) {
 94 |     new_scan_ = false;
 95 |     // savePointCloudMap(cloud_filtered);
 96 |   }
 97 | 
 98 |   double distanceR_res = 5.0; // CANDO: Can modify this value
 99 |   double time_res = 1.0;      // CANDO: Can modify this value
100 |   auto current_time = std::chrono::system_clock::now();
101 |   if (!new_scan_) {
102 |     std::chrono::duration<double> prev_scan_seconds = current_time - prev_scan_time_;
103 |     if (prev_scan_seconds.count() > time_res && minDistance(Point(current_pose_.position.x, current_pose_.position.y, current_pose_.position.z), scan_poses_) > distanceR_res) {
104 |       scan_poses_.push_back(Point(current_pose_.position.x, current_pose_.position.y, current_pose_.position.z));
105 |       new_scan_ = true;
106 |     }
107 |   }
108 | 
109 |   prev_scan_time_ = std::chrono::system_clock::now();
110 | }
111 | 
112 | /////////////////////////////////////////////////////
113 | // Other methods: mostly are helper functions
114 | /////////////////////////////////////////////////////
115 | 
116 | Eigen::Matrix4d LidarMapping::transform3D(double yaw, double pitch, double roll, double xt, double yt, double zt) {
117 | 
118 |   Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity();
119 | 
120 |   matrix(0, 3) = xt;
121 |   matrix(1, 3) = yt;
122 |   matrix(2, 3) = zt;
123 | 
124 |   matrix(0, 0) = cos(yaw) * cos(pitch);
125 |   matrix(0, 1) = cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll);
126 |   matrix(0, 2) = cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(roll);
127 |   matrix(1, 0) = sin(yaw) * cos(pitch);
128 |   matrix(1, 1) = sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll);
129 |   matrix(1, 2) = sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll);
130 |   matrix(2, 0) = -sin(pitch);
131 |   matrix(2, 1) = cos(pitch) * sin(roll);
132 |   matrix(2, 2) = cos(pitch) * cos(roll);
133 | 
134 |   return matrix;
135 | }
136 | 
137 | // angle around z axis
138 | Eigen::Quaternionf LidarMapping::getQuaternion(float theta) {
139 |   Eigen::Matrix3f rotation_mat;
140 |   rotation_mat << cos(theta), -sin(theta), 0, sin(theta), cos(theta), 0, 0, 0, 1;
141 |   Eigen::Quaternionf q(rotation_mat);
142 |   return q;
143 | }
144 | 
145 | Pose LidarMapping::getPose(Eigen::Matrix4d matrix) {
146 |   Pose pose(Point(matrix(0, 3), matrix(1, 3), matrix(2, 3)),
147 |             Rotate(atan2(matrix(2, 1), matrix(2, 2)),
148 |                    atan2(-matrix(2, 0), sqrt(matrix(2, 1) * matrix(2, 1) +
149 |                                              matrix(2, 2) * matrix(2, 2))),
150 |                    atan2(matrix(1, 0), matrix(0, 0))));
151 |   return pose;
152 | }
153 | 
154 | double LidarMapping::getDistance(Point p1, Point p2) {
155 |   return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
156 | }
157 | 
158 | double LidarMapping::minDistance(Point p1, std::vector<Point> points) {
159 |   if (points.size() > 0) {
160 |     double dist = getDistance(p1, points[0]);
161 |     for (int index = 1; index < points.size(); index++) {
162 |       double new_dist = getDistance(p1, points[index]);
163 |       if (new_dist < dist) {
164 |         dist = new_dist;
165 |       }
166 |     }
167 |     return dist;
168 |   }
169 |   return -1;
170 | }
171 | 
172 | void LidarMapping::print4x4Matrix(const Eigen::Matrix4d &matrix) {
173 |   printf("Rotation matrix :\n");
174 |   printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1),
175 |          matrix(0, 2));
176 |   printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1),
177 |          matrix(1, 2));
178 |   printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1),
179 |          matrix(2, 2));
180 |   printf("Translation vector :\n");
181 |   printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3),
182 |          matrix(2, 3));
183 | }
184 | 
185 | void LidarMapping::print4x4Matrixf(const Eigen::Matrix4f &matrix) {
186 |   printf("Rotation matrix :\n");
187 |   printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1),
188 |          matrix(0, 2));
189 |   printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1),
190 |          matrix(1, 2));
191 |   printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1),
192 |          matrix(2, 2));
193 |   printf("Translation vector :\n");
194 |   printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3),
195 |          matrix(2, 3));
196 | }
197 | 
198 | PointCloudT::Ptr LidarMapping::downsamplePointCloud(const PointCloudT& pcl_cloud) {
199 |   PointCloudT::Ptr cloud(new PointCloudT);
200 |   *cloud = pcl_cloud;
201 |   cloud->width = cloud->points.size();
202 |   cloud->height = 1;
203 | 
204 |   // Downsample the map point cloud using a voxel filter
205 |   PointCloudT::Ptr cloud_filtered(new PointCloudT);
206 |   pcl::VoxelGrid<PointT> vg;
207 |   vg.setInputCloud(cloud);
208 |   double filterRes = 0.5;
209 |   vg.setLeafSize(filterRes, filterRes, filterRes);
210 |   vg.filter(*cloud_filtered);
211 | 
212 |   return cloud_filtered;
213 | }
214 | 
215 | void LidarMapping::savePointCloudMap(const PointCloudT::Ptr cloud_filtered) {
216 |   // save the point cloud map
217 |   pcl::io::savePCDFileASCII("town_map.pcd", *cloud_filtered);
218 |   ROS_INFO("Saved pcd map.");
219 | }
220 | 


--------------------------------------------------------------------------------