29 #include <boost/bind.hpp> 30 #include <boost/thread/thread.hpp> 31 #include <gazebo/sensors/Sensor.hh> 32 #include <gazebo/sensors/SensorTypes.hh> 41 this->depth_info_connect_count_ = 0;
42 this->depth_image_connect_count_ = 0;
43 this->last_depth_image_camera_info_update_time_ = common::Time(0);
49 DepthCameraPlugin::Load(_parent, _sdf);
53 this->
width_ = this->width;
55 this->
depth_ = this->depth;
57 this->
camera_ = this->depthCamera;
60 if (!_sdf->HasElement(
"imageTopicName")) {
64 if (!_sdf->HasElement(
"cameraInfoTopicName")) {
69 if (!_sdf->HasElement(
"depthImageTopicName")) {
73 _sdf->GetElement(
"depthImageTopicName")->Get<std::string>();
76 if (!_sdf->HasElement(
"depthImageCameraInfoTopicName")) {
80 _sdf->GetElement(
"depthImageCameraInfoTopicName")->Get<std::string>();
85 if (!_sdf->HasElement(
"depthNoiseModelName")) {
86 noise_model =
"Kinect";
88 "depthNoiseModelName not defined, assuming 'Kinect'");
90 noise_model = _sdf->GetElement(
"depthNoiseModelName")->Get<std::string>();
93 if (boost::iequals(noise_model,
"Kinect")) {
97 }
else if(boost::iequals(noise_model,
"PMD")) {
103 else if (boost::iequals(noise_model,
"D435")) {
105 this->noise_model.reset(model);
107 if (_sdf->HasElement(
"horizontal_fov")) {
108 model->
h_fov = _sdf->GetElement(
"horizontal_fov")->Get<
float>();
111 if (_sdf->HasElement(
"baseline")) {
112 model->
baseline = _sdf->GetElement(
"baseline")->Get<
float>();
116 if (_sdf->HasElement(
"D435NoiseSubpixelErr")) {
118 _sdf->GetElement(
"D435NoiseSubpixelErr")->Get<
float>();
121 if (_sdf->HasElement(
"D435MaxStdev")) {
122 model->
max_stdev = _sdf->GetElement(
"D435MaxStdev")->Get<
float>();
126 "NoisyDepth",
"D435 Depth noise configuration: " 127 <<
"\tHorizontal FoV: " << model->
h_fov << std::endl
128 <<
"\tBaseline: " << model->
baseline << std::endl
131 <<
"\tNoise StDev cutoff: " << model->
max_stdev);
134 "Invalid depthNoiseModelName (%s), assuming 'Kinect'",
135 noise_model.c_str());
142 if (_sdf->HasElement(
"depthNoiseMinDist")) {
143 this->noise_model->min_depth =
144 _sdf->GetElement(
"depthNoiseMinDist")->Get<
float>();
147 if (_sdf->HasElement(
"depthNoiseMaxDist")) {
148 this->noise_model->max_depth =
149 _sdf->GetElement(
"depthNoiseMaxDist")->Get<
float>();
159 ros::AdvertiseOptions::create<sensor_msgs::Image>(
168 ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
179 this->parentSensor->SetActive(
true);
195 unsigned int _height,
197 const std::string &_format) {
203 if (this->parentSensor->IsActive()) {
205 this->parentSensor->SetActive(
false);
213 this->parentSensor->SetActive(
true);
221 unsigned int _height,
223 const std::string &_format) {
229 if (this->parentSensor->IsActive()) {
232 this->parentSensor->SetActive(
false);
239 this->parentSensor->SetActive(
true);
256 this->
lock_.unlock();
260 const uint32_t cols_arg,
261 const uint32_t step_arg,
262 const float *data_arg,
263 sensor_msgs::Image *image_msg) {
264 if(data_arg ==
nullptr){
265 ROS_WARN_NAMED(
"NoisyDepth",
"Invalid data array received - nullptr.");
270 image_msg->height = rows_arg;
271 image_msg->width = cols_arg;
272 image_msg->step =
sizeof(
float) * cols_arg;
273 image_msg->data.resize(rows_arg * cols_arg *
sizeof(
float));
274 image_msg->is_bigendian = 0;
276 float *
dest = (
float *)(&(image_msg->data[0]));
277 memcpy(dest, data_arg,
sizeof(
float) * width * height);
291 #if GAZEBO_MAJOR_VERSION >= 8 292 common::Time cur_time = this->
world_->SimTime();
294 common::Time cur_time = this->
world_->GetSimTime();
std::string format(const std::string &fmt, Args ... args)
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
std::string image_topic_name_
virtual void Advertise()
Advertise depth image.
common::Time sensor_update_time_
#define ROS_WARN_NAMED(name,...)
event::ConnectionPtr load_connection_
GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin)
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
void DepthImageDisconnect()
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
void FillDepthImage(const float *_src)
#define ROS_INFO_STREAM_NAMED(name, args)
std::string depth_image_topic_name_
sensor_msgs::Image depth_image_msg_
ros::CallbackQueue camera_queue_
boost::shared_ptr< int > image_connect_count_
std::string depth_image_camera_info_topic_name_
void publish(const boost::shared_ptr< M > &message) const
event::ConnectionPtr OnLoad(const boost::function< void()> &)
const std::string TYPE_32FC1
common::Time last_depth_image_camera_info_update_time_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int depth_image_connect_count_
std::string camera_info_topic_name_
void DepthInfoDisconnect()
ros::Publisher depth_image_camera_info_pub_
void PutCameraData(const unsigned char *_src)
rendering::CameraPtr camera_
std::unique_ptr< DepthNoiseModel > noise_model
ros::NodeHandle * rosnode_
int depth_info_connect_count_
common::Time depth_sensor_update_time_
virtual void PublishCameraInfo()
ros::Publisher depth_image_pub_
bool FillDepthImageHelper(const uint32_t rows_arg, const uint32_t cols_arg, const uint32_t step_arg, const float *data_arg, sensor_msgs::Image *image_msg)
sensors::SensorPtr parentSensor_
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
boost::shared_ptr< void > VoidPtr