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; }