Init gmapping

.gitignore

@@ -0,0 +1,2 @@
+build**
+data/gmapping.bag

README.md

@@ -0,0 +1,46 @@
+# GMapping
+
+> G. Grisetti, C. Stachniss and W. Burgard, "Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters," in IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34-46, Feb. 2007, doi: 10.1109/TRO.2006.889486.
+
+## Introduction
+
+The most codes in this repository were adapted from [here](https://github.com/Wleisure95/laser_slam), while the original codes of gmapping can be found [here](https://github.com/ros-perception/openslam_gmapping). Note that most redundant codes in the original repository were not kept and I used more modern C++ to rewrite the codes in [Google style](https://google.github.io/styleguide/cppguide.html).
+
+## Dependencies
+
+- [ROS](http://wiki.ros.org/ROS/Installation)
+- [glog](https://github.com/google/glog)
+
+## Compilation
+
+```bash
+mkdir -p ~/laser_slam_ws/src
+cd ~/laser_slam_ws/src/
+git clone [email protected]:TongLing916/gmapping_clean.git
+cd ..
+catkin_make -j
+```
+
+## Execution
+
+```bash
+# Terminal 1
+cd ~/laser_slam_ws
+source devel/setup.bash
+roslaunch gmapping gmapping_sim.launch
+```
+
+```bash
+# Terminal 2
+cd ~/laser_slam_ws/src/gmapping_clean/data
+rosbag play --clock gmapping.bag
+```
+
+```bash
+# Terminal 3
+rosrun rviz rviz
+```
+
+## Demo
+
+![](https://raw.githubusercontent.com/TongLing916/gmapping_clean/main/data/gmapping.PNG)

gmapping/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gmapping)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14 -O3 -Wall")
+
+find_package(catkin REQUIRED 
+COMPONENTS 
+    roscpp
+    rospy
+    std_msgs
+    tf 
+    sensor_msgs 
+    nav_msgs
+    openslam_gmapping
+)
+
+catkin_package(
+ INCLUDE_DIRS include
+)
+
+include_directories(
+  ${PROJECT_SOURCE_DIR}/include
+  ${PROJECT_SOURCE_DIR}/../openslam_gmapping/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node 
+    app/main.cc
+
+    src/slam_gmapping.cc
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  -pthread
+  ${catkin_LIBRARIES}
+)

gmapping/app/main.cc

@@ -0,0 +1,22 @@
+#include <glog/logging.h>
+#include <ros/ros.h>
+
+#include "slam_gmapping.h"
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging("GMapping");
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+
+  ros::init(argc, argv, "gmapping_node");
+
+  gmapping::SlamGMapping gm;
+  gm.StartLiveSlam();
+
+  ros::spin();
+
+  google::ShutdownGoogleLogging();
+
+  return 0;
+}

gmapping/include/slam_gmapping.h

@@ -0,0 +1,156 @@
+#pragma once
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include <message_filters/subscriber.h>
+#include <nav_msgs/GetMap.h>
+#include <ros/publisher.h>
+#include <ros/ros.h>
+#include <ros/service_server.h>
+#include <ros/subscriber.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/message_filter.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include "grid_fast_slam/grid_slam_processor.h"
+#include "sensor/odometry_sensor.h"
+#include "sensor/range_sensor.h"
+#include "type/oriented_point.h"
+
+namespace gmapping {
+
+class SlamGMapping {
+ public:
+  enum Status { kUnknown = -1, kFree = 0, kOccupied = 100 };
+
+ public:
+  SlamGMapping();
+
+  void StartLiveSlam();
+
+ private:
+  void Initialize();
+
+  bool InitializeMapper(const sensor_msgs::LaserScan& scan);
+
+  bool MapCallBack(nav_msgs::GetMap::Request& req,
+                   nav_msgs::GetMap::Response& res);
+
+  void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& scan);
+
+  void PublishLoop(const double transform_publish_period);
+
+  void PublishTransform();
+
+  bool AddScan(const sensor_msgs::LaserScan& scan,
+               OrientedPoint2d* const odom_pose);
+
+  /**
+   * @brief Update map using the particle with max weight
+   * @details This function is just for visualization.
+   */
+  void UpdateMap(const sensor_msgs::LaserScan& scan);
+
+  double ComputePoseEntropy();
+
+  bool GetOdomPose(const ros::Time& t, OrientedPoint2d* const pose);
+
+  int GetMapIdx(const int s, const int x, const int y) { return s * y + x; }
+
+ private:
+  ros::NodeHandle private_nh_;
+
+  tf::Transform map_2_odom_;
+  std::mutex map_2_odom_mtx_;
+  int laser_cnt_;
+
+  unsigned long seed_;
+
+  bool got_first_scan_;
+  bool got_map_;
+  std::mutex map_mtx_;
+  nav_msgs::GetMap::Response map_;
+
+  std::shared_ptr<GridSlamProcessor> grid_slam_processor_;
+  std::shared_ptr<RangeSensor> range_sensor_;
+  std::shared_ptr<OdometrySensor> odometry_sensor_;
+
+  std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;
+
+  std::shared_ptr<std::thread> th_transfrom_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::LaserScan>>
+      scan_filter_sub_;
+  std::shared_ptr<tf::MessageFilter<sensor_msgs::LaserScan>> scan_filter_;
+
+  ros::NodeHandle nh_;
+  ros::Publisher entropy_publisher_;
+  ros::Publisher map_publisher_;
+  ros::Publisher map_metadata_publisher_;
+  ros::ServiceServer dynamic_map_server_;
+  tf::TransformListener tf_;
+
+  /**
+   * @brief The angles in the laser, going from -x to x
+   * @details Adjustment is made to get the laser between symmetrical bounds as
+   * that's what gmapping expects.
+   */
+  std::vector<double> laser_angles_;
+
+  // Parameters used by ROS wrapper
+  int throttle_scans_;
+  std::string base_frame_;
+  std::string laser_frame_;
+  std::string map_frame_;
+  std::string odom_frame_;
+  double transform_publish_period_;
+  double tf_delay_;
+  ros::Duration map_update_interval_;
+
+  // Parameters used by GMapping
+  double max_range_;      // Laser's max range
+  double max_urange_;     // Laser's max usable range
+  double minimum_score_;  // Minimum score for considering matching outcome good
+  double sigma_;          // Variance for computing score
+  int kernel_size_;       // Size of searching window when computing score
+  double lstep_;          // Linear step size for optimization
+  double astep_;          // Angular step size for optimization
+  int iterations_;        // Number of optimization iterations
+  double lsigma_;         // Variance for computing likelihood
+  double ogain_;          // Smoothing factor for the likelihood
+  int lskip_;             // Number of beams we skip when computing likelihood
+  double srr_;            // Linear error's variance due to linear motion
+  double srt_;            // Angular error's variance due to linear motion
+  double str_;            // Linear error's variance due to angular motion
+  double stt_;            // Angular error's variance due to angular motion
+  double linear_update_;
+  double angular_update_;
+  double temporal_update_;
+  double resample_threshold_;
+  int num_particles_;
+  double xmin_;
+  double ymin_;
+  double xmax_;
+  double ymax_;
+
+  /** Map resolution */
+  double delta_;
+
+  double occ_thresh_;
+  double ll_sample_range_;  // Translational sampling range
+  double ll_sample_step_;   // Translational sampling step
+  double la_sample_range_;  // Angular sampling range
+  double la_sample_step_;   // Angular sampling step
+
+  size_t laser_beam_cnt_;
+  bool do_reverse_range_;
+
+  // The pose in the original laser frame, of the corresponding centered laser
+  // with z facing up
+  tf::Stamped<tf::Pose> centered_laser_pose_;
+};
+
+}  // namespace gmapping

gmapping/launch/gmapping_sim.launch

@@ -0,0 +1,57 @@
+<launch>
+
+  <param name="use_sim_time" value="true"/>
+  <arg name="scan_topic" default="sick_scan" />
+
+  <node pkg="gmapping" type="gmapping_node" name="gmapping_node" output="screen">
+
+    <remap from="scan" to="$(arg scan_topic)"/>
+
+    <!--  GMapping Wrapper paremeters -->
+    <param name="throttle_scans" value="1"/>
+    <param name="base_frame" value="base_link" />
+    <param name="odom_frame" value="odom"/>--
+    <param name="map_update_interval" value="5.0"/>                   <!-- Map update rate -->
+
+    <!-- Laser Parameters -->
+    <param name="maxRange" value="5.51" />                            <!-- Laser max range -->
+    <param name="maxUrange" value="5.5"/>                             <!-- Laser max usable range -->
+    <param name="sigma" value="0.05"/>                                <!-- Scan matching - gaussian sigma -->
+    <param name="kernelSize" value="1"/>                              <!-- Scan matching - search window size -->
+    <param name="lstep" value="0.05"/>                                <!-- Scan matching - linear step size -->
+    <param name="astep" value="0.05"/>                                <!-- Scan matching - angular step size -->
+    <param name="iterations" value="5"/>                              <!-- Scan matching - pptimization iterations -->
+    <param name="lsigma" value="0.075"/>                              <!-- Scan matching - likelihood sigma -->
+    <param name="ogain" value="3.0"/>                                 <!-- Likelihood gain -->
+    <param name="lskip" value="0"/>                                   <!-- Beam skip size  -->
+    <param name="minimumScore" value="50"/>                           <!-- scan matching - threshold to accept matching -->
+
+    <!-- Motion model parameters -->
+    <param name="srr" value="0.01"/>                                  <!-- Translational error due to translation -->
+    <param name="srt" value="0.005"/>                                 <!-- Angular error due to translation -->
+    <param name="str" value="0.01"/>        	                        <!-- Translational error due to rotation -->
+    <param name="stt" value="0.005"/>                                 <!-- Angular error due to rotation -->
+
+
+    <!-- Other parameters -->
+    <param name="linearUpdate" value="0.2"/>                          <!-- Linear threshold for processing scan -->
+    <param name="angularUpdate" value="0.2"/>                         <!-- Angular threshold for processing scan -->
+    <param name="temporalUpdate" value="5"/>                          <!-- Temporal threshold for processing scan -->
+    <param name="resampleThreshold" value="0.5"/>                     <!-- Threshold of effective particle' percentage -->
+    <param name="particles" value="30"/>                              <!-- Number of particles -->
+
+    <!-- Likelihood sampling parameters -->
+    <param name="llsamplerange" value="0.02"/>                          <!-- Range of linear sampling -->
+    <param name="llsamplestep" value="0.02"/>                           <!-- Step of linear sampling -->
+    <param name="lasamplerange" value="0.005"/>                         <!-- Range of angular sampling -->
+    <param name="lasamplestep" value="0.005"/>                          <!-- Step of angular samplinp -->
+
+    <!-- Map size and resolution -->
+    <param name="xmin" value="-1.0"/>                                  
+    <param name="ymin" value="-1.0"/>                                   
+    <param name="xmax" value="1.0"/>                                    
+    <param name="ymax" value="1.0"/>                                    
+    <param name="delta" value="0.025"/>                                 
+
+  </node>
+</launch>

gmapping/package.xml

@@ -0,0 +1,35 @@
+<package>
+  <name>gmapping</name>
+  <version>1.3.8</version>
+  <description>This package contains a ROS wrapper for OpenSlam's Gmapping. 
+  The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 
+  as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy
+  grid map (like a building floorplan) from laser and pose data collected by a mobile robot.
+  </description>
+  <author>Brian Gerkey</author>
+  <maintainer email="[email protected]">Vincent Rabaud</maintainer>
+  <license>CreativeCommons-by-nc-sa-2.0</license>
+
+  <url>http://ros.org/wiki/gmapping</url>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>openslam_gmapping</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>openslam_gmapping</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>