Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00037 #include "depth_sensor_pose/depth_sensor_pose_node.h"
00038
00039 #define MAX_NODE_RATE 30
00040
00041 using namespace depth_sensor_pose;
00042
00043
00044 DepthSensorPoseNode::DepthSensorPoseNode(ros::NodeHandle& n, ros::NodeHandle& pnh):
00045 node_rate_hz_(1), pnh_(pnh), it_(n), srv_(pnh)
00046 {
00047 boost::mutex::scoped_lock lock(connection_mutex_);
00048
00049
00050 srv_.setCallback(boost::bind(&DepthSensorPoseNode::reconfigureCb, this, _1, _2));
00051
00052
00053
00054 pub_height_ = n.advertise<std_msgs::Float64>("depth_sensor_pose/height", 2,
00055 boost::bind(&DepthSensorPoseNode::connectCb, this),
00056 boost::bind(&DepthSensorPoseNode::disconnectCb, this));
00057
00058 pub_angle_ = n.advertise<std_msgs::Float64>("depth_sensor_pose/tilt_angle", 2,
00059 boost::bind(&DepthSensorPoseNode::connectCb, this),
00060 boost::bind(&DepthSensorPoseNode::disconnectCb, this));
00061
00062
00063 pub_ = it_.advertise("depth_sensor_pose/depth", 1,
00064 boost::bind(&DepthSensorPoseNode::connectCb, this),
00065 boost::bind(&DepthSensorPoseNode::disconnectCb, this));
00066 }
00067
00068
00069 DepthSensorPoseNode::~DepthSensorPoseNode()
00070 {
00071 sub_.shutdown();
00072 }
00073
00074
00075 void DepthSensorPoseNode::setNodeRate(const float rate)
00076 {
00077 if (rate <= MAX_NODE_RATE)
00078 node_rate_hz_ = rate;
00079 else
00080 node_rate_hz_ = MAX_NODE_RATE;
00081 }
00082
00083
00084 float DepthSensorPoseNode::getNodeRate()
00085 {
00086 return node_rate_hz_;
00087 }
00088
00089
00090 void DepthSensorPoseNode::depthCb( const sensor_msgs::ImageConstPtr& depth_msg,
00091 const sensor_msgs::CameraInfoConstPtr& info_msg)
00092 {
00093 try
00094 {
00095
00096 estimator_.estimateParams(depth_msg, info_msg);
00097
00098 std_msgs::Float64 height, tilt_angle;
00099 height.data = estimator_.getSensorMountHeight();
00100 tilt_angle.data = estimator_.getSensorTiltAngle();
00101
00102 ROS_ERROR("height = %.4f angle = %.4f", estimator_.getSensorMountHeight(),
00103 estimator_.getSensorTiltAngle());
00104
00105 pub_height_.publish(height);
00106 pub_angle_.publish(tilt_angle);
00107
00108
00109 if (estimator_.getPublishDepthEnable())
00110 pub_.publish(estimator_.new_depth_msg_);
00111 }
00112 catch (std::runtime_error& e)
00113 {
00114 ROS_ERROR_THROTTLE(1.0, "Could not to run estimatation procedure: %s", e.what());
00115 }
00116 }
00117
00118
00119 void DepthSensorPoseNode::connectCb()
00120 {
00121 boost::mutex::scoped_lock lock(connection_mutex_);
00122 if (!sub_ && (pub_height_.getNumSubscribers() > 0 || pub_angle_.getNumSubscribers() > 0
00123 || pub_.getNumSubscribers() > 0))
00124 {
00125 ROS_DEBUG("Connecting to depth topic.");
00126 image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
00127 sub_ = it_.subscribeCamera("image", 1, &DepthSensorPoseNode::depthCb, this, hints);
00128 }
00129 }
00130
00131
00132 void DepthSensorPoseNode::disconnectCb()
00133 {
00134 boost::mutex::scoped_lock lock(connection_mutex_);
00135 if (pub_height_.getNumSubscribers() == 0 && pub_angle_.getNumSubscribers() == 0
00136 && pub_.getNumSubscribers() == 0)
00137 {
00138 ROS_DEBUG("Unsubscribing from depth topic.");
00139 sub_.shutdown();
00140 }
00141 }
00142
00143
00144 void DepthSensorPoseNode::reconfigureCb( depth_sensor_pose::DepthSensorPoseConfig& config,
00145 uint32_t level )
00146 {
00147 node_rate_hz_ = (float) config.rate;
00148
00149 estimator_.setRangeLimits(config.range_min, config.range_max);
00150 estimator_.setSensorMountHeightMin(config.mount_height_min);
00151 estimator_.setSensorMountHeightMax(config.mount_height_max);
00152 estimator_.setSensorTiltAngleMin(config.tilt_angle_min);
00153 estimator_.setSensorTiltAngleMax(config.tilt_angle_max);
00154
00155 estimator_.setPublishDepthEnable(config.publish_depth);
00156 estimator_.setCamModelUpdate(config.cam_model_update);
00157 estimator_.setUsedDepthHeight((unsigned int)config.used_depth_height);
00158 estimator_.setDepthImgStepRow(config.depth_img_step_row);
00159 estimator_.setDepthImgStepCol(config.depth_img_step_col);
00160
00161 estimator_.setRansacDistanceThresh(config.ransac_dist_thresh);
00162 estimator_.setRansacMaxIter(config.ransac_max_iter);
00163 estimator_.setGroundMaxPoints(config.ground_max_points);
00164
00165 estimator_.setReconfParamsUpdated(true);
00166 }