void ColorizeDistanceFromPlane::colorize(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
{
boost::mutex::scoped_lock lock(mutex_);
if (coefficients_msg->coefficients.size() == 0) {
return;
}
// convert all the data into pcl format
pcl::PointCloud<PointT>::Ptr cloud
(new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*cloud_msg, *cloud);
std::vector<pcl::ModelCoefficients::Ptr> coefficients
= pcl_conversions::convertToPCLModelCoefficients(
coefficients_msg->coefficients);
// first, build ConvexPolygon::Ptr
std::vector<ConvexPolygon::Ptr> convexes;
for (size_t i = 0; i < polygons->polygons.size(); i++) {
if (polygons->polygons[i].polygon.points.size() > 0) {
ConvexPolygon convex =
ConvexPolygon::fromROSMsg(polygons->polygons[i].polygon);
ConvexPolygon::Ptr convex_ptr
= boost::make_shared<ConvexPolygon>(convex);
convexes.push_back(convex_ptr);
}
else {
JSK_NODELET_ERROR_STREAM(__PRETTY_FUNCTION__ << ":: there is no points in the polygon");
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud
(new pcl::PointCloud<pcl::PointXYZRGB>);
for (size_t i = 0; i < cloud->points.size(); i++) {
PointT p = cloud->points[i];
pcl::PointXYZRGB p_output;
pointFromXYZToXYZ<PointT, pcl::PointXYZRGB>(p, p_output);
double d = distanceToConvexes(p, convexes);
if (d != DBL_MAX) {
uint32_t color = colorForDistance(d);
p_output.rgb = *reinterpret_cast<float*>(&color);
output_cloud->points.push_back(p_output);
}
}
sensor_msgs::PointCloud2 ros_output;
pcl::toROSMsg(*output_cloud, ros_output);
ros_output.header = cloud_msg->header;
pub_.publish(ros_output);
}
bool CollisionDetector::checkCollision(const sensor_msgs::JointState& joint,
const geometry_msgs::PoseStamped& pose)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("checkCollision is called.");
// calculate the sensor transformation
tf::StampedTransform sensor_to_world_tf;
try {
tf_listener_.lookupTransform(world_frame_id_, cloud_frame_id_, cloud_stamp_, sensor_to_world_tf);
} catch (tf::TransformException& ex) {
JSK_NODELET_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
return false;
}
// transform point cloud
Eigen::Matrix4f sensor_to_world;
pcl_ros::transformAsMatrix(sensor_to_world_tf, sensor_to_world);
pcl::transformPointCloud(cloud_, cloud_, sensor_to_world);
self_mask_->assumeFrameFromJointAngle(joint, pose);
// check containment for all point cloud
bool contain_flag = false;
pcl::PointXYZ p;
for (size_t i = 0; i < cloud_.size(); i++) {
p = cloud_.at(i);
if (finite(p.x) && finite(p.y) && finite(p.z) &&
self_mask_->getMaskContainment(p.x, p.y, p.z) == robot_self_filter::INSIDE) {
contain_flag = true;
break;
}
}
if (contain_flag) {
JSK_NODELET_INFO("collision!");
} else {
JSK_NODELET_INFO("no collision!");
}
return contain_flag;
}
