Remove flag (#13)

include/emcl/emcl.h

@@ -37,7 +37,6 @@ struct EMCLParam
   int reset_counter = 0;
   int reset_count_limit = 0;
   int laser_step = 0;
-  float move_dist_th = 0.0;
   float init_x = 0.0;
   float init_y = 0.0;
   float init_yaw = 0.0;
@@ -209,8 +208,6 @@ class EMCL
 
   std::vector<Particle> particles_;
 
-  bool flag_move_;
-
   std::random_device rd_;
   std::mt19937 gen_{rd_()};
 
@@ -225,9 +222,8 @@ class EMCL
 
   std::optional<sensor_msgs::LaserScan> laser_scan_;
   std::optional<nav_msgs::OccupancyGrid> map_;
-  std::optional<nav_msgs::Odometry> initial_odom_;
-  nav_msgs::Odometry prev_odom_;
-  nav_msgs::Odometry last_odom_;
+  std::optional<nav_msgs::Odometry> prev_odom_;
+  std::optional<nav_msgs::Odometry> last_odom_;
 };
 
 #endif  // EMCL_EMCL_H

launch/emcl.launch

@@ -1,17 +1,13 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="hz" default="10"/>
-    <arg name="is_visible" default="true"/>
-    <arg name="move_dist_th" default="0.025"/>
     <arg name="config_file_path" default="$(find emcl_ros)/launch/test.yaml"/>
     <arg name="initial_pose_topic" default="/initialpose"/>
     <arg name="laser_scan_topic" default="/scan"/>
     <arg name="map_topic" default="/map"/>
     <arg name="odom_topic" default="/odom"/>
     <node pkg="emcl_ros" type="emcl_node" name="emcl" output="screen">
         <param name="hz" value="$(arg hz)"/>
-        <param name="is_visible" value="$(arg is_visible)"/>
-        <param name="move_dist_th" value="$(arg move_dist_th)"/>
         <rosparam command="load" file="$(arg config_file_path)"/>
         <remap from="/initialpose" to="$(arg initial_pose_topic)"/>
         <remap from="/scan" to="$(arg laser_scan_topic)"/>

src/emcl.cpp

@@ -10,7 +10,7 @@
 
 #include "emcl/emcl.h"
 
-EMCL::EMCL() : private_nh_("~"), flag_move_(false)
+EMCL::EMCL() : private_nh_("~")
 {
   load_params();
 
@@ -32,7 +32,6 @@ EMCL::EMCL() : private_nh_("~"), flag_move_(false)
 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));
-  flag_move_ = false;
 }
 
 void EMCL::laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg) { laser_scan_ = *msg; }
