void ProjectImagePoint::project(
const geometry_msgs::PointStamped::ConstPtr& msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
if (!camera_info_) {
JSK_NODELET_WARN(
"[ProjectImagePoint::project] camera info is not yet available");
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(camera_info_);
cv::Point3d ray = model.projectPixelTo3dRay(
cv::Point2d(msg->point.x, msg->point.y));
geometry_msgs::Vector3Stamped vector;
vector.header.frame_id = camera_info_->header.frame_id;
vector.header = msg->header;
vector.vector.x = ray.x;
vector.vector.y = ray.y;
vector.vector.z = ray.z;
pub_vector_.publish(vector);
if (ray.z == 0.0) {
JSK_NODELET_ERROR("Z value of projected ray is 0");
return;
}
double alpha = z_ / ray.z;
geometry_msgs::PointStamped point;
point.header = msg->header;
point.header.frame_id = camera_info_->header.frame_id;
point.point.x = ray.x * alpha;
point.point.y = ray.y * alpha;
point.point.z = ray.z * alpha;
pub_.publish(point);
}
void TiltLaserListener::getPointCloudFromLocalBuffer(
const std::vector<sensor_msgs::PointCloud2::ConstPtr>& target_clouds,
sensor_msgs::PointCloud2& output_cloud)
{
if (target_clouds.size() > 0) {
output_cloud.fields = target_clouds[0]->fields;
output_cloud.is_bigendian = target_clouds[0]->is_bigendian;
output_cloud.is_dense = true;
output_cloud.point_step = target_clouds[0]->point_step;
size_t point_num = 0;
size_t data_num = 0;
for (size_t i = 0; i < target_clouds.size(); i++) {
data_num += target_clouds[i]->row_step;
point_num += target_clouds[i]->width * target_clouds[i]->height;
}
output_cloud.data.reserve(data_num);
for (size_t i = 0; i < target_clouds.size(); i++) {
std::copy(target_clouds[i]->data.begin(),
target_clouds[i]->data.end(),
std::back_inserter(output_cloud.data));
}
output_cloud.header.frame_id = target_clouds[0]->header.frame_id;
output_cloud.width = point_num;
output_cloud.height = 1;
output_cloud.row_step = data_num;
}
else {
JSK_NODELET_WARN("target_clouds size is 0");
}
}
void PlaneSupportedCuboidEstimator::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_INFO("cloudCallback");
if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
JSK_NODELET_WARN("Not yet polygon is available");
return;
}
if (!tracker_) {
NODELET_INFO("initTracker");
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons_.push_back(polygon);
}
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
ParticleCloud::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
support_plane_updated_ = false;
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
else {
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() > fast_cloud_threshold_) {
estimate(msg);
}
}
}
void GridSampler::configCallback(Config &config, uint32_t level)
{
boost::mutex::scoped_lock(mutex_);
if (config.grid_size == 0.0) {
JSK_NODELET_WARN("grid_size == 0.0 is prohibited");
return;
}
else {
grid_size_ = config.grid_size;
min_indices_ = config.min_indices;
}
}
void FeatureRegistration::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
JSK_NODELET_ERROR("Not yet reference data is available");
return;
}
pcl::PointCloud<pcl::PointNormal>::Ptr
cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr
object_aligned (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr
feature (new pcl::PointCloud<pcl::FPFHSignature33>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::fromROSMsg(*feature_msg, *feature);
pcl::SampleConsensusPrerejective<pcl::PointNormal,
pcl::PointNormal,
pcl::FPFHSignature33> align;
align.setInputSource(reference_cloud_);
align.setSourceFeatures(reference_feature_);
align.setInputTarget(cloud);
align.setTargetFeatures(feature);
align.setMaximumIterations(max_iterations_); // Number of RANSAC iterations
align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness(correspondence_randomness_); // Number of nearest features to use
align.setSimilarityThreshold(similarity_threshold_); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance(max_correspondence_distance_); // Inlier threshold
align.setInlierFraction(inlier_fraction_); // Required inlier fraction for accepting a pose hypothesis
align.align (*object_aligned);
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Affine3f transformation(align.getFinalTransformation());
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(transformation, ros_pose.pose);
ros_pose.header = cloud_msg->header;
pub_pose_.publish(ros_pose);
pcl::PointCloud<pcl::PointNormal>::Ptr
result_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloud(
*reference_cloud_, *result_cloud, transformation);
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*object_aligned, ros_cloud);
ros_cloud.header = cloud_msg->header;
pub_cloud_.publish(ros_cloud);
//pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
}
else {
JSK_NODELET_WARN("failed to align pointcloud");
}
}
void GeometricConsistencyGrouping::recognize(
const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
return;
}
pcl::PointCloud<pcl::SHOT352>::Ptr
scene_feature (new pcl::PointCloud<pcl::SHOT352>);
pcl::PointCloud<pcl::PointNormal>::Ptr
scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud (reference_feature_);
for (size_t i = 0; i < scene_feature->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
continue;
}
int found_neighs
= match_search.nearestKSearch(scene_feature->at(i), 1,
neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// (SHOT descriptor distances are between 0 and 1 by design)
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
gc_clusterer.setGCSize(gc_size_);
gc_clusterer.setGCThreshold(gc_thresh_);
gc_clusterer.setInputCloud(reference_cloud_);
gc_clusterer.setSceneCloud(scene_cloud);
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
std::vector<pcl::Correspondences> clustered_corrs;
std::vector<Eigen::Matrix4f,
Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
gc_clusterer.recognize(rototranslations, clustered_corrs);
if (rototranslations.size() > 0) {
JSK_NODELET_INFO("detected %lu objects", rototranslations.size());
Eigen::Matrix4f result_mat = rototranslations[0];
Eigen::Affine3f affine(result_mat);
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(affine, ros_pose.pose);
ros_pose.header = scene_cloud_msg->header;
pub_output_.publish(ros_pose);
}
else {
JSK_NODELET_WARN("Failed to find object");
}
}
void PointCloudLocalization::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
//JSK_NODELET_INFO("cloudCallback");
latest_cloud_ = cloud_msg;
if (localize_requested_){
JSK_NODELET_INFO("localization is requested");
try {
pcl::PointCloud<pcl::PointNormal>::Ptr
local_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*latest_cloud_, *local_cloud);
JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
latest_cloud_->header.frame_id.c_str(),
global_frame_.c_str());
if (tf_listener_->waitForTransform(
latest_cloud_->header.frame_id,
global_frame_,
latest_cloud_->header.stamp,
ros::Duration(1.0))) {
pcl::PointCloud<pcl::PointNormal>::Ptr
input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl_ros::transformPointCloudWithNormals(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
else {
pcl_ros::transformPointCloud(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
applyDownsampling(input_cloud, *input_downsampled_cloud);
if (isFirstTime()) {
all_cloud_ = input_downsampled_cloud;
first_time_ = false;
}
else {
// run ICP
ros::ServiceClient client
= pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
jsk_pcl_ros::ICPAlign icp_srv;
if (clip_unseen_pointcloud_) {
// Before running ICP, remove pointcloud where we cannot see
// First, transform reference pointcloud, that is all_cloud_, into
// sensor frame.
// And after that, remove points which are x < 0.
tf::StampedTransform global_sensor_tf_transform
= lookupTransformWithDuration(
tf_listener_,
global_frame_,
sensor_frame_,
cloud_msg->header.stamp,
ros::Duration(1.0));
Eigen::Affine3f global_sensor_transform;
tf::transformTFToEigen(global_sensor_tf_transform,
global_sensor_transform);
pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*all_cloud_,
*sensor_cloud,
global_sensor_transform.inverse());
// Remove negative-x points
pcl::PassThrough<pcl::PointNormal> pass;
pass.setInputCloud(sensor_cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(0.0, 100.0);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pass.filter(*filtered_cloud);
JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
// Convert the pointcloud to global frame again
pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*filtered_cloud,
*global_filtered_cloud,
global_sensor_transform);
pcl::toROSMsg(*global_filtered_cloud,
icp_srv.request.target_cloud);
}
else {
pcl::toROSMsg(*all_cloud_,
icp_srv.request.target_cloud);
}
pcl::toROSMsg(*input_downsampled_cloud,
icp_srv.request.reference_cloud);
if (client.call(icp_srv)) {
Eigen::Affine3f transform;
tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
Eigen::Vector3f transform_pos(transform.translation());
float roll, pitch, yaw;
pcl::getEulerAngles(transform, roll, pitch, yaw);
JSK_NODELET_INFO("aligned parameter --");
JSK_NODELET_INFO(" - pos: [%f, %f, %f]",
transform_pos[0],
transform_pos[1],
transform_pos[2]);
JSK_NODELET_INFO(" - rot: [%f, %f, %f]", roll, pitch, yaw);
pcl::PointCloud<pcl::PointNormal>::Ptr
transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl::transformPointCloudWithNormals(*input_cloud,
*transformed_input_cloud,
transform);
}
else {
pcl::transformPointCloud(*input_cloud,
*transformed_input_cloud,
transform);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>);
*concatenated_cloud = *all_cloud_ + *transformed_input_cloud;
// update *all_cloud
applyDownsampling(concatenated_cloud, *all_cloud_);
// update localize_transform_
tf::Transform icp_transform;
tf::transformEigenToTF(transform, icp_transform);
{
boost::mutex::scoped_lock tf_lock(tf_mutex_);
localize_transform_ = localize_transform_ * icp_transform;
}
}
else {
JSK_NODELET_ERROR("Failed to call ~icp_align");
return;
}
}
localize_requested_ = false;
}
else {
JSK_NODELET_WARN("No tf transformation is available");
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
}
}
void CollisionDetector::initSelfMask()
{
// genearte urdf model
std::string content;
urdf::Model urdf_model;
if (nh_->getParam("robot_description", content)) {
if (!urdf_model.initString(content)) {
JSK_NODELET_ERROR("Unable to parse URDF description!");
}
}
std::string root_link_id;
std::string world_frame_id;
pnh_->param<std::string>("root_link_id", root_link_id, "BODY");
pnh_->param<std::string>("world_frame_id", world_frame_id, "map");
double default_padding, default_scale;
pnh_->param("self_see_default_padding", default_padding, 0.01);
pnh_->param("self_see_default_scale", default_scale, 1.0);
std::vector<robot_self_filter::LinkInfo> links;
std::string link_names;
if (!pnh_->hasParam("self_see_links")) {
JSK_NODELET_WARN("No links specified for self filtering.");
} else {
XmlRpc::XmlRpcValue ssl_vals;;
pnh_->getParam("self_see_links", ssl_vals);
if (ssl_vals.getType() != XmlRpc::XmlRpcValue::TypeArray) {
JSK_NODELET_WARN("Self see links need to be an array");
} else {
if (ssl_vals.size() == 0) {
JSK_NODELET_WARN("No values in self see links array");
} else {
for (int i = 0; i < ssl_vals.size(); i++) {
robot_self_filter::LinkInfo li;
if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
JSK_NODELET_WARN("Self see links entry %d is not a structure. Stopping processing of self see links",i);
break;
}
if (!ssl_vals[i].hasMember("name")) {
JSK_NODELET_WARN("Self see links entry %d has no name. Stopping processing of self see links",i);
break;
}
li.name = std::string(ssl_vals[i]["name"]);
if (!ssl_vals[i].hasMember("padding")) {
JSK_NODELET_DEBUG("Self see links entry %d has no padding. Assuming default padding of %g",i,default_padding);
li.padding = default_padding;
} else {
li.padding = ssl_vals[i]["padding"];
}
if (!ssl_vals[i].hasMember("scale")) {
JSK_NODELET_DEBUG("Self see links entry %d has no scale. Assuming default scale of %g",i,default_scale);
li.scale = default_scale;
} else {
li.scale = ssl_vals[i]["scale"];
}
links.push_back(li);
}
}
}
}
self_mask_ = boost::shared_ptr<robot_self_filter::SelfMaskUrdfRobot>(new robot_self_filter::SelfMaskUrdfRobot(tf_listener_, tf_broadcaster_, links, urdf_model, root_link_id, world_frame_id));
}
