35 #define BOOST_PARAMETER_MAX_ARITY 7 42 DiagnosticNodelet::onInit();
45 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
46 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
48 srv_->setCallback (f);
51 if (pnh_->getParam(
"rate", rate)) {
52 timer_ = pnh_->createTimer(
58 pub_ = advertise<sensor_msgs::PointCloud2>(
75 Config &config, uint32_t level)
90 sensor_msgs::PointCloud2 dummy_cloud;
92 dummy_cloud.header.frame_id =
"map";
93 generate(boost::make_shared<sensor_msgs::PointCloud2>(dummy_cloud));
97 const sensor_msgs::PointCloud2::ConstPtr& msg)
101 pcl::PointCloud<pcl::PointXYZ>::Ptr
102 cloud (
new pcl::PointCloud<pcl::PointXYZ>);
106 cloud->points.resize(point_num);
108 for (
int phi_i = 0; phi_i < phi_num; phi_i++) {
110 Eigen::Affine3f trans =
getPlane(phi);
111 for (
int theta_i = 0; theta_i <
scan_num_; theta_i++) {
114 cloud->points[i] = p;
118 sensor_msgs::PointCloud2 ros_cloud;
120 ros_cloud.header.stamp = msg->header.stamp;
125 ros_cloud.header.frame_id = cloud->header.frame_id;
132 Eigen::Vector3f norm(0,
sin(phi),
cos(phi));
133 Eigen::Quaternionf
rot;
134 rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), norm);
135 Eigen::Affine3f trans = Eigen::Affine3f::Identity();
141 double r,
double theta,
const Eigen::Affine3f& trans)
143 Eigen::Vector3f local_p(r *
cos(theta), r *
sin(theta), 0);
144 Eigen::Vector3f world_p = trans * local_p;
146 p.getVector3fMap() = world_p;
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void unsubscribe()
Shutdown subscribers.
void publish(const boost::shared_ptr< M > &message) const
virtual void configCallback(Config &config, uint32_t level)
dynamic_reconfigure callback
virtual Eigen::Affine3f getPlane(double phi)
Transformation for scan plane specified by phi angle.
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::SphericalPointCloudSimulator, nodelet::Nodelet)
virtual void generate(const sensor_msgs::PointCloud2::ConstPtr &msg)
Callback function for ~input topic.
This is a class for jsk_pcl/SphericalPointCloudSimulator nodelet.
SphericalPointCloudSimulatorConfig Config
virtual pcl::PointXYZ getPoint(double r, double theta, const Eigen::Affine3f &trans)
compute a point according to a parametric model of spherical tilting laser.
virtual void timerCallback(const ros::TimerEvent &event)
Timer callback for fixed rate publishing.
virtual void onInit()
Initialization function.
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void subscribe()
Subscribe another pointcloud to synchronize timestamp.
std::string frame_id_
frame_id of output pointcloud. If it is empty string, frame_id of ~input is copied.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)