FeatureTrackerNode class Connects to the ROS communication system to get messages from the sensors and publish results Templated in the state of to be estimated by this RE level (ROS type) The received messages are passed to the FeatureTracker object to be processed. More...

#include <FeatureTrackerNode.h>

Inheritance diagram for omip::FeatureTrackerNode:

[legend]

List of all members.

Public Member Functions
virtual void	AdvanceBagCallbackFromShapeReconstruction (const boost::shared_ptr< std_msgs::Bool const > &flag)
virtual void	AdvanceBagCallbackFromShapeTracker (const boost::shared_ptr< omip_msgs::ShapeTrackerStates const > &st_states)
virtual void	DynamicReconfigureCallback (feature_tracker::FeatureTrackerDynReconfConfig &config, uint32_t level)
	Callback for the Dynamic Reconfigure parameters.
	FeatureTrackerNode ()
virtual void	measurementCallback (const sensor_msgs::PointCloud2ConstPtr &pc_msg, const sensor_msgs::ImageConstPtr &depth_image_msg, const sensor_msgs::ImageConstPtr &rgb_image_msg)
	Combined callback for the measurements of this RE level: point cloud, depth image and rgb image.
virtual void	measurementCallback (const sensor_msgs::ImageConstPtr &depth_image_msg, const sensor_msgs::ImageConstPtr &rgb_image_msg)
	Lighter version of the combined callback for the measurements of this RE level: depth image and rgb image.
virtual void	measurementCallback (const boost::shared_ptr< ft_measurement_ros_t const > &ft_measurement_ros)
	Empty measurement callback, just to implement correctly the RENode interface.
virtual void	OcclusionMaskImgCallback (const sensor_msgs::ImageConstPtr &occlusion_mask_img)
	Callback for the mask of the self occlusion of the robot. Only for the WAM.
virtual void	PublishDepthEdgesImg ()
	Publish an image with the edges detected on the depth map.
virtual void	PublishDepthImg ()
virtual void	PublishDetectingMaskImg ()
	Publish the mask used to detect new features. Combines edges, max depth and mask of current features (to not detect them)
virtual void	PublishFullRGBDPC ()
	Publish the full RGB-D point cloud used in the last iteration (used to synchronize the PC to the other published messages)
virtual void	PublishPredictedAndLastFeaturesImg ()
	Publish an image with the last feature locations and the predicted feature locations.
virtual void	PublishPredictionMaskImg ()
	Publish the mask of the predictions.
virtual void	PublishRGBImg ()
virtual void	PublishTrackedFeaturesImg ()
	Publish an image with the last tracked features on it.
virtual void	PublishTrackedFeaturesWithPredictionMaskImg ()
	Publish an image with the last tracked features together with the mask of the predictions.
virtual void	PublishTrackingMaskImg ()
	Publish the mask used to track features. Combines edges, predictions mask and max depth.
virtual void	ReadRosBag ()
	Read messages from a rosbag instead of receiving them from a sensor Internally it "publishes" the messages that are then received by the measurement callback.
virtual void	RepublishPredictedFeatLocation ()
virtual void	run ()
virtual void	statePredictionCallback (const boost::shared_ptr< ft_state_ros_t const > &predicted_next_state)
	Callback for the predictions about the state of this RE level coming from the higher level of the hierarchy.
virtual	~FeatureTrackerNode ()
Protected Member Functions
virtual void	_InitializeVariables ()
virtual void	_publishPredictedMeasurement () const
	Publish the prediction about the next measurement by this RE level.
virtual void	_publishState () const
	Publish the current state of this RE level.