@@ -41,24 +40,9 @@ void EMCL::map_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg) { map_ = *
 
 void EMCL::odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
 {
-  if (!initial_odom_.has_value())
-  {
-    initial_odom_ = *msg;
-    prev_odom_ = *msg;
-  }
-  else
-  {
+  if (last_odom_.has_value())
     prev_odom_ = last_odom_;
-  }
   last_odom_ = *msg;
-
-  if (!flag_move_)
-  {
-    const float dist_from_init_x = prev_odom_.pose.pose.position.x - initial_odom_.value().pose.pose.position.x;
-    const float dist_from_init_y = prev_odom_.pose.pose.position.y - initial_odom_.value().pose.pose.position.y;
-    if (emcl_param_.move_dist_th < hypot(dist_from_init_x, dist_from_init_y))
-      flag_move_ = true;
-  }
 }
 
 void EMCL::process(void)
@@ -67,19 +51,8 @@ void EMCL::process(void)
 
   while (ros::ok())
   {
-    if (map_.has_value() && initial_odom_.has_value() && laser_scan_.has_value())
-    {
-      if (flag_move_)
-      {
-        localize();
-      }
-      else
-      {
-        broadcast_odom_state();
-        publish_estimated_pose();
-        publish_particles();
-      }
-    }
+    if (map_.has_value() && prev_odom_.has_value() && laser_scan_.has_value())
+      localize();
     ros::spinOnce();
     loop_rate.sleep();
   }
@@ -117,9 +90,9 @@ void EMCL::broadcast_odom_state(void)
   const float map_to_base_x = emcl_pose_.x();
   const float map_to_base_y = emcl_pose_.y();
 
-  const float odom_to_base_yaw = tf2::getYaw(last_odom_.pose.pose.orientation);
-  const float odom_to_base_x = last_odom_.pose.pose.position.x;
-  const float odom_to_base_y = last_odom_.pose.pose.position.y;
+  const float odom_to_base_yaw = tf2::getYaw(last_odom_.value().pose.pose.orientation);
+  const float odom_to_base_x = last_odom_.value().pose.pose.position.x;
+  const float odom_to_base_y = last_odom_.value().pose.pose.position.y;
 
   const float map_to_odom_yaw = normalize_angle(map_to_base_yaw - odom_to_base_yaw);
   const float map_to_odom_x =
@@ -135,7 +108,7 @@ void EMCL::broadcast_odom_state(void)
   odom_state.header.stamp = ros::Time::now();
 
   odom_state.header.frame_id = map_.value().header.frame_id;
-  odom_state.child_frame_id = last_odom_.header.frame_id;
+  odom_state.child_frame_id = last_odom_.value().header.frame_id;
 
   odom_state.transform.translation.x = isnan(map_to_odom_x) ? 0.0 : map_to_odom_x;
   odom_state.transform.translation.y = isnan(map_to_odom_y) ? 0.0 : map_to_odom_y;
@@ -183,11 +156,11 @@ void EMCL::localize(void)
 
 void EMCL::motion_update(void)
 {
-  const float last_yaw = tf2::getYaw(last_odom_.pose.pose.orientation);
-  const float prev_yaw = tf2::getYaw(prev_odom_.pose.pose.orientation);
+  const float last_yaw = tf2::getYaw(last_odom_.value().pose.pose.orientation);
+  const float prev_yaw = tf2::getYaw(prev_odom_.value().pose.pose.orientation);
 
-  const float dx = last_odom_.pose.pose.position.x - prev_odom_.pose.pose.position.x;
-  const float dy = last_odom_.pose.pose.position.y - prev_odom_.pose.pose.position.y;
+  const float dx = last_odom_.value().pose.pose.position.x - prev_odom_.value().pose.pose.position.x;
+  const float dy = last_odom_.value().pose.pose.position.y - prev_odom_.value().pose.pose.position.y;
   const float dyaw = normalize_angle(last_yaw - prev_yaw);
 
   const float length = hypot(dx, dy);

src/parameters.cpp

@@ -28,8 +28,6 @@ void EMCL::load_params(void)
   // - l -
   private_nh_.param<int>("laser_step", emcl_param_.laser_step, 10);
   private_nh_.param<float>("likelihood_th", emcl_param_.likelihood_th, 0.02);
-  // - m -
-  private_nh_.param<float>("move_dist_th", emcl_param_.move_dist_th, 0.025);
   // - p -
   private_nh_.param<int>("particle_num", emcl_param_.particle_num, 420);
   // - r -
@@ -61,7 +59,6 @@ void EMCL::print_params(void)
   ROS_INFO_STREAM("  init_yaw_dev: " << emcl_param_.init_yaw_dev);
   ROS_INFO_STREAM("  laser_step: " << emcl_param_.laser_step);
   ROS_INFO_STREAM("  likelihood_th: " << emcl_param_.likelihood_th);
-  ROS_INFO_STREAM("  move_dist_th: " << emcl_param_.move_dist_th);
   ROS_INFO_STREAM("  particle_num: " << emcl_param_.particle_num);
   ROS_INFO_STREAM("  reset_count_limit: " << emcl_param_.reset_count_limit);
   ROS_INFO_STREAM("  sensor_noise_ratio: " << emcl_param_.sensor_noise_ratio);