Skip to content

Commit

Permalink
Camera: Add support for video capture and range zoom (PX4#1031)
Browse files Browse the repository at this point in the history 
* Add support for video capture and range zoom

* Use MAV_RESULT_DENIED for bad params
  • Loading branch information
@DonLakeFlyer
DonLakeFlyer authored Mar 1, 2024
1 parent 33ac87a commit da7206e
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 7 deletions.
8 changes: 7 additions & 1 deletion include/gazebo_camera_manager_plugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include <gazebo/rendering/rendering.hh>
#include <SITLGps.pb.h>
#include <ignition/math.hh>
#include <chrono>

namespace gazebo
{
Expand Down Expand Up @@ -61,6 +62,8 @@ class GAZEBO_VISIBLE CameraManagerPlugin : public SensorPlugin
void _handle_storage_info(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_take_photo(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_stop_take_photo(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_start_video_capture(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_stop_video_capture(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_request_camera_settings(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_request_video_stream_information(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
void _handle_request_video_stream_status(const mavlink_message_t *pMsg, struct sockaddr* srcaddr);
Expand Down Expand Up @@ -90,7 +93,8 @@ class GAZEBO_VISIBLE CameraManagerPlugin : public SensorPlugin
enum {
CAPTURE_DISABLED,
CAPTURE_SINGLE,
CAPTURE_ELAPSED
CAPTURE_ELAPSED,
CAPTURE_VIDEO
};

int _captureMode{CAPTURE_DISABLED};
Expand All @@ -117,6 +121,8 @@ class GAZEBO_VISIBLE CameraManagerPlugin : public SensorPlugin
int _systemID{1};
int _componentID{MAV_COMP_ID_CAMERA};
int _mavlinkCamPort{14530};

std::chrono::time_point<std::chrono::high_resolution_clock> _start_video_capture_time;
};

} /* namespace gazebo */
82 changes: 76 additions & 6 deletions src/gazebo_camera_manager_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -313,6 +313,12 @@ void CameraManagerPlugin::_handle_message(mavlink_message_t *msg, struct sockadd
case MAV_CMD_IMAGE_STOP_CAPTURE:
_handle_stop_take_photo(msg, srcaddr);
break;
case MAV_CMD_VIDEO_START_CAPTURE:
_handle_start_video_capture(msg, srcaddr);
break;
case MAV_CMD_VIDEO_STOP_CAPTURE:
_handle_stop_video_capture(msg, srcaddr);
break;
case MAV_CMD_REQUEST_CAMERA_INFORMATION:
_handle_camera_info(msg, srcaddr);
break;
Expand All @@ -335,7 +341,6 @@ void CameraManagerPlugin::_handle_message(mavlink_message_t *msg, struct sockadd
_handle_set_camera_mode(msg, srcaddr);
break;
case MAV_CMD_SET_CAMERA_ZOOM:
//Control the Zoom of the camera
_handle_camera_zoom(msg, srcaddr);
break;
case MAV_CMD_RESET_CAMERA_SETTINGS:
Expand Down Expand Up @@ -524,7 +529,7 @@ void CameraManagerPlugin::_handle_take_photo(const mavlink_message_t *pMsg, stru
} else {
gzerr << "Bad Start Capture argments: " << cmd.param2 << " " << cmd.param3 << endl;
_send_cmd_ack(pMsg->sysid, pMsg->compid,
MAV_CMD_IMAGE_START_CAPTURE, MAV_RESULT_UNSUPPORTED, srcaddr);
MAV_CMD_IMAGE_START_CAPTURE, MAV_RESULT_DENIED, srcaddr);
}
}

Expand All @@ -540,6 +545,55 @@ void CameraManagerPlugin::_handle_stop_take_photo(const mavlink_message_t *pMsg,
MAV_CMD_IMAGE_STOP_CAPTURE, MAV_RESULT_ACCEPTED, srcaddr);
}

void CameraManagerPlugin::_handle_start_video_capture(const mavlink_message_t *pMsg, struct sockaddr* srcaddr)
{
gzdbg << "Handle Start Video Capture" << endl;

mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(pMsg, &cmd);
std::lock_guard<std::mutex> guard(_captureMutex);

if (_captureMode != CAPTURE_DISABLED) {
// We are already capturing
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_START_CAPTURE, MAV_RESULT_TEMPORARILY_REJECTED, srcaddr);
return;
}

if (cmd.param1 != 0 || cmd.param2 != 0) {
gzerr << "VIDEO_START_CAPTURE: param1 and param2 must be 0\n";
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_START_CAPTURE, MAV_RESULT_DENIED, srcaddr);
return;
}

_captureMode = CAPTURE_VIDEO;
_start_video_capture_time = std::chrono::high_resolution_clock::now();
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_START_CAPTURE, MAV_RESULT_ACCEPTED, srcaddr);
}

void CameraManagerPlugin::_handle_stop_video_capture(const mavlink_message_t *pMsg, struct sockaddr* srcaddr)
{
gzdbg << "Handle Stop Video Capture" << endl;

mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(pMsg, &cmd);
std::lock_guard<std::mutex> guard(_captureMutex);

if (cmd.param1 != 0) {
gzerr << "VIDEO_STOP_CAPTURE: param1 must be 0\n";
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_START_CAPTURE, MAV_RESULT_DENIED, srcaddr);
return;
}

if (_captureMode != CAPTURE_VIDEO) {
// We are not capturing
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_START_CAPTURE, MAV_RESULT_TEMPORARILY_REJECTED, srcaddr);
return;
}

_captureMode = CAPTURE_DISABLED;
_send_cmd_ack(pMsg->sysid, pMsg->compid, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_RESULT_ACCEPTED, srcaddr);
}

void CameraManagerPlugin::_handle_request_camera_capture_status(const mavlink_message_t *pMsg, struct sockaddr* srcaddr)
{
mavlink_command_long_t cmd;
Expand Down Expand Up @@ -712,6 +766,10 @@ void CameraManagerPlugin::_handle_camera_zoom(const mavlink_message_t *pMsg, str
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 if (cmd.param1 == ZOOM_TYPE_RANGE) {
auto zoomRange = std::min(std::max(cmd.param2, 0.0f), 100.0f);
_zoom = 1.0f + (zoomRange / 100.0f) * (_maxZoom - 1.0f);
_camera->SetHFOV(_hfov / _zoom);
} else {
_zoom = std::max(std::min(float(_zoom + 0.1 * cmd.param2), _maxZoom), 1.0f);
_camera->SetHFOV(_hfov / _zoom);
Expand All @@ -721,9 +779,21 @@ void CameraManagerPlugin::_handle_camera_zoom(const mavlink_message_t *pMsg, str
void CameraManagerPlugin::_send_capture_status(struct sockaddr* srcaddr)
{
_captureMutex.lock();
int status = _captureMode == CAPTURE_DISABLED ? 0 : (_captureMode == CAPTURE_SINGLE ? 1 : 3);
uint8_t image_status = 0; // idle
uint8_t video_status = 0; // idle
uint32_t recording_time_ms = 0;
if (_captureMode == CAPTURE_SINGLE) {
image_status = 1; // active
} else if (_captureMode == CAPTURE_ELAPSED) {
image_status = 3; // time lapse
} else if (_captureMode == CAPTURE_VIDEO) {
video_status = 1; // active
auto current_time = std::chrono::high_resolution_clock::now();
recording_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - _start_video_capture_time).count();
}
float interval = CAPTURE_ELAPSED ? (float)_captureInterval : 0.0f;
_captureMutex.unlock();

gzdbg << "Send capture status" << endl;
float available_mib = 0.0f;
boost::filesystem::space_info si = boost::filesystem::space(".");
Expand All @@ -742,10 +812,10 @@ void CameraManagerPlugin::_send_capture_status(struct sockaddr* srcaddr)
MAVLINK_COMM_1,
&msg,
current_time.Double() * 1e3,
status, // image status
0, // video status (Idle)
image_status, // image status
video_status, // video status
interval, // image interval
0, // recording time in ms
recording_time_ms, // recording time in ms
available_mib, // available storage capacity
_imageCounter); // total number of images
_send_mavlink_message(&msg, srcaddr);
Expand Down

0 comments on commit da7206e

Please sign in to comment.