cliff_detector_node.cpp
Go to the documentation of this file.
2 
3 namespace cliff_detector {
4 
6  node_rate_hz_(2), pnh_(pnh), it_(n), reconf_srv_(pnh) {
7  boost::mutex::scoped_lock lock(connection_mutex_);
8 
9  // Set callback for dynamic reconfigure server
10  reconf_srv_.setCallback(boost::bind(&CliffDetectorNode::reconfigureCb, this, _1, _2));
11 
12  // New depth image publisher
13  pub_ = it_.advertise("cliff_detector/depth", 1,
14  boost::bind(&CliffDetectorNode::connectCb, this),
15  boost::bind(&CliffDetectorNode::disconnectCb, this));
16 
17  // Publisher for stairs points msg
18  pub_points_ = n.advertise<depth_nav_msgs::Point32List>("cliff_detector/points", 2);
19 }
20 
22  sub_.shutdown();
23 }
24 
25 void CliffDetectorNode::setNodeRate(const unsigned int rate) {
26  if (rate <= 30) {
27  node_rate_hz_ = rate;
28  }
29  else {
30  node_rate_hz_ = 30;
31  }
32 }
33 
35  return node_rate_hz_;
36 }
37 
38 void CliffDetectorNode::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
39  const sensor_msgs::CameraInfoConstPtr& info_msg) {
40  try {
41  // Run cliff detector based on depth image
42  detector_.detectCliff(depth_msg, info_msg);
43 
44  // Publish stairs points msg
46 
47  // Publishes new depth image with added cliff
50  }
51  }
52  catch (std::runtime_error& e) {
53  ROS_ERROR_THROTTLE(1.0, "Could not perform stairs detection: %s", e.what());
54  }
55 }
56 
58  boost::mutex::scoped_lock lock(connection_mutex_);
59  if (sub_ == nullptr && pub_.getNumSubscribers() > 0) {
60  ROS_DEBUG("Connecting to depth topic.");
62  sub_ = it_.subscribeCamera("image", 1, &CliffDetectorNode::depthCb, this, hints);
63  }
64 }
65 
67  boost::mutex::scoped_lock lock(connection_mutex_);
68  if (pub_.getNumSubscribers() == 0) {
69  ROS_DEBUG("Unsubscribing from depth topic.");
70  sub_.shutdown();
71  }
72 }
73 
75  cliff_detector::CliffDetectorConfig& config, [[maybe_unused]] uint32_t level)
76 {
77  node_rate_hz_ = (unsigned int) config.rate;
78 
79  detector_.setRangeLimits(config.range_min, config.range_max);
80  detector_.setSensorMountHeight(config.sensor_mount_height);
81  detector_.setSensorTiltAngle(config.sensor_tilt_angle);
82  detector_.setPublishDepthEnable(config.publish_depth);
83  detector_.setCamModelUpdate(config.cam_model_update);
84 
85  detector_.setUsedDepthHeight((unsigned int)config.used_depth_height);
86  detector_.setBlockSize(config.block_size);
87  detector_.setBlockPointsThresh(config.block_points_thresh);
88  detector_.setDepthImgStepRow(config.depth_img_step_row);
89  detector_.setDepthImgStepCol(config.depth_img_step_col);
90  detector_.setGroundMargin(config.ground_margin);
91 
93 }
94 
95 } // namespace cliff_detector
void setDepthImgStepCol(const int step)
setDepthImgStepCol
void detectCliff(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
detectCliff detects descending stairs based on the information in a depth image
void publish(const boost::shared_ptr< M > &message) const
depth_nav_msgs::Point32List stairs_points_msg_
image_transport::ImageTransport it_
Subscribes to synchronized Image CameraInfo pairs.
sensor_msgs::Image new_depth_msg_
ros::NodeHandle pnh_
Private node handler used to generate the transport hints in the connectCb.
CameraSubscriber subscribeCamera(const std::string &base_topic, uint32_t queue_size, const CameraSubscriber::Callback &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
boost::mutex connection_mutex_
Prevents the connectCb and disconnectCb from being called until everything is initialized.
image_transport::Publisher pub_
Publisher for image_transport.
bool getPublishDepthEnable() const
getPublishDepthEnable
image_transport::CameraSubscriber sub_
Subscriber for image_transport.
void setParametersConfigurated(const bool u)
setParametersConfigurated
void setDepthImgStepRow(const int step)
setDepthImgStepRow
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
uint32_t getNumSubscribers() const
void setUsedDepthHeight(const unsigned int height)
setUsedDepthHeight
void setCamModelUpdate(const bool u)
Sets the number of image rows to use in data processing.
#define ROS_ERROR_THROTTLE(rate,...)
cliff_detector::CliffDetector detector_
Contains cliff detection method implementation.
void disconnectCb()
disconnectCb is called when a subscriber stop subscribing
void publish(const sensor_msgs::Image &message) const
void setSensorMountHeight(const float height)
setSensorMountHeight sets the height of sensor mount (in meters) from ground
void setBlockSize(const int size)
setBlockSize sets size of square block (subimage) used in cliff detector
void setPublishDepthEnable(const bool enable)
setPublishDepthEnable
unsigned int getNodeRate()
getNodeRate gets rate of data processing loop in node.
void reconfigureCb(cliff_detector::CliffDetectorConfig &config, uint32_t level)
reconfigureCb is dynamic reconfigure callback
ros::Publisher pub_points_
Publisher for publishing messages with stairs points.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void setNodeRate(const unsigned int rate)
setNodeRate sets rate of processing data loop in node.
void setGroundMargin(const float margin)
setGroundMargin sets the floor margin (in meters)
void setBlockPointsThresh(const int thresh)
setBlockPointsThresh sets threshold value of pixels in block to set block as stairs ...
void connectCb()
connectCb is callback which is called when new subscriber connected.
void setSensorTiltAngle(const float angle)
setSensorTiltAngle sets the sensor tilt angle (in degrees)
void depthCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
depthCb is callback which is called when new depth image appear
CliffDetectorNode(ros::NodeHandle &n, ros::NodeHandle &pnh)
dynamic_reconfigure::Server< cliff_detector::CliffDetectorConfig > reconf_srv_
Dynamic reconfigure server.
#define ROS_DEBUG(...)
void setRangeLimits(const float rmin, const float rmax)
setRangeLimits sets the minimum and maximum range of depth value from RGBD sensor.


cliff_detector
Author(s): Michal Drwiega (http://www.mdrwiega.com)
autogenerated on Wed May 5 2021 02:56:04