gazebo_ros_depth_camera.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/*
 * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor.
 * Author: John Hsu
 * Date: 24 Sept 2008
 */
 
#ifndef GAZEBO_ROS_DEPTH_CAMERA_HH
#define GAZEBO_ROS_DEPTH_CAMERA_HH
 
// ros stuff
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
 
// ros messages stuff
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/fill_image.h>
#include <std_msgs/Float64.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
 
// gazebo stuff
#include <sdf/Param.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/DepthCameraPlugin.hh>
 
// dynamic reconfigure stuff
#include <gazebo_plugins/GazeboRosCameraConfig.h>
#include <dynamic_reconfigure/server.h>
 
// boost stuff
#include <boost/thread/mutex.hpp>
 
// camera stuff
#include <gazebo_plugins/gazebo_ros_camera_utils.h>
 
namespace gazebo
{
  class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils
  {
    public: GazeboRosDepthCamera();
 
    public: ~GazeboRosDepthCamera();
 
    public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) override;
 
    public: virtual void Advertise();
 
    protected: virtual void OnNewDepthFrame(const float *_image,
                   unsigned int _width, unsigned int _height,
                   unsigned int _depth, const std::string &_format) override;
 
    protected: virtual void OnNewRGBPointCloud(const float *_pcd,
                    unsigned int _width, unsigned int _height,
                    unsigned int _depth, const std::string &_format) override;
 
    protected: virtual void OnNewImageFrame(const unsigned char *_image,
                   unsigned int _width, unsigned int _height,
                   unsigned int _depth, const std::string &_format) override;
 
#if GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 12
     protected: virtual void OnNewReflectanceFrame(const float * _reflectance,
                    unsigned int _width, unsigned int _height,
                    unsigned int _depth, const std::string &_format) override;
 
    protected: virtual void OnNewNormalsFrame(const float * _normals,
                   unsigned int _width, unsigned int _height,
                   unsigned int _depth, const std::string &_format) override;
#endif
 
    private: void FillPointdCloud(const float *_src);
 
    private: void FillDepthImage(const float *_src);
 
    private: int point_cloud_connect_count_;
    private: void PointCloudConnect();
    private: void PointCloudDisconnect();
 
    private: int reflectance_connect_count_;
    private: void ReflectanceConnect();
    private: void ReflectanceDisconnect();
 
    private: int normals_connect_count_;
    private: void NormalsConnect();
    private: void NormalsDisconnect();
 
    private: int depth_image_connect_count_;
    private: void DepthImageConnect();
    private: void DepthImageDisconnect();
    private: common::Time last_depth_image_camera_info_update_time_;
 
    private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg,
                                  uint32_t rows_arg, uint32_t cols_arg,
                                  uint32_t step_arg, void* data_arg);
 
    private: bool FillDepthImageHelper( sensor_msgs::Image& image_msg,
                                  uint32_t rows_arg, uint32_t cols_arg,
                                  uint32_t step_arg, void* data_arg);
 
    private: ros::Publisher point_cloud_pub_;
    private: ros::Publisher depth_image_pub_;
    private: ros::Publisher reflectance_pub_;
    private: ros::Publisher normal_pub_;
 
    private: sensor_msgs::PointCloud2 point_cloud_msg_;
    private: sensor_msgs::Image depth_image_msg_;
    private: sensor_msgs::Image reflectance_msg_;
 
    private: float * pcd_ = nullptr;
 
    private: double point_cloud_cutoff_;
 
    private: int reduce_normals_;
 
    private: std::string point_cloud_topic_name_;
 
    private: std::string reflectance_topic_name_;
 
    private: std::string normals_topic_name_;
 
    private: void InfoConnect();
    private: void InfoDisconnect();
 
    using GazeboRosCameraUtils::PublishCameraInfo;
    protected: virtual void PublishCameraInfo();
 
    private: std::string depth_image_topic_name_;
    private: std::string depth_image_camera_info_topic_name_;
    private: int depth_info_connect_count_;
    private: void DepthInfoConnect();
    private: void DepthInfoDisconnect();
    private: bool use_depth_image_16UC1_format_;
 
    // overload with our own
    private: common::Time depth_sensor_update_time_;
    protected: ros::Publisher depth_image_camera_info_pub_;
 
    private: event::ConnectionPtr load_connection_;
  };
 
}
#endif