void HeightmapToPointCloud::onInit()
{
DiagnosticNodelet::onInit();
pub_config_ = pnh_->advertise<jsk_recognition_msgs::HeightmapConfig>(
"output/config", 1);
sub_config_ = pnh_->subscribe(
getHeightmapConfigTopic(pnh_->resolveName("input")), 1,
&HeightmapToPointCloud::configCallback, this);
pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
onInitPostProcess();
}
void HeightmapMorphologicalFiltering::onInit()
{
DiagnosticNodelet::onInit();
pub_config_ = pnh_->advertise<jsk_recognition_msgs::HeightmapConfig>("output/config",
1, true);
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&HeightmapMorphologicalFiltering::configCallback, this, _1, _2);
srv_->setCallback (f);
pnh_->param("max_queue_size", max_queue_size_, 10);
pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
sub_config_ = pnh_->subscribe(getHeightmapConfigTopic(pnh_->resolveName("input")), 1,
&HeightmapMorphologicalFiltering::configTopicCallback,
this);
}
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();
}
