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(tf_, links);
00058 }
00059
00060 ~TestSelfFilter(void)
00061 {
00062 delete sf_;
00063 }
00064
00065 void gotIntersection(const btVector3 &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 sensor_msgs::PointCloud in;
00102
00103 in.header.stamp = ros::Time::now();
00104 in.header.frame_id = "base_link";
00105 in.channels.resize(1);
00106 in.channels[0].name = "stamps";
00107
00108 const unsigned int N = 500000;
00109 in.points.resize(N);
00110 in.channels[0].values.resize(N);
00111 for (unsigned int i = 0 ; i < N ; ++i)
00112 {
00113 in.points[i].x = uniform(1.5);
00114 in.points[i].y = uniform(1.5);
00115 in.points[i].z = uniform(1.5);
00116 in.channels[0].values[i] = (double)i/(double)N;
00117 }
00118
00119 for (unsigned int i = 0 ; i < 1000 ; ++i)
00120 {
00121 ros::Duration(0.001).sleep();
00122 ros::spinOnce();
00123 }
00124
00125 ros::WallTime tm = ros::WallTime::now();
00126 std::vector<int> mask;
00127 sf_->maskIntersection(in, "laser_tilt_mount_link", 0.01, mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
00128
00129 printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
00130
00131
00132 int k = 0;
00133 for (unsigned int i = 0 ; i < mask.size() ; ++i)
00134 {
00135
00136
00137
00138 if (mask[i] != robot_self_filter::INSIDE) continue;
00139
00140 k++;
00141 }
00142
00143 ros::spin();
00144 }
00145
00146 protected:
00147
00148 double uniform(double magnitude)
00149 {
00150 return (2.0 * drand48() - 1.0) * magnitude;
00151 }
00152
00153 tf::TransformListener tf_;
00154 robot_self_filter::SelfMask *sf_;
00155 ros::Publisher vmPub_;
00156 ros::NodeHandle nodeHandle_;
00157 int id_;
00158 };
00159
00160 int main(int argc, char **argv)
00161 {
00162 ros::init(argc, argv, "robot_self_filter");
00163
00164 TestSelfFilter t;
00165 sleep(1);
00166 t.run();
00167
00168 return 0;
00169 }
00170
00171
00172