40 #include <visualization_msgs/Marker.h> 51 std::vector<robot_self_filter::LinkInfo> links;
72 visualization_msgs::Marker mk;
76 mk.header.frame_id =
"base_link";
78 mk.ns =
"test_self_filter";
81 mk.action = visualization_msgs::Marker::ADD;
82 mk.pose.position.x = x;
83 mk.pose.position.y = y;
84 mk.pose.position.z = z;
85 mk.pose.orientation.w = 1.0;
87 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
101 pcl::PointCloud<pcl::PointXYZ> in;
104 in.header.frame_id =
"base_link";
106 const unsigned int N = 500000;
108 for (
unsigned int i = 0 ; i < N ; ++i)
115 for (
unsigned int i = 0 ; i < 1000 ; ++i)
122 std::vector<int> mask;
129 for (
unsigned int i = 0 ; i < mask.size() ; ++i)
146 return (2.0 * drand48() - 1.0) * magnitude;
156 int main(
int argc,
char **argv)
158 ros::init(argc, argv,
"robot_self_filter");
void sendPoint(double x, double y, double z)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void maskIntersection(const PointCloud &data_in, const std::string &sensor_frame, const double min_sensor_dist, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL)
Compute the intersection mask for a given pointcloud. If a mask element can have one of the values IN...
ROSCPP_DECL void spin(Spinner &spinner)
double uniform(double magnitude)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
void gotIntersection(const tf::Vector3 &pt)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
robot_self_filter::SelfMask< pcl::PointXYZ > * sf_
ros::NodeHandle nodeHandle_
int main(int argc, char **argv)
tf::TransformListener tf_
ROSCPP_DECL void spinOnce()