Add a test for landmark measurement

src/mcl_3dl.cpp

@@ -192,6 +192,8 @@ class MCL3dlNode
       s.odom_err_integ_ang_ = Vec3();
     };
     pf_->predict(integ_reset_func);
+
+    publishParticles();
   }
 
   void cbOdom(const nav_msgs::Odometry::ConstPtr& msg)
@@ -746,27 +748,7 @@ class MCL3dlNode
       }
     }
 
-    geometry_msgs::PoseArray pa;
-    if (has_odom_)
-      pa.header.stamp = odom_last_ + tf_tolerance_base_ + *params_.tf_tolerance_;
-    else
-      pa.header.stamp = ros::Time::now() + tf_tolerance_base_ + *params_.tf_tolerance_;
-    pa.header.frame_id = frame_ids_["map"];
-    for (size_t i = 0; i < pf_->getParticleSize(); i++)
-    {
-      geometry_msgs::Pose pm;
-      auto p = pf_->getParticle(i);
-      p.rot_.normalize();
-      pm.position.x = p.pos_.x_;
-      pm.position.y = p.pos_.y_;
-      pm.position.z = p.pos_.z_;
-      pm.orientation.x = p.rot_.x_;
-      pm.orientation.y = p.rot_.y_;
-      pm.orientation.z = p.rot_.z_;
-      pm.orientation.w = p.rot_.w_;
-      pa.poses.push_back(pm);
-    }
-    pub_particle_.publish(pa);
+    publishParticles();
 
     pf_->resample(State6DOF(
         Vec3(params_.resample_var_x_,
@@ -887,9 +869,20 @@ class MCL3dlNode
            rot_rpy.z_)
               .finished();
 
-      return nd(diff_vec);
+      const auto n = nd(diff_vec);
+      return n;
     };
     pf_->measure(measure_func);
+
+    pf_->resample(State6DOF(
+        Vec3(params_.resample_var_x_,
+             params_.resample_var_y_,
+             params_.resample_var_z_),
+        Vec3(params_.resample_var_roll_,
+             params_.resample_var_pitch_,
+             params_.resample_var_yaw_)));
+
+    publishParticles();
   }
   void cbImu(const sensor_msgs::Imu::ConstPtr& msg)
   {
@@ -975,6 +968,7 @@ class MCL3dlNode
                         mcl_3dl_msgs::ResizeParticleResponse& response)
   {
     pf_->resizeParticle(request.size);
+    publishParticles();
     return true;
   }
   bool cbExpansionReset(std_srvs::TriggerRequest& request,
@@ -987,6 +981,7 @@ class MCL3dlNode
         Vec3(params_.expansion_var_roll_,
              params_.expansion_var_pitch_,
              params_.expansion_var_yaw_)));
+    publishParticles();
     return true;
   }
   bool cbGlobalLocalization(std_srvs::TriggerRequest& request,
@@ -1056,6 +1051,31 @@ class MCL3dlNode
     return true;
   }
 
+  void publishParticles()
+  {
+    geometry_msgs::PoseArray pa;
+    if (has_odom_)
+      pa.header.stamp = odom_last_ + tf_tolerance_base_ + *params_.tf_tolerance_;
+    else
+      pa.header.stamp = ros::Time::now() + tf_tolerance_base_ + *params_.tf_tolerance_;
+    pa.header.frame_id = frame_ids_["map"];
+    for (size_t i = 0; i < pf_->getParticleSize(); i++)
+    {
+      geometry_msgs::Pose pm;
+      auto p = pf_->getParticle(i);
+      p.rot_.normalize();
+      pm.position.x = p.pos_.x_;
+      pm.position.y = p.pos_.y_;
+      pm.position.z = p.pos_.z_;
+      pm.orientation.x = p.rot_.x_;
+      pm.orientation.y = p.rot_.y_;
+      pm.orientation.z = p.rot_.z_;
+      pm.orientation.w = p.rot_.w_;
+      pa.poses.push_back(pm);
+    }
+    pub_particle_.publish(pa);
+  }
+
 public:
   MCL3dlNode()
     : nh_("")

test/CMakeLists.txt

