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); }