Skip to content

Commit

Permalink
syncronize and split camera_info with images.
Browse files Browse the repository at this point in the history 
- give each image from cameras its own topic
- add configuration for camera topics

This changes cameras to publish a syncronized camera_info for
each image. e.g for rgb image:
/mujoco_server/cameras/camera_name/rgb/camera_info
/mujoco_server/cameras/camera_name/rgb/image_raw

Additional configuration options to change topics:
  camera_name/topic: defaults to "/mujoco_server/cameras/camera_name"
  camera_name/name_rgb: default to "rbg"
  camera_name/name_depth: default to "depth"
  camera_name/name_segment: default to "segmented"
  • Loading branch information
@LeroyR @DavidPL1
LeroyR authored and DavidPL1 committed Nov 20, 2024
1 parent f640ba0 commit 36b6f99
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 40 deletions.
22 changes: 14 additions & 8 deletions mujoco_ros/include/mujoco_ros/offscreen_camera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,10 +53,11 @@ namespace mujoco_ros::rendering {
class OffscreenCamera
{
public:
OffscreenCamera(const uint8_t cam_id, const std::string &cam_name, const int width, const int height,
const streamType stream_type, const bool use_segid, const float pub_freq,
image_transport::ImageTransport *it, const ros::NodeHandle &parent_nh, const mjModel *model,
mjData *data, mujoco_ros::MujocoEnv *env_ptr);
OffscreenCamera(const uint8_t cam_id, const std::string &base_topic, const std::string &rgb_topic,
const std::string &depth_topic, const std::string &segment_topic, const std::string &cam_name,
const int width, const int height, const streamType stream_type, const bool use_segid,
const float pub_freq, const ros::NodeHandle &parent_nh, const mjModel *model, mjData *data,
mujoco_ros::MujocoEnv *env_ptr);

~OffscreenCamera()
{
Expand All @@ -66,11 +67,14 @@ class OffscreenCamera
rgb_pub_.shutdown();
depth_pub_.shutdown();
segment_pub_.shutdown();
camera_info_pub_.shutdown();
rgb_camera_info_pub_.shutdown();
depth_camera_info_pub_.shutdown();
segment_camera_info_pub_.shutdown();
};

uint8_t cam_id_;
std::string cam_name_;
std::string topic_;
int width_, height_;
streamType stream_type_ = streamType::RGB;
bool use_segid_ = true;
Expand All @@ -82,10 +86,14 @@ class OffscreenCamera
mjvSceneState scn_state_; // Update depends on vopt, so every CamStream needs one

ros::Time last_pub_;
ros::Publisher camera_info_pub_;
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Publisher rgb_pub_;
ros::Publisher rgb_camera_info_pub_;
image_transport::Publisher depth_pub_;
ros::Publisher depth_camera_info_pub_;
image_transport::Publisher segment_pub_;
ros::Publisher segment_camera_info_pub_;

void renderAndPublish(mujoco_ros::OffscreenRenderContext *offscreen);

Expand All @@ -98,8 +106,6 @@ class OffscreenCamera
private:
std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;

void publishCameraInfo();

bool renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext *offscreen, const bool rgb, const bool depth,
const bool segment);

Expand Down
59 changes: 32 additions & 27 deletions mujoco_ros/src/offscreen_camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,36 +47,42 @@

namespace mujoco_ros::rendering {

OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_name, const int width, const int height,
OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &base_topic, const std::string &rgb_topic,
const std::string &depth_topic, const std::string &segment_topic,
const std::string &cam_name, const int width, const int height,
const streamType stream_type, const bool use_segid, const float pub_freq,
image_transport::ImageTransport *it, const ros::NodeHandle &parent_nh,
const mjModel *model, mjData *data, mujoco_ros::MujocoEnv *env_ptr)
const ros::NodeHandle &parent_nh, const mjModel *model, mjData *data,
mujoco_ros::MujocoEnv *env_ptr)
: cam_id_(cam_id)
, cam_name_(cam_name)
, width_(width)
, height_(height)
, stream_type_(stream_type)
, use_segid_(use_segid)
, pub_freq_(pub_freq)
, nh_(parent_nh, base_topic)
, it_(nh_)
{
last_pub_ = ros::Time::now();
ros::NodeHandle nh(parent_nh, "cameras/" + cam_name);

mjv_defaultOption(&vopt_);
mjv_defaultSceneState(&scn_state_);
mjv_makeSceneState(const_cast<mjModel *>(model), data, &scn_state_, Viewer::kMaxGeom);

if (stream_type & streamType::RGB) {
ROS_DEBUG_NAMED("mujoco_env", "\tCreating rgb publisher");
rgb_pub_ = it->advertise("cameras/" + cam_name + "/rgb", 1);
rgb_pub_ = it_.advertise(rgb_topic + "/image_raw", 1);
rgb_camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(rgb_topic + "/camera_info", 1, true);
}
if (stream_type & streamType::DEPTH) {
ROS_DEBUG_NAMED("mujoco_env", "\tCreating depth publisher");
depth_pub_ = it->advertise("cameras/" + cam_name + "/depth", 1);
depth_pub_ = it_.advertise(depth_topic + "/image_raw", 1);
depth_camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(depth_topic + "/camera_info", 1, true);
}
if (stream_type & streamType::SEGMENTED) {
ROS_DEBUG_NAMED("mujoco_env", "\tCreating segmentation publisher");
segment_pub_ = it->advertise("cameras/" + cam_name + "/segmented", 1);
segment_pub_ = it_.advertise(segment_topic + "/image_raw", 1);
segment_camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(segment_topic + "/camera_info", 1, true);
}

ROS_DEBUG_STREAM_NAMED("mujoco_env", "\tSetting up camera stream(s) of type '"
Expand Down Expand Up @@ -120,7 +126,7 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na
env_ptr->registerStaticTransform(cam_transform);

// init camera info manager
camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(nh, cam_name);
camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(nh_, cam_name);

// Get camera info
mjtNum cam_pos[3];
Expand Down Expand Up @@ -153,7 +159,6 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na
mju_copy(ci.K.c_array() + 6, extrinsic + 8, 3);

camera_info_manager_->setCameraInfo(ci);
camera_info_pub_ = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, true);
}

