7 #include <dynamic_reconfigure/server.h> 8 #include <std_msgs/Float64.h> 9 #include <sensor_msgs/Image.h> 12 #include <opencv2/highgui/highgui.hpp> 13 #include <opencv2/imgproc/imgproc.hpp> 14 #include <cv_bridge/cv_bridge.h> 16 #include <depth_sensor_pose/DepthSensorPoseConfig.h> 54 void depthCallback(
const sensor_msgs::ImageConstPtr& depth_msg,
55 const sensor_msgs::CameraInfoConstPtr& info_msg);
96 dynamic_reconfigure::Server<depth_sensor_pose::DepthSensorPoseConfig>
dyn_rec_srv_;
DepthSensorPoseNode(ros::NodeHandle &pnh)
void depthCallback(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
depthCb is callback which is called when new depth image appear
void disconnectCallback()
disconnectCb is called when a subscriber stop subscribing
image_transport::ImageTransport it_
Subscribes to synchronized Image CameraInfo pairs.
std::mutex connection_mutex_
Prevents the connectCb and disconnectCb from being called until everything is initialized.
void reconfigureCallback(depth_sensor_pose::DepthSensorPoseConfig &config, uint32_t level)
reconfigureCb is dynamic reconfigure callback
image_transport::CameraSubscriber sub_
Subscriber for image_transport.
image_transport::Publisher pub_
Publisher for image_transport.
float getNodeRate()
getNodeRate gets rate of data processing loop in node.
depth_sensor_pose::DepthSensorPose estimator_
Object which contain estimation methods.
DepthSensorPoseNode & operator=(const DepthSensorPoseNode &)=delete
void connectCallback()
connectCb is callback which is called when new subscriber connected.
void setNodeRate(const float rate)
setNodeRate sets rate of processing data loop in node.
ros::NodeHandle pnh_
Private node handler.
ros::Publisher pub_height_
Publisher for estimated sensor height.
ros::Publisher pub_angle_
Publisher for estimated sensor tilt angle.
dynamic_reconfigure::Server< depth_sensor_pose::DepthSensorPoseConfig > dyn_rec_srv_
Dynamic reconfigure server.
float node_rate_hz_
Node loop frequency in Hz.