void ColorizeRandomForest::onInit(void)
{
// not implemented yet
PCLNodelet::onInit();
sub_input_ = pnh_->subscribe("input", 1, &ColorizeRandomForest::extract, this);
pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("cloth_result", 1);
pub2_ = pnh_->advertise<sensor_msgs::PointCloud2>("noncloth_result", 1);
srand(time(NULL));
double tmp_radius = 0.03, tmp_pass = 0.03, tmp_pass2 = 0.06;
sum_num_ = 100;
if (!pnh_->getParam("rs", tmp_radius))
{
JSK_ROS_WARN("~rs is not specified, set 1");
}
if (!pnh_->getParam("po", tmp_pass))
{
JSK_ROS_WARN("~po is not specified, set 1");
}
if (!pnh_->getParam("po2", tmp_pass2))
{
JSK_ROS_WARN("~po is not specified, set 1");
}
if (!pnh_->getParam("sum_num", sum_num_))
{
JSK_ROS_WARN("~sum_num is not specified, set 1");
}
radius_search_ = tmp_radius;
pass_offset_ = tmp_pass;
pass_offset2_ = tmp_pass2;
}
void RGBDecomposer::decompose(
const sensor_msgs::Image::ConstPtr& image_msg)
{
if ((image_msg->width == 0) && (image_msg->height == 0)) {
JSK_ROS_WARN("invalid image input");
return;
}
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
image_msg, image_msg->encoding);
cv::Mat image = cv_ptr->image;
if (image_msg->encoding == sensor_msgs::image_encodings::RGB8) {
cv::cvtColor(image, image, CV_RGB2BGR);
}
std::vector<cv::Mat> bgr_planes;
cv::split(image, bgr_planes);
cv::Mat red = bgr_planes[2];
cv::Mat blue = bgr_planes[0];
cv::Mat green = bgr_planes[1];
pub_r_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
red).toImageMsg());
pub_g_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
green).toImageMsg());
pub_b_.publish(cv_bridge::CvImage(
image_msg->header,
sensor_msgs::image_encodings::MONO8,
blue).toImageMsg());
}
TEST(LogUtils, testJSKROSXXX){
JSK_ROS_DEBUG("Testing JSK_ROS_DEBUG: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO("Testing JSK_ROS_INFO: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN("Testing JSK_ROS_WARN: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR("Testing JSK_ROS_ERROR: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL("Testing JSK_ROS_FATAL: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM("Testing " << "JSK_ROS_DEBUG_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM("Testing " << "JSK_ROS_INFO_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM("Testing " << "JSK_ROS_WARN_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM("Testing " << "JSK_ROS_ERROR_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM("Testing " << "JSK_ROS_FATAL_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_DEBUG_THROTTLE(1, "Testing JSK_ROS_DEBUG_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO_THROTTLE(1, "Testing JSK_ROS_INFO_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN_THROTTLE(1, "Testing JSK_ROS_WARN_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR_THROTTLE(1, "Testing JSK_ROS_ERROR_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL_THROTTLE(1, "Testing JSK_ROS_FATAL_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_DEBUG_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_INFO_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_WARN_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_ERROR_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_FATAL_STREAM_THROTTLE: " << ros::Time::now().toNSec());
}
void TfTransformCloud::onInit(void)
{
DiagnosticNodelet::onInit();
if (!pnh_->getParam("target_frame_id", target_frame_id_))
{
JSK_ROS_WARN("~target_frame_id is not specified, using %s", "/base_footprint");
}
pnh_->param("duration", duration_, 1.0);
pnh_->param("use_latest_tf", use_latest_tf_, false);
pnh_->param("tf_queue_size", tf_queue_size_, 10);
tf_listener_ = TfListenerSingleton::getInstance();
pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
}
void GaussianBlur::apply(
const sensor_msgs::Image::ConstPtr& image_msg)
{
if ((image_msg->width == 0) && (image_msg->height == 0)) {
JSK_ROS_WARN("invalid image input");
return;
}
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
image_msg, image_msg->encoding);
cv::Mat image = cv_ptr->image;
cv::Mat applied_image;
if (kernel_size_ % 2 == 1) {
cv::GaussianBlur(image, applied_image, cv::Size(kernel_size_, kernel_size_), sigma_x_, sigma_y_);
} else {
cv::GaussianBlur(image, applied_image, cv::Size(kernel_size_+1, kernel_size_+1), sigma_x_, sigma_y_);
}
pub_.publish(cv_bridge::CvImage(
image_msg->header,
image_msg->encoding,
applied_image).toImageMsg());
}
void KMeans::apply(
const sensor_msgs::Image::ConstPtr& image_msg)
{
if ((image_msg->width == 0) && (image_msg->height == 0)) {
JSK_ROS_WARN("invalid image input");
return;
}
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
image_msg, image_msg->encoding);
cv::Mat image = cv_ptr->image;
cv::Mat reshaped_img = image.reshape(1, image.cols * image.rows);
cv::Mat reshaped_img32f;
reshaped_img.convertTo(reshaped_img32f, CV_32FC1, 1.0 / 255.0);
cv::Mat labels;
cv::TermCriteria criteria = cv::TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 10, 1.0);
cv::Mat centers;
cv::kmeans(reshaped_img32f, n_clusters_, labels, criteria, /*attempts=*/1, /*flags=*/cv::KMEANS_PP_CENTERS, centers);
cv::Mat rgb_image(image.rows, image.cols, CV_8UC3);
cv::MatIterator_<cv::Vec3b> rgb_first = rgb_image.begin<cv::Vec3b>();
cv::MatIterator_<cv::Vec3b> rgb_last = rgb_image.end<cv::Vec3b>();
cv::MatConstIterator_<int> label_first = labels.begin<int>();
cv::Mat centers_u8;
centers.convertTo(centers_u8, CV_8UC1, 255.0);
cv::Mat centers_u8c3 = centers_u8.reshape(3);
while ( rgb_first != rgb_last ) {
const cv::Vec3b& rgb = centers_u8c3.ptr<cv::Vec3b>(*label_first)[0];
*rgb_first = rgb;
++rgb_first;
++label_first;
}
pub_.publish(cv_bridge::CvImage(
image_msg->header,
image_msg->encoding,
rgb_image).toImageMsg());
}
bool jsk_pcl_ros::PointcloudScreenpoint::extract_point (pcl::PointCloud< pcl::PointXYZ > &in_pts, int reqx, int reqy,
float &resx, float &resy, float &resz) {
int x, y;
x = reqx < 0.0 ? ceil(reqx - 0.5) : floor(reqx + 0.5);
y = reqy < 0.0 ? ceil(reqy - 0.5) : floor(reqy + 0.5);
JSK_ROS_WARN("Request : %d %d", x, y);
if (checkpoint (in_pts, x, y, resx, resy, resz)) {
return true;
} else {
for (int n = 1; n < crop_size_; n++) {
for (int y2 = 0; y2 <= n; y2++) {
int x2 = n - y2;
if (checkpoint (in_pts, x + x2, y + y2, resx, resy, resz)) {
return true;
}
if (x2 != 0 && y2 != 0) {
if (checkpoint (in_pts, x - x2, y - y2, resx, resy, resz)) {
return true;
}
}
if (x2 != 0) {
if (checkpoint (in_pts, x - x2, y + y2, resx, resy, resz)) {
return true;
}
}
if (y2 != 0) {
if (checkpoint (in_pts, x + x2, y - y2, resx, resy, resz)) {
return true;
}
}
}
}
}
return false;
}
