f200_nodelet.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  Copyright (c) 2017, Intel Corporation
3  All rights reserved.
4 
5  Redistribution and use in source and binary forms, with or without
6  modification, are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice, this
9  list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software without
17  specific prior written permission.
18 
19  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *******************************************************************************/
30 
31 #include <string>
32 #include <vector>
33 
35 
37 
38 namespace realsense_camera
39 {
40  /*
41  * Initialize the nodelet.
42  */
43  void F200Nodelet::onInit()
44  {
47  cv_type_[RS_STREAM_COLOR] = CV_8UC3;
48  unit_step_size_[RS_STREAM_COLOR] = sizeof(unsigned char) * 3;
49 
50  format_[RS_STREAM_DEPTH] = RS_FORMAT_Z16;
52  cv_type_[RS_STREAM_DEPTH] = CV_16UC1;
53  unit_step_size_[RS_STREAM_DEPTH] = sizeof(uint16_t);
54 
57  cv_type_[RS_STREAM_INFRARED] = CV_8UC1;
58  unit_step_size_[RS_STREAM_INFRARED] = sizeof(unsigned char);
59 
60  max_z_ = F200_MAX_Z;
61 
62  SyncNodelet::onInit();
63  }
64 
65  /*
66  * Determine fastest stream -- overrides base class
67  */
68  void F200Nodelet::setStreams()
69  {
70  // enable camera streams
71  BaseNodelet::setStreams();
72 
73  // Find the fastest updating, enabled stream
74  fastest_stream_ = RS_STREAM_DEPTH; // default to depth
75 
76  if (fps_[RS_STREAM_COLOR] > fps_[RS_STREAM_DEPTH])
77  {
78  if (enable_[RS_STREAM_COLOR])
79  {
80  fastest_stream_ = RS_STREAM_COLOR;
81  }
82  }
83  }
84 
85  /*
86  * Set Dynamic Reconfigure Server and return the dynamic params.
87  */
88  std::vector<std::string> F200Nodelet::setDynamicReconfServer()
89  {
90  dynamic_reconf_server_.reset(new dynamic_reconfigure::Server<realsense_camera::f200_paramsConfig>(pnh_));
91 
92  // Get dynamic options from the dynamic reconfigure server.
93  realsense_camera::f200_paramsConfig params_config;
94  dynamic_reconf_server_->getConfigDefault(params_config);
95  std::vector<realsense_camera::f200_paramsConfig::AbstractParamDescriptionConstPtr> param_desc =
96  params_config.__getParamDescriptions__();
97  std::vector<std::string> dynamic_params;
98  for (realsense_camera::f200_paramsConfig::AbstractParamDescriptionConstPtr param_desc_ptr : param_desc)
99  {
100  dynamic_params.push_back((* param_desc_ptr).name);
101  }
102 
103  return dynamic_params;
104  }
105 
106  /*
107  * Start Dynamic Reconfigure Callback.
108  */
109  void F200Nodelet::startDynamicReconfCallback()
110  {
111  dynamic_reconf_server_->setCallback(boost::bind(&F200Nodelet::configCallback, this, _1, _2));
112  }
113 
114  /*
115  * Get the dynamic param values.
116  */
117  void F200Nodelet::configCallback(realsense_camera::f200_paramsConfig &config, uint32_t level)
118  {
119  ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options");
120 
121  // set the depth enable
122  BaseNodelet::setDepthEnable(config.enable_depth);
123 
124  // Set common options
125  rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0);
126  rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0);
127  rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0);
128  rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0);
129  rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0);
130  rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0);
131  rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0);
132  rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0);
134  config.color_enable_auto_exposure, 0);
135  if (config.color_enable_auto_exposure == 0)
136  {
137  rs_set_device_option(rs_device_, RS_OPTION_COLOR_EXPOSURE, config.color_exposure, 0);
138  }
140  config.color_enable_auto_white_balance, 0);
141  if (config.color_enable_auto_white_balance == 0)
142  {
143  rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0);
144  }
145 
146  // Set F200 specific options
147  rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, config.f200_laser_power, 0);
148  rs_set_device_option(rs_device_, RS_OPTION_F200_ACCURACY, config.f200_accuracy, 0);
149  rs_set_device_option(rs_device_, RS_OPTION_F200_MOTION_RANGE, config.f200_motion_range, 0);
150  rs_set_device_option(rs_device_, RS_OPTION_F200_FILTER_OPTION, config.f200_filter_option, 0);
151  rs_set_device_option(rs_device_, RS_OPTION_F200_CONFIDENCE_THRESHOLD, config.f200_confidence_threshold, 0);
152  }
153 } // namespace realsense_camera
RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE
void rs_set_device_option(rs_device *device, rs_option option, double value, rs_error **error)
RS_OPTION_COLOR_BACKLIGHT_COMPENSATION
RS_OPTION_F200_MOTION_RANGE
RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE
PLUGINLIB_EXPORT_CLASS(depth_image_proc::ConvertMetricNodelet, nodelet::Nodelet)
const float F200_MAX_Z
Definition: constants.h:103
const std::string TYPE_8UC1
RS_OPTION_COLOR_HUE
RS_OPTION_COLOR_GAIN
RS_FORMAT_Z16
RS_OPTION_COLOR_CONTRAST
RS_OPTION_COLOR_WHITE_BALANCE
RS_OPTION_COLOR_EXPOSURE
GLint level
RS_OPTION_F200_CONFIDENCE_THRESHOLD
RS_OPTION_COLOR_SHARPNESS
const std::string TYPE_16UC1
#define ROS_INFO_STREAM(args)
RS_OPTION_COLOR_SATURATION
RS_OPTION_F200_FILTER_OPTION
RS_FORMAT_RGB8
RS_OPTION_F200_LASER_POWER
RS_STREAM_DEPTH
RS_FORMAT_Y8
RS_STREAM_INFRARED
RS_OPTION_COLOR_BRIGHTNESS
RS_OPTION_COLOR_GAMMA
RS_STREAM_COLOR
RS_OPTION_F200_ACCURACY


realsense_camera
Author(s): Rajvi Jingar , Reagan Lopez , Matt Hansen , Mark Horn
autogenerated on Mon Jun 10 2019 14:40:37