add demo bag link and update launch file for the demo

Author: MaverickPeter

README.md

@@ -84,6 +84,14 @@ In order to get the GEM to run with your robot, you will need to adapt a few par
 
     roslaunch elevation_mapping_demos simple_demo.launch
 
+## Simple Demo
+
+You can get our demo bag from this link: https://drive.google.com/file/d/1hv3ovZnAVSpL0T6GZkJQ14Ptm4w_1ALc/view?usp=sharing
+
+    rosbag play test.bag --clock --pause
+    roslaunch elevation_mapping_demos simple_demo.launch
+
+
 ## Nodes
 
 ### Node: elevation_mapping

elevation_mapping/LICENSE

@@ -61,8 +61,8 @@ ElevationMapping::ElevationMapping(ros::NodeHandle& nodeHandle, string robot_nam
       robotMotionMapUpdater_(nodeHandle),
       isContinouslyFusing_(false),
       ignoreRobotMotionUpdates_(false),
-      vel_sub(nodeHandle, "/robot0/pointcloud", 1),
-      cameraR_sub(nodeHandle, "/robot0/image_rect", 1),
+      vel_sub(nodeHandle, "/voxel_grid/output", 1),
+      cameraR_sub(nodeHandle, "/stereo_grey/left/image_raw", 1),
       sync(MySyncPolicy(10), vel_sub, cameraR_sub)
 {
   ROS_INFO("Elevation mapping node started.");

elevation_mapping/elevation_mapping/src/ElevationMapping.cpp

@@ -1,9 +1,9 @@
 robot_id: "0"
 robot_name: "robot0"
 map_frame_id: "/robot0/map"
-sensor_frame_id: "/velodyne0"
-robot_base_frame_id: "/velodyne0"
-track_point_frame_id: "/velodyne0"
+sensor_frame_id: "/PandarQT"
+robot_base_frame_id: "/PandarQT"
+track_point_frame_id: "/PandarQT"
 robot_pose_with_covariance_topic: "/robot0/laser_pose"
 robot_pose_cache_size: 200
 robot_local_map_size: 20
@@ -13,5 +13,5 @@ track_point_z: 0.0
 octomap_resolution: 0.01
 map_saving_file: "./map.pcd"
 submap_saving_dir: "./submaps/"
-camera_params_yaml: "./kitti_intrinsic.yaml"
+camera_params_yaml: "./yq_intrinsic.yaml"
 orthomosaic_saving_dir: "./image/"

elevation_mapping/elevation_mapping_demos/config/elevation_maps/detection.yaml

@@ -7,10 +7,10 @@
   <arg name="lidar_topic"  default="/$(arg robot_name)/pointcloud"/>
 
   <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping_$(arg robot_id)" output="screen">
-    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/detection.yaml" />
-    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/detection.yaml" />
+    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/simple_demo_robot.yaml" />
+    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/simple_demo_map.yaml" />
     <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/velodyne_VLP16.yaml" />
-    <!-- <remap from="/voxel_grid/output" to="$(arg lidar_topic)"/> -->
+    <remap from="/voxel_grid/output" to="$(arg lidar_topic)"/>
     <remap from="/stereo_grey/left/image_raw" to="$(arg camera_topic)"/>
 
   </node>