Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031
00032 #include <ros/master.h>
00033 #include <geometry_msgs/TransformStamped.h>
00034 #include <sensor_msgs/point_cloud2_iterator.h>
00035 #include <sensor_msgs/PointCloud2.h>
00036 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00037 #include <tf2_ros/transform_broadcaster.h>
00038
00039 #include <string>
00040
00041 #include <gtest/gtest.h>
00042
00043 namespace
00044 {
00045 void GenerateSinglePointPointcloud2(
00046 sensor_msgs::PointCloud2& cloud,
00047 const float x, const float y, const float z)
00048 {
00049 cloud.height = 1;
00050 cloud.width = 1;
00051 cloud.is_bigendian = false;
00052 cloud.is_dense = false;
00053 sensor_msgs::PointCloud2Modifier modifier(cloud);
00054 modifier.setPointCloud2FieldsByString(1, "xyz");
00055 sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
00056 sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
00057 sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
00058 modifier.resize(1);
00059 *iter_x = x;
00060 *iter_y = y;
00061 *iter_z = z;
00062 }
00063 void publishSinglePointPointcloud2(
00064 ros::Publisher& pub,
00065 const float x, const float y, const float z,
00066 const std::string frame_id,
00067 const ros::Time stamp)
00068 {
00069 sensor_msgs::PointCloud2 cloud;
00070 cloud.header.frame_id = frame_id;
00071 cloud.header.stamp = stamp;
00072 GenerateSinglePointPointcloud2(cloud, x, y, z);
00073 pub.publish(cloud);
00074 }
00075 }
00076
00077 TEST(TransformFailure, NoDeadAgainstTransformFailure)
00078 {
00079 ros::NodeHandle nh("");
00080 tf2_ros::TransformBroadcaster tfb;
00081 ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00082 ros::Publisher pub_mapcloud = nh.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
00083
00084 ros::Rate rate(10);
00085 publishSinglePointPointcloud2(
00086 pub_mapcloud,
00087 0.0, 0.0, 0.0, "map", ros::Time::now());
00088
00089 while (ros::ok())
00090 {
00091 ros::spinOnce();
00092 rate.sleep();
00093
00094 ros::V_string nodes;
00095 ros::master::getNodes(nodes);
00096 bool found = false;
00097 for (auto node : nodes)
00098 {
00099 if (node == "/mcl_3dl")
00100 {
00101 found = true;
00102 break;
00103 }
00104 }
00105 if (found)
00106 break;
00107 }
00108 std::cerr << "mcl_3dl started" << std::endl;
00109 ros::Duration(1.0).sleep();
00110 int cnt = 0;
00111
00112 while (ros::ok())
00113 {
00114 ++cnt;
00115 geometry_msgs::TransformStamped trans;
00116 trans.header.stamp = ros::Time::now() + ros::Duration(0.1);
00117 trans.transform.rotation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1));
00118
00119 trans.header.frame_id = "laser_link_base";
00120 trans.child_frame_id = "laser_link";
00121 tfb.sendTransform(trans);
00122
00123 trans.header.frame_id = "map";
00124 trans.child_frame_id = "odom";
00125 tfb.sendTransform(trans);
00126
00127 if (cnt > 10)
00128 {
00129 trans.header.frame_id = "base_link";
00130 trans.child_frame_id = "laser_link_base";
00131 tfb.sendTransform(trans);
00132 }
00133 if (cnt > 20)
00134 {
00135 trans.header.frame_id = "odom";
00136 trans.child_frame_id = "base_link";
00137 tfb.sendTransform(trans);
00138 }
00139 if (cnt > 30)
00140 break;
00141
00142 publishSinglePointPointcloud2(
00143 pub_cloud,
00144 0.0, 0.0, 0.0, "laser_link", ros::Time::now());
00145
00146 ros::spinOnce();
00147 rate.sleep();
00148 }
00149 ASSERT_TRUE(ros::ok());
00150 }
00151
00152 int main(int argc, char** argv)
00153 {
00154 testing::InitGoogleTest(&argc, argv);
00155 ros::init(argc, argv, "test_transform_failure");
00156
00157 return RUN_ALL_TESTS();
00158 }