#include <CPCtoOCRegistration.h>
Public Types | |
typedef tPointCloud | tCloud |
Used cloud type. | |
Public Member Functions | |
COcPatchMaker () | |
Constructor. | |
bool | computeCloud (const SMapWithParameters &par, const ros::Time &time) |
Get output pointcloud. | |
tCloud & | getCloud () |
Get cloud. | |
virtual void | init (ros::NodeHandle &node_handle) |
Initialize plugin - called in server constructor. | |
void | setCloudFrameId (const std::string &fid) |
Set output cloud frame id. | |
Protected Member Functions | |
virtual void | handleOccupiedNode (tButServerOcTree::iterator &it, const SMapWithParameters &mp) |
hook that is called when traversing occupied nodes of the updated Octree (does nothing here) | |
bool | inSensorCone (const cv::Point2d &uv) const |
Test if point is in camera cone. | |
void | onCameraChangedCB (const sensor_msgs::CameraInfo::ConstPtr &cam_info) |
On camera position changed callback. | |
void | publishInternal (const ros::Time ×tamp) |
Called when new scan was inserted and now all can be published. | |
void | spinThread () |
Protected Attributes | |
ros::CallbackQueue | callback_queue_ |
bool | m_bCamModelInitialized |
Is camera model initialized? | |
bool | m_bPublishCloud |
Should i publish pointcloud. | |
bool | m_bSpinThread |
Spin out own input callback thread. | |
bool | m_bTransformCamera |
Should camera position and orientation be transformed? | |
sensor_msgs::CameraInfo | m_camera_info_buffer |
Camera info buffer. | |
image_geometry::PinholeCameraModel | m_camera_model |
Camera model. | |
image_geometry::PinholeCameraModel | m_camera_model_buffer |
cv::Size | m_camera_size |
Camera size. | |
cv::Size | m_camera_size_buffer |
int | m_camera_stereo_offset_left |
Camera offsets. | |
int | m_camera_stereo_offset_right |
std::string | m_cameraFrameId |
Camera frame id. | |
std::string | m_cameraInfoTopic |
boost::recursive_mutex | m_camPosMutex |
ros::Subscriber | m_camPosSubscriber |
Subscriber - camera position. | |
tCloud | m_cloud |
Output point cloud data. | |
ros::Time | m_DataTimeStamp |
Time stamp. | |
btMatrix3x3 | m_fracMatrix |
View fraction computation matrix. | |
double | m_fracX |
Fraction of the field of view taken from the octomap (x-direction) | |
double | m_fracY |
Fraction of the field of view taken from the octomap (y-direction) | |
std::string | m_ocFrameId |
Crawled octomap frame id. | |
std::string | m_pcFrameId |
Output frame id. | |
ros::Publisher | m_pubConstrainedPC |
Cloud publishers. | |
tf::TransformListener | m_tfListener |
Transform listener. | |
ros::Time | m_time_stamp |
tf::Transform | m_to_sensorTf |
PC to sensor transformation. | |
volatile bool | need_to_terminate_ |
ros::NodeHandle | node_handle_ |
boost::scoped_ptr< boost::thread > | spin_thread_ |
Get visible pointcloud from octomap module
Definition at line 54 of file CPCtoOCRegistration.h.
typedef tPointCloud srs_env_model::COcPatchMaker::tCloud |
Used cloud type.
Definition at line 59 of file CPCtoOCRegistration.h.
bool srs_env_model::COcPatchMaker::computeCloud | ( | const SMapWithParameters & | par, |
const ros::Time & | time | ||
) |
Get output pointcloud.
Get output pointcloud
Definition at line 106 of file CPCtoOCRegistration.cpp.
tCloud& srs_env_model::COcPatchMaker::getCloud | ( | ) | [inline] |
Get cloud.
Definition at line 72 of file CPCtoOCRegistration.h.
void srs_env_model::COcPatchMaker::handleOccupiedNode | ( | tButServerOcTree::iterator & | it, |
const SMapWithParameters & | mp | ||
) | [protected, virtual] |
hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
Definition at line 242 of file CPCtoOCRegistration.cpp.
void srs_env_model::COcPatchMaker::init | ( | ros::NodeHandle & | node_handle | ) | [virtual] |
Initialize plugin - called in server constructor.
Initialize module - called in server constructor
Definition at line 57 of file CPCtoOCRegistration.cpp.
bool srs_env_model::COcPatchMaker::inSensorCone | ( | const cv::Point2d & | uv | ) | const [protected] |
Test if point is in camera cone.
Test if point is in camera cone
Definition at line 313 of file CPCtoOCRegistration.cpp.
void srs_env_model::COcPatchMaker::onCameraChangedCB | ( | const sensor_msgs::CameraInfo::ConstPtr & | cam_info | ) | [protected] |
On camera position changed callback.
On camera position changed callback
Definition at line 287 of file CPCtoOCRegistration.cpp.
void srs_env_model::COcPatchMaker::publishInternal | ( | const ros::Time & | timestamp | ) | [protected] |
Called when new scan was inserted and now all can be published.
Called when new scan was inserted and now all can be published
Definition at line 353 of file CPCtoOCRegistration.cpp.
void srs_env_model::COcPatchMaker::setCloudFrameId | ( | const std::string & | fid | ) | [inline] |
Set output cloud frame id.
Definition at line 75 of file CPCtoOCRegistration.h.
void srs_env_model::COcPatchMaker::spinThread | ( | ) | [protected] |
Main loop when spinning our own thread - process callbacks in our callback queue - process pending goals
Definition at line 338 of file CPCtoOCRegistration.cpp.
Definition at line 120 of file CPCtoOCRegistration.h.
Is camera model initialized?
Definition at line 124 of file CPCtoOCRegistration.h.
bool srs_env_model::COcPatchMaker::m_bPublishCloud [protected] |
Should i publish pointcloud.
Definition at line 133 of file CPCtoOCRegistration.h.
bool srs_env_model::COcPatchMaker::m_bSpinThread [protected] |
Spin out own input callback thread.
Definition at line 115 of file CPCtoOCRegistration.h.
bool srs_env_model::COcPatchMaker::m_bTransformCamera [protected] |
Should camera position and orientation be transformed?
Definition at line 97 of file CPCtoOCRegistration.h.
sensor_msgs::CameraInfo srs_env_model::COcPatchMaker::m_camera_info_buffer [protected] |
Camera info buffer.
Definition at line 139 of file CPCtoOCRegistration.h.
Camera model.
Definition at line 136 of file CPCtoOCRegistration.h.
Definition at line 136 of file CPCtoOCRegistration.h.
cv::Size srs_env_model::COcPatchMaker::m_camera_size [protected] |
Camera size.
Definition at line 142 of file CPCtoOCRegistration.h.
cv::Size srs_env_model::COcPatchMaker::m_camera_size_buffer [protected] |
Definition at line 142 of file CPCtoOCRegistration.h.
int srs_env_model::COcPatchMaker::m_camera_stereo_offset_left [protected] |
Camera offsets.
Definition at line 127 of file CPCtoOCRegistration.h.
int srs_env_model::COcPatchMaker::m_camera_stereo_offset_right [protected] |
Definition at line 127 of file CPCtoOCRegistration.h.
std::string srs_env_model::COcPatchMaker::m_cameraFrameId [protected] |
Camera frame id.
Definition at line 100 of file CPCtoOCRegistration.h.
std::string srs_env_model::COcPatchMaker::m_cameraInfoTopic [protected] |
Definition at line 106 of file CPCtoOCRegistration.h.
boost::recursive_mutex srs_env_model::COcPatchMaker::m_camPosMutex [protected] |
Definition at line 112 of file CPCtoOCRegistration.h.
Subscriber - camera position.
Definition at line 109 of file CPCtoOCRegistration.h.
tCloud srs_env_model::COcPatchMaker::m_cloud [protected] |
Output point cloud data.
Definition at line 130 of file CPCtoOCRegistration.h.
Time stamp.
Definition at line 154 of file CPCtoOCRegistration.h.
View fraction computation matrix.
Definition at line 166 of file CPCtoOCRegistration.h.
double srs_env_model::COcPatchMaker::m_fracX [protected] |
Fraction of the field of view taken from the octomap (x-direction)
Definition at line 160 of file CPCtoOCRegistration.h.
double srs_env_model::COcPatchMaker::m_fracY [protected] |
Fraction of the field of view taken from the octomap (y-direction)
Definition at line 163 of file CPCtoOCRegistration.h.
std::string srs_env_model::COcPatchMaker::m_ocFrameId [protected] |
Crawled octomap frame id.
Definition at line 151 of file CPCtoOCRegistration.h.
std::string srs_env_model::COcPatchMaker::m_pcFrameId [protected] |
Output frame id.
Definition at line 103 of file CPCtoOCRegistration.h.
Cloud publishers.
Definition at line 148 of file CPCtoOCRegistration.h.
Transform listener.
Definition at line 145 of file CPCtoOCRegistration.h.
ros::Time srs_env_model::COcPatchMaker::m_time_stamp [protected] |
Definition at line 154 of file CPCtoOCRegistration.h.
PC to sensor transformation.
Definition at line 157 of file CPCtoOCRegistration.h.
volatile bool srs_env_model::COcPatchMaker::need_to_terminate_ [protected] |
Definition at line 121 of file CPCtoOCRegistration.h.
Definition at line 119 of file CPCtoOCRegistration.h.
boost::scoped_ptr<boost::thread> srs_env_model::COcPatchMaker::spin_thread_ [protected] |
Definition at line 118 of file CPCtoOCRegistration.h.