@@ -41,6 +41,11 @@ add_rostest_gtest(test_global_localization tests/global_localization_rostest.tes
 target_link_libraries(test_global_localization ${catkin_LIBRARIES})
 add_dependencies(test_global_localization mcl_3dl)
 
+add_rostest_gtest(test_landmark tests/landmark_rostest.test
+    src/test_landmark.cpp)
+target_link_libraries(test_landmark ${catkin_LIBRARIES})
+add_dependencies(test_landmark mcl_3dl)
+
 add_rostest_gtest(test_mcl_3dl_compat tests/mcl_3dl_compat_rostest.test
     src/test_mcl_3dl_compat.cpp)
 target_link_libraries(test_mcl_3dl_compat ${catkin_LIBRARIES})

test/src/test_landmark.cpp

@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) 2019, the mcl_3dl authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <gtest/gtest.h>
+
+namespace
+{
+inline geometry_msgs::PoseWithCovarianceStamped generatePoseWithCov(
+    const float y, const float y_var, const float var_measure = 0.0)
+{
+  geometry_msgs::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = ros::Time::now();
+  pose.pose.pose.position.y = y;
+  pose.pose.pose.orientation.w = 1.0;
+  pose.pose.covariance[6 * 0 + 0] = var_measure;
+  pose.pose.covariance[6 * 1 + 1] = y_var;
+  pose.pose.covariance[6 * 2 + 2] = var_measure;
+  pose.pose.covariance[6 * 3 + 3] = var_measure;
+  pose.pose.covariance[6 * 4 + 4] = var_measure;
+  pose.pose.covariance[6 * 5 + 5] = var_measure;
+  return pose;
+}
+std::pair<float, float> getMean(const std::vector<geometry_msgs::Pose>& poses)
+{
+  float mean = 0;
+  for (const geometry_msgs::Pose p : poses)
+  {
+    mean += p.position.y;
+  }
+  mean /= poses.size();
+
+  float root_mean = 0;
+  for (const geometry_msgs::Pose p : poses)
+  {
+    root_mean += std::pow(p.position.y - mean, 2.0f);
+  }
+  root_mean /= poses.size();
+
+  return std::pair<float, float>(mean, root_mean);
+}
+}  // namespace
+TEST(Landmark, Measurement)
+{
+  geometry_msgs::PoseArray::ConstPtr poses;
+
+  const boost::function<void(const geometry_msgs::PoseArray::ConstPtr&)> cb_pose =
+      [&poses](const geometry_msgs::PoseArray::ConstPtr& msg) -> void
+  {
+    poses = msg;
+  };
+
+  ros::NodeHandle nh("");
+  ros::Subscriber sub_pose = nh.subscribe("mcl_3dl/particles", 1, cb_pose);
+  ros::Publisher pub_init =
+      nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, true);
+  ros::Publisher pub_landmark =
+      nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("mcl_measurement", 1, true);
+
+  ros::Duration(1.0).sleep();
+  pub_init.publish(generatePoseWithCov(2.0, 1.0));
+  ros::Duration(0.1).sleep();
+  ros::spinOnce();
+
+  ASSERT_TRUE(static_cast<bool>(poses));
+  for (const geometry_msgs::Pose p : poses->poses)
+  {
+    ASSERT_FLOAT_EQ(p.position.x, 0.0f);
+    ASSERT_FLOAT_EQ(p.position.z, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.x, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.y, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.z, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.w, 1.0f);
+  }
+  const std::pair<float, float> mean_init = getMean(poses->poses);
+  ASSERT_NEAR(mean_init.first, 2.0f, 0.1f);
+  ASSERT_NEAR(mean_init.second, 1.0f, 0.1f);
+
+  poses = nullptr;
+  pub_landmark.publish(generatePoseWithCov(2.6, 1.0, 1000.0));
+  ros::Duration(0.1).sleep();
+  ros::spinOnce();
+
+  ASSERT_TRUE(static_cast<bool>(poses));
+  for (const geometry_msgs::Pose p : poses->poses)
+  {
+    ASSERT_FLOAT_EQ(p.position.x, 0.0f);
+    ASSERT_FLOAT_EQ(p.position.z, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.x, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.y, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.z, 0.0f);
+    ASSERT_FLOAT_EQ(p.orientation.w, 1.0f);
+  }
+  const std::pair<float, float> mean_measured = getMean(poses->poses);
+  ASSERT_NEAR(mean_measured.first, 2.3f, 0.1f);
+  ASSERT_NEAR(mean_measured.second, 0.5f, 0.1f);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_landmark");
+
+  return RUN_ALL_TESTS();
+}

test/tests/landmark_rostest.test

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+  <node pkg="mcl_3dl" type="mcl_3dl" name="mcl_3dl" output="screen">
+    <param name="compatible" value="1" />
+    <param name="lpf_step" value="0" />
+    <param name="num_particles" value="4096" />
+    <param name="resample_var_x" value="0.0" />
+    <param name="resample_var_y" value="0.0" />
+    <param name="resample_var_z" value="0.0" />
+    <param name="resample_var_roll" value="0.0" />
+    <param name="resample_var_pitch" value="0.0" />
+    <param name="resample_var_yaw" value="0.0" />
+  </node>
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="stf_base_link" args="1 0 0 0 0 0 odom base_link" />
+
+  <test test-name="test_landmark" pkg="mcl_3dl" type="test_landmark" time-limit="90" />
+</launch>