Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
jsk_pcl_ros::HintedStickFinder Class Reference

#include <hinted_stick_finder.h>

Inheritance diagram for jsk_pcl_ros::HintedStickFinder:
Inheritance graph
[legend]

Public Types

typedef message_filters::sync_policies::ApproximateTime< geometry_msgs::PolygonStamped, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2 > ASyncPolicy
 
typedef HintedStickFinderConfig Config
 
- Public Types inherited from jsk_topic_tools::DiagnosticNodelet
typedef boost::shared_ptr< DiagnosticNodeletPtr
 

Public Member Functions

 HintedStickFinder ()
 
- Public Member Functions inherited from jsk_topic_tools::DiagnosticNodelet
 DiagnosticNodelet (const std::string &name)
 
- Public Member Functions inherited from jsk_topic_tools::ConnectionBasedNodelet
 ConnectionBasedNodelet ()
 
- Public Member Functions inherited from nodelet::Nodelet
void init (const std::string &name, const M_string &remapping_args, const V_string &my_argv, ros::CallbackQueueInterface *st_queue=NULL, ros::CallbackQueueInterface *mt_queue=NULL)
 
 Nodelet ()
 
virtual ~Nodelet ()
 

Protected Member Functions

virtual void cloudCallback (const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
 Non synchronized message callback for ~input pointcloud. More...
 
virtual void configCallback (Config &config, uint32_t level)
 
virtual void detect (const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const sensor_msgs::CameraInfo::ConstPtr &camera_info_msg, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
 Synchronized message callback. More...
 
virtual void filterPointCloud (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const jsk_recognition_utils::ConvexPolygon::Ptr polygon, pcl::PointIndices &output_indices)
 
virtual void fittingCylinder (const pcl::PointCloud< pcl::PointXYZ >::Ptr &filtered_cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_nromals, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
 
virtual void hintCallback (const geometry_msgs::PolygonStamped::ConstPtr &hint_msg)
 Non synchronized message callback for ~input/hint/line. More...
 
virtual void infoCallback (const sensor_msgs::CameraInfo::ConstPtr &info_msg)
 Non synchronized message callback for ~input/camera_info. More...
 
virtual void normalEstimate (const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::PointIndices::Ptr indices, pcl::PointCloud< pcl::Normal > &normals, pcl::PointCloud< pcl::PointXYZ > &normals_cloud)
 
virtual void onInit ()
 
virtual jsk_recognition_utils::ConvexPolygon::Ptr polygonFromLine (const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const image_geometry::PinholeCameraModel &model, Eigen::Vector3f &a, Eigen::Vector3f &b)
 
virtual bool rejected2DHint (const jsk_recognition_utils::Cylinder::Ptr &cylinder, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
 Check direction of cylinder in 2-D image coordinate system and if it is larger than eps_2d_angle_, return false. More...
 
virtual void subscribe ()
 
virtual void unsubscribe ()
 
- Protected Member Functions inherited from jsk_topic_tools::DiagnosticNodelet
virtual void updateDiagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat)
 
- Protected Member Functions inherited from jsk_topic_tools::ConnectionBasedNodelet
ros::Publisher advertise (ros::NodeHandle &nh, std::string topic, int queue_size)
 
image_transport::CameraPublisher advertiseCamera (ros::NodeHandle &nh, image_transport::ImageTransport &it, const std::string &topic, int queue_size)
 
image_transport::CameraPublisher advertiseCamera (ros::NodeHandle &nh, const std::string &topic, int queue_size)
 
image_transport::Publisher advertiseImage (ros::NodeHandle &nh, image_transport::ImageTransport &it, const std::string &topic, int queue_size)
 
image_transport::Publisher advertiseImage (ros::NodeHandle &nh, const std::string &topic, int queue_size)
 
virtual void cameraConnectionBaseCallback ()
 
virtual void cameraConnectionCallback (const image_transport::SingleSubscriberPublisher &pub)
 
virtual void cameraInfoConnectionCallback (const ros::SingleSubscriberPublisher &pub)
 
virtual void connectionCallback (const ros::SingleSubscriberPublisher &pub)
 
virtual void imageConnectionCallback (const image_transport::SingleSubscriberPublisher &pub)
 
virtual bool isSubscribed ()
 
virtual void onInitPostProcess ()
 
virtual void warnNeverSubscribedCallback (const ros::WallTimerEvent &event)
 
virtual void warnOnInitPostProcessCalledCallback (const ros::WallTimerEvent &event)
 
- Protected Member Functions inherited from nodelet::Nodelet
ros::CallbackQueueInterfacegetMTCallbackQueue () const
 
ros::NodeHandlegetMTNodeHandle () const
 
ros::NodeHandlegetMTPrivateNodeHandle () const
 
const V_stringgetMyArgv () const
 
const std::stringgetName () const
 
ros::NodeHandlegetNodeHandle () const
 
ros::NodeHandlegetPrivateNodeHandle () const
 
const M_stringgetRemappingArgs () const
 
ros::CallbackQueueInterfacegetSTCallbackQueue () const
 
std::string getSuffixedName (const std::string &suffix) const
 

Protected Attributes

int cylinder_fitting_trial_
 
double eps_2d_angle_
 
double eps_angle_
 
double filter_distance_
 
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_
 
geometry_msgs::PolygonStamped::ConstPtr latest_hint_
 
int max_iteration_
 
double max_radius_
 
int min_inliers_
 
double min_probability_
 
double min_radius_
 
boost::mutex mutex_
 
bool not_synchronize_
 Run in continuous mode. continuous mode means this nodelet does not synchronize hint and input messages but keep processing with old hint information. More...
 
double outlier_threshold_
 
ros::Publisher pub_coefficients_
 
ros::Publisher pub_cylinder_marker_
 
ros::Publisher pub_cylinder_pose_
 
ros::Publisher pub_inliers_
 
ros::Publisher pub_line_filtered_indices_
 
ros::Publisher pub_line_filtered_normal_
 
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
 
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
 
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
 
ros::Subscriber sub_no_sync_camera_info_
 
ros::Subscriber sub_no_sync_cloud_
 
ros::Subscriber sub_no_sync_polygon_
 
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_normal_
 
message_filters::Subscriber< geometry_msgs::PolygonStamped > sub_polygon_
 
boost::shared_ptr< message_filters::Synchronizer< ASyncPolicy > > sync_
 
bool use_normal_
 True if use ~input has normal fields. More...
 
- Protected Attributes inherited from jsk_topic_tools::DiagnosticNodelet
TimeredDiagnosticUpdater::Ptr diagnostic_updater_
 
const std::string name_
 
jsk_topic_tools::VitalChecker::Ptr vital_checker_
 
- Protected Attributes inherited from jsk_topic_tools::ConnectionBasedNodelet
bool always_subscribe_
 
std::vector< image_transport::CameraPublishercamera_publishers_
 
boost::mutex connection_mutex_
 
ConnectionStatus connection_status_
 
bool ever_subscribed_
 
std::vector< image_transport::Publisherimage_publishers_
 
boost::shared_ptr< ros::NodeHandlenh_
 
bool on_init_post_process_called_
 
boost::shared_ptr< ros::NodeHandlepnh_
 
std::vector< ros::Publisherpublishers_
 
bool subscribed_
 
ros::WallTimer timer_warn_never_subscribed_
 
ros::WallTimer timer_warn_on_init_post_process_called_
 
bool verbose_connection_
 

Detailed Description

Definition at line 59 of file hinted_stick_finder.h.

Member Typedef Documentation

typedef message_filters::sync_policies::ApproximateTime< geometry_msgs::PolygonStamped, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> jsk_pcl_ros::HintedStickFinder::ASyncPolicy

Definition at line 65 of file hinted_stick_finder.h.

typedef HintedStickFinderConfig jsk_pcl_ros::HintedStickFinder::Config

Definition at line 66 of file hinted_stick_finder.h.

Constructor & Destructor Documentation

jsk_pcl_ros::HintedStickFinder::HintedStickFinder ( )
inline

Definition at line 67 of file hinted_stick_finder.h.

Member Function Documentation

void jsk_pcl_ros::HintedStickFinder::cloudCallback ( const sensor_msgs::PointCloud2::ConstPtr &  cloud_msg)
protectedvirtual

Non synchronized message callback for ~input pointcloud.

Definition at line 96 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::configCallback ( Config config,
uint32_t  level 
)
protectedvirtual

Definition at line 353 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::detect ( const geometry_msgs::PolygonStamped::ConstPtr &  polygon_msg,
const sensor_msgs::CameraInfo::ConstPtr &  camera_info_msg,
const sensor_msgs::PointCloud2::ConstPtr &  cloud_msg 
)
protectedvirtual

Synchronized message callback.

Definition at line 140 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::filterPointCloud ( const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud,
const jsk_recognition_utils::ConvexPolygon::Ptr  polygon,
pcl::PointIndices &  output_indices 
)
protectedvirtual

Definition at line 304 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::fittingCylinder ( const pcl::PointCloud< pcl::PointXYZ >::Ptr filtered_cloud,
const pcl::PointCloud< pcl::Normal >::Ptr cloud_nromals,
const Eigen::Vector3f &  a,
const Eigen::Vector3f &  b 
)
protectedvirtual

Definition at line 223 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::hintCallback ( const geometry_msgs::PolygonStamped::ConstPtr &  hint_msg)
protectedvirtual

Non synchronized message callback for ~input/hint/line.

Definition at line 111 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::infoCallback ( const sensor_msgs::CameraInfo::ConstPtr &  info_msg)
protectedvirtual

Non synchronized message callback for ~input/camera_info.

Definition at line 118 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::normalEstimate ( const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud,
const pcl::PointIndices::Ptr  indices,
pcl::PointCloud< pcl::Normal > &  normals,
pcl::PointCloud< pcl::PointXYZ > &  normals_cloud 
)
protectedvirtual

Definition at line 187 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::onInit ( void  )
protectedvirtual

Reimplemented from jsk_topic_tools::DiagnosticNodelet.

Definition at line 49 of file hinted_stick_finder_nodelet.cpp.

jsk_recognition_utils::ConvexPolygon::Ptr jsk_pcl_ros::HintedStickFinder::polygonFromLine ( const geometry_msgs::PolygonStamped::ConstPtr &  polygon_msg,
const image_geometry::PinholeCameraModel model,
Eigen::Vector3f &  a,
Eigen::Vector3f &  b 
)
protectedvirtual

Definition at line 327 of file hinted_stick_finder_nodelet.cpp.

bool jsk_pcl_ros::HintedStickFinder::rejected2DHint ( const jsk_recognition_utils::Cylinder::Ptr cylinder,
const Eigen::Vector3f &  a,
const Eigen::Vector3f &  b 
)
protectedvirtual

Check direction of cylinder in 2-D image coordinate system and if it is larger than eps_2d_angle_, return false.

Parameters
cylinderCylinder object
a3-D ray to start point of 2-D line
b3-D ray to end point of 2-D line

Definition at line 207 of file hinted_stick_finder_nodelet.cpp.

void jsk_pcl_ros::HintedStickFinder::subscribe ( )
protectedvirtual
void jsk_pcl_ros::HintedStickFinder::unsubscribe ( )
protectedvirtual

Member Data Documentation

int jsk_pcl_ros::HintedStickFinder::cylinder_fitting_trial_
protected

Definition at line 163 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::eps_2d_angle_
protected

Definition at line 165 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::eps_angle_
protected

Definition at line 161 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::filter_distance_
protected

Definition at line 158 of file hinted_stick_finder.h.

sensor_msgs::CameraInfo::ConstPtr jsk_pcl_ros::HintedStickFinder::latest_camera_info_
protected

Definition at line 178 of file hinted_stick_finder.h.

geometry_msgs::PolygonStamped::ConstPtr jsk_pcl_ros::HintedStickFinder::latest_hint_
protected

Definition at line 179 of file hinted_stick_finder.h.

int jsk_pcl_ros::HintedStickFinder::max_iteration_
protected

Definition at line 160 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::max_radius_
protected

Definition at line 156 of file hinted_stick_finder.h.

int jsk_pcl_ros::HintedStickFinder::min_inliers_
protected

Definition at line 164 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::min_probability_
protected

Definition at line 162 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::min_radius_
protected

Definition at line 157 of file hinted_stick_finder.h.

boost::mutex jsk_pcl_ros::HintedStickFinder::mutex_
protected

Definition at line 136 of file hinted_stick_finder.h.

bool jsk_pcl_ros::HintedStickFinder::not_synchronize_
protected

Run in continuous mode. continuous mode means this nodelet does not synchronize hint and input messages but keep processing with old hint information.

Definition at line 176 of file hinted_stick_finder.h.

double jsk_pcl_ros::HintedStickFinder::outlier_threshold_
protected

Definition at line 159 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_coefficients_
protected

Definition at line 149 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_cylinder_marker_
protected

Definition at line 146 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_cylinder_pose_
protected

Definition at line 147 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_inliers_
protected

Definition at line 148 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_line_filtered_indices_
protected

Definition at line 144 of file hinted_stick_finder.h.

ros::Publisher jsk_pcl_ros::HintedStickFinder::pub_line_filtered_normal_
protected

Definition at line 145 of file hinted_stick_finder.h.

boost::shared_ptr<dynamic_reconfigure::Server<Config> > jsk_pcl_ros::HintedStickFinder::srv_
protected

Definition at line 154 of file hinted_stick_finder.h.

message_filters::Subscriber<sensor_msgs::PointCloud2> jsk_pcl_ros::HintedStickFinder::sub_cloud_
protected

Definition at line 140 of file hinted_stick_finder.h.

message_filters::Subscriber<sensor_msgs::CameraInfo> jsk_pcl_ros::HintedStickFinder::sub_info_
protected

Definition at line 139 of file hinted_stick_finder.h.

ros::Subscriber jsk_pcl_ros::HintedStickFinder::sub_no_sync_camera_info_
protected

Definition at line 152 of file hinted_stick_finder.h.

ros::Subscriber jsk_pcl_ros::HintedStickFinder::sub_no_sync_cloud_
protected

Definition at line 151 of file hinted_stick_finder.h.

ros::Subscriber jsk_pcl_ros::HintedStickFinder::sub_no_sync_polygon_
protected

Definition at line 153 of file hinted_stick_finder.h.

message_filters::Subscriber<sensor_msgs::PointCloud2> jsk_pcl_ros::HintedStickFinder::sub_normal_
protected

Definition at line 141 of file hinted_stick_finder.h.

message_filters::Subscriber<geometry_msgs::PolygonStamped> jsk_pcl_ros::HintedStickFinder::sub_polygon_
protected

Definition at line 138 of file hinted_stick_finder.h.

boost::shared_ptr<message_filters::Synchronizer<ASyncPolicy> > jsk_pcl_ros::HintedStickFinder::sync_
protected

Definition at line 142 of file hinted_stick_finder.h.

bool jsk_pcl_ros::HintedStickFinder::use_normal_
protected

True if use ~input has normal fields.

Definition at line 170 of file hinted_stick_finder.h.


The documentation for this class was generated from the following files:


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47