diff --git a/src/mujoco_cameras.cpp b/src/mujoco_cameras.cpp index 6a865af..9bb7278 100644 --- a/src/mujoco_cameras.cpp +++ b/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(cam.width)); + max_height = std::max(max_height, static_cast(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_); diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 6440b55..f8f2918 100644 --- a/src/mujoco_system_interface.cpp +++ b/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 diff --git a/test/test_resources/test_robot.xml b/test/test_resources/test_robot.xml index 5f1a39b..f504d99 100644 --- a/test/test_resources/test_robot.xml +++ b/test/test_resources/test_robot.xml @@ -17,7 +17,7 @@ - +