bool OffscreenCamera::shouldRender(const ros::Time &t)
Expand All @@ -166,10 +171,13 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext
const bool depth, const bool segment)
{
if ((!rgb && !depth) || // nothing to render
(rgb_pub_.getNumSubscribers() == 0 && depth_pub_.getNumSubscribers() == 0) || // no subscribers
(!rgb && depth_pub_.getNumSubscribers() == 0) ||
(!depth && ((rgb_pub_.getNumSubscribers() == 0 && !segment) || // would render depth but no depth subscribers
(segment && segment_pub_.getNumSubscribers() == 0)))) { // would render rgb but no rgb subscribers
(rgb_camera_info_pub_.getNumSubscribers() == 0 && rgb_pub_.getNumSubscribers() == 0 &&
depth_camera_info_pub_.getNumSubscribers() == 0 && depth_pub_.getNumSubscribers() == 0) || // no subscribers
(!rgb && depth_camera_info_pub_.getNumSubscribers() == 0 && depth_pub_.getNumSubscribers() == 0) ||
(!depth && ((rgb_camera_info_pub_.getNumSubscribers() == 0 && rgb_pub_.getNumSubscribers() == 0 &&
!segment) || // would render depth but no depth subscribers
(segment && segment_camera_info_pub_.getNumSubscribers() == 0 &&
segment_pub_.getNumSubscribers() == 0)))) { // would render rgb but no rgb subscribers
return false;
}

Expand All @@ -194,11 +202,16 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext
}
glfwSwapBuffers(offscreen->window.get());

// create info msg
auto ros_time = ros::Time(scn_state_.data.time);
sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
camera_info_msg.header.stamp = ros_time;

