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