Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_ONVIF/AP_ONVIF.cpp

504 lines
19 KiB
C++
Raw Permalink Blame History

/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Michael Oborne and Siddharth Bharat Purohit, Cubepilot Pty.
*/
#include "AP_ONVIF.h"
#if ENABLE_ONVIF
#include <AP_ONVIF/MediaBinding.nsmap>
#include "onvifhelpers.h"
// For ChibiOS we will use HW RND # generator
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal;
#if 0
#define DEBUG_PRINT(fmt,args...) do {GCS_SEND_TEXT(MAV_SEVERITY_INFO ,"AP_ONVIF:" fmt "\n", ## args); } while(0)
#else
#define DEBUG_PRINT(fmt,args...)
#endif
#define ERROR_PRINT(fmt,args...) do {GCS_SEND_TEXT(MAV_SEVERITY_ERROR , "AP_ONVIF:" fmt "\n", ## args); } while(0)
const char *wsse_PasswordDigestURI = "http://docs.oasis-open.org/wss/2004/01/oasis-200401-wss-username-token-profile-1.0#PasswordDigest";
const char *wsse_Base64BinaryURI = "http://docs.oasis-open.org/wss/2004/01/oasis-200401-wss-soap-message-security-1.0#Base64Binary";
AP_ONVIF *AP_ONVIF::_singleton;
static AP_ONVIF onvif;
#define DEVICE_ENDPOINT_LOC "/onvif/device_service"
#define MEDIA_ENDPOINT_LOC "/onvif/media_service"
#define PTZ_ENDPOINT_LOC "/onvif/ptz_service"
// Default constructor
AP_ONVIF::AP_ONVIF()
{
if (_singleton != nullptr) {
AP_HAL::panic("AP_ONVIF must be singleton");
}
_singleton = this;
}
// Start ONVIF client with username, password and service host url
bool AP_ONVIF::start(const char *user, const char *pass, const char *hostname)
{
if (!initialised) {
soap = soap_new1(SOAP_XML_CANONICAL | SOAP_C_UTFSTRING);
if (soap == nullptr) {
ERROR_PRINT("AP_ONVIF: Failed to allocate soap");
return false;
}
soap->connect_timeout = soap->recv_timeout = soap->send_timeout = 30; // 30 sec
if (proxy_device == nullptr) {
proxy_device = NEW_NOTHROW DeviceBindingProxy(soap);
}
if (proxy_media == nullptr) {
proxy_media = NEW_NOTHROW MediaBindingProxy(soap);
}
if (proxy_ptz == nullptr) {
proxy_ptz = NEW_NOTHROW PTZBindingProxy(soap);
}
if (proxy_device == nullptr ||
proxy_media == nullptr ||
proxy_ptz == nullptr) {
ERROR_PRINT("AP_ONVIF: Failed to allocate gSOAP Proxy objects.");
return false;
}
initialised = true;
}
if (strlen(user) > username_len) {
if (username != nullptr) {
free(username);
username = nullptr;
}
}
username_len = strlen(user);
if (username == nullptr) {
username = (char*)malloc(username_len + 1);
}
if (strlen(pass) > password_len) {
if (password != nullptr) {
free(password);
password = nullptr;
}
}
password_len = strlen(pass);
if (password == nullptr) {
password = (char*)malloc(password_len+1);
}
if (strlen(hostname) > hostname_len) {
// free if not nullptr
if (device_endpoint != nullptr) {
free(device_endpoint);
device_endpoint = nullptr;
}
if (media_endpoint != nullptr) {
free(media_endpoint);
media_endpoint = nullptr;
}
if (ptz_endpoint != nullptr) {
free(ptz_endpoint);
ptz_endpoint = nullptr;
}
}
hostname_len = strlen(hostname);
if (device_endpoint == nullptr) {
device_endpoint = (char*)malloc(hostname_len + strlen(DEVICE_ENDPOINT_LOC) + 1);
}
if (media_endpoint == nullptr) {
media_endpoint = (char*)malloc(hostname_len + strlen(MEDIA_ENDPOINT_LOC) + 1);
}
if (ptz_endpoint == nullptr) {
ptz_endpoint = (char*)malloc(hostname_len + strlen(PTZ_ENDPOINT_LOC) + 1);
}
if (device_endpoint == nullptr ||
media_endpoint == nullptr ||
ptz_endpoint == nullptr ||
username == nullptr ||
password == nullptr) {
ERROR_PRINT("Failed to Allocate strings");
return false;
}
strcpy(username, user);
strcpy(password, pass);
snprintf(device_endpoint, hostname_len + strlen(DEVICE_ENDPOINT_LOC) + 1, "%s" DEVICE_ENDPOINT_LOC, hostname);
snprintf(media_endpoint, hostname_len + strlen(MEDIA_ENDPOINT_LOC) + 1, "%s" MEDIA_ENDPOINT_LOC, hostname);
snprintf(ptz_endpoint, hostname_len + strlen(PTZ_ENDPOINT_LOC) + 1, "%s" PTZ_ENDPOINT_LOC, hostname);
/// TODO: Need to find a way to store this in parameter system
// or it could be just storage, we will see
proxy_device->soap_endpoint = device_endpoint;
if (!probe_onvif_server()) {
ERROR_PRINT("Failed to probe onvif server.");
return false;
}
return true;
}
void AP_ONVIF::report_error()
{
ERROR_PRINT("ONVIF ERROR:");
if (soap_check_state(soap)) {
ERROR_PRINT("Error: soap struct state not initialized");
} else if (soap->error) {
const char **c, *v = NULL, *s, *d;
c = soap_faultcode(soap);
if (!*c) {
soap_set_fault(soap);
c = soap_faultcode(soap);
}
if (soap->version == 2) {
v = soap_fault_subcode(soap);
}
s = soap_fault_string(soap);
d = soap_fault_detail(soap);
ERROR_PRINT("%s%d fault %s [%s]\n%s\nDetail: %s",(soap->version ? "SOAP 1." : "Error "),
(soap->version ? (int)soap->version : soap->error),
*c, (v ? v : "no subcode"), (s ? s : "[no reason]"),
(d ? d : "[no detail]"));
}
}
// detect onvif server present on the network
bool AP_ONVIF::probe_onvif_server()
{
{
_tds__GetDeviceInformation GetDeviceInformation;
_tds__GetDeviceInformationResponse GetDeviceInformationResponse;
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_device->GetDeviceInformation(&GetDeviceInformation, GetDeviceInformationResponse)) {
ERROR_PRINT("Failed to fetch Device Information");
report_error();
goto err;
}
DEBUG_PRINT("Manufacturer: %s",GetDeviceInformationResponse.Manufacturer);
DEBUG_PRINT("Model: %s",GetDeviceInformationResponse.Model);
DEBUG_PRINT("FirmwareVersion: %s",GetDeviceInformationResponse.FirmwareVersion);
DEBUG_PRINT("SerialNumber: %s",GetDeviceInformationResponse.SerialNumber);
DEBUG_PRINT("HardwareId: %s",GetDeviceInformationResponse.HardwareId);
}
// get device capabilities and print media
{
_tds__GetCapabilities GetCapabilities;
_tds__GetCapabilitiesResponse GetCapabilitiesResponse;
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_device->GetCapabilities(&GetCapabilities, GetCapabilitiesResponse)) {
ERROR_PRINT("Failed to fetch Device Capabilities");
report_error();
goto err;
}
if (!GetCapabilitiesResponse.Capabilities || !GetCapabilitiesResponse.Capabilities->Media) {
ERROR_PRINT("Missing device capabilities info");
goto err;
} else {
DEBUG_PRINT("XAddr: %s", GetCapabilitiesResponse.Capabilities->Media->XAddr);
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities) {
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTPMulticast) {
DEBUG_PRINT("RTPMulticast: %s",(*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTPMulticast ? "yes" : "no"));
}
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORETCP) {
DEBUG_PRINT("RTP_TCP: %s", (*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORETCP ? "yes" : "no"));
}
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORERTSP_USCORETCP) {
DEBUG_PRINT("RTP_RTSP_TCP: %s", (*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORERTSP_USCORETCP ? "yes" : "no"));
}
}
}
// set the Media proxy endpoint to XAddr
proxy_media->soap_endpoint = media_endpoint;
}
// get device profiles
{
_trt__GetProfiles GetProfiles;
_trt__GetProfilesResponse GetProfilesResponse;
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_media->GetProfiles(&GetProfiles, GetProfilesResponse)){
ERROR_PRINT("Failed to fetch profiles");
report_error();
goto err;
}
if (GetProfilesResponse.__sizeProfiles > 0) {
DEBUG_PRINT("Profiles Received %lu", (unsigned long)GetProfilesResponse.__sizeProfiles);
} else {
ERROR_PRINT("Error: No Profiles Received");
goto err;
}
// for each profile get snapshot
for (uint32_t i = 0; i < (uint32_t)GetProfilesResponse.__sizeProfiles; i++) {
DEBUG_PRINT("Profile name: %s", GetProfilesResponse.Profiles[i]->Name);
}
// Just use first one for now
if (profile_token_size < (strlen(GetProfilesResponse.Profiles[0]->token) + 1)) {
if (profile_token != nullptr) {
free(profile_token);
}
profile_token = (char*)malloc(strlen(GetProfilesResponse.Profiles[0]->token) + 1);
profile_token_size = strlen(GetProfilesResponse.Profiles[0]->token) + 1;
if (profile_token == nullptr) {
goto err;
}
}
strcpy(profile_token, GetProfilesResponse.Profiles[0]->token);
proxy_ptz->soap_endpoint = ptz_endpoint;
}
// get PTZ Token
{
_tptz__GetConfigurations GetConfigurations;
_tptz__GetConfigurationsResponse GetConfigurationsResponse;
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_ptz->GetConfigurations(&GetConfigurations, GetConfigurationsResponse)) {
ERROR_PRINT("Failed to fetch Configurations");
report_error();
goto err;
}
if (GetConfigurationsResponse.__sizePTZConfiguration > 0) {
DEBUG_PRINT("PTZ Tokens Received");
} else {
ERROR_PRINT("Error: No Profiles Received");
goto err;
}
for (uint32_t i = 0; i < (uint32_t)GetConfigurationsResponse.__sizePTZConfiguration; i++) {
DEBUG_PRINT("PTZ: %s", GetConfigurationsResponse.PTZConfiguration[i]->Name);
}
//GetConfigurationsResponse.PTZConfiguration[0]->token
pan_tilt_limit_max = Vector2f(GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->XRange->Max,
GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->YRange->Max);
pan_tilt_limit_min = Vector2f(GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->XRange->Min,
GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->YRange->Min);
zoom_min = GetConfigurationsResponse.PTZConfiguration[0]->ZoomLimits->Range->XRange->Min;
zoom_max = GetConfigurationsResponse.PTZConfiguration[0]->ZoomLimits->Range->XRange->Max;
DEBUG_PRINT("Pan: %f %f Tilt: %f %f", pan_tilt_limit_min.x, pan_tilt_limit_max.x,
pan_tilt_limit_min.y, pan_tilt_limit_max.y);
}
// Get PTZ status
{
_tptz__GetStatus GetStatus;
_tptz__GetStatusResponse GetStatusResponse;
GetStatus.ProfileToken = profile_token;
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_ptz->GetStatus(&GetStatus, GetStatusResponse)) {
DEBUG_PRINT("Failed to recieve PTZ status");
goto err;
}
if (GetStatusResponse.PTZStatus->Error) {
DEBUG_PRINT("ErrorStatus: %s", GetStatusResponse.PTZStatus->Error);
}
if (GetStatusResponse.PTZStatus->MoveStatus->PanTilt) {
DEBUG_PRINT("PTStatus: %d", *GetStatusResponse.PTZStatus->MoveStatus->PanTilt);
}
if (GetStatusResponse.PTZStatus->MoveStatus->Zoom) {
DEBUG_PRINT("ZoomStatus: %d", *GetStatusResponse.PTZStatus->MoveStatus->Zoom);
}
DEBUG_PRINT("Pan: %f Tilt: %f", GetStatusResponse.PTZStatus->Position->PanTilt->x,
GetStatusResponse.PTZStatus->Position->PanTilt->y);
DEBUG_PRINT("Zoom: %f", GetStatusResponse.PTZStatus->Position->Zoom->x);
}
soap_destroy(soap);
soap_end(soap);
return true;
err:
soap_destroy(soap);
soap_end(soap);
return false;
}
// Generate Random Nonce value
bool AP_ONVIF::rand_nonce(char *nonce, size_t noncelen)
{
if (noncelen <= 4) {
// invalid size
return false;
}
uint32_t r = (uint32_t)(hal.util->get_hw_rtc()/1000000ULL);
(void)memcpy((void *)nonce, (const void *)&r, 4);
return hal.util->get_random_vals((uint8_t*)&nonce[4], noncelen - 4);
}
#define TEST_NONCE "LKqI6G/AikKCQrN0zqZFlg=="
#define TEST_TIME "2010-09-16T07:50:45Z"
#define TEST_PASS "userpassword"
#define TEST_RESULT "tuOSpGlFlIXsozq4HFNeeGeFLEI="
#define TEST 0
bool AP_ONVIF::set_credentials()
{
soap_wsse_delete_Security(soap);
soap_wsse_add_Timestamp(soap, "Time", 60);
_wsse__Security *security = soap_wsse_add_Security(soap);
const char *created = soap_dateTime2s(soap, (time_t)(hal.util->get_hw_rtc()/1000000ULL));
char HA[SHA1_DIGEST_SIZE] {};
char HABase64fin[29] {};
char nonce[16];
char *nonceBase64enc = nullptr;
char *nonceBase64fin;
sha1_ctx ctx;
uint16_t HABase64len;
char *HABase64enc = nullptr;
uint16_t noncelen;
/* generate a nonce */
if (!rand_nonce(nonce, 16)) {
return false;
}
sha1_begin(&ctx);
#if TEST
char* test_nonce = (char*)base64_decode((const unsigned char*)TEST_NONCE, strlen(TEST_NONCE), &noncelen);
sha1_hash((const unsigned char*)test_nonce, noncelen, &ctx);
sha1_hash((const unsigned char*)TEST_TIME, strlen(TEST_TIME), &ctx);
sha1_hash((const unsigned char*)TEST_PASS, strlen(TEST_PASS), &ctx);
nonceBase64enc = (char*)base64_encode((unsigned char*)test_nonce, 16, &noncelen); // this call also mallocs
DEBUG_PRINT("Created:%s Hash64:%s", TEST_TIME, HABase64fin);
#else
sha1_hash((const unsigned char*)nonce, 16, &ctx);
sha1_hash((const unsigned char*)created, strlen(created), &ctx);
sha1_hash((const unsigned char*)password, strlen(password), &ctx);
nonceBase64enc = (char*)base64_encode((unsigned char*)nonce, 16, &noncelen); // this call also mallocs
#endif
if (nonceBase64enc == nullptr) {
return false;
}
// move to something we can track
nonceBase64fin = (char*)soap_malloc(soap, noncelen);
memcpy(nonceBase64fin, nonceBase64enc, noncelen);
free(nonceBase64enc);
sha1_end((unsigned char*)HA, &ctx);
HABase64enc = (char*)base64_encode((unsigned char*)HA, SHA1_DIGEST_SIZE, &HABase64len);
if (HABase64enc == nullptr) {
return false;
}
if (HABase64len > 29) {
//things have gone truly bad time to panic
ERROR_PRINT("Error: Invalid Base64 Encode!");
free(HABase64enc);
return false;
}
memcpy(HABase64fin, HABase64enc, HABase64len);
free(HABase64enc);
if (soap_wsse_add_UsernameTokenText(soap, "Auth", username, HABase64fin)) {
report_error();
return false;
}
/* populate the remainder of the password, nonce, and created */
security->UsernameToken->Password->Type = (char*)wsse_PasswordDigestURI;
security->UsernameToken->Nonce = (struct wsse__EncodedString*)soap_malloc(soap, sizeof(struct wsse__EncodedString));
security->UsernameToken->Salt = NULL;
security->UsernameToken->Iteration = NULL;
if (!security->UsernameToken->Nonce) {
ERROR_PRINT("Failed to allocate NONCE");
return false;
}
soap_default_wsse__EncodedString(soap, security->UsernameToken->Nonce);
security->UsernameToken->Nonce->__item = nonceBase64fin;
security->UsernameToken->Nonce->EncodingType = (char*)wsse_Base64BinaryURI;
security->UsernameToken->wsu__Created = soap_strdup(soap, created);
return true;
}
// Turn ONVIF camera to mentioned pan, tilt and zoom, normalised
// between limits
bool AP_ONVIF::set_absolutemove(float x, float y, float z)
{
_tptz__AbsoluteMove AbsoluteMove;
_tptz__AbsoluteMoveResponse AbsoluteMoveResponse;
AbsoluteMove.Position = soap_new_tt__PTZVector(soap);
if (AbsoluteMove.Position == nullptr) {
ERROR_PRINT("Failed to allocate AbsoluteMove.Position");
goto err;
}
AbsoluteMove.Position->PanTilt = soap_new_tt__Vector2D(soap);
if (AbsoluteMove.Position->PanTilt == nullptr) {
ERROR_PRINT("Failed to allocate AbsoluteMove.Position->PanTilt");
goto err;
}
AbsoluteMove.Position->Zoom = soap_new_tt__Vector1D(soap);
if (AbsoluteMove.Position->Zoom == nullptr) {
ERROR_PRINT("Failed to allocate AbsoluteMove.Position->Zoom");
goto err;
}
AbsoluteMove.Position->PanTilt->x = constrain_float(x, pan_tilt_limit_min.x, pan_tilt_limit_max.x);
AbsoluteMove.Position->PanTilt->y = constrain_float(y, pan_tilt_limit_min.y, pan_tilt_limit_max.y);
AbsoluteMove.Position->Zoom->x = constrain_float(z, zoom_min, zoom_max);
AbsoluteMove.Speed = NULL;
AbsoluteMove.ProfileToken = profile_token;
// DEBUG_PRINT("Setting AbsoluteMove: %f %f %f", AbsoluteMove.Position->PanTilt->x,
// AbsoluteMove.Position->PanTilt->y,
// AbsoluteMove.Position->Zoom->x);
if (!set_credentials()) {
ERROR_PRINT("Failed to setup credentials");
goto err;
}
if (proxy_ptz->AbsoluteMove(&AbsoluteMove, AbsoluteMoveResponse)) {
ERROR_PRINT("Failed to sent AbsoluteMove cmd");
report_error();
goto err;
}
soap_destroy(soap);
soap_end(soap);
return true;
err:
soap_destroy(soap);
soap_end(soap);
return false;
}
#endif //#if ENABLE_ONVIF
Reference in New Issue View Git Blame Copy Permalink