void pc_motion_filter::PcMotionFilter::cloud_in_cb(const sensor_msgs::PointCloud2::ConstPtr msg)
{
update();
ros::Time now = msg->header.stamp;
ros::Time past = now;
past.sec -= stillness_duration;
geometry_msgs::TransformStamped difference_msg;
btTransform difference;
BOOST_FOREACH(std::string& frame_id, frame_ids){
try{
difference_msg = tf_buffer->lookupTransform(msg->header.frame_id, now, msg->header.frame_id, past, frame_id, ros::Duration(0.1));
difference = tf2::transformToBullet(difference_msg);
double linear_distance = difference.getOrigin().length();//sqrt(t.x*t.x+t.y*t.y+t.z*t.z);
double angular_distance = difference.getRotation().getAngle();
if (linear_distance > linear_throshold) {
NODELET_DEBUG_STREAM("above linear threshold: "<<linear_distance<<" between "<<frame_id<<" and "<<msg->header.frame_id);
return;
}
if (fabs(angular_distance) > angular_threshold) {
NODELET_DEBUG_STREAM("above angular threshold: "<<angles::to_degrees(angular_distance)<<" between "<<frame_id<<" and "<<msg->header.frame_id);
return;
}
} catch (tf2::TransformException &ex) {
NODELET_WARN_STREAM_THROTTLE(1.0, "Could NOT transform "<<msg->header.frame_id<<" to "<<frame_id<<": "<< ex.what());
//return;
}
}
cloud_out_publisher.publish(msg);
}
bool
PrimitiveShapeClassifier::checkFrameId(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
{
std::string cloud_topic = ros::names::resolve(sub_cloud_.getTopic());
std::string normal_topic = ros::names::resolve(sub_normal_.getTopic());
std::string indices_topic = ros::names::resolve(sub_indices_.getTopic());
std::string polygons_topic = ros::names::resolve(sub_polygons_.getTopic());
if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_normal->header)) {
NODELET_ERROR_STREAM(cloud_topic << " and " << normal_topic << " must have same frame_id");
return false;
}
if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_indices->header)) {
NODELET_ERROR_STREAM(cloud_topic << " and " << indices_topic << " must have same frame_id");
return false;
}
if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_polygons->header)) {
NODELET_ERROR_STREAM(cloud_topic << " and " << polygons_topic << " must have same frame_id");
return false;
}
NODELET_DEBUG_STREAM("Frame id is ok: " << ros_cloud->header.frame_id);
return true;
}
void pc_motion_filter::PcMotionFilter::update()
{
ros::NodeHandle& nh_priv = getPrivateNodeHandle();
nh_priv.getParamCached("linear_threshold", linear_throshold);
nh_priv.getParamCached("degree_threshold", degree_threshold);
nh_priv.getParamCached("stillness_duration", stillness_duration);
nh_priv.getParamCached("frame_ids", frame_ids);
angular_threshold = angles::from_degrees(degree_threshold);
NODELET_DEBUG_STREAM("lin: "<<linear_throshold<<", ang: "<<angles::to_degrees(angular_threshold)<<", still: "<<stillness_duration);
}
void BarcodeReaderNodelet::cleanCb()
{
for (boost::unordered_map<std::string, ros::Time>::iterator it = barcode_memory_.begin();
it != barcode_memory_.end(); ++it)
{
if (ros::Time::now() > it->second)
{
NODELET_DEBUG_STREAM("Cleaned " << it->first << " from memory");
barcode_memory_.erase(it);
}
}
}
// What we want to do here is to look at all the points that are *not* included in the indices list,
// (i.e. these are the outliers, the points not on the floor) and determine which are the closest to the camera
void cloudIndicesModelCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const PointIndicesConstPtr& indices, const pcl::ModelCoefficientsConstPtr& model)
{
boost::mutex::scoped_lock lock (callback_mutex);
NODELET_DEBUG_STREAM("Got cloud with timestamp " << cloud_msg->header.stamp << " + indices with timestamp "
<< indices->header.stamp << " + model with timestamp " << model->header.stamp);
double dt;
if (first)
{
first = false;
dt = 0;
}
else
{
dt = (cloud_msg->header.stamp - prev_cloud_time).toSec();
prev_cloud_time = cloud_msg->header.stamp;
}
if (model->values.size() > 0)
{
// Determine altitude:
kinect_z = reject_outliers(-fabs(model->values[3])) - imu_to_kinect_offset;
calcVelocity(kinect_z, dt, kinect_vz);
// Detect obstacles:
pcl::PointXYZ obstacle_location;
bool obstacle_found = detectObstacle(cloud_msg, indices, model, obstacle_location);
if (obstacle_found)
{
publishObstacleMarker(obstacle_location);
NODELET_DEBUG("Detected obstacle at: [%f, %f, %f]", obstacle_location.x, obstacle_location.y,
obstacle_location.z);
}
publishObstacle(obstacle_found, obstacle_location);
}
else
{
NODELET_WARN("No planar model found -- cannot determine altitude or obstacles");
}
updateMask();
if (not use_backup_estimator_alt)
{
publishOdom();
}
if (debug_throttle_rate > 0)
{
ros::Duration(1 / debug_throttle_rate).sleep();
}
}
void ColorHistogramMatcher::feature(
const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
const jsk_pcl_ros::ClusterPointIndices::ConstPtr& input_indices)
{
boost::mutex::scoped_lock(mutex_);
if (!reference_set_) {
NODELET_WARN("reference histogram is not available yet");
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input_cloud, *pcl_cloud);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud (new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloudXYZRGBtoXYZHSV(*pcl_cloud, *hsv_cloud);
// compute histograms first
std::vector<std::vector<float> > histograms;
histograms.resize(input_indices->cluster_indices.size());
pcl::ExtractIndices<pcl::PointXYZHSV> extract;
extract.setInputCloud(hsv_cloud);
// for debug
jsk_pcl_ros::ColorHistogramArray histogram_array;
histogram_array.header = input_cloud->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
pcl::IndicesPtr indices (new std::vector<int>(input_indices->cluster_indices[i].indices));
extract.setIndices(indices);
pcl::PointCloud<pcl::PointXYZHSV> segmented_cloud;
extract.filter(segmented_cloud);
std::vector<float> histogram;
computeHistogram(segmented_cloud, histogram, policy_);
histograms[i] = histogram;
ColorHistogram ros_histogram;
ros_histogram.header = input_cloud->header;
ros_histogram.histogram = histogram;
histogram_array.histograms.push_back(ros_histogram);
}
all_histogram_pub_.publish(histogram_array);
// compare histograms
jsk_pcl_ros::ClusterPointIndices result;
result.header = input_indices->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
const double coefficient = bhattacharyyaCoefficient(histograms[i], reference_histogram_);
NODELET_DEBUG_STREAM("coefficient: " << i << "::" << coefficient);
if (coefficient > coefficient_thr_) {
result.cluster_indices.push_back(input_indices->cluster_indices[i]);
}
}
result_pub_.publish(result);
}
void NormalEstimationOMP::onInit()
{
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
DiagnosticNodelet::onInit();
pnh_->param("number_of_threads", num_of_threads_, 0);
NODELET_DEBUG_STREAM("num_of_threads: " << num_of_threads_);
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&NormalEstimationOMP::configCallback, this, _1, _2);
srv_->setCallback (f);
pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
pub_with_xyz_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_with_xyz", 1);
pub_latest_time_ = advertise<std_msgs::Float32>(
*pnh_, "output/latest_time", 1);
pub_average_time_ = advertise<std_msgs::Float32>(
*pnh_, "output/average_time", 1);
onInitPostProcess();
}
~SafetyControllerNodelet()
{
NODELET_DEBUG_STREAM("Waiting for update thread to finish.");
shutdown_requested_ = true;
update_thread_.join();
}
void GridSampler::sample(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock(mutex_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*msg, *pcl_cloud);
Eigen::Vector4f minpt, maxpt;
pcl::getMinMax3D<pcl::PointXYZRGB>(*pcl_cloud, minpt, maxpt);
int x_bin_num = ceil((maxpt[0] - minpt[0]) / grid_size_);
int y_bin_num = ceil((maxpt[1] - minpt[1]) / grid_size_);
int z_bin_num = ceil((maxpt[2] - minpt[2]) / grid_size_);
// x y z
std::map<int, std::map<int, std::map<int, std::vector<size_t > > > > grid;
for (size_t i = 0; i < pcl_cloud->points.size(); i++) {
pcl::PointXYZRGB point = pcl_cloud->points[i];
if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
// skip nan
continue;
}
int xbin = int((point.x - minpt[0]) / grid_size_);
int ybin = int((point.y - minpt[1]) / grid_size_);
int zbin = int((point.z - minpt[2]) / grid_size_);
std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit
= grid.find(xbin);
if (xit == grid.end()) { // cannot find x bin
NODELET_DEBUG_STREAM("no x bin" << xbin);
std::map<int, std::vector<size_t> > new_z;
std::vector<size_t> new_indices;
new_indices.push_back(i);
new_z[zbin] = new_indices;
std::map<int, std::map<int, std::vector<size_t> > > new_y;
new_y[ybin] = new_z;
grid[xbin] = new_y;
}
else {
NODELET_DEBUG_STREAM("found x bin" << xbin);
std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
std::map<int, std::map<int, std::vector<size_t> > >::iterator yit
= ybins.find(ybin);
if (yit == ybins.end()) { // cannot find y bin
NODELET_DEBUG_STREAM("no y bin" << ybin);
std::map<int, std::vector<size_t> > new_z;
std::vector<size_t> new_indices;
new_indices.push_back(i);
new_z[zbin] = new_indices;
xit->second[ybin] = new_z;
}
else {
NODELET_DEBUG_STREAM("found y bin" << ybin);
std::map<int, std::vector<size_t> > zbins = yit->second;
std::map<int, std::vector<size_t> >::iterator zit
= zbins.find(zbin);
if (zit == zbins.end()) {
NODELET_DEBUG_STREAM("no z bin" << zbin);
std::vector<size_t> new_indices;
new_indices.push_back(i);
xit->second[ybin][zbin] = new_indices;
}
else {
NODELET_DEBUG_STREAM("found z bin" << zbin);
xit->second[ybin][zbin].push_back(i);
}
}
}
}
// publish the result
jsk_recognition_msgs::ClusterPointIndices output;
output.header = msg->header;
for (std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit = grid.begin();
xit != grid.end();
xit++) {
std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
for (std::map<int, std::map<int, std::vector<size_t> > >::iterator yit = ybins.begin();
yit != ybins.end();
yit++) {
std::map<int, std::vector<size_t> > zbins = yit->second;
for (std::map<int, std::vector<size_t> >::iterator zit = zbins.begin();
zit != zbins.end();
zit++) {
std::vector<size_t> indices = zit->second;
NODELET_DEBUG_STREAM("size: " << indices.size());
if (indices.size() > min_indices_) {
PCLIndicesMsg ros_indices;
ros_indices.header = msg->header;
for (size_t j = 0; j < indices.size(); j++) {
ros_indices.indices.push_back(indices[j]);
}
output.cluster_indices.push_back(ros_indices);
}
}
}
}
pub_.publish(output);
}
bool
PrimitiveShapeClassifier::estimate(const pcl::PointCloud<PointT>::Ptr& cloud,
const pcl::PointCloud<pcl::Normal>::Ptr& normal,
const pcl::ModelCoefficients::Ptr& plane,
pcl::PointIndices::Ptr& boundary_indices,
pcl::PointCloud<PointT>::Ptr& projected_cloud,
float& circle_likelihood,
float& box_likelihood)
{
// estimate boundaries
pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>);
pcl::BoundaryEstimation<PointT, pcl::Normal, pcl::Boundary> b;
b.setInputCloud(cloud);
b.setInputNormals(normal);
b.setSearchMethod(typename pcl::search::KdTree<PointT>::Ptr(new pcl::search::KdTree<PointT>));
b.setAngleThreshold(DEG2RAD(70));
b.setRadiusSearch(0.03);
b.compute(*boundaries);
// set boundaries as indices
for (size_t i = 0; i < boundaries->points.size(); ++i)
if ((int)boundaries->points[i].boundary_point)
boundary_indices->indices.push_back(i);
// extract boundaries
pcl::PointCloud<PointT>::Ptr boundary_cloud(new pcl::PointCloud<PointT>);
pcl::ExtractIndices<PointT> ext;
ext.setInputCloud(cloud);
ext.setIndices(boundary_indices);
ext.filter(*boundary_cloud);
// thresholding with min points num
if (boundary_indices->indices.size() < min_points_num_)
return false;
// project to supporting plane
pcl::ProjectInliers<PointT> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(boundary_cloud);
proj.setModelCoefficients(plane);
proj.filter(*projected_cloud);
// estimate circles
pcl::PointIndices::Ptr circle_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr circle_coeffs(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr line_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr line_coeffs(new pcl::ModelCoefficients);
pcl::SACSegmentation<PointT> seg;
seg.setInputCloud(projected_cloud);
seg.setOptimizeCoefficients(true);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(sac_max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
seg.setDistanceThreshold(sac_distance_threshold_);
seg.setRadiusLimits(sac_radius_limit_min_, sac_radius_limit_max_);
seg.segment(*circle_inliers, *circle_coeffs);
seg.setOptimizeCoefficients(true);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(sac_max_iterations_);
seg.setModelType(pcl::SACMODEL_LINE);
seg.setDistanceThreshold(sac_distance_threshold_);
seg.segment(*line_inliers, *line_coeffs);
// compute likelihood
circle_likelihood =
1.0f * circle_inliers->indices.size() / projected_cloud->points.size();
box_likelihood =
1.0f * line_inliers->indices.size() / projected_cloud->points.size();
NODELET_DEBUG_STREAM("Projected cloud has " << projected_cloud->points.size() << " points");
NODELET_DEBUG_STREAM(circle_inliers->indices.size() << " circle inliers found");
NODELET_DEBUG_STREAM("circle confidence: " << circle_likelihood);
NODELET_DEBUG_STREAM(line_inliers->indices.size() << " line inliers found");
NODELET_DEBUG_STREAM("box confidence: " << box_likelihood);
return true;
}
void
PrimitiveShapeClassifier::process(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
{
boost::mutex::scoped_lock lock(mutex_);
if (!checkFrameId(ros_cloud, ros_normal, ros_indices, ros_polygons)) return;
pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*ros_cloud, *input);
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*ros_normal, *normal);
pcl::ExtractIndices<PointT> ext_input;
ext_input.setInputCloud(input);
pcl::ExtractIndices<pcl::Normal> ext_normal;
ext_normal.setInputCloud(normal);
std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
= jsk_recognition_utils::Polygon::fromROSMsg(*ros_polygons);
jsk_recognition_msgs::ClassificationResult result;
result.header = ros_cloud->header;
result.classifier = "primitive_shape_classifier";
result.target_names.push_back("box");
result.target_names.push_back("circle");
result.target_names.push_back("other");
pcl::PointCloud<PointT>::Ptr projected_cloud(new pcl::PointCloud<PointT>);
std::vector<pcl::PointIndices::Ptr> boundary_indices;
NODELET_DEBUG_STREAM("Cluster num: " << ros_indices->cluster_indices.size());
for (size_t i = 0; i < ros_indices->cluster_indices.size(); ++i)
{
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
indices->indices = ros_indices->cluster_indices[i].indices;
NODELET_DEBUG_STREAM("Estimating cluster #" << i << " (" << indices->indices.size() << " points)");
pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
ext_input.setIndices(indices);
ext_input.filter(*cluster_cloud);
pcl::PointCloud<pcl::Normal>::Ptr cluster_normal(new pcl::PointCloud<pcl::Normal>);
ext_normal.setIndices(indices);
ext_normal.filter(*cluster_normal);
pcl::ModelCoefficients::Ptr support_plane(new pcl::ModelCoefficients);
if (!getSupportPlane(cluster_cloud, polygons, support_plane))
{
NODELET_ERROR_STREAM("cloud " << i << " has no support plane. skipped");
continue;
}
pcl::PointIndices::Ptr b(new pcl::PointIndices);
pcl::PointCloud<PointT>::Ptr pc(new pcl::PointCloud<PointT>);
float circle_likelihood, box_likelihood;
estimate(cluster_cloud, cluster_normal, support_plane,
b, pc,
circle_likelihood, box_likelihood);
boundary_indices.push_back(std::move(b));
*projected_cloud += *pc;
if (circle_likelihood > circle_threshold_) {
// circle
result.labels.push_back(1);
result.label_names.push_back("circle");
result.label_proba.push_back(circle_likelihood);
} else if (box_likelihood > box_threshold_) {
// box
result.labels.push_back(0);
result.label_names.push_back("box");
result.label_proba.push_back(box_likelihood);
} else {
// other
result.labels.push_back(3);
result.label_names.push_back("other");
result.label_proba.push_back(0.0);
}
}
// publish results
sensor_msgs::PointCloud2 ros_projected_cloud;
pcl::toROSMsg(*projected_cloud, ros_projected_cloud);
ros_projected_cloud.header = ros_cloud->header;
pub_projected_cloud_.publish(ros_projected_cloud);
jsk_recognition_msgs::ClusterPointIndices ros_boundary_indices;
ros_boundary_indices.header = ros_cloud->header;
for (size_t i = 0; i < boundary_indices.size(); ++i)
{
pcl_msgs::PointIndices ri;
pcl_conversions::moveFromPCL(*boundary_indices[i], ri);
ros_boundary_indices.cluster_indices.push_back(ri);
}
pub_boundary_indices_.publish(ros_boundary_indices);
pub_class_.publish(result);
}
