32 #include <geometry_msgs/PoseArray.h> 33 #include <geometry_msgs/PoseWithCovarianceStamped.h> 34 #include <mcl_3dl_msgs/Status.h> 35 #include <nav_msgs/Odometry.h> 36 #include <sensor_msgs/Imu.h> 37 #include <sensor_msgs/PointCloud2.h> 39 #include <std_srvs/Trigger.h> 46 #include <gtest/gtest.h> 50 void generateSamplePointcloud2(
51 sensor_msgs::PointCloud2& cloud,
57 cloud.is_bigendian =
false;
58 cloud.is_dense =
false;
60 modifier.setPointCloud2Fields(
62 "x", 1, sensor_msgs::PointField::FLOAT32,
63 "y", 1, sensor_msgs::PointField::FLOAT32,
64 "z", 1, sensor_msgs::PointField::FLOAT32,
65 "intensity", 1, sensor_msgs::PointField::FLOAT32);
71 Point(
const float x,
const float y,
const float z)
78 std::vector<Point> points;
80 for (
float x = -1;
x < 1;
x += 0.05)
82 for (
float y = -1;
y < 1;
y += 0.05)
84 points.push_back(
Point(
x / 2 + offset_x,
y + offset_y, 1.0 + offset_z));
85 points.push_back(
Point(
x / 2 + offset_x,
y + offset_y, -1.0 + offset_z));
86 points.push_back(
Point(1.0 / 2 + offset_x,
y + offset_y,
x + offset_z));
87 points.push_back(
Point(-1.0 / 2 + offset_x,
y + offset_y,
x + offset_z));
88 points.push_back(
Point(
x / 2 + offset_x, 1.0 + offset_y,
y + offset_z));
89 points.push_back(
Point(
x / 2 + offset_x, -1.0 + offset_y,
y + offset_z));
93 modifier.resize(points.size());
94 cloud.width = points.size();
99 for (
const Point& p : points)
110 inline sensor_msgs::PointCloud2 generateMapMsg(
111 const float offset_x,
112 const float offset_y,
113 const float offset_z)
115 sensor_msgs::PointCloud2 cloud;
116 generateSamplePointcloud2(cloud, offset_x, offset_y, offset_z);
117 cloud.header.frame_id =
"map";
120 inline sensor_msgs::PointCloud2 generateCloudMsg()
122 sensor_msgs::PointCloud2 cloud;
123 generateSamplePointcloud2(cloud, 0, 0, 0);
124 cloud.header.frame_id =
"base_link";
128 inline sensor_msgs::Imu generateImuMsg()
130 sensor_msgs::Imu imu;
131 imu.header.frame_id =
"base_link";
133 imu.orientation.w = 1;
134 imu.linear_acceleration.z = 9.8;
137 inline nav_msgs::Odometry generateOdomMsg()
139 nav_msgs::Odometry odom;
140 odom.header.frame_id =
"odom";
142 odom.pose.pose.position.x = 1;
143 odom.pose.pose.orientation.w = 1;
146 inline geometry_msgs::PoseWithCovarianceStamped generateInitialPose()
148 geometry_msgs::PoseWithCovarianceStamped pose;
149 pose.header.frame_id =
"map";
151 pose.pose.pose.orientation.w = 1.0;
152 pose.pose.covariance[6 * 0 + 0] = 0.05 * 0.05;
153 pose.pose.covariance[6 * 1 + 1] = 0.05 * 0.05;
154 pose.pose.covariance[6 * 2 + 2] = 0.05 * 0.05;
155 pose.pose.covariance[6 * 3 + 3] = 0.0;
156 pose.pose.covariance[6 * 4 + 4] = 0.0;
157 pose.pose.covariance[6 * 5 + 5] = 0.05 * 0.05;
176 geometry_msgs::PoseArray::ConstPtr
poses_;
177 geometry_msgs::PoseWithCovarianceStamped::ConstPtr
pose_cov_;
180 void cbPose(
const geometry_msgs::PoseArray::ConstPtr& msg)
184 void cbStatus(
const mcl_3dl_msgs::Status::ConstPtr& msg)
188 void cbPoseCov(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
197 bool found_true_positive(
false);
198 for (
const auto& pose : poses_->poses)
204 if (tf_diff.
getOrigin().length() < 2e-1 &&
206 found_true_positive =
true;
208 return found_true_positive;
215 pub_init_.
publish(generateInitialPose());
217 for (
int i = 0; i < 100; i++)
235 pub_mapcloud_ = nh_.
advertise<sensor_msgs::PointCloud2>(
"mapcloud", 1,
true);
236 pub_cloud_ = nh_.
advertise<sensor_msgs::PointCloud2>(
"cloud", 1);
237 pub_imu_ = nh_.
advertise<sensor_msgs::Imu>(
"imu/data", 1);
238 pub_odom_ = nh_.
advertise<nav_msgs::Odometry>(
"odom", 1);
240 nh_.
advertise<geometry_msgs::PoseWithCovarianceStamped>(
"initialpose", 1,
true);
242 src_expansion_resetting_ =
243 nh_.
serviceClient<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(
244 "expansion_resetting");
250 const float offset_x = 1;
251 const float offset_y = 0;
252 const float offset_z = 0;
253 pub_mapcloud_.publish(generateMapMsg(offset_x, offset_y, offset_z));
258 for (
int i = 0; i < 40; ++i)
262 if (status_ && status_->status == mcl_3dl_msgs::Status::EXPANSION_RESETTING)
264 pub_cloud_.publish(generateCloudMsg());
265 pub_imu_.publish(generateImuMsg());
266 pub_odom_.publish(generateOdomMsg());
270 ASSERT_TRUE(static_cast<bool>(status_));
271 ASSERT_TRUE(static_cast<bool>(poses_));
276 tf2::Vector3(offset_x, offset_y, offset_z))));
281 const float offset_x = 1;
282 const float offset_y = 0;
283 const float offset_z = 0;
284 pub_mapcloud_.publish(generateMapMsg(offset_x, offset_y, offset_z));
290 for (
int i = 0; i < 40; ++i)
294 if (i > 5 && status_ && status_->status != mcl_3dl_msgs::Status::EXPANSION_RESETTING)
297 pub_cloud_.publish(generateCloudMsg());
298 pub_imu_.publish(generateImuMsg());
299 pub_odom_.publish(generateOdomMsg());
304 std_srvs::TriggerRequest req;
305 std_srvs::TriggerResponse res;
306 ASSERT_TRUE(src_expansion_resetting_.call(req, res));
310 for (
int i = 0; i < 40; ++i)
316 ASSERT_NE(status_->status, mcl_3dl_msgs::Status::EXPANSION_RESETTING);
318 pub_cloud_.publish(generateCloudMsg());
319 pub_imu_.publish(generateImuMsg());
320 pub_odom_.publish(generateOdomMsg());
324 ASSERT_TRUE(static_cast<bool>(status_));
325 ASSERT_TRUE(static_cast<bool>(poses_));
330 tf2::Vector3(offset_x, offset_y, offset_z))));
333 int main(
int argc,
char** argv)
335 testing::InitGoogleTest(&argc, argv);
336 ros::init(argc, argv,
"test_expansion_resetting");
339 return RUN_ALL_TESTS();
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::Subscriber sub_pose_
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
mcl_3dl_msgs::Status::ConstPtr status_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Publisher pub_mapcloud_
void cbPoseCov(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
TEST_F(ExpansionResetting, ExpandAndResume)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void cbPose(const geometry_msgs::PoseArray::ConstPtr &msg)
ros::Subscriber sub_pose_cov_
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
ros::Publisher pub_cloud_
void fromMsg(const A &, B &b)
ros::ServiceClient src_expansion_resetting_
TFSIMD_FORCE_INLINE const tfScalar & y() const
int main(int argc, char **argv)
bool findTruePose(const tf2::Transform &true_pose)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double getYaw(const A &a)
void cbStatus(const mcl_3dl_msgs::Status::ConstPtr &msg)
geometry_msgs::PoseArray::ConstPtr poses_
ROSCPP_DECL void spinOnce()
ros::Subscriber sub_status_
geometry_msgs::PoseWithCovarianceStamped::ConstPtr pose_cov_