Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Modification of sensor data publisher #11

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Modification of sensor data publisher #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
216 changes: 200 additions & 16 deletions src/plugin/BodyROSItem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,18 +147,41 @@ void BodyROSItem::createSensors(BodyPtr body)
accelSensors_.assign(devices.extract<AccelerationSensor>());
imus_.assign(devices.extract<Imu>());
visionSensors_.assign(devices.extract<Camera>());
rangeVisionSensors_.assign(devices.extract<RangeCamera>());
// rangeVisionSensors_.assign(devices.extract<RangeCamera>());
rangeVisionSensors_.clear();
rangeVisionSensors_depthimage_.clear();
rangeSensors_.assign(devices.extract<RangeSensor>());

for (size_t i=0; i < visionSensors_.size(); ++i) {
if (Camera* sensor = visionSensors_[i]) {
RangeCamera* camera = dynamic_cast<RangeCamera*>(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<RangeCamera*>(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();
Expand Down Expand Up @@ -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);
});
Expand All @@ -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_) {
Expand All @@ -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<bool (std_srvs::SetBoolRequest&, std_srvs::SetBoolResponse&)> 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) {
Expand Down Expand Up @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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);
}


Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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<Vector3f>& 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();
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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();
Expand Down
10 changes: 7 additions & 3 deletions src/plugin/BodyROSItem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem
DeviceList<Imu> imus_;
DeviceList<Camera> visionSensors_;
DeviceList<RangeCamera> rangeVisionSensors_;
DeviceList<RangeCamera> rangeVisionSensors_depthimage_;
DeviceList<RangeSensor> rangeSensors_;
double timeStep_;

Expand All @@ -97,8 +98,9 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem
std::vector<ros::Publisher> rateGyroSensorPublishers;
std::vector<ros::Publisher> accelSensorPublishers;
std::vector<ros::Publisher> imuPublishers;
std::vector<image_transport::Publisher> visionSensorPublishers;
std::vector<ros::Publisher> rangeVisionSensorPublishers;
std::vector<image_transport::CameraPublisher> visionSensorPublishers;
std::vector<ros::Publisher> rangeVisionSensorPublishers_pointcloud;
std::vector<image_transport::CameraPublisher> rangeVisionSensorPublishers_depthimage;
std::vector<ros::Publisher> rangeSensorPublishers;
std::vector<ros::Publisher> rangeSensorPcPublishers;

Expand All @@ -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(
Expand Down