26 #include <boost/thread/thread.hpp> 27 #include <boost/bind.hpp> 31 #include <gazebo/sensors/Sensor.hh> 33 #include <gazebo/sensors/SensorTypes.hh> 42 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosOpenniKinect)
48 this->point_cloud_connect_count_ = 0;
49 this->depth_info_connect_count_ = 0;
50 this->depth_image_connect_count_ = 0;
51 this->last_depth_image_camera_info_update_time_ = common::Time(0);
64 DepthCameraPlugin::Load(_parent, _sdf);
68 this->
width_ = this->width;
70 this->
depth_ = this->depth;
72 this->
camera_ = this->depthCamera;
75 if (!_sdf->HasElement(
"imageTopicName"))
77 if (!_sdf->HasElement(
"cameraInfoTopicName"))
81 if (!_sdf->HasElement(
"pointCloudTopicName"))
87 if (!_sdf->HasElement(
"depthImageTopicName"))
92 if (!_sdf->HasElement(
"depthImageCameraInfoTopicName"))
97 if (!_sdf->HasElement(
"pointCloudCutoff"))
101 if (!_sdf->HasElement(
"pointCloudCutoffMax"))
113 ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
121 ros::AdvertiseOptions::create< sensor_msgs::Image >(
129 ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
143 this->parentSensor->SetActive(
true);
152 this->parentSensor->SetActive(
false);
160 this->parentSensor->SetActive(
true);
185 unsigned int _width,
unsigned int _height,
unsigned int _depth,
186 const std::string &_format)
192 if (this->parentSensor->IsActive())
198 this->parentSensor->SetActive(
false);
214 this->parentSensor->SetActive(
true);
222 unsigned int _width,
unsigned int _height,
unsigned int _depth,
223 const std::string &_format)
231 if (this->parentSensor->IsActive())
237 this->parentSensor->SetActive(
false);
249 this->parentSensor->SetActive(
true);
275 this->
lock_.unlock();
297 this->
lock_.unlock();
303 sensor_msgs::PointCloud2 &point_cloud_msg,
304 uint32_t rows_arg, uint32_t cols_arg,
305 uint32_t step_arg,
void* data_arg)
310 pcd_modifier.
resize(rows_arg*cols_arg);
311 point_cloud_msg.is_dense =
true;
318 float* toCopyFrom = (
float*)data_arg;
321 double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
322 double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));
325 for (uint32_t j=0; j<rows_arg; j++)
328 if (rows_arg>1) pAngle = atan2( (
double)j - 0.5*(
double)(rows_arg-1), fl);
331 for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
334 if (cols_arg>1) yAngle = atan2( (
double)i - 0.5*(
double)(cols_arg-1), fl);
337 double depth = toCopyFrom[index++];
346 *iter_x = depth * tan(yAngle);
347 *iter_y = depth * tan(pAngle);
352 *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN ();
353 point_cloud_msg.is_dense =
false;
357 uint8_t* image_src = (uint8_t*)(&(this->
image_msg_.data[0]));
358 if (this->
image_msg_.data.size() == rows_arg*cols_arg*3)
361 iter_rgb[0] = image_src[i*3+j*cols_arg*3+0];
362 iter_rgb[1] = image_src[i*3+j*cols_arg*3+1];
363 iter_rgb[2] = image_src[i*3+j*cols_arg*3+2];
365 else if (this->
image_msg_.data.size() == rows_arg*cols_arg)
368 iter_rgb[0] = image_src[i+j*cols_arg];
369 iter_rgb[1] = image_src[i+j*cols_arg];
370 iter_rgb[2] = image_src[i+j*cols_arg];
383 point_cloud_msg.height = rows_arg;
384 point_cloud_msg.width = cols_arg;
385 point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width;
392 sensor_msgs::Image& image_msg,
393 uint32_t rows_arg, uint32_t cols_arg,
394 uint32_t step_arg,
void* data_arg)
397 image_msg.height = rows_arg;
398 image_msg.width = cols_arg;
399 image_msg.step =
sizeof(float) * cols_arg;
400 image_msg.data.resize(rows_arg * cols_arg *
sizeof(
float));
401 image_msg.is_bigendian = 0;
403 const float bad_point = std::numeric_limits<float>::quiet_NaN();
405 float* dest = (
float*)(&(image_msg.data[0]));
406 float* toCopyFrom = (
float*)data_arg;
410 for (uint32_t j = 0; j < rows_arg; j++)
412 for (uint32_t i = 0; i < cols_arg; i++)
414 float depth = toCopyFrom[index++];
419 dest[i + j * cols_arg] = depth;
423 dest[i + j * cols_arg] = bad_point;
432 ROS_DEBUG_NAMED(
"openni_kinect",
"publishing default camera info, then openni kinect camera info");
438 #if GAZEBO_MAJOR_VERSION >= 8 439 common::Time cur_time = this->
world_->SimTime();
441 common::Time cur_time = this->
world_->GetSimTime();
ros::Publisher depth_image_pub_
std::string image_topic_name_
ROS image topic name.
void DepthImageDisconnect()
void publish(const boost::shared_ptr< M > &message) const
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
common::Time sensor_update_time_
double point_cloud_cutoff_
Minimum range of the point cloud.
int depth_image_connect_count_
Keep track of number of connctions for point clouds.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
std::string depth_image_camera_info_topic_name_
~GazeboRosOpenniKinect()
Destructor.
common::Time last_depth_image_camera_info_update_time_
virtual void PublishCameraInfo()
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
ros::Publisher point_cloud_pub_
A pointer to the ROS node. A node will be instantiated if it does not exist.
ros::CallbackQueue camera_queue_
void DepthInfoDisconnect()
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
sensor_msgs::Image image_msg_
ROS image message.
#define ROS_DEBUG_NAMED(name,...)
void PointCloudDisconnect()
bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
event::ConnectionPtr OnLoad(const boost::function< void()> &)
virtual void Advertise()
Advertise point cloud and depth image.
sensor_msgs::Image depth_image_msg_
const std::string TYPE_32FC1
bool FillDepthImageHelper(sensor_msgs::Image &image_msg, uint32_t height, uint32_t width, uint32_t step, void *data_arg)
bool initialized_
True if camera util is initialized.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int point_cloud_connect_count_
Keep track of number of connctions for point clouds.
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
event::ConnectionPtr load_connection_
std::string camera_info_topic_name_
void FillPointdCloud(const float *_src)
push point cloud data into ros topic
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
rendering::CameraPtr camera_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
ros::Publisher depth_image_camera_info_pub_
std::string depth_image_topic_name_
image where each pixel contains the depth data
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
double point_cloud_cutoff_max_
Maximum range of the point cloud.
int depth_info_connect_count_
sensors::SensorPtr parentSensor_
void setPointCloud2FieldsByString(int n_fields,...)
sensor_msgs::PointCloud2 point_cloud_msg_
PointCloud2 point cloud message.
boost::shared_ptr< void > VoidPtr
std::string point_cloud_topic_name_
ROS image topic name.
void FillDepthImage(const float *_src)
push depth image data into ros topic
common::Time depth_sensor_update_time_