void StaticPolygonArrayPublisher::onInit()
{
ConnectionBasedNodelet::onInit();
pnh_->param("use_periodic", use_periodic_, false);
pnh_->param("use_message", use_message_, false);
pnh_->param("use_trigger", use_trigger_, false);
pnh_->param("periodic_rate", periodic_rate_, 10.0);
bool frame_id_read_p
= jsk_topic_tools::readVectorParameter(*pnh_, "frame_ids",
frame_ids_);
if (!frame_id_read_p) {
JSK_NODELET_FATAL("failed to read frame_ids from ~frame_ids");
return;
}
bool polygon_read_p = readPolygonArray("polygon_array");
if (!polygon_read_p) {
JSK_NODELET_FATAL("failed to read polygons from ~polygon_array");
return;
}
if (frame_ids_.size() != polygons_.polygons.size()) {
JSK_NODELET_FATAL("the size of frame_ids(%lu) does not match the size of polygons(%lu)",
frame_ids_.size(), polygons_.polygons.size());
return;
}
else {
for (size_t i = 0; i < frame_ids_.size(); i++) {
polygons_.polygons[i].header.frame_id = frame_ids_[i];
coefficients_.coefficients[i].header.frame_id = frame_ids_[i];
}
}
if (!use_periodic_ && !use_message_ && !use_trigger_) {
JSK_NODELET_FATAL("~use_preiodic, ~use_trigger nor ~use_message is not true");
return;
}
polygons_.header.frame_id = frame_ids_[0];
coefficients_.header.frame_id = frame_ids_[0];
if (!use_periodic_) {
polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(
*pnh_, "output_polygons", 1);
coefficients_pub_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
*pnh_, "output_coefficients", 1);
}
else {
polygon_pub_ = pnh_->advertise<jsk_recognition_msgs::PolygonArray>(
"output_polygons", 1);
coefficients_pub_ = pnh_->advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
"output_coefficients", 1);
subscribe();
timer_ = pnh_->createTimer(ros::Duration(1.0 / periodic_rate_), &StaticPolygonArrayPublisher::timerCallback, this);
}
}
/*
parameter format is:
polygon_array: [[[0, 0, 0], [0, 0, 1], [1, 0, 0]], ...]
*/
bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name)
{
if (pnh_->hasParam(param_name)) {
XmlRpc::XmlRpcValue v;
pnh_->param(param_name, v, v);
if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) { // polygons
XmlRpc::XmlRpcValue polygon_v = v[toplevel_i];
geometry_msgs::PolygonStamped polygon;
if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
polygon_v.size() >= 3) {
for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) { // each polygon, vertices
XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i];
if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
vertex_v.size() == 3 ) { // vertex_v := [x, y, z]
double x = getXMLDoubleValue(vertex_v[0]);
double y = getXMLDoubleValue(vertex_v[1]);
double z = getXMLDoubleValue(vertex_v[2]);
geometry_msgs::Point32 point;
point.x = x;
point.y = y;
point.z = z;
polygon.polygon.points.push_back(point);
}
else {
JSK_NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3",
param_name.c_str(), toplevel_i, secondlevel_i);
return false;
}
}
polygons_.polygons.push_back(polygon);
// estimate model coefficients
coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon));
}
else {
JSK_NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
return false;
}
}
return true;
}
else {
JSK_NODELET_FATAL("%s is not array", param_name.c_str());
return false;
}
}
else {
JSK_NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
return false;
}
return true;
}
void AddColorFromImage::addColor(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::CameraInfo::ConstPtr& info_msg)
{
if ((cloud_msg->header.frame_id != image_msg->header.frame_id) ||
(cloud_msg->header.frame_id != info_msg->header.frame_id)) {
JSK_NODELET_FATAL("frame_id is not collect: [%s, %s, %s",
cloud_msg->header.frame_id.c_str(),
image_msg->header.frame_id.c_str(),
info_msg->header.frame_id.c_str());
return;
}
vital_checker_->poke();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud
(new pcl::PointCloud<pcl::PointXYZRGB>);
rgb_cloud->points.resize(cloud->points.size());
rgb_cloud->is_dense = cloud->is_dense;
rgb_cloud->width = cloud->width;
rgb_cloud->height = cloud->height;
cv::Mat image = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8)->image;
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(info_msg);
for (size_t i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZRGB p;
p.x = cloud->points[i].x;
p.y = cloud->points[i].y;
p.z = cloud->points[i].z;
//p.getVector3fMap() = cloud->points[i].getVector3fMap();
if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
// compute RGB
cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
if (uv.x > 0 && uv.x < image_msg->width &&
uv.y > 0 && uv.y < image_msg->height) {
cv::Vec3b rgb = image.at<cv::Vec3b>(uv.y, uv.x);
p.r = rgb[2];
p.g = rgb[1];
p.b = rgb[0];
}
}
rgb_cloud->points[i] = p;
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*rgb_cloud, ros_cloud);
ros_cloud.header = cloud_msg->header;
pub_.publish(ros_cloud);
}
void PolygonArrayTransformer::onInit()
{
ConnectionBasedNodelet::onInit();
if (!pnh_->getParam("frame_id", frame_id_)) {
JSK_NODELET_FATAL("~frame_id is not specified");
return;
}
listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
polygons_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_polygons", 1);
coefficients_pub_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
*pnh_, "output_coefficients", 1);
}
void HeightmapTimeAccumulation::onInit()
{
ConnectionBasedNodelet::onInit();
pub_config_ = pnh_->advertise<jsk_recognition_msgs::HeightmapConfig>(
"output/config", 1, true);
sub_config_ = pnh_->subscribe(
getHeightmapConfigTopic(pnh_->resolveName("input")), 1,
&HeightmapTimeAccumulation::configCallback, this);
if (!pnh_->getParam("center_frame_id", center_frame_id_)) {
JSK_NODELET_FATAL("no ~center_frame_id is specified");
return;
}
if (!pnh_->getParam("fixed_frame_id", fixed_frame_id_)) {
JSK_NODELET_FATAL("no ~fixed_frame_id is specified");
return;
}
int tf_queue_size;
pnh_->param("tf_queue_size", tf_queue_size, 10);
prev_from_center_to_fixed_ = Eigen::Affine3f::Identity();
tf_ = TfListenerSingleton::getInstance();
pub_output_ = pnh_->advertise<sensor_msgs::Image>("output", 1, true);
sub_previous_pointcloud_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
"input/prev_pointcloud", 5,
&HeightmapTimeAccumulation::prevPointCloud, this);
sub_heightmap_.subscribe(*pnh_, "input", 10);
tf_filter_.reset(new tf::MessageFilter<sensor_msgs::Image>(
sub_heightmap_,
*tf_,
fixed_frame_id_,
tf_queue_size));
tf_filter_->registerCallback(
boost::bind(
&HeightmapTimeAccumulation::accumulate, this, _1));
srv_reset_ = pnh_->advertiseService("reset", &HeightmapTimeAccumulation::resetCallback, this);
onInitPostProcess();
}
void JointStateStaticFilter::onInit()
{
DiagnosticNodelet::onInit();
double vital_rate;
pnh_->param("vital_rate", vital_rate, 1.0);
joint_vital_.reset(
new jsk_topic_tools::VitalChecker(1 / vital_rate));
if (!jsk_topic_tools::readVectorParameter(*pnh_,
"joint_names",
joint_names_) ||
joint_names_.size() == 0) {
JSK_NODELET_FATAL("NO ~joint_names is specified");
return;
}
pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
onInitPostProcess();
}
void PointcloudDatabaseServer::onInit()
{
PCLNodelet::onInit();
std::vector<std::string> pcd_files;
pub_points_ = pnh_->advertise<jsk_recognition_msgs::PointsArray>("output", 1);
if (!jsk_topic_tools::readVectorParameter(*pnh_, "models", pcd_files)
|| pcd_files.size() == 0) {
JSK_NODELET_FATAL("no models is specified");
return;
}
for (size_t i = 0; i< pcd_files.size(); i++) {
PointCloudData::Ptr data(new PointCloudData(pcd_files[i]));
point_clouds_.push_back(data);
}
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind(
&PointcloudDatabaseServer::configCallback, this, _1, _2);
srv_->setCallback (f);
pnh_->getParam("duration", duration_);
}
void PointCloudLocalization::tfTimerCallback(
const ros::TimerEvent& event)
{
boost::mutex::scoped_lock lock(tf_mutex_);
try {
ros::Time stamp = event.current_real;
if (initialize_from_tf_ && first_time_) {
// Update localize_transform_ to points initialize_tf
tf::StampedTransform transform = lookupTransformWithDuration(
tf_listener_,
initialize_tf_, odom_frame_, stamp, ros::Duration(1.0));
localize_transform_ = transform;
}
tf_broadcast_.sendTransform(tf::StampedTransform(localize_transform_,
stamp,
global_frame_,
odom_frame_));
}
catch (tf2::TransformException& e) {
JSK_NODELET_FATAL("Failed to lookup transformation: %s", e.what());
}
}