virtual void	_ReadParameters ()
virtual void	_SubscribeAndAdvertiseTopics ()
Protected Attributes
double	_advance_frame_max_wait_time
int	_advance_frame_mechanism
double	_advance_frame_min_wait_time
ros::Subscriber	_advance_sub
bool	_advance_sub_returned_true
bool	_attention_to_motion
BagSubscriber< sensor_msgs::Image >	_bag_depth_img_sub
std::string	_bag_file_name
BagSubscriber < sensor_msgs::PointCloud2 >	_bag_full_rgbd_pc_sub
BagSubscriber< sensor_msgs::Image >	_bag_rgb_img_sub
sensor_msgs::CameraInfo	_camera_info_msg
ros::Publisher	_camera_info_pub
ros::Publisher	_camera_info_pub2
bool	_ci_initialized
std::string	_ci_topic
cv_bridge::CvImagePtr	_cv_ptr_depth
cv_bridge::CvImagePtr	_cv_ptr_occlusion_msk
cv_bridge::CvImagePtr	_cv_ptr_rgb
bool	_data_from_bag
image_transport::Publisher	_depth_edges_img_pub
image_transport::Publisher	_depth_img_pub
message_filters::Subscriber < sensor_msgs::Image >	_depth_img_sub
std::string	_depth_img_topic
image_transport::Publisher	_detecting_mask_img_pub
dynamic_reconfigure::Server < feature_tracker::FeatureTrackerDynReconfConfig > ::CallbackType	_dr_callback
dynamic_reconfigure::Server < feature_tracker::FeatureTrackerDynReconfConfig >	_dr_srv
std::string	_ft_ns
PointCloudPCL::ConstPtr	_full_rgbd_pc
ros::Publisher	_full_rgbd_pc_repub
message_filters::Subscriber < sensor_msgs::PointCloud2 >	_full_rgbd_pc_sub
std::string	_full_rgbd_pc_topic
image_transport::ImageTransport	_image_transport
tf::Transform	_initial_ee_tf
message_filters::Synchronizer < FTrackerLightSyncPolicy > *	_light_synchronizer
double	_loop_period_ns
ros::Subscriber	_occlusion_mask_img_sub
std::string	_occlusion_mask_img_topic
bool	_occlusion_mask_positive
ros::Publisher	_pred_feat_locs_repub
image_transport::Publisher	_predicted_and_past_feats_img_pub
FeatureCloudPCLwc::Ptr	_predicted_locations_pc
std::string	_predicted_locations_pc_topic
image_transport::Publisher	_prediction_mask_img_pub
int	_processing_factor
bool	_publishing_depth_edges_img
bool	_publishing_depth_img
bool	_publishing_detecting_msk_img
bool	_publishing_full_pc
bool	_publishing_predicted_and_past_feats_img
bool	_publishing_predicting_msk_img
bool	_publishing_rgb_img
bool	_publishing_tracked_feats_img
bool	_publishing_tracked_feats_with_pred_msk_img
bool	_publishing_tracking_msk_img
bool	_republishing_predicted_feat_locs
image_transport::Publisher	_rgb_img_pub
message_filters::Subscriber < sensor_msgs::Image >	_rgb_img_sub
std::string	_rgb_img_topic
double	_sensor_fps
ros::Publisher	_shutdown_publisher
bool	_subscribe_to_pc
message_filters::Synchronizer < FTrackerSyncPolicy > *	_synchronizer
ros::Publisher	_tf_repub
ros::Publisher	_time_repub
image_transport::Publisher	_tracked_feats_img_pub
image_transport::Publisher	_tracked_feats_with_pm_img_pub
std::string	_tracker_type
image_transport::Publisher	_tracking_mask_img_pub
Private Types
typedef message_filters::sync_policies::ApproximateTime < sensor_msgs::Image, sensor_msgs::Image >	FTrackerLightSyncPolicy
typedef message_filters::sync_policies::ApproximateTime < sensor_msgs::PointCloud2, sensor_msgs::Image, sensor_msgs::Image >	FTrackerSyncPolicy

Detailed Description

FeatureTrackerNode class Connects to the ROS communication system to get messages from the sensors and publish results Templated in the state of to be estimated by this RE level (ROS type) The received messages are passed to the FeatureTracker object to be processed.

Definition at line 84 of file FeatureTrackerNode.h.

Member Typedef Documentation

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> omip::FeatureTrackerNode::FTrackerLightSyncPolicy [private]

Definition at line 89 of file FeatureTrackerNode.h.

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image, sensor_msgs::Image> omip::FeatureTrackerNode::FTrackerSyncPolicy [private]

Definition at line 88 of file FeatureTrackerNode.h.

Constructor & Destructor Documentation

FeatureTrackerNode::FeatureTrackerNode ( )

Constructor

Definition at line 30 of file FeatureTrackerNode.cpp.

FeatureTrackerNode::~FeatureTrackerNode ( ) [virtual]

Destructor

Definition at line 67 of file FeatureTrackerNode.cpp.

Member Function Documentation

void FeatureTrackerNode::_InitializeVariables ( ) [protected, virtual]

Initialize variables

Definition at line 651 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::_publishPredictedMeasurement ( ) const [protected, virtual]

Publish the prediction about the next measurement by this RE level.

Implements omip::RecursiveEstimatorNodeInterface< ft_measurement_ros_t, ft_state_ros_t, FeatureTracker >.

Definition at line 558 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::_publishState ( ) const [protected, virtual]

Publish the current state of this RE level.

Implements omip::RecursiveEstimatorNodeInterface< ft_measurement_ros_t, ft_state_ros_t, FeatureTracker >.

Definition at line 546 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::_ReadParameters ( ) [protected, virtual]

Read parameters from configuration file

