diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp index 011caaf..be5eba5 100644 --- a/src/plugin/BodyROSItem.cpp +++ b/src/plugin/BodyROSItem.cpp @@ -147,18 +147,41 @@ void BodyROSItem::createSensors(BodyPtr body) accelSensors_.assign(devices.extract()); imus_.assign(devices.extract()); visionSensors_.assign(devices.extract()); - rangeVisionSensors_.assign(devices.extract()); + // rangeVisionSensors_.assign(devices.extract()); + rangeVisionSensors_.clear(); + rangeVisionSensors_depthimage_.clear(); rangeSensors_.assign(devices.extract()); for (size_t i=0; i < visionSensors_.size(); ++i) { if (Camera* sensor = visionSensors_[i]) { RangeCamera* camera = dynamic_cast(sensor); if (camera) { - rangeVisionSensors_.push_back(camera); + if (camera->isOrganized()) { + rangeVisionSensors_depthimage_.push_back(camera); + } else { + rangeVisionSensors_.push_back(camera); + } } } } + auto itr = visionSensors_.begin(); + while(itr != visionSensors_.end()){ + if (Camera* sensor = *itr){ + RangeCamera* camera = dynamic_cast(sensor); + if (sensor->imageType() == cnoid::Camera::NO_IMAGE) { + itr = visionSensors_.erase(itr); + } else if (camera && !camera->isOrganized()) { + itr = visionSensors_.erase(itr); + }else{ + ++itr; + } + }else{ + ++itr; + } + } + + forceSensorPublishers.clear(); forceSensorPublishers.reserve(forceSensors_.size()); forceSensorSwitchServers.clear(); @@ -247,8 +270,8 @@ void BodyROSItem::createSensors(BodyPtr body) for (CameraPtr sensor : visionSensors_) { std::string name = sensor->name(); std::replace(name.begin(), name.end(), '-', '_'); - const image_transport::Publisher publisher - = it.advertise(name + "/image_raw", 1); + const image_transport::CameraPublisher publisher + = it.advertiseCamera(name + "/color/image_raw", 1); sensor->sigStateChanged().connect([this, sensor, publisher]() { updateVisionSensor(sensor, publisher); }); @@ -261,8 +284,8 @@ void BodyROSItem::createSensors(BodyPtr body) sensor->name().c_str(), sensor->frameRate()); } - rangeVisionSensorPublishers.clear(); - rangeVisionSensorPublishers.reserve(rangeVisionSensors_.size()); + rangeVisionSensorPublishers_pointcloud.clear(); + rangeVisionSensorPublishers_pointcloud.reserve(rangeVisionSensors_.size()); rangeVisionSensorSwitchServers.clear(); rangeVisionSensorSwitchServers.reserve(rangeVisionSensors_.size()); for (RangeCameraPtr sensor : rangeVisionSensors_) { @@ -273,8 +296,28 @@ void BodyROSItem::createSensors(BodyPtr body) sensor->sigStateChanged().connect([this, sensor, publisher]() { updateRangeVisionSensor(sensor, publisher); }); - rangeVisionSensorPublishers.push_back(publisher); - // adds a server only for the camera whose type is COLOR_DEPTH or POINT_CLOUD. + rangeVisionSensorPublishers_pointcloud.push_back(publisher); + + boost::function requestCallback + = boost::bind(&BodyROSItem::switchDevice, this, _1, _2, sensor); + rangeVisionSensorSwitchServers.push_back( + rosNode->advertiseService(name + "/set_enabled", requestCallback)); + ROS_INFO("Create point cloud camera %s (%f Hz)", sensor->name().c_str(), sensor->frameRate()); + + } + + rangeVisionSensorPublishers_depthimage.clear(); + rangeVisionSensorPublishers_depthimage.reserve(rangeVisionSensors_.size()); + for (RangeCameraPtr sensor : rangeVisionSensors_depthimage_) { + std::string name = sensor->name(); + std::replace(name.begin(), name.end(), '-', '_'); + const image_transport::CameraPublisher publisher + = it.advertiseCamera(name + "/depth/image_raw", 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateRangeVisionSensor_depthimage(sensor, publisher); + }); + rangeVisionSensorPublishers_depthimage.push_back(publisher); + // adds a server only for the camera whose type is COLOR_DEPTH. // Without this exception, a new service server may be a duplicate // of one added to 'visionSensorSwitchServers'. if (sensor->imageType() == Camera::NO_IMAGE) { @@ -369,6 +412,9 @@ void BodyROSItem::updateForceSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } geometry_msgs::WrenchStamped force; force.header.stamp.fromSec(io->currentTime()); force.header.frame_id = sensor->name(); @@ -388,6 +434,9 @@ void BodyROSItem::updateRateGyroSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::Imu gyro; gyro.header.stamp.fromSec(io->currentTime()); gyro.header.frame_id = sensor->name(); @@ -404,6 +453,9 @@ void BodyROSItem::updateAccelSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::Imu accel; accel.header.stamp.fromSec(io->currentTime()); accel.header.frame_id = sensor->name(); @@ -420,6 +472,9 @@ void BodyROSItem::updateImu if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::Imu imu; imu.header.stamp.fromSec(io->currentTime()); imu.header.frame_id = sensor->name(); @@ -434,11 +489,14 @@ void BodyROSItem::updateImu void BodyROSItem::updateVisionSensor -(const CameraPtr& sensor, const image_transport::Publisher& publisher) +(const CameraPtr& sensor, const image_transport::CameraPublisher& publisher) { if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::Image vision; vision.header.stamp.fromSec(io->currentTime()); vision.header.frame_id = sensor->name(); @@ -455,7 +513,46 @@ void BodyROSItem::updateVisionSensor vision.step = sensor->image().width() * sensor->image().numComponents(); vision.data.resize(vision.step * vision.height); std::memcpy(&(vision.data[0]), &(sensor->image().pixels()[0]), vision.step * vision.height); - publisher.publish(vision); + sensor_msgs::CameraInfo info; + info.header.stamp = vision.header.stamp; + info.header.frame_id = vision.header.frame_id ; + info.width = vision.width; + info.height = vision.height; + info.distortion_model = "plumb_bob"; + info.D.resize(5); + for (int i = 0; i < 5; i++ ) info.D[i] = 0; + { + double fovy2 = sensor->fieldOfView() / 2.0; + double width = sensor->resolutionX(); + double height = sensor->resolutionY(); + double minlength = std::min(width, height); + double fu, fv; + fv = fu = minlength / tan(fovy2) / 2.0; + double u0 = (width - 1)/2.0; + double v0 = (height - 1)/2.0; + + info.K[0] = fu; + info.K[1] = 0.0; + info.K[2] = u0; + info.K[3] = 0.0; + info.K[4] = fv; + info.K[5] = v0; + info.K[6] = info.K[7] = 0.0; info.K[8] = 1.0; + + info.P[0] = fu; + info.P[1] = 0.0; + info.P[2] = u0; + info.P[4] = 0.0; + info.P[5] = fv; + info.P[6] = v0; + info.P[3] = info.P[7] = info.P[8] = info.P[9] = info.P[11] = 0.0; + info.P[10] = 1.0; + } + + for (int i = 0; i < 9; i++ ) info.R[i] = 0; + info.R[0] = info.R[4] = info.R[8] = 1; + + publisher.publish(vision, info); } @@ -465,16 +562,18 @@ void BodyROSItem::updateRangeVisionSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::PointCloud2 range; range.header.stamp.fromSec(io->currentTime()); range.header.frame_id = sensor->name(); - range.width = sensor->resolutionX(); - range.height = sensor->resolutionY(); + range.width = sensor->constPoints().size(); + range.height = 1; range.is_bigendian = false; range.is_dense = true; - range.row_step = range.point_step * range.width; if (sensor->imageType() == cnoid::Camera::COLOR_IMAGE) { - range.fields.resize(6); + range.fields.resize(4); range.fields[3].name = "rgb"; range.fields[3].offset = 12; range.fields[3].count = 1; @@ -498,6 +597,7 @@ void BodyROSItem::updateRangeVisionSensor range.fields.resize(3); range.point_step = 12; } + range.row_step = range.point_step * range.width; range.fields[0].name = "x"; range.fields[0].offset = 0; range.fields[0].datatype = sensor_msgs::PointField::FLOAT32; @@ -533,12 +633,87 @@ void BodyROSItem::updateRangeVisionSensor } +void BodyROSItem::updateRangeVisionSensor_depthimage +(const RangeCameraPtr& sensor, const image_transport::CameraPublisher& publisher) +{ + if(!sensor->on()){ + return; + } + if(publisher.getNumSubscribers()==0){ + return; + } + sensor_msgs::Image vision; + vision.header.stamp.fromSec(io->currentTime()); + vision.header.frame_id = sensor->name(); + vision.height = sensor->resolutionY(); + vision.width = sensor->resolutionX(); + vision.encoding = sensor_msgs::image_encodings::MONO16; + + vision.is_bigendian = 0; + vision.step = sensor->resolutionX() * sizeof(uint16_t); + vision.data.resize(vision.step * vision.height); + + uint16_t* dst = (uint16_t*)&(vision.data[0]); + const std::vector& points = sensor->constPoints(); + for (int y = 0; y < vision.height; y++ ) { + for (int x = 0; x < vision.width; x++ ) { + int idx = y * vision.width + x; + dst[idx] = (uint16_t)(points[idx].z() * 1000); + } + } + + sensor_msgs::CameraInfo info; + info.header.stamp = vision.header.stamp; + info.header.frame_id = vision.header.frame_id ; + info.width = vision.width; + info.height = vision.height; + info.distortion_model = "plumb_bob"; + info.D.resize(5); + for (int i = 0; i < 5; i++ ) info.D[i] = 0; + { + double fovy2 = sensor->fieldOfView() / 2.0; + double width = sensor->resolutionX(); + double height = sensor->resolutionY(); + double minlength = std::min(width, height); + double fu, fv; + fv = fu = minlength / tan(fovy2) / 2.0; + double u0 = (width - 1)/2.0; + double v0 = (height - 1)/2.0; + + info.K[0] = fu; + info.K[1] = 0.0; + info.K[2] = u0; + info.K[3] = 0.0; + info.K[4] = fv; + info.K[5] = v0; + info.K[6] = info.K[7] = 0.0; info.K[8] = 1.0; + + info.P[0] = fu; + info.P[1] = 0.0; + info.P[2] = u0; + info.P[4] = 0.0; + info.P[5] = fv; + info.P[6] = v0; + info.P[3] = info.P[7] = info.P[8] = info.P[9] = info.P[11] = 0.0; + info.P[10] = 1.0; + } + + for (int i = 0; i < 9; i++ ) info.R[i] = 0; + info.R[0] = info.R[4] = info.R[8] = 1; + + publisher.publish(vision, info); +} + + void BodyROSItem::updateRangeSensor (const RangeSensorPtr& sensor, const ros::Publisher& publisher) { if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::LaserScan range; range.header.stamp.fromSec(io->currentTime()); range.header.frame_id = sensor->name(); @@ -571,6 +746,9 @@ void BodyROSItem::update3DRangeSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::PointCloud2 range; // Header Info range.header.stamp.fromSec(io->currentTime()); @@ -645,6 +823,9 @@ void BodyROSItem::update3DRangeSensor if(!sensor->on()){ return; } + if(publisher.getNumSubscribers()==0){ + return; + } sensor_msgs::PointCloud range; // Header Info range.header.stamp.fromSec(io->currentTime()); @@ -708,8 +889,11 @@ void BodyROSItem::stopPublishing() for (size_t i = 0; i < visionSensorPublishers.size(); ++i) { visionSensorPublishers[i].shutdown(); } - for (size_t i = 0; i < rangeVisionSensorPublishers.size(); ++i) { - rangeVisionSensorPublishers[i].shutdown(); + for (size_t i = 0; i < rangeVisionSensorPublishers_pointcloud.size(); i++) { + rangeVisionSensorPublishers_pointcloud[i].shutdown(); + } + for (size_t i = 0; i < rangeVisionSensorPublishers_depthimage.size(); i++) { + rangeVisionSensorPublishers_depthimage[i].shutdown(); } for (size_t i = 0; i < rangeSensorPublishers.size(); ++i) { rangeSensorPublishers[i].shutdown(); diff --git a/src/plugin/BodyROSItem.h b/src/plugin/BodyROSItem.h index 092e0e7..326e49d 100644 --- a/src/plugin/BodyROSItem.h +++ b/src/plugin/BodyROSItem.h @@ -75,6 +75,7 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem DeviceList imus_; DeviceList visionSensors_; DeviceList rangeVisionSensors_; + DeviceList rangeVisionSensors_depthimage_; DeviceList rangeSensors_; double timeStep_; @@ -97,8 +98,9 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem std::vector rateGyroSensorPublishers; std::vector accelSensorPublishers; std::vector imuPublishers; - std::vector visionSensorPublishers; - std::vector rangeVisionSensorPublishers; + std::vector visionSensorPublishers; + std::vector rangeVisionSensorPublishers_pointcloud; + std::vector rangeVisionSensorPublishers_depthimage; std::vector rangeSensorPublishers; std::vector rangeSensorPcPublishers; @@ -119,9 +121,11 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem const AccelerationSensorPtr& sensor, const ros::Publisher& publisher); void updateImu(const ImuPtr& sensor, const ros::Publisher& publisher); void updateVisionSensor( - const CameraPtr& sensor, const image_transport::Publisher& publisher); + const CameraPtr& sensor, const image_transport::CameraPublisher& publisher); void updateRangeVisionSensor( const RangeCameraPtr& sensor, const ros::Publisher& publisher); + void updateRangeVisionSensor_depthimage( + const RangeCameraPtr& sensor, const image_transport::CameraPublisher& publisher); void updateRangeSensor( const RangeSensorPtr& sensor, const ros::Publisher& publisher); void update3DRangeSensor(