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
00031
00032
00033
00034
00037 #include <cstdio>
00038
00039 #include <ros/ros.h>
00040 #include <visualization_msgs/Marker.h>
00041 #include "robot_self_filter_color/self_mask_color.h"
00042
00043 #include <pcl/point_types.h>
00044 #include <pcl/point_cloud.h>
00045
00046 class TestSelfFilter
00047 {
00048 public:
00049
00050 TestSelfFilter(void)
00051 {
00052 id_ = 1;
00053 vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
00054 std::vector<robot_self_filter_color::LinkInfo> links;
00055 robot_self_filter_color::LinkInfo li;
00056 li.name="base_link";
00057 li.padding = .05;
00058 li.scale = 1.0;
00059 links.push_back(li);
00060 sf_ = new robot_self_filter_color::SelfMask(tf_, links);
00061 }
00062
00063 ~TestSelfFilter(void)
00064 {
00065 delete sf_;
00066 }
00067
00068 void gotIntersection(const tf::Vector3 &pt)
00069 {
00070 sendPoint(pt.x(), pt.y(), pt.z());
00071 }
00072
00073 void sendPoint(double x, double y, double z)
00074 {
00075 visualization_msgs::Marker mk;
00076
00077 mk.header.stamp = ros::Time::now();
00078
00079 mk.header.frame_id = "base_link";
00080
00081 mk.ns = "test_self_filter";
00082 mk.id = id_++;
00083 mk.type = visualization_msgs::Marker::SPHERE;
00084 mk.action = visualization_msgs::Marker::ADD;
00085 mk.pose.position.x = x;
00086 mk.pose.position.y = y;
00087 mk.pose.position.z = z;
00088 mk.pose.orientation.w = 1.0;
00089
00090 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
00091
00092 mk.color.a = 1.0;
00093 mk.color.r = 1.0;
00094 mk.color.g = 0.04;
00095 mk.color.b = 0.04;
00096
00097 mk.lifetime = ros::Duration(10);
00098
00099 vmPub_.publish(mk);
00100 }
00101
00102 void run(void)
00103 {
00104 pcl::PointCloud<pcl::PointXYZRGB> in;
00105
00106 in.header.stamp = ros::Time::now();
00107 in.header.frame_id = "base_link";
00108
00109
00110
00111 const unsigned int N = 500000;
00112 in.points.resize(N);
00113
00114 for (unsigned int i = 0 ; i < N ; ++i)
00115 {
00116 in.points[i].x = uniform(1.5);
00117 in.points[i].y = uniform(1.5);
00118 in.points[i].z = uniform(1.5);
00119
00120 }
00121
00122 for (unsigned int i = 0 ; i < 1000 ; ++i)
00123 {
00124 ros::Duration(0.001).sleep();
00125 ros::spinOnce();
00126 }
00127
00128 ros::WallTime tm = ros::WallTime::now();
00129 std::vector<int> mask;
00130 sf_->maskIntersection(in, "laser_tilt_mount_link", 0.01, mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
00131
00132 printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
00133
00134
00135 int k = 0;
00136 for (unsigned int i = 0 ; i < mask.size() ; ++i)
00137 {
00138
00139
00140
00141 if (mask[i] != robot_self_filter_color::INSIDE) continue;
00142
00143 k++;
00144 }
00145
00146 ros::spin();
00147 }
00148
00149 protected:
00150
00151 double uniform(double magnitude)
00152 {
00153 return (2.0 * drand48() - 1.0) * magnitude;
00154 }
00155
00156 tf::TransformListener tf_;
00157 robot_self_filter_color::SelfMask *sf_;
00158 ros::Publisher vmPub_;
00159 ros::NodeHandle nodeHandle_;
00160 int id_;
00161 };
00162
00163 int main(int argc, char **argv)
00164 {
00165 ros::init(argc, argv, "robot_self_filter_color");
00166
00167 TestSelfFilter t;
00168 sleep(1);
00169 t.run();
00170
00171 return 0;
00172 }
00173
00174
00175