void TfTransformCloud::transform(const sensor_msgs::PointCloud2ConstPtr &input)
{
vital_checker_->poke();
sensor_msgs::PointCloud2 output;
try
{
if (use_latest_tf_) {
sensor_msgs::PointCloud2 latest_pointcloud(*input);
latest_pointcloud.header.stamp = ros::Time(0);
if (pcl_ros::transformPointCloud(target_frame_id_, latest_pointcloud, output,
*tf_listener_)) {
output.header.stamp = input->header.stamp;
pub_cloud_.publish(output);
}
}
else {
if (pcl_ros::transformPointCloud(target_frame_id_, *input, output,
*tf_listener_)) {
pub_cloud_.publish(output);
}
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (...)
{
NODELET_ERROR("Unknown transform error");
}
}
void SnapIt::convexAlignPolygonCallback(
const geometry_msgs::PolygonStamped::ConstPtr& poly_msg)
{
boost::mutex::scoped_lock lock(mutex_);
geometry_msgs::PoseArray pose_array;
pose_array.header = poly_msg->header;
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(poly_msg->header.frame_id,
poly_msg->header.stamp,
polygons_);
for (size_t i = 0; i < poly_msg->polygon.points.size(); i++) {
geometry_msgs::Point32 p = poly_msg->polygon.points[i];
Eigen::Vector3f pose_point(p.x, p.y, p.z);
int min_index = findNearestConvex(pose_point, convexes);
if (min_index == -1) {
JSK_NODELET_ERROR("cannot project onto convex");
return;
}
else {
jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
Eigen::Affine3f pose_eigen = Eigen::Affine3f::Identity();
pose_eigen.translate(pose_point);
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = poly_msg->header;
pose_array.poses.push_back(aligned_pose.pose);
}
}
convex_aligned_pose_array_pub_.publish(pose_array);
}
void TiltLaserListener::onInit()
{
DiagnosticNodelet::onInit();
skip_counter_ = 0;
if (pnh_->hasParam("joint_name")) {
pnh_->getParam("joint_name", joint_name_);
}
else {
JSK_NODELET_ERROR("no ~joint_state is specified");
return;
}
pnh_->param("overwrap_angle", overwrap_angle_, 0.0);
std::string laser_type;
pnh_->param("skip_number", skip_number_, 1);
pnh_->param("laser_type", laser_type, std::string("tilt_half_down"));
if (laser_type == "tilt_half_up") {
laser_type_ = TILT_HALF_UP;
}
else if (laser_type == "tilt_half_down") {
laser_type_ = TILT_HALF_DOWN;
}
else if (laser_type == "tilt") {
laser_type_ = TILT;
}
else if (laser_type == "infinite_spindle") {
laser_type_ = INFINITE_SPINDLE;
}
else if (laser_type == "infinite_spindle_half") {
laser_type_ = INFINITE_SPINDLE_HALF;
}
else {
JSK_NODELET_ERROR("unknown ~laser_type: %s", laser_type.c_str());
return;
}
pnh_->param("not_use_laser_assembler_service", not_use_laser_assembler_service_, false);
pnh_->param("use_laser_assembler", use_laser_assembler_, false);
if (use_laser_assembler_) {
if (not_use_laser_assembler_service_) {
sub_cloud_
= pnh_->subscribe("input/cloud", 100, &TiltLaserListener::cloudCallback, this);
}
else {
assemble_cloud_srv_
= pnh_->serviceClient<laser_assembler::AssembleScans2>("assemble_scans2");
}
cloud_pub_
= pnh_->advertise<sensor_msgs::PointCloud2>("output_cloud", 1);
}
prev_angle_ = 0;
prev_velocity_ = 0;
start_time_ = ros::Time::now();
clear_cache_service_ = pnh_->advertiseService(
"clear_cache", &TiltLaserListener::clearCacheCallback,
this);
trigger_pub_ = advertise<jsk_recognition_msgs::TimeRange>(*pnh_, "output", 1);
sub_ = pnh_->subscribe("input", 1, &TiltLaserListener::jointCallback, this);
}
void TiltLaserListener::publishTimeRange(
const ros::Time& stamp,
const ros::Time& start,
const ros::Time& end)
{
jsk_recognition_msgs::TimeRange range;
range.header.stamp = stamp;
range.start = start;
range.end = end;
trigger_pub_.publish(range);
if (use_laser_assembler_) {
if (skip_counter_++ % skip_number_ == 0) {
laser_assembler::AssembleScans2 srv;
srv.request.begin = start;
srv.request.end = end;
try {
if (!not_use_laser_assembler_service_) {
if (assemble_cloud_srv_.call(srv)) {
sensor_msgs::PointCloud2 output_cloud = srv.response.cloud;
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
else {
JSK_NODELET_ERROR("Failed to call assemble cloud service");
}
}
else {
// Assemble cloud from local buffer
std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds;
{
boost::mutex::scoped_lock lock(cloud_mutex_);
for (size_t i = 0; i < cloud_buffer_.size(); i++) {
ros::Time the_stamp = cloud_buffer_[i]->header.stamp;
if (the_stamp > start && the_stamp < end) {
target_clouds.push_back(cloud_buffer_[i]);
}
}
cloud_buffer_.removeBefore(start);
}
sensor_msgs::PointCloud2 output_cloud;
getPointCloudFromLocalBuffer(target_clouds, output_cloud);
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
}
catch (...) {
JSK_NODELET_ERROR("Exception in calling assemble cloud service");
}
}
}
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> SnapIt::createConvexes(
const std::string& frame_id, const ros::Time& stamp,
jsk_recognition_msgs::PolygonArray::ConstPtr polygons)
{
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> result;
try
{
for (size_t i = 0; i < polygons->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
jsk_recognition_utils::Vertices vertices;
tf::StampedTransform transform = lookupTransformWithDuration(
tf_listener_,
polygon.header.frame_id, frame_id, stamp, ros::Duration(5.0));
for (size_t j = 0; j < polygon.polygon.points.size(); j++) {
Eigen::Vector4d p;
p[0] = polygon.polygon.points[j].x;
p[1] = polygon.polygon.points[j].y;
p[2] = polygon.polygon.points[j].z;
p[3] = 1;
Eigen::Affine3d eigen_transform;
tf::transformTFToEigen(transform, eigen_transform);
Eigen::Vector4d transformed_pointd = eigen_transform.inverse() * p;
Eigen::Vector3f transformed_point;
transformed_point[0] = transformed_pointd[0];
transformed_point[1] = transformed_pointd[1];
transformed_point[2] = transformed_pointd[2];
vertices.push_back(transformed_point);
}
std::reverse(vertices.begin(), vertices.end());
jsk_recognition_utils::ConvexPolygon::Ptr convex(new jsk_recognition_utils::ConvexPolygon(vertices));
result.push_back(convex);
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (tf2::ExtrapolationException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
return result;
}
void SnapIt::convexAlignCallback(
const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
convex_aligned_pub_.publish(pose_msg);
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(pose_msg->header.frame_id,
pose_msg->header.stamp,
polygons_);
Eigen::Affine3d pose_eigend;
Eigen::Affine3f pose_eigen;
tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
convertEigenAffine3(pose_eigend, pose_eigen);
Eigen::Vector3f pose_point(pose_eigen.translation());
int min_index = findNearestConvex(pose_point, convexes);
if (min_index != -1) {
jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = pose_msg->header;
convex_aligned_pub_.publish(aligned_pose);
}
else {
convex_aligned_pub_.publish(pose_msg); // shoud we publish this?
}
}
void HeightmapToPointCloud::convert(const sensor_msgs::Image::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!config_msg_) {
JSK_NODELET_ERROR("no ~input/config is yet available");
return;
}
cv::Mat float_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_32FC1)->image;
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.reserve(float_image.rows * float_image.cols);
double dx = (max_x_ - min_x_) / float_image.cols;
double dy = (max_y_ - min_y_) / float_image.rows;
for (size_t j = 0; j < float_image.rows; j++) {
for (size_t i = 0; i < float_image.cols; i++) {
float v = float_image.at<float>(j, i);
pcl::PointXYZ p;
if (v != -FLT_MAX) {
p.y = j * dy + min_y_ + dy / 2.0;
p.x = i * dx + min_x_ + dx / 2.0;
p.z = v;
cloud.points.push_back(p);
}
}
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(cloud, ros_cloud);
ros_cloud.header = msg->header;
pub_.publish(ros_cloud);
}
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 OctreeVoxelGrid::generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr& input_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (resolution_ == 0.0) {
pub_cloud_.publish(input_msg);
}
else {
if (point_type_ == "xyz") {
generateVoxelCloudImpl<pcl::PointXYZ>(input_msg);
}
else if (point_type_ == "xyznormal") {
generateVoxelCloudImpl<pcl::PointNormal>(input_msg);
}
else if (point_type_ == "xyzrgb") {
generateVoxelCloudImpl<pcl::PointXYZRGB>(input_msg);
}
else if (point_type_ == "xyzrgbnormal") {
generateVoxelCloudImpl<pcl::PointXYZRGBNormal>(input_msg);
}
else {
JSK_NODELET_ERROR("unknown point type: %s", point_type_.c_str());
}
}
std_msgs::Float32 resolution;
resolution.data = resolution_;
pub_octree_resolution_.publish(resolution);
}
void PolygonArrayColorLikelihood::likelihood(
const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr& histogram_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_) {
return;
}
if (polygon_msg->polygons.size() != histogram_msg->histograms.size()) {
JSK_NODELET_ERROR("length of polygon and histogram are not same");
return;
}
cv::MatND reference_histogram
= jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
reference_->bins);
cv::normalize(reference_histogram, reference_histogram, 1, reference_histogram.rows, cv::NORM_L2,
-1, cv::Mat());
jsk_recognition_msgs::PolygonArray new_msg(*polygon_msg);
for (size_t i = 0; i < new_msg.polygons.size(); i++) {
cv::MatND hist
= jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
histogram_msg->histograms[i].bins);
cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2,
-1, cv::Mat());
double d = compareHist(reference_histogram, hist);
if (polygon_msg->likelihood.size() == 0) {
new_msg.likelihood.push_back(d);
}
else {
new_msg.likelihood[i] *= d;
}
}
pub_.publish(new_msg);
}
void BorderEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& msg,
const sensor_msgs::CameraInfo::ConstPtr& info)
{
if (msg->height == 1) {
JSK_NODELET_ERROR("[BorderEstimator::estimate] pointcloud must be organized");
return;
}
pcl::RangeImagePlanar range_image;
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg, cloud);
Eigen::Affine3f dummytrans = Eigen::Affine3f::Identity();
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
range_image.createFromPointCloudWithFixedSize (cloud,
msg->width,
msg->height,
cx, cy,
fx, fy,
dummytrans);
range_image.setUnseenToMaxRange();
computeBorder(range_image, msg->header);
}
void LabDecomposer::decompose(
const sensor_msgs::Image::ConstPtr& image_msg)
{
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
image_msg, image_msg->encoding);
cv::Mat image = cv_ptr->image;
cv::Mat lab_image;
std::vector<cv::Mat> lab_planes;
if (image_msg->encoding == sensor_msgs::image_encodings::BGR8) {
cv::cvtColor(image, lab_image, CV_BGR2Lab);
}
else if (image_msg->encoding == sensor_msgs::image_encodings::RGB8) {
cv::cvtColor(image, lab_image, CV_RGB2Lab);
}
else {
JSK_NODELET_ERROR("unsupported format to Lab: %s", image_msg->encoding.c_str());
return;
}
cv::split(lab_image, lab_planes);
cv::Mat l = lab_planes[0];
cv::Mat a = lab_planes[1];
cv::Mat b = lab_planes[2];
pub_l_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
l).toImageMsg());
pub_a_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
a).toImageMsg());
pub_b_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
b).toImageMsg());
}
void DepthCalibration::calibrate(
const sensor_msgs::Image::ConstPtr& msg,
const sensor_msgs::CameraInfo::ConstPtr& camera_info)
{
boost::mutex::scoped_lock lock(mutex_);
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}
catch (cv_bridge::Exception& e)
{
JSK_NODELET_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image = cv_ptr->image;
cv::Mat output_image = image.clone();
double cu = camera_info->P[2];
double cv = camera_info->P[6];
for(int v = 0; v < image.rows; v++) {
for(int u = 0; u < image.cols; u++) {
float z = image.at<float>(v, u);
if (isnan(z)) {
output_image.at<float>(v, u) = z;
}
else {
output_image.at<float>(v, u) = applyModel(z, u, v, cu, cv);
}
//JSK_NODELET_INFO("z: %f", z);
}
}
sensor_msgs::Image::Ptr ros_image = cv_bridge::CvImage(msg->header, "32FC1", output_image).toImageMsg();
pub_.publish(ros_image);
}
void HeightmapTimeAccumulation::accumulate(
const sensor_msgs::Image::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!config_) {
JSK_NODELET_ERROR("no ~input/config is yet available");
return;
}
tf::StampedTransform tf_transform;
tf_->lookupTransform(fixed_frame_id_, center_frame_id_,
msg->header.stamp,
tf_transform);
Eigen::Affine3f from_center_to_fixed;
tf::transformTFToEigen(tf_transform, from_center_to_fixed);
cv::Mat new_heightmap = cv_bridge::toCvShare(
msg, sensor_msgs::image_encodings::TYPE_32FC1)->image;
// Transform prev_cloud_ to current frame
Eigen::Affine3f from_prev_to_current
= prev_from_center_to_fixed_.inverse() * from_center_to_fixed;
pcl::PointCloud<pcl::PointXYZ> transformed_pointcloud;
pcl::transformPointCloud(prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse());
for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
pcl::PointXYZ p = transformed_pointcloud.points[i];
if (isValidPoint(p)) {
cv::Point index = toIndex(p, new_heightmap);
if (isValidIndex(index, new_heightmap)) {
if (!isValidCell(index, new_heightmap)) {
new_heightmap.at<float>(index.y, index.x) = p.z;
}
}
}
}
publishHeightmap(new_heightmap, msg->header);
prev_from_center_to_fixed_ = from_center_to_fixed;
}
void PolygonArrayAngleLikelihood::likelihood(
const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
jsk_recognition_msgs::PolygonArray new_msg(*msg);
try
{
// Resolve tf
// ConstPtrmpute position of target_frame_id
// respected from msg->header.frame_id
tf::StampedTransform transform;
tf_listener_->lookupTransform(
target_frame_id_, msg->header.frame_id, msg->header.stamp, transform);
Eigen::Affine3f pose;
tf::transformTFToEigen(transform, pose);
// Use x
Eigen::Vector3f reference_axis = pose.rotation() * axis_;
double min_distance = DBL_MAX;
double max_distance = - DBL_MAX;
std::vector<double> distances;
for (size_t i = 0; i < msg->polygons.size(); i++) {
jsk_recognition_utils::Polygon::Ptr polygon
= jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
Eigen::Vector3f n = polygon->getNormal();
double distance = std::abs(reference_axis.dot(n));
min_distance = std::min(distance, min_distance);
max_distance = std::max(distance, max_distance);
distances.push_back(distance);
}
// Normalization
for (size_t i = 0; i < distances.size(); i++) {
// double normalized_distance
// = (distances[i] - min_distance) / (max_distance - min_distance);
double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1));
if (msg->likelihood.size() == 0) {
new_msg.likelihood.push_back(likelihood);
}
else {
new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
}
}
pub_.publish(new_msg);
}
catch (...)
{
JSK_NODELET_ERROR("Unknown transform error");
}
}
void LineSegmentCollector::onInit()
{
DiagnosticNodelet::onInit();
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&LineSegmentCollector::configCallback, this, _1, _2);
srv_->setCallback (f);
if (!pnh_->getParam("fixed_frame_id", fixed_frame_id_)) {
JSK_NODELET_ERROR("no ~fixed_frame_id is specified");
return;
}
std::string rotate_type_str;
pnh_->param("rotate_type", rotate_type_str, std::string("tilt_two_way"));
if (rotate_type_str == "tilt") {
rotate_type_ = ROTATION_TILT;
}
else if (rotate_type_str == "tilt_two_way") {
rotate_type_ = ROTATION_TILT_TWO_WAY;
}
else if (rotate_type_str == "spindle") {
rotate_type_ = ROTATION_SPINDLE;
}
else {
JSK_NODELET_ERROR("unknown ~rotate_type: %s", rotate_type_str.c_str());
return;
}
pub_point_cloud_
= advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output/inliers", 1);
pub_coefficients_
= advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output/coefficients", 1);
pub_polygons_
= advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output/polygons", 1);
debug_pub_inliers_before_plane_
= advertise<jsk_recognition_msgs::ClusterPointIndices>(
*pnh_, "debug/connect_segments/inliers", 1);
}
void HSVDecomposer::decompose(
const sensor_msgs::Image::ConstPtr& image_msg)
{
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
image_msg, image_msg->encoding);
cv::Mat image = cv_ptr->image;
cv::Mat hsv_image;
std::vector<cv::Mat> hsv_planes;
if (image_msg->encoding == sensor_msgs::image_encodings::BGR8) {
cv::cvtColor(image, hsv_image, CV_BGR2HSV);
}
else if (image_msg->encoding == sensor_msgs::image_encodings::RGB8) {
cv::cvtColor(image, hsv_image, CV_RGB2HSV);
}
else if (image_msg->encoding == sensor_msgs::image_encodings::BGRA8 ||
image_msg->encoding == sensor_msgs::image_encodings::BGRA16) {
cv::Mat tmp_image;
cv::cvtColor(image, tmp_image, CV_BGRA2BGR);
cv::cvtColor(tmp_image, hsv_image, CV_BGR2HSV);
}
else if (image_msg->encoding == sensor_msgs::image_encodings::RGBA8 ||
image_msg->encoding == sensor_msgs::image_encodings::RGBA16) {
cv::Mat tmp_image;
cv::cvtColor(image, tmp_image, CV_RGBA2BGR);
cv::cvtColor(tmp_image, hsv_image, CV_BGR2HSV);
}
else {
JSK_NODELET_ERROR("unsupported format to HSV: %s", image_msg->encoding.c_str());
return;
}
cv::split(hsv_image, hsv_planes);
cv::Mat hue = hsv_planes[0];
cv::Mat saturation = hsv_planes[1];
cv::Mat value = hsv_planes[2];
pub_h_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
hue).toImageMsg());
pub_s_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
saturation).toImageMsg());
pub_v_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
value).toImageMsg());
}
double ColorHistogramLabelMatch::coefficients(
const cv::Mat& ref_hist,
const cv::Mat& target_hist)
{
if (coefficient_method_ == 0) {
return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0;
}
else if (coefficient_method_ == 1) {
double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR);
return 1/ (x * x + 1);
}
else if (coefficient_method_ == 2) {
return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT);
}
else if (coefficient_method_ == 3) {
return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA);
}
else if (coefficient_method_ == 4 || coefficient_method_ == 5) {
cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
//JSK_NODELET_INFO("ref_hist.cols = %d", ref_hist.cols);
for (size_t i = 0; i < ref_hist.cols; i++) {
ref_sig.at<float>(i, 0) = ref_hist.at<float>(0, i);
target_sig.at<float>(i, 0) = target_hist.at<float>(0, i);
ref_sig.at<float>(i, 1) = i;
target_sig.at<float>(i, 1) = i;
}
if (coefficient_method_ == 4) {
double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1);
return 1.0 / (1.0 + x * x);
}
else {
double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2);
return 1.0 / (1.0 + x * x);
}
}
else {
JSK_NODELET_ERROR("unknown coefficiet method: %d", coefficient_method_);
return 0;
}
}
void SnapIt::polygonAlignCallback(
const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
polygon_aligned_pub_.publish(pose_msg);
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(pose_msg->header.frame_id,
pose_msg->header.stamp,
polygons_);
Eigen::Affine3d pose_eigend;
Eigen::Affine3f pose_eigen;
tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
convertEigenAffine3(pose_eigend, pose_eigen);
Eigen::Vector3f pose_point(pose_eigen.translation());
double min_distance = DBL_MAX;
jsk_recognition_utils::ConvexPolygon::Ptr min_convex;
for (size_t i = 0; i < convexes.size(); i++) {
jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
double d = convex->distanceToPoint(pose_point);
if (d < min_distance) {
min_convex = convex;
min_distance = d;
}
}
if (min_convex) {
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = pose_msg->header;
polygon_aligned_pub_.publish(aligned_pose);
}
else {
polygon_aligned_pub_.publish(pose_msg);
}
}
void ApplyMaskImage::apply(
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::Image::ConstPtr& mask_msg)
{
vital_checker_->poke();
cv::Mat image;
if (jsk_recognition_utils::isBGRA(image_msg->encoding)) {
cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
cv::cvtColor(tmp_image, image, cv::COLOR_BGRA2BGR);
}
else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) {
cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
cv::cvtColor(tmp_image, image, cv::COLOR_RGBA2BGR);
}
else { // BGR, RGB or GRAY
image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
}
cv::Mat mask = cv_bridge::toCvShare(mask_msg, "mono8")->image;
if (image.cols != mask.cols || image.rows != mask.rows) {
JSK_NODELET_ERROR("size of image and mask is different");
JSK_NODELET_ERROR("image: %dx%dx", image.cols, image.rows);
JSK_NODELET_ERROR("mask: %dx%dx", mask.cols, mask.rows);
return;
}
if (clip_) {
cv::Rect region = jsk_recognition_utils::boundingRectOfMaskImage(mask);
mask = mask(region);
image = image(region);
}
pub_mask_.publish(cv_bridge::CvImage(
mask_msg->header,
"mono8",
mask).toImageMsg());
cv::Mat masked_image;
image.copyTo(masked_image, mask);
cv::Mat output_image;
if (mask_black_to_transparent_) {
if (sensor_msgs::image_encodings::isMono(image_msg->encoding)) {
cv::cvtColor(masked_image, output_image, CV_GRAY2BGRA);
}
else if (jsk_recognition_utils::isRGB(image_msg->encoding)) {
cv::cvtColor(masked_image, output_image, CV_RGB2BGRA);
}
else { // BGR, BGRA or RGBA
cv::cvtColor(masked_image, output_image, CV_BGR2BGRA);
}
for (size_t j=0; j < mask.rows; j++) {
for (int i=0; i < mask.cols; i++) {
if (mask.at<uchar>(j, i) == 0) {
cv::Vec4b color = output_image.at<cv::Vec4b>(j, i);
color[3] = 0; // mask black -> transparent
output_image.at<cv::Vec4b>(j, i) = color;
}
}
}
// publish bgr8 image
pub_image_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::BGRA8,
output_image).toImageMsg());
}
else {
if (jsk_recognition_utils::isBGRA(image_msg->encoding)) {
cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2BGRA);
}
else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) {
cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2RGBA);
}
else { // BGR, RGB or GRAY
masked_image.copyTo(output_image);
}
pub_image_.publish(cv_bridge::CvImage(
image_msg->header,
image_msg->encoding,
output_image).toImageMsg());
}
}
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 TiltLaserListener::publishTimeRange(
const ros::Time& stamp,
const ros::Time& start,
const ros::Time& end)
{
jsk_recognition_msgs::TimeRange range;
range.header.stamp = stamp;
range.start = start;
range.end = end;
trigger_pub_.publish(range);
// publish velocity
// only publish if twist_frame_id is not empty
if (!twist_frame_id_.empty()) {
// simply compute from the latest velocity
if (buffer_.size() >= 2) {
// at least, we need two joint angles to
// compute velocity
StampedJointAngle::Ptr latest = buffer_[buffer_.size() - 1];
StampedJointAngle::Ptr last_second = buffer_[buffer_.size() - 2];
double value_diff = latest->getValue() - last_second->getValue();
double time_diff = (latest->header.stamp - last_second->header.stamp).toSec();
double velocity = value_diff / time_diff;
geometry_msgs::TwistStamped twist;
twist.header.frame_id = twist_frame_id_;
twist.header.stamp = latest->header.stamp;
if (laser_type_ == INFINITE_SPINDLE_HALF || // roll laser
laser_type_ == INFINITE_SPINDLE) {
twist.twist.angular.x = velocity;
}
else { // tilt laser
twist.twist.angular.y = velocity;
}
twist_pub_.publish(twist);
}
}
if (use_laser_assembler_) {
if (skip_counter_++ % skip_number_ == 0) {
laser_assembler::AssembleScans2 srv;
srv.request.begin = start;
srv.request.end = end;
try {
if (!not_use_laser_assembler_service_) {
if (assemble_cloud_srv_.call(srv)) {
sensor_msgs::PointCloud2 output_cloud = srv.response.cloud;
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
else {
JSK_NODELET_ERROR("Failed to call assemble cloud service");
}
}
else {
// Assemble cloud from local buffer
std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds;
{
boost::mutex::scoped_lock lock(cloud_mutex_);
if (cloud_buffer_.size() == 0) {
return;
}
for (size_t i = 0; i < cloud_buffer_.size(); i++) {
ros::Time the_stamp = cloud_buffer_[i]->header.stamp;
if (the_stamp > start && the_stamp < end) {
target_clouds.push_back(cloud_buffer_[i]);
}
}
if (clear_assembled_scans_) {
cloud_buffer_.removeBefore(end);
}
else {
cloud_buffer_.removeBefore(start);
}
}
sensor_msgs::PointCloud2 output_cloud;
getPointCloudFromLocalBuffer(target_clouds, output_cloud);
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
}
catch (...) {
JSK_NODELET_ERROR("Exception in calling assemble cloud service");
}
}
}
}
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));
}
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 PlaneSupportedCuboidEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
// 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);
polygon->decomposeToTriangles();
polygons_.push_back(polygon);
}
try {
// 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_);
if (!tracker_) {
NODELET_INFO("initTracker");
pcl::PointCloud<pcl::tracking::ParticleCuboid>::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_);
}
else {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
tracker_->setInputCloud(cloud);
// Before call compute() method, we prepare candidate_cloud_ for
// weight phase.
candidate_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
std::set<int> all_indices;
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism_extract;
pcl::PointCloud<pcl::PointXYZ>::Ptr
boundaries (new pcl::PointCloud<pcl::PointXYZ>);
polygon->boundariesToPointCloud<pcl::PointXYZ>(*boundaries);
boundaries->points.push_back(boundaries->points[0]);
prism_extract.setInputCloud(cloud);
prism_extract.setViewPoint(viewpoint_[0], viewpoint_[1], viewpoint_[2]);
prism_extract.setHeightLimits(init_local_position_z_min_, init_local_position_z_max_);
prism_extract.setInputPlanarHull(boundaries);
pcl::PointIndices output_indices;
prism_extract.segment(output_indices);
for (size_t i = 0; i < output_indices.indices.size(); i++) {
all_indices.insert(output_indices.indices[i]);
}
}
pcl::ExtractIndices<pcl::PointXYZ> ex;
ex.setInputCloud(cloud);
pcl::PointIndices::Ptr indices (new pcl::PointIndices);
indices->indices = std::vector<int>(all_indices.begin(), all_indices.end());
ex.setIndices(indices);
ex.filter(*candidate_cloud_);
sensor_msgs::PointCloud2 ros_candidate_cloud;
pcl::toROSMsg(*candidate_cloud_, ros_candidate_cloud);
ros_candidate_cloud.header = msg->header;
pub_candidate_cloud_.publish(ros_candidate_cloud);
// check the number of candidate points
if (candidate_cloud_->points.size() == 0) {
JSK_NODELET_ERROR("No candidate cloud");
return;
}
tree_.setInputCloud(candidate_cloud_);
if (support_plane_updated_) {
// Compute assignment between particles and polygons
NODELET_INFO("polygon updated");
updateParticlePolygonRelationship(tracker_->getParticles());
}
ROS_INFO("start tracker_->compute()");
tracker_->compute();
ROS_INFO("done tracker_->compute()");
Particle result = tracker_->getResult();
jsk_recognition_msgs::BoundingBoxArray box_array;
box_array.header = msg->header;
box_array.boxes.push_back(result.toBoundingBox());
box_array.boxes[0].header = msg->header;
pub_result_.publish(box_array);
}
support_plane_updated_ = false;
ParticleCloud::Ptr particles = tracker_->getParticles();
// 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);
}
catch (tf2::TransformException& e) {
JSK_ROS_ERROR("tf exception");
}
}
void PolygonArrayTransformer::transform(const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
{
if (polygons->polygons.size() != coefficients->coefficients.size()) {
JSK_NODELET_ERROR("the size of polygons(%lu) does not match with the size of coefficients(%lu)",
polygons->polygons.size(),
coefficients->coefficients.size());
return;
}
jsk_recognition_msgs::PolygonArray transformed_polygon_array;
jsk_recognition_msgs::ModelCoefficientsArray transformed_model_coefficients_array;
transformed_polygon_array.header = polygons->header;
transformed_model_coefficients_array.header = coefficients->header;
transformed_polygon_array.header.frame_id = frame_id_;
transformed_model_coefficients_array.header.frame_id = frame_id_;
for (size_t i = 0; i < polygons->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
PCLModelCoefficientMsg coefficient = coefficients->coefficients[i];
if (polygon.header.frame_id != coefficient.header.frame_id) {
JSK_NODELET_ERROR("frame_id of polygon[%lu] is %s and frame_id of coefficient[%lu] is %s, they does not point to the same frame_id",
i, polygon.header.frame_id.c_str(),
i, coefficient.header.frame_id.c_str());
return;
}
listener_->waitForTransform(coefficient.header.frame_id,
frame_id_,
coefficient.header.stamp,
ros::Duration(1.0));
if (listener_->canTransform(coefficient.header.frame_id,
frame_id_,
coefficient.header.stamp)) {
tf::StampedTransform transform; // header -> frame_id_
listener_->lookupTransform(coefficient.header.frame_id, frame_id_,
//ros::Time(0.0), transform);
coefficient.header.stamp, transform);
Eigen::Affine3d eigen_transform;
tf::transformTFToEigen(transform, eigen_transform);
PCLModelCoefficientMsg transformed_coefficient;
//transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient);
geometry_msgs::PolygonStamped transformed_polygon;
if (polygon.polygon.points.size() == 0) {
transformed_polygon.header = polygon.header;
transformed_polygon.header.frame_id = frame_id_;
transformed_polygon_array.polygons.push_back(transformed_polygon);
transformed_coefficient.values = coefficient.values;
transformed_coefficient.header = polygon.header;
transformed_coefficient.header.frame_id = frame_id_;
transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
}
else {
transformPolygon(eigen_transform, polygon, transformed_polygon);
//computeCoefficients(transformed_polygon, transformed_coefficient);
transformModelCoefficient(eigen_transform, coefficient,
transformed_coefficient);
if (isnan(transformed_coefficient.values[0]) ||
isnan(transformed_coefficient.values[1]) ||
isnan(transformed_coefficient.values[2]) ||
isnan(transformed_coefficient.values[3])) {
continue;
}
transformed_polygon_array.polygons.push_back(transformed_polygon);
transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
}
}
else {
JSK_NODELET_ERROR("cannot lookup transform from %s to %s at %f",
frame_id_.c_str(), coefficient.header.frame_id.c_str(),
coefficient.header.stamp.toSec());
return;
}
}
polygons_pub_.publish(transformed_polygon_array);
coefficients_pub_.publish(transformed_model_coefficients_array);
}
