void JointStateStaticFilter::jointStateCallback(
const sensor_msgs::JointState::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("jointCallback");
// filter out joints based on joint names
std::vector<double> joints = filterJointState(msg);
if (joints.size() == 0) {
JSK_NODELET_DEBUG("cannot find the joints from the input message");
return;
}
joint_vital_->poke();
// check the previous state...
if (previous_joints_.size() > 0) {
// compute velocity
for (size_t i = 0; i < previous_joints_.size(); i++) {
// JSK_NODELET_INFO("[%s] diff: %f", joint_names_[i].c_str(),
// fabs(previous_joints_[i] - joints[i]));
if (fabs(previous_joints_[i] - joints[i]) > eps_) {
buf_.push_front(boost::make_tuple<ros::Time, bool>(
msg->header.stamp, false));
previous_joints_ = joints;
return;
}
}
buf_.push_front(boost::make_tuple<ros::Time, bool>(
msg->header.stamp, true));
}
previous_joints_ = joints;
}
void CollisionDetector::pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("update pointcloud.");
pcl::fromROSMsg(*msg, cloud_);
cloud_frame_id_ = msg->header.frame_id;
cloud_stamp_ = msg->header.stamp;
}
void FeatureRegistration::referenceCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("update reference");
reference_cloud_.reset(new pcl::PointCloud<pcl::PointNormal>);
reference_feature_.reset(new pcl::PointCloud<pcl::FPFHSignature33>);
pcl::fromROSMsg(*cloud_msg, *reference_cloud_);
pcl::fromROSMsg(*feature_msg, *reference_feature_);
}
void PolygonArrayTransformer::transformModelCoefficient(const Eigen::Affine3d& transform,
const PCLModelCoefficientMsg& coefficient,
PCLModelCoefficientMsg& result)
{
jsk_recognition_utils::Plane plane(coefficient.values);
jsk_recognition_utils::Plane transformed_plane = plane.transform(transform);
result.header.stamp = coefficient.header.stamp;
result.header.frame_id = frame_id_;
transformed_plane.toCoefficients(result.values);
JSK_NODELET_DEBUG("[%f, %f, %f, %f] => [%f, %f, %f, %f]",
coefficient.values[0], coefficient.values[1], coefficient.values[2], coefficient.values[3],
result.values[0], result.values[1], result.values[2], result.values[3]);
}
void JointStateStaticFilter::filter(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("Pointcloud Callback");
vital_checker_->poke();
if (isStatic(msg->header.stamp)) {
JSK_ROS_DEBUG("static");
pub_.publish(msg);
}
else {
JSK_ROS_DEBUG("not static");
}
diagnostic_updater_->update();
}
bool JointStateStaticFilter::isStatic(
const ros::Time& stamp)
{
double min_diff = DBL_MAX;
bool min_value = false;
for (boost::circular_buffer<StampedBool>::iterator it = buf_.begin();
it != buf_.end();
++it) {
double diff = fabs((it->get<0>() - stamp).toSec());
if (diff < min_diff) {
min_value = it->get<1>();
min_diff = diff;
}
}
JSK_NODELET_DEBUG("min_diff: %f", min_diff);
return min_value;
}
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;
}
void ColorHistogramLabelMatch::match(
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::Image::ConstPtr& label_msg,
const sensor_msgs::Image::ConstPtr& mask_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (histogram_.empty()) {
JSK_NODELET_DEBUG("no reference histogram is available");
return;
}
cv::Mat image
= cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
cv::Mat label
= cv_bridge::toCvShare(label_msg, label_msg->encoding)->image;
cv::Mat whole_mask
= cv_bridge::toCvShare(mask_msg, mask_msg->encoding)->image;
cv::Mat coefficient_image = cv::Mat::zeros(
image_msg->height, image_msg->width, CV_32FC1);
std::vector<int> labels;
getLabels(label, labels);
cv::Mat coefficients_heat_image = cv::Mat::zeros(
image_msg->height, image_msg->width, CV_8UC3); // BGR8
int hist_size = histogram_.cols;
float range[] = { min_value_, max_value_ };
const float* hist_range = { range };
double min_coef = DBL_MAX;
double max_coef = - DBL_MAX;
for (size_t i = 0; i < labels.size(); i++) {
int label_index = labels[i];
cv::Mat label_mask = cv::Mat::zeros(label.rows, label.cols, CV_8UC1);
getMaskImage(label, label_index, label_mask);
double coef = 0.0;
// get label_mask & whole_mask and check is it all black or not
cv::Mat masked_label;
label_mask.copyTo(masked_label, whole_mask);
if (isMasked(label_mask, masked_label)) {
coef = masked_coefficient_;
}
else {
cv::MatND hist;
bool uniform = true; bool accumulate = false;
cv::calcHist(&image, 1, 0, label_mask, hist, 1,
&hist_size, &hist_range, uniform, accumulate);
cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2, -1,
cv::Mat());
cv::Mat hist_mat = cv::Mat::zeros(1, hist_size, CV_32FC1);
for (size_t j = 0; j < hist_size; j++) {
hist_mat.at<float>(0, j) = hist.at<float>(0, j);
}
//cv::Mat hist_mat = hist;
coef = coefficients(hist_mat, histogram_);
if (min_coef > coef) {
min_coef = coef;
}
if (max_coef < coef) {
max_coef = coef;
}
}
std_msgs::ColorRGBA coef_color = jsk_topic_tools::heatColor(coef);
for (size_t j = 0; j < coefficients_heat_image.rows; j++) {
for (size_t i = 0; i < coefficients_heat_image.cols; i++) {
if (label_mask.at<uchar>(j, i) == 255) {
coefficients_heat_image.at<cv::Vec3b>(j, i)
= cv::Vec3b(int(coef_color.b * 255),
int(coef_color.g * 255),
int(coef_color.r * 255));
coefficient_image.at<float>(j, i) = coef;
}
}
}
}
JSK_NODELET_INFO("coef: %f - %f", min_coef, max_coef);
pub_debug_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::BGR8,
coefficients_heat_image).toImageMsg());
pub_coefficient_image_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::TYPE_32FC1,
coefficient_image).toImageMsg());
// apply threshold operation
cv::Mat threshold_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_32FC1);
if (threshold_method_ == 0) { // smaller than
cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
cv::THRESH_BINARY_INV);
}
else if (threshold_method_ == 1) { // greater than
cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
cv::THRESH_BINARY);
}
else if (threshold_method_ == 2 || threshold_method_ == 3) {
// convert image into 8UC to apply otsu' method
cv::Mat otsu_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_8UC1);
cv::Mat otsu_result_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_8UC1);
coefficient_image.convertTo(otsu_image, 8, 255.0);
cv::threshold(otsu_image, otsu_result_image, coef_threshold_, 255,
cv::THRESH_OTSU);
// convert it into float image again
if (threshold_method_ == 2) {
otsu_result_image.convertTo(threshold_image, 32, 1 / 255.0);
}
else if (threshold_method_ == 3) {
otsu_result_image.convertTo(threshold_image, 32, - 1 / 255.0, 1.0);
}
}
cv::Mat threshold_uchar_image = cv::Mat(threshold_image.rows,
threshold_image.cols,
CV_8UC1);
threshold_image.convertTo(threshold_uchar_image, 8, 255.0);
// convert image from float to uchar
pub_result_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::MONO8,
threshold_uchar_image).toImageMsg());
}
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));
}
