diff --git a/include/gazebo_geotagged_images_plugin.h b/include/gazebo_geotagged_images_plugin.h index c0fd2b97ca81..21e02dc34477 100644 --- a/include/gazebo_geotagged_images_plugin.h +++ b/include/gazebo_geotagged_images_plugin.h @@ -85,6 +85,7 @@ class GAZEBO_VISIBLE GeotaggedImagesPlugin : public SensorPlugin int _captureCount; double _captureInterval; int _fd; + int _zoom_cmd; enum { CAPTURE_DISABLED, diff --git a/src/gazebo_geotagged_images_plugin.cpp b/src/gazebo_geotagged_images_plugin.cpp index 0f1beb9747f7..1efb166f00e3 100644 --- a/src/gazebo_geotagged_images_plugin.cpp +++ b/src/gazebo_geotagged_images_plugin.cpp @@ -54,6 +54,7 @@ GeotaggedImagesPlugin::GeotaggedImagesPlugin() , _hfov(1.57) , _zoom(1.0) , _maxZoom(8.0) + , _zoom_cmd(0) { } @@ -428,6 +429,14 @@ void GeotaggedImagesPlugin::cameraThread() { _last_heartbeat = current_time; _send_heartbeat(); } + + //Move camera zoom incase of continuous zoom + // _zoom_cmd is set by MAV_CMD_SET_CAMERA_ZOOM + if (_zoom_cmd!=0) { + _zoom = std::max(std::min(float(_zoom + 0.05 * _zoom_cmd), _maxZoom), 1.0f); + _camera->SetHFOV(_hfov / _zoom); + } + } } @@ -689,8 +698,13 @@ void GeotaggedImagesPlugin::_handle_camera_zoom(const mavlink_message_t *pMsg, s _send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_SET_CAMERA_ZOOM, MAV_RESULT_ACCEPTED, srcaddr); - _zoom = std::max(std::min(float(_zoom + 0.1 * cmd.param2), _maxZoom), 1.0f); - _camera->SetHFOV(_hfov / _zoom); + if (cmd.param1 == ZOOM_TYPE_CONTINUOUS) { + _zoom = std::max(std::min(float(_zoom + 0.1 * cmd.param2), _maxZoom), 1.0f); + _zoom_cmd = cmd.param2; + } else { + _zoom = std::max(std::min(float(_zoom + 0.1 * cmd.param2), _maxZoom), 1.0f); + _camera->SetHFOV(_hfov / _zoom); + } } void GeotaggedImagesPlugin::_send_capture_status(struct sockaddr* srcaddr)