Add 3d mapping using kinect

vatanaksoytezer · Mar 30, 2018 · 7977891 · 7977891
1 parent e7dc7a7
commit 7977891
Show file tree

Hide file tree

Showing 8 changed files with 90 additions and 14 deletions.
diff --git a/README.md b/README.md
@@ -28,22 +28,30 @@ Installation Instructions - Ubuntu 16.04 with ROS Kinetic
 Basic Usage
 -----------
   ```
-  $ roslaunch zephyr_gazebo firefly_indoor_slam.launch
+  $ roslaunch zephyr_gazebo firefly_indoor_slam_with_kinect.launch
+  $ roslaunch zephyr_mapping kinect_rtabmap.launch
   ```
 
-After executing the command, 2 seperate GUIs should show up. These are:
+After executing the commands, 2 seperate GUIs should show up. These are:
 1. Gazebo: Gazebo is the physics integrated simulator, which the MAV goes accordingly. 
 2. Rviz: The camera feed, maps and depth points is visualized in Rviz. 
 
 MAV will automatically start navigating and mapping between pre-determined waypoints once the GUIs show up. These waypoints are located and can be changed from rotors_gazebo/resource/example_waypoints.txt file. For more information, please visit [RotorS github page](https://github.com/ethz-asl/rotors_simulator).
 
-Viewing Generated Map
+Viewing Generated 2D Map
 ---------------------
   ```
   $ rosrun map_server map_saver -f ~/catkin_ws/src
   ```
 Example maps generated from this package can be found under test directory of this repository.
 
+Viewing Generated 3D Map
+---------------------
+  ```
+  $  rtabmap-databaseViewer ~/.ros/rtabmap.db
+  ```
+Example maps generated from this package can be found under test directory of this repository.
+
 Future Work
 -----------
  - Autonomous navigation without waypoints

diff --git a/rtabmap.db b/rtabmap.db
diff --git a/test/kinect_topics.txt b/test/kinect_topics.txt
@@ -0,0 +1,58 @@
+/clicked_point
+/clock
+/firefly/command/motor_speed
+/firefly/command/pose
+/firefly/command/trajectory
+/firefly/gazebo/command/motor_speed
+/firefly/ground_truth/imu
+/firefly/ground_truth/odometry
+/firefly/ground_truth/pose
+/firefly/ground_truth/pose_with_covariance
+/firefly/ground_truth/position
+/firefly/ground_truth/transform
+/firefly/imu
+/firefly/joint_states
+/firefly/kinect_camera/depth/camera_info
+/firefly/kinect_camera/depth/image_raw
+/firefly/kinect_camera/depth/points
+/firefly/kinect_camera/rgb/camera_info
+/firefly/kinect_camera/rgb/image_raw
+/firefly/kinect_camera/rgb/image_raw/compressed
+/firefly/kinect_camera/rgb/image_raw/compressed/parameter_descriptions
+/firefly/kinect_camera/rgb/image_raw/compressed/parameter_updates
+/firefly/kinect_camera/rgb/image_raw/compressedDepth
+/firefly/kinect_camera/rgb/image_raw/compressedDepth/parameter_descriptions
+/firefly/kinect_camera/rgb/image_raw/compressedDepth/parameter_updates
+/firefly/kinect_camera/rgb/image_raw/theora
+/firefly/kinect_camera/rgb/image_raw/theora/parameter_descriptions
+/firefly/kinect_camera/rgb/image_raw/theora/parameter_updates
+/firefly/motor_speed
+/firefly/motor_speed/0
+/firefly/motor_speed/1
+/firefly/motor_speed/2
+/firefly/motor_speed/3
+/firefly/motor_speed/4
+/firefly/motor_speed/5
+/firefly/odometry_sensor1/odometry
+/firefly/odometry_sensor1/pose
+/firefly/odometry_sensor1/pose_with_covariance
+/firefly/odometry_sensor1/position
+/firefly/odometry_sensor1/transform
+/firefly/scan
+/firefly/wind_speed
+/gazebo/link_states
+/gazebo/model_states
+/gazebo/parameter_descriptions
+/gazebo/parameter_updates
+/gazebo/set_link_state
+/gazebo/set_model_state
+/initialpose
+/map
+/map_metadata
+/map_updates
+/move_base_simple/goal
+/rosout
+/rosout_agg
+/slam_gmapping/entropy
+/tf
+/tf_static
diff --git a/test/rtabmap_test_1.db b/test/rtabmap_test_1.db
diff --git a/test/topics.txt → test/vi_topics.txt b/test/topics.txt → test/vi_topics.txt
diff --git a/zephyr_gazebo/resource/rviz/firefly_with_kinect.rviz b/zephyr_gazebo/resource/rviz/firefly_with_kinect.rviz
@@ -249,8 +249,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 0.0802771449
-        Min Value: 0.0796459764
+        Max Value: 0.931960762
+        Min Value: -0.09943185
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -286,6 +286,16 @@ Visualization Manager:
       Unreliable: false
       Use Timestamp: false
       Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /firefly/octomap_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -329,7 +339,7 @@ Visualization Manager:
       Pitch: 0.70039767
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.32084537
+      Yaw: 3.15084505
     Saved: ~
 Window Geometry:
   "&Camera":

diff --git a/zephyr_mapping/launch/rtabmap.launch → zephyr_mapping/launch/kinect_rtabmap.launch b/zephyr_mapping/launch/rtabmap.launch → zephyr_mapping/launch/kinect_rtabmap.launch
@@ -18,7 +18,7 @@
   <arg name="stereo"          default="false"/>
 
   <!-- Choose visualization -->
-  <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rtabmapviz"              default="false" /> 
   <arg name="rviz"                    default="false" />
 
   <!-- Localization-only mode -->
@@ -29,7 +29,7 @@
   <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
   <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
-  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
+  <arg name="frame_id"                default="firefly/base_link"/>     <!-- Fixed frame id, you may set "base_link" or "kinect_camera_depth_optical_frame" if they are published -->
   <arg name="namespace"               default="firefly"/>
   <arg name="database_path"           default="~/.ros/rtabmap.db"/>
   <arg name="queue_size"              default="10"/>
@@ -42,9 +42,9 @@
   <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
 
   <!-- RGB-D related topics -->
-  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
-  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
-  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+  <arg name="rgb_topic"               default="kinect_camera/rgb/image_raw" />
+  <arg name="depth_topic"             default="kinect_camera/depth/image_raw" />
+  <arg name="camera_info_topic"       default="kinect_camera/rgb/camera_info" />
 
   <!-- stereo related topics -->
   <arg name="stereo_namespace"        default="/stereo_camera"/>
@@ -63,8 +63,8 @@
   <arg name="subscribe_scan_cloud"    default="false"/>
   <arg name="scan_cloud_topic"        default="/scan_cloud"/>
 
-  <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
-  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="visual_odometry"         default="false"/>          <!-- Launch rtabmap visual odometry node -->
+  <arg name="odom_topic"              default="odometry_sensor1/odometry"/>         <!-- Odometry topic used if visual_odometry is false -->
   <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
   <arg name="odom_args"               default="$(arg rtabmap_args)"/>
 

diff --git a/zephyr_mapping/launch/rtab_mapping.launch → zephyr_mapping/launch/stereo_rtabmap.launch b/zephyr_mapping/launch/rtab_mapping.launch → zephyr_mapping/launch/stereo_rtabmap.launch
@@ -39,7 +39,7 @@
   <arg name="scan_cloud_topic"        default="/scan_cloud"/>
 
   <arg name="visual_odometry"         default="false"/>          <!-- Generate visual odometry -->
-  <arg name="odom_topic"              default="vi_sensor/ground_truth/odometry"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="odom_topic"              default="/firefly/odometry_sensor1/odometry"/>         <!-- Odometry topic used if visual_odometry is false -->
   <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
 
   <arg name="namespace"               default="firefly"/>