[Jazzy] Ensure the cameras context will support the maximum camera resolutions

src/mujoco_cameras.cpp

@@ -164,14 +164,23 @@ void MujocoCameras::update_loop()
   // Turn rangefinder rendering off so we don't get rays in camera images
   mjv_opt_.flags[mjtVisFlag::mjVIS_RANGEFINDER] = 0;
 
+  // Initialize data for camera rendering
+  mj_camera_data_ = mj_makeData(mj_model_);
+  RCLCPP_INFO(node_->get_logger(), "Starting the camera rendering loop, publishing at %f Hz", camera_publish_rate_);
+
   // create scene and context
   mjv_makeScene(mj_model_, &mjv_scn_, 2000);
   mjr_makeContext(mj_model_, &mjr_con_, mjFONTSCALE_150);
 
-  // Initialize data for camera rendering
-  mj_camera_data_ = mj_makeData(mj_model_);
-  RCLCPP_INFO_STREAM(node_->get_logger(),
-                     "Starting the camera rendering loop, publishing at " << camera_publish_rate_ << " Hz");
+  // Ensure the context will support the largest cameras
+  int max_width = 1, max_height = 1;
+  for (const auto& cam : cameras_)
+  {
+    max_width = std::max(max_width, static_cast<int>(cam.width));
+    max_height = std::max(max_height, static_cast<int>(cam.height));
+  }
+  mjr_resizeOffscreen(max_width, max_height, &mjr_con_);
+  RCLCPP_INFO(node_->get_logger(), "Resized offscreen buffer to %d x %d", max_width, max_height);
 
   // TODO: Support per-camera publish rates?
   rclcpp::Rate rate(camera_publish_rate_);

src/mujoco_system_interface.cpp

@@ -434,6 +434,10 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
     return hardware_interface::CallbackReturn::FAILURE;
   }
 
+  // Disable the rangefinder flag at startup so that we don't get the yellow lines.
+  // We can still turn this on manually if desired.
+  sim_->opt.flags[mjVIS_RANGEFINDER] = false;
+
   // When the interface is activated, we start the physics engine.
   physics_thread_ = std::thread([this]() {
     // Load the simulation and do an initial forward pass

test/test_resources/test_robot.xml

@@ -17,7 +17,7 @@
         <site name="camera_color_frame" pos="1.0 0 0.1" quat="1.0 0 0 0"/>
         <site name="camera_color_optical_frame" pos="1.0 0 0.1" quat="-0.49999816339744824 0.4999999999966268 0.5000018366025517 -0.4999999999966268"/>
         <site name="camera_color_mujoco_frame" pos="1.0 0 0.1" quat="0.5000006633911983 0.4999974999995599 0.49999933659630164 0.5000024999995599"/>
-        <camera name="camera" fovy="58" mode="fixed" resolution="640 480" pos="1.0 0 0.1" quat="-0.4999999999966268 -0.49999816339744824 -0.4999999999966268 -0.5000018366025517"/>
+        <camera name="camera" fovy="58" mode="fixed" resolution="1280 720" pos="1.0 0 0.1" quat="-0.4999999999966268 -0.49999816339744824 -0.4999999999966268 -0.5000018366025517"/>
       </body>
       <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" class="collision"/>
       <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" rgba="1 0 0 1" class="visual"/>