From 9297e938c77b40ee9ca6b9d9e1539da5ce541cb0 Mon Sep 17 00:00:00 2001 From: Mike Hamer Date: Wed, 28 Nov 2012 12:53:25 +0100 Subject: [PATCH] Added fullspeed_video Will now publish video frames as soon as received if fullspeed_video=true. Unfortunately to do this we needed to do a few dirty hacks, implying that our architecture is no longer so suitable for the future direction of the driver. We need to talk about an architectural redesign perhaps... --- launch/ardrone_aggressive.launch | 1 + src/ardrone_driver.cpp | 55 +++++++++++++++----------------- src/ardrone_driver.h | 3 +- src/ardrone_sdk.cpp | 2 ++ src/ardrone_sdk.h | 2 ++ src/video.cpp | 4 +++ src/video.h | 1 + 7 files changed, 37 insertions(+), 31 deletions(-) diff --git a/launch/ardrone_aggressive.launch b/launch/ardrone_aggressive.launch index fbec5f6..ad88810 100644 --- a/launch/ardrone_aggressive.launch +++ b/launch/ardrone_aggressive.launch @@ -65,6 +65,7 @@ + [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index d6e1053..08cd773 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -140,6 +140,7 @@ void ARDroneDriver::run() ROS_INFO(" Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h ROS_INFO(" ROS Loop Rate: %d", looprate); ROS_INFO(" Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off"); + ROS_INFO(" Instant New Video Publish: %s", fullspeed_video ? "On" : "Off"); ROS_INFO(" Drone Send Speed: %s", ardrone_application_default_config.navdata_demo==0 ? "200Hz (navdata_demo=0)" : "15Hz (navdata_demo=1)"); // TODO: Enabled Navdata Demo vp_os_mutex_unlock(&navdata_lock); @@ -149,13 +150,16 @@ void ARDroneDriver::run() } } } else { - vp_os_mutex_lock(&video_lock); - copy_current_frame_id = current_frame_id; - vp_os_mutex_unlock(&video_lock); - if (copy_current_frame_id != last_frame_id) + if(!fullspeed_video) { - last_frame_id = copy_current_frame_id; - publish_video(); + vp_os_mutex_lock(&video_lock); + copy_current_frame_id = current_frame_id; + vp_os_mutex_unlock(&video_lock); + if (copy_current_frame_id != last_frame_id) + { + last_frame_id = copy_current_frame_id; + publish_video(); + } } vp_os_mutex_lock(&navdata_lock); @@ -366,9 +370,9 @@ void ARDroneDriver::publish_video() image_msg.step = D1_STREAM_WIDTH*3; image_msg.data.resize(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); std::copy(buffer, buffer+(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3), image_msg.data.begin()); - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); if (cam_state == ZAP_CHANNEL_HORI) { @@ -394,14 +398,14 @@ void ARDroneDriver::publish_video() image_msg.data.clear(); image_msg.data.resize(D1_VERTSTREAM_WIDTH*D1_VERTSTREAM_HEIGHT*3); _it = image_msg.data.begin(); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); for (int row = 0; row < D1_VERTSTREAM_HEIGHT ; row++) { int _b = row * D1_STREAM_WIDTH * 3; int _e = _b + image_msg.step; _it = std::copy(buffer + _b, buffer + _e, _it); } - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH; cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT; @@ -424,14 +428,14 @@ void ARDroneDriver::publish_video() image_msg.data.clear(); image_msg.data.resize((D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH)*D1_STREAM_HEIGHT*3); _it = image_msg.data.begin(); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); for (int row = 0; row < D1_STREAM_HEIGHT; row++) { int _b = (row * D1_STREAM_WIDTH * 3) + (D1_MODE2_PIP_WIDTH * 3); int _e = _b + image_msg.step; _it = std::copy(buffer + _b, buffer + _e, _it); } - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); cinfo_msg_hori.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH; cinfo_msg_hori.height = D1_STREAM_HEIGHT; @@ -446,14 +450,14 @@ void ARDroneDriver::publish_video() image_msg.data.clear(); image_msg.data.resize(D1_MODE2_PIP_WIDTH * D1_MODE2_PIP_HEIGHT * 3); _it = image_msg.data.begin(); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); for (int row = 0; row < D1_MODE2_PIP_HEIGHT; row++) { int _b = row * D1_STREAM_WIDTH * 3; int _e = _b + image_msg.step; _it = std::copy(buffer + _b, buffer + _e, _it); } - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); cinfo_msg_vert.width = D1_MODE2_PIP_WIDTH; cinfo_msg_vert.height = D1_MODE2_PIP_HEIGHT; @@ -479,9 +483,9 @@ void ARDroneDriver::publish_video() { int _b = (row * (D1_STREAM_WIDTH * 3)) + (D1_MODE3_PIP_WIDTH * 3); int _e = _b + image_msg.step; - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); _it = std::copy(buffer + _b, buffer + _e, _it); - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); } cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH; @@ -497,13 +501,13 @@ void ARDroneDriver::publish_video() image_msg.data.clear(); image_msg.data.resize(D1_MODE3_PIP_WIDTH * D1_MODE3_PIP_HEIGHT * 3); _it = image_msg.data.begin(); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); for (int row = 0; row < D1_MODE3_PIP_HEIGHT; row++) { int _b = row * D1_STREAM_WIDTH * 3; int _e = _b + image_msg.step; _it = std::copy(buffer + _b, buffer + _e, _it); - }vp_os_mutex_unlock(&video_lock); + }if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); cinfo_msg_hori.width = D1_MODE3_PIP_WIDTH; cinfo_msg_hori.height = D1_MODE3_PIP_HEIGHT; @@ -544,9 +548,9 @@ void ARDroneDriver::publish_video() image_msg.is_bigendian = false; image_msg.step = D2_STREAM_WIDTH*3; image_msg.data.resize(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3); - vp_os_mutex_lock(&video_lock); + if(!fullspeed_video) vp_os_mutex_lock(&video_lock); std::copy(buffer, buffer+(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3), image_msg.data.begin()); - vp_os_mutex_unlock(&video_lock); + if(!fullspeed_video) vp_os_mutex_unlock(&video_lock); // We only put the width and height in here. @@ -574,17 +578,8 @@ void ARDroneDriver::publish_video() } -void ARDroneDriver::publish_navdata() +void ARDroneDriver::publish_navdata(navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time) { - // Thread safe copy of interesting Navdata data - // TODO: This is a very expensive task, can we optimize here? - // maybe ignoring the copy when it is not needed. - vp_os_mutex_lock(&navdata_lock); - navdata_raw = shared_raw_navdata; - navdata_receive_time = shared_navdata_receive_time; - vp_os_mutex_unlock(&navdata_lock); - - if ((do_caliberation) && (!caliberated)) { acc_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_X]); diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index a242707..d17d6cb 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -1,6 +1,7 @@ #ifndef _ARDRONE_DRIVER_H_ #define _ARDRONE_DRIVER_H_ +class ARDroneDriver; #include #include #include @@ -52,8 +53,8 @@ public: #include "NavdataMessageDefinitions.h" #undef NAVDATA_STRUCTS_HEADER_PUBLIC + void publish_video(); private: - void publish_video(); void publish_navdata(); void publish_tf(); bool readCovParams(std::string param_name, boost::array &cov_array); diff --git a/src/ardrone_sdk.cpp b/src/ardrone_sdk.cpp index 6be2638..3e408d5 100644 --- a/src/ardrone_sdk.cpp +++ b/src/ardrone_sdk.cpp @@ -16,6 +16,7 @@ ARDroneDriver* rosDriver; int32_t looprate; bool fullspeed_navdata; +bool fullspeed_video; int32_t should_exit; @@ -61,6 +62,7 @@ extern "C" { ros::param::param("~looprate",looprate,50); ros::param::param("~fullspeed_navdata",fullspeed_navdata,false); + ros::param::param("~fullspeed_video",fullspeed_video,false); // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below) diff --git a/src/ardrone_sdk.h b/src/ardrone_sdk.h index 8375228..bb92474 100644 --- a/src/ardrone_sdk.h +++ b/src/ardrone_sdk.h @@ -47,6 +47,7 @@ extern video_decoder_config_t vec; #define NB_DRIVER_POST_STAGES 10 extern navdata_unpacked_t shared_raw_navdata; +extern ARDroneDriver *rosDriver; extern ros::Time shared_navdata_receive_time; extern vp_os_mutex_t navdata_lock; @@ -55,6 +56,7 @@ extern vp_os_mutex_t twist_lock; extern int32_t looprate; extern bool fullspeed_navdata; +extern bool fullspeed_video; extern int32_t should_exit; diff --git a/src/video.cpp b/src/video.cpp index 0f9c55f..2aa3857 100644 --- a/src/video.cpp +++ b/src/video.cpp @@ -18,6 +18,10 @@ extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_ vp_os_mutex_lock(&video_lock); memcpy(buffer, in->buffers[0], in->size); current_frame_id++; + if(fullspeed_video) + { + rosDriver->publish_video(); + } vp_os_mutex_unlock(&video_lock); // vp_os_mutex_unlock(&video_update_lock); return (SUCCESS); diff --git a/src/video.h b/src/video.h index 57bf9af..904b240 100644 --- a/src/video.h +++ b/src/video.h @@ -2,6 +2,7 @@ #define _VIDEO_H_ #include "ardrone_sdk.h" +#include "ardrone_driver.h" // The maximum memory allocation #define MAX_STREAM_WIDTH 640