36 #define BOOST_PARAMETER_MAX_ARITY 7 42 #include <pcl/common/transforms.h> 48 DiagnosticNodelet::onInit();
49 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
59 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
63 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
76 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
80 pose_msg->header.frame_id)) {
82 cloud_msg->header.frame_id.c_str(),
83 pose_msg->header.frame_id.c_str());
88 pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (
new pcl::PointCloud<pcl::PointXYZ>);
89 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (
new pcl::PointCloud<pcl::PointXYZ>);
91 Eigen::Affine3f inverse_transform = transform.inverse();
92 pcl::transformPointCloud(*cloud, *transformed_cloud, inverse_transform);
93 sensor_msgs::PointCloud2 ros_cloud;
95 ros_cloud.header = cloud_msg->header;
#define NODELET_ERROR(...)
virtual void transform(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
void publish(const boost::shared_ptr< M > &message) const
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_pose_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
virtual void unsubscribe()
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool isSameFrameId(const std::string &a, const std::string &b)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudRelativeFromPoseStamped, nodelet::Nodelet)