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