Definition at line 563 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::_SubscribeAndAdvertiseTopics ( ) [protected, virtual]

Subscribe and advertise topics for this node

Definition at line 715 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::AdvanceBagCallbackFromShapeReconstruction ( const boost::shared_ptr< std_msgs::Bool const > & flag ) [virtual]

Definition at line 814 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::AdvanceBagCallbackFromShapeTracker ( const boost::shared_ptr< omip_msgs::ShapeTrackerStates const > & st_states ) [virtual]

Definition at line 820 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::DynamicReconfigureCallback	(	feature_tracker::FeatureTrackerDynReconfConfig &	config,
		uint32_t	level
	)		`[virtual]`

Callback for the Dynamic Reconfigure parameters.

Parameters:

config	Values from the Dynamic Reconfigure server
level

Definition at line 232 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::measurementCallback	(	const sensor_msgs::PointCloud2ConstPtr &	pc_msg,
		const sensor_msgs::ImageConstPtr &	depth_image_msg,
		const sensor_msgs::ImageConstPtr &	rgb_image_msg
	)		`[virtual]`

Combined callback for the measurements of this RE level: point cloud, depth image and rgb image.

Parameters:

pc_msg	Input full RGBD point cloud
depth_image_msg	Input depth image
rgb_image_msg	Input RGB image

Definition at line 71 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::measurementCallback	(	const sensor_msgs::ImageConstPtr &	depth_image_msg,
		const sensor_msgs::ImageConstPtr &	rgb_image_msg
	)		`[virtual]`

Lighter version of the combined callback for the measurements of this RE level: depth image and rgb image.

Parameters:

depth_image_msg	Input depth image
rgb_image_msg	Input RGB image

Definition at line 85 of file FeatureTrackerNode.cpp.

virtual void omip::FeatureTrackerNode::measurementCallback ( const boost::shared_ptr< ft_measurement_ros_t const > & ft_measurement_ros ) [inline, virtual]

Empty measurement callback, just to implement correctly the RENode interface.

Parameters:

ft_measurement_ros Empty message. Only to implement the interface

Definition at line 132 of file FeatureTrackerNode.h.

void FeatureTrackerNode::OcclusionMaskImgCallback ( const sensor_msgs::ImageConstPtr & occlusion_mask_img ) [virtual]

Callback for the mask of the self occlusion of the robot. Only for the WAM.

Parameters:

occlusion_mask_img Mask image with the area of the visual field covered by the WAM robot

Definition at line 408 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishDepthEdgesImg ( ) [virtual]

Publish an image with the edges detected on the depth map.

Definition at line 477 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishDepthImg ( ) [virtual]

Definition at line 428 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishDetectingMaskImg ( ) [virtual]

Publish the mask used to detect new features. Combines edges, max depth and mask of current features (to not detect them)

Definition at line 511 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishFullRGBDPC ( ) [virtual]

Publish the full RGB-D point cloud used in the last iteration (used to synchronize the PC to the other published messages)

Definition at line 442 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishPredictedAndLastFeaturesImg ( ) [virtual]

Publish an image with the last feature locations and the predicted feature locations.

Definition at line 522 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishPredictionMaskImg ( ) [virtual]

Publish the mask of the predictions.

Definition at line 489 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishRGBImg ( ) [virtual]

Definition at line 414 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishTrackedFeaturesImg ( ) [virtual]

Publish an image with the last tracked features on it.

Definition at line 454 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishTrackedFeaturesWithPredictionMaskImg ( ) [virtual]

Publish an image with the last tracked features together with the mask of the predictions.

Definition at line 465 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::PublishTrackingMaskImg ( ) [virtual]

Publish the mask used to track features. Combines edges, predictions mask and max depth.

Definition at line 500 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::ReadRosBag ( ) [virtual]

Read messages from a rosbag instead of receiving them from a sensor Internally it "publishes" the messages that are then received by the measurement callback.

Definition at line 255 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::RepublishPredictedFeatLocation ( ) [virtual]

Definition at line 533 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::run ( ) [virtual]

Reimplemented from omip::RecursiveEstimatorNodeInterface< ft_measurement_ros_t, ft_state_ros_t, FeatureTracker >.

Definition at line 826 of file FeatureTrackerNode.cpp.

void FeatureTrackerNode::statePredictionCallback ( const boost::shared_ptr< ft_state_ros_t const > & predicted_next_state ) [virtual]

Callback for the predictions about the state of this RE level coming from the higher level of the hierarchy.

Parameters:

predicted_next_state Predictions about the next state (next feature 3D locations) from the RBT

Definition at line 222 of file FeatureTrackerNode.cpp.