35 #define BOOST_PARAMETER_MAX_ARITY 7 52 const sensor_msgs::CameraInfo& info,
double distance,
53 pcl::PointCloud<pcl::PointXYZ>&
cloud)
57 cloud.points.resize(info.width * info.height);
58 cloud.is_dense =
true;
59 for (
size_t j = 0; j < info.height; j++) {
60 for (
size_t i = 0; i < info.width; i++) {
63 p.x = ray.x * distance;
64 p.y = ray.y * distance;
65 p.z = ray.z * distance;
66 cloud.points[j * info.width + i] = p;
69 cloud.width = info.width;
70 cloud.height = info.height;
75 DiagnosticNodelet::onInit();
76 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
77 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
79 srv_->setCallback (f);
81 pub_ = advertise<sensor_msgs::PointCloud2>(
89 sub_ = pnh_->subscribe(
99 Config &config, uint32_t level)
102 distance_ = config.distance;
106 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
109 vital_checker_->poke();
110 pcl::PointCloud<pcl::PointXYZ>::Ptr
111 cloud(
new pcl::PointCloud<pcl::PointXYZ>);
112 impl_.generate(*info_msg, distance_, *cloud);
113 sensor_msgs::PointCloud2 ros_cloud;
115 ros_cloud.header = info_msg->header;
116 pub_.publish(ros_cloud);
Nodelet implementation of jsk_pcl/PlanarPointCloudSimulator.
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
virtual void generate(const sensor_msgs::CameraInfo &info, double distance, pcl::PointCloud< pcl::PointXYZ > &cloud)
Generate a pointcloud according to width, height and distance.
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PlanarPointCloudSimulatorNodelet, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
virtual ~PlanarPointCloudSimulator()
virtual void generate(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
Callback function for ~input.
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
PlanarPointCloudSimulator()
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
PlanarPointCloudSimulatorConfig Config