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) {
NODELET_FATAL("failed to read frame_ids from ~frame_ids");
return;
}
bool polygon_read_p = readPolygonArray("polygon_array");
if (!polygon_read_p) {
NODELET_FATAL("failed to read polygons from ~polygon_array");
return;
}
if (frame_ids_.size() != polygons_.polygons.size()) {
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_) {
NODELET_FATAL("~use_periodic, ~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 {
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 {
NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
return false;
}
}
return true;
}
else {
NODELET_FATAL("%s is not array", param_name.c_str());
return false;
}
}
else {
NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
return false;
}
return true;
}
void PointCloudToMaskImage::convert(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *pc);
if (!pc->isOrganized())
{
NODELET_FATAL("Input point cloud is not organized.");
return;
}
cv::Mat mask_image = cv::Mat::zeros(cloud_msg->height, cloud_msg->width, CV_8UC1);
for (size_t index = 0; index < pc->points.size(); index++)
{
if (isnan(pc->points[index].x) || isnan(pc->points[index].y) || isnan(pc->points[index].z))
{
continue;
}
int width_index = index % cloud_msg->width;
int height_index = index / cloud_msg->width;
mask_image.at<uchar>(height_index, width_index) = 255;
}
cv_bridge::CvImage mask_bridge(cloud_msg->header,
sensor_msgs::image_encodings::MONO8,
mask_image);
pub_.publish(mask_bridge.toImageMsg());
}
void NormalFlipToFrame::onInit()
{
DiagnosticNodelet::onInit();
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
if (!pnh_->getParam("frame_id", frame_id_)) {
NODELET_FATAL("[%s] no ~frame_id is specified", __PRETTY_FUNCTION__);
}
pnh_->param("strict_tf", strict_tf_, false);
pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
}
void VitalCheckerNodelet::onInit()
{
DiagnosticNodelet::onInit();
if (pnh_->hasParam("title")) {
pnh_->getParam("title", title_);
}
else {
NODELET_FATAL("no ~title is specified");
return;
}
sub_ = pnh_->subscribe<topic_tools::ShapeShifter>(
"input", 1,
&VitalCheckerNodelet::inputCallback, this);
}
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) {
NODELET_FATAL("NO ~joint_names is specified");
return;
}
pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
}
void MUX::onInit()
{
advertised_ = false;
pnh_ = getPrivateNodeHandle();
topics_ = readStringArray("topics", pnh_);
if (topics_.size() < 1) {
NODELET_FATAL("need to specify at least one topic in ~topics");
return;
}
pub_selected_ = pnh_.advertise<std_msgs::String>("selected", 1, true);
selected_topic_ = topics_[0];
subscribeSelectedTopic();
// in original mux node, it subscribes all the topics first, however
// in our version, we never subscribe topic which are not selected.
// service advertise: _select, select, add, list, delete
ss_select_ = pnh_.advertiseService("select", &MUX::selectTopicCallback, this);
ss_add_ = pnh_.advertiseService("add", &MUX::addTopicCallback, this);
ss_list_ = pnh_.advertiseService("list", &MUX::listTopicCallback, this);
ss_del_ = pnh_.advertiseService("delete", &MUX::deleteTopicCallback, this);
}
void PointcloudDatabaseServer::onInit()
{
PCLNodelet::onInit();
pub_points_ = pnh_->advertise<jsk_recognition_msgs::PointsArray>("output", 1);
if (!jsk_topic_tools::readVectorParameter(*pnh_, "models", files_)
|| files_.size() == 0) {
NODELET_FATAL("no models is specified");
return;
}
for (size_t i = 0; i< files_.size(); i++) {
PointCloudData::Ptr data(new PointCloudData(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 NormalEstimationIntegralImage::compute(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock(mutex_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(*msg, *input);
pcl::PointCloud<pcl::Normal> output;
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
if (estimation_method_ == 0) {
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
}
else if (estimation_method_ == 1) {
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
}
else if (estimation_method_ == 2) {
ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
}
else {
NODELET_FATAL("unknown estimation method: %d", estimation_method_);
return;
}
if (border_policy_ignore_) {
ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal>::BORDER_POLICY_IGNORE);
}
else {
ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal>::BORDER_POLICY_MIRROR);
}
ne.setMaxDepthChangeFactor(max_depth_change_factor_);
ne.setNormalSmoothingSize(normal_smoothing_size_);
ne.setDepthDependentSmoothing(depth_dependent_smoothing_);
ne.setInputCloud(input);
ne.compute(output);
sensor_msgs::PointCloud2 ros_output;
pcl::toROSMsg(output, ros_output);
pub_.publish(ros_output);
}
void DriverNodelet::setupDevice ()
{
// Initialize the openni device
FreenectDriver& driver = FreenectDriver::getInstance();
// Enable debugging in libfreenect if requested
if (libfreenect_debug_)
driver.enableDebug();
do {
driver.updateDeviceList ();
if (driver.getNumberDevices () == 0)
{
NODELET_INFO ("No devices connected.... waiting for devices to be connected");
boost::this_thread::sleep(boost::posix_time::seconds(3));
continue;
}
NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
{
NODELET_INFO("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'",
deviceIdx + 1, driver.getBus(deviceIdx), driver.getAddress(deviceIdx),
driver.getProductName(deviceIdx), driver.getProductID(deviceIdx),
driver.getVendorName(deviceIdx), driver.getVendorID(deviceIdx),
driver.getSerialNumber(deviceIdx));
}
try {
string device_id;
if (!getPrivateNodeHandle().getParam("device_id", device_id))
{
NODELET_WARN ("~device_id is not set! Using first device.");
device_ = driver.getDeviceByIndex(0);
}
else if (device_id.find ('@') != string::npos)
{
size_t pos = device_id.find ('@');
unsigned bus = atoi (device_id.substr (0, pos).c_str ());
unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
device_ = driver.getDeviceByAddress (bus, address);
}
else if (device_id[0] == '#')
{
unsigned index = atoi (device_id.c_str() + 1);
NODELET_INFO ("Searching for device with index = %d", index);
device_ = driver.getDeviceByIndex (index - 1);
}
else
{
NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
device_ = driver.getDeviceBySerialNumber (device_id);
}
}
catch (exception& e)
{
if (!device_)
{
NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", e.what ());
boost::this_thread::sleep(boost::posix_time::seconds(3));
continue;
}
else
{
NODELET_FATAL ("Could not retrieve device. Reason: %s", e.what ());
exit (-1);
}
}
} while (!device_);
NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
device_->registerImageCallback(&DriverNodelet::rgbCb, *this);
device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
device_->registerIRCallback (&DriverNodelet::irCb, *this);
}
