Use service for map acquisition (#15)

ToshikiNakamura0412 · May 26, 2024 · 04921c2 · 04921c2
1 parent f9584a5
commit 04921c2
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 10 deletions.
diff --git a/include/emcl/emcl.h b/include/emcl/emcl.h
@@ -12,6 +12,7 @@
 #include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/GetMap.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Odometry.h>
 #include <optional>
@@ -102,18 +103,17 @@ class EMCL
    */
   void laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg);
 
-  /**
-   * @brief Callback function for map
-   * @param msg Map
-   */
-  void map_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
   /**
    * @brief Callback function for odom
    * @param msg Odom
    */
   void odom_callback(const nav_msgs::Odometry::ConstPtr &msg);
 
+  /**
+   * @brief Client function for map
+   */
+  void get_map(void);
+
   /**
    * @brief Calculate the normal distribution
    * @param mean Mean
@@ -214,7 +214,6 @@ class EMCL
   ros::Publisher particle_cloud_pub_;
   ros::Subscriber initial_pose_sub_;
   ros::Subscriber laser_scan_sub_;
-  ros::Subscriber map_sub_;
   ros::Subscriber odom_sub_;
 
   std::optional<nav_msgs::OccupancyGrid> map_;

diff --git a/src/emcl.cpp b/src/emcl.cpp
@@ -18,7 +18,6 @@ EMCL::EMCL() : private_nh_("~")
   particle_cloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particle_cloud", 1);
   initial_pose_sub_ = nh_.subscribe("/initialpose", 1, &EMCL::initial_pose_callback, this);
   laser_scan_sub_ = nh_.subscribe("/scan", 1, &EMCL::laser_scan_callback, this);
-  map_sub_ = nh_.subscribe("/map", 1, &EMCL::map_callback, this);
   odom_sub_ = nh_.subscribe("/odom", 1, &EMCL::odom_callback, this);
 
   ROS_INFO_STREAM(ros::this_node::getName() << " node has started..");
@@ -27,13 +26,24 @@ EMCL::EMCL() : private_nh_("~")
   particles_.reserve(emcl_param_.particle_num);
   odom_model_ = OdomModel(odom_model_param_.ff, odom_model_param_.fr, odom_model_param_.rf, odom_model_param_.rr);
   initialize(emcl_param_.init_x, emcl_param_.init_y, emcl_param_.init_yaw);
+  get_map();
 }
 
 void EMCL::initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
 {
   initialize(msg->pose.pose.position.x, msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation));
 }
 
+void EMCL::get_map(void)
+{
+  nav_msgs::GetMap::Request req;
+  nav_msgs::GetMap::Response resp;
+  while (!ros::service::call("static_map", req, resp))
+    ros::Duration(0.5).sleep();
+  map_ = resp.map;
+  ROS_WARN("Received a map");
+}
+
 void EMCL::laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
 {
   if (!map_.has_value() || !prev_odom_.has_value())
@@ -62,8 +72,6 @@ void EMCL::laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
   }
 }
 
-void EMCL::map_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg) { map_ = *msg; }
-
 void EMCL::odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
 {
   if (last_odom_.has_value())