if (rgb) {
// Publish RGB image
sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image>();
rgb_msg->header.frame_id = cam_name_ + "_optical_frame";
rgb_msg->header.stamp = ros::Time(scn_state_.data.time);
rgb_msg->header.stamp = ros_time;
rgb_msg->width = static_cast<decltype(rgb_msg->width)>(viewport.width);
rgb_msg->height = static_cast<decltype(rgb_msg->height)>(viewport.height);
rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
Expand All @@ -216,16 +229,18 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext

if (segment) {
segment_pub_.publish(rgb_msg);
segment_camera_info_pub_.publish(camera_info_msg);
} else {
rgb_pub_.publish(rgb_msg);
rgb_camera_info_pub_.publish(camera_info_msg);
}
}

if (depth) {
// Publish DEPTH image
sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
depth_msg->header.frame_id = cam_name_ + "_optical_frame";
depth_msg->header.stamp = ros::Time(scn_state_.data.time);
depth_msg->header.stamp = ros_time;
depth_msg->width = util::as_unsigned(viewport.width);
depth_msg->height = util::as_unsigned(viewport.height);
depth_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
Expand All @@ -249,7 +264,9 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext
}

depth_pub_.publish(depth_msg);
depth_camera_info_pub_.publish(camera_info_msg);
}

return true;
}

Expand Down Expand Up @@ -297,18 +314,6 @@ void OffscreenCamera::renderAndPublish(mujoco_ros::OffscreenRenderContext *offsc
offscreen->scn.flags[mjRND_SEGMENT] = 1;
rendered = renderAndPubIfNecessary(offscreen, segment, true, segment);
}

if (rendered) {
publishCameraInfo();
}
}

void OffscreenCamera::publishCameraInfo()
{
sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo();
camera_info_msg.header.stamp = ros::Time::now();

camera_info_pub_.publish(camera_info_msg);
}

} // namespace mujoco_ros::rendering
21 changes: 16 additions & 5 deletions mujoco_ros/src/offscreen_rendering.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,9 @@ OffscreenRenderContext::~OffscreenRenderContext()

void MujocoEnv::initializeRenderResources()
{
image_transport::ImageTransport it(*nh_);
bool config_exists, use_segid;
rendering::streamType stream_type;
std::string cam_name, cam_config_path;
std::string cam_name, cam_config_path, base_topic, rgb, depth, segment;
float pub_freq;
int max_res_h = 0, max_res_w = 0;

Expand Down Expand Up @@ -92,19 +91,31 @@ void MujocoEnv::initializeRenderResources()
res_w_string += "/width";
std::string res_h_string(param_path);
res_h_string += "/height";
std::string base_topic_string(param_path);
base_topic_string += "/topic";
std::string rgb_topic_string(param_path);
rgb_topic_string += "/name_rgb";
std::string depth_topic_string(param_path);
depth_topic_string += "/name_depth";
std::string segment_topic_string(param_path);
segment_topic_string += "/name_segment";

stream_type = rendering::streamType(this->nh_->param<int>(stream_type_string, rendering::streamType::RGB));
pub_freq = this->nh_->param<float>(pub_freq_string, 15);
use_segid = this->nh_->param<bool>(segid_string, true);
res_w = this->nh_->param<int>(res_w_string, 720);
res_h = this->nh_->param<int>(res_h_string, 480);
base_topic = this->nh_->param<std::string>(base_topic_string, "cameras/" + cam_name);
rgb = this->nh_->param<std::string>(rgb_topic_string, "rgb");
depth = this->nh_->param<std::string>(depth_topic_string, "depth");
segment = this->nh_->param<std::string>(segment_topic_string, "segmented");

max_res_h = std::max(res_h, max_res_h);
max_res_w = std::max(res_w, max_res_w);

offscreen_.cams.emplace_back(std::make_unique<rendering::OffscreenCamera>(cam_id, cam_name, res_w, res_h,
stream_type, use_segid, pub_freq, &it,
*nh_, model_.get(), data_.get(), this));
offscreen_.cams.emplace_back(std::make_unique<rendering::OffscreenCamera>(
cam_id, base_topic, rgb, depth, segment, cam_name, res_w, res_h, stream_type, use_segid, pub_freq, *nh_,
model_.get(), data_.get(), this));
}

if (model_->vis.global.offheight < max_res_h || model_->vis.global.offwidth < max_res_w) {
Expand Down

0 comments on commit 36b6f99

Please sign in to comment.