wellton/ardrone_autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Video modes ported to be compatible with both Drone 1 & 2. Some old codes removed.

Ver Código-Fonte
Esse commit está contido em:
Mani Monajjemi
2012-06-28 16:15:35 -07:00
pai 7a7ef11451
commit 43a1f8c64b
7 arquivos alterados com 256 adições e 347 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
src/ardrone_driver.cpp
+205 -172
Ver Arquivo
@@ -19,15 +19,10 @@ ARDroneDriver::ARDroneDriver()
navdata_pub = node_handle.advertise<ardrone_brown::Navdata>("/ardrone/navdata", 1);
//toggleCam_sub = node_handle.subscribe("/ardrone/togglecam", 10, &toggleCamCallback);
#ifdef _USING_SDK_1_7_
//int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later
//int set_navdata_demo_value = DEFAULT_NAVDATA_DEMO;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
#else
//Ensure that the horizontal camera is running
ardrone_at_set_toy_configuration("video:video_channel","0");
#endif
// ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detect_dtype, NULL);
// ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_h, &detect_hori_type, NULL);
@@ -74,16 +69,12 @@ void ARDroneDriver::run()
{
configDone = true;
fprintf(stderr, "\nSending some critical initial configuration after some delay...\n");
#ifdef _USING_SDK_1_7_
//Ensure that the horizontal camera is running
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &mCodec, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (bitrate_ctrl_mode, &vbcMode, NULL);
#else
//Ensure that the horizontal camera is running
ardrone_at_set_toy_configuration("video:video_channel","0");
#endif
//Ensure that the horizontal camera is running
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &mCodec, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (bitrate_ctrl_mode, &vbcMode, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detect_dtype, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_v, &detect_vert_type, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_v_hsync, &detect_disable_placeholder, NULL);
@@ -108,165 +99,207 @@ void ARDroneDriver::run()
void ARDroneDriver::publish_video()
{
/*
* Information on buffer and image sizes.
* Buffer is always in QVGA size, however for different Camera Modes
* The picture and PIP sizes are different.
*
* image_raw and buffer are always 320x240. In order to preserve backward compatibilty image_raw remains
* always as before. Two new set of topics are added for two new cameras : /ardrone/front/xxx and /ardrone/bottom/xxx
*
* In Camera State 0 front image relays the buffer and image_raw and bottom image are not updated.
*
* In Camera State 1 bottom image is a 174x144 crop of the buffer. The front image is not updated
*
* In Camera State 2 bottom image is a PIP cut of size (87x72) from buffer.
* The bottom image is a (320-87)x(240) cut of the buffer.
*
* In Camera State 3 front image is a PIP cut of size (58x42) from buffer.
* The bottom image is a (174-58)x144 crop of the buffer.
*/
sensor_msgs::Image image_msg;
sensor_msgs::CameraInfo cinfo_msg;
sensor_msgs::Image::_data_type::iterator _it;
if (IS_ARDRONE1)
{
/*
* Information on buffer and image sizes.
* Buffer is always in QVGA size, however for different Camera Modes
* The picture and PIP sizes are different.
*
* image_raw and buffer are always 320x240. In order to preserve backward compatibilty image_raw remains
* always as before. Two new set of topics are added for two new cameras : /ardrone/front/xxx and /ardrone/bottom/xxx
*
* In Camera State 0 front image relays the buffer and image_raw and bottom image are not updated.
*
* In Camera State 1 bottom image is a 174x144 crop of the buffer. The front image is not updated
*
* In Camera State 2 bottom image is a PIP cut of size (87x72) from buffer.
* The bottom image is a (320-87)x(240) cut of the buffer.
*
* In Camera State 3 front image is a PIP cut of size (58x42) from buffer.
* The bottom image is a (174-58)x144 crop of the buffer.
*/
sensor_msgs::Image image_msg;
sensor_msgs::CameraInfo cinfo_msg;
sensor_msgs::Image::_data_type::iterator _it;
image_msg.width = STREAM_WIDTH;
image_msg.height = STREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = STREAM_WIDTH*3;
image_msg.data.resize(STREAM_WIDTH*STREAM_HEIGHT*3);
std::copy(buffer, buffer+(STREAM_WIDTH*STREAM_HEIGHT*3), image_msg.data.begin());
image_msg.width = D1_STREAM_WIDTH;
image_msg.height = D1_STREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = D1_STREAM_WIDTH*3;
image_msg.data.resize(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3);
std::copy(buffer, buffer+(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3), image_msg.data.begin());
// We only put the width and height in here.
cinfo_msg.width = STREAM_WIDTH;
cinfo_msg.height = STREAM_HEIGHT;
image_pub.publish(image_msg, cinfo_msg);
if (cam_state == ZAP_CHANNEL_HORI)
{
/*
* Horizontal camera is activated, only /ardrone/front/ is being updated
*/
hori_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_VERT)
{
/*
* Vertical camera is activated, only /ardrone/bottom/ is being updated
*/
image_msg.width = VERTSTREAM_WIDTH;
image_msg.height = VERTSTREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = VERTSTREAM_WIDTH*3;
image_msg.data.clear();
image_msg.data.resize(VERTSTREAM_WIDTH*VERTSTREAM_HEIGHT*3);
_it = image_msg.data.begin();
for (int row = 0; row < VERTSTREAM_HEIGHT ; row++)
{
int _b = row * STREAM_WIDTH * 3;
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = VERTSTREAM_WIDTH;
cinfo_msg.height = VERTSTREAM_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT)
{
/*
* The Picture in Picture is activated with vertical camera inside the horizontal
* camera. Both /ardrone/front and /ardrone/bottom is being updated
*/
// Front (Cropping the first 88 columns)
image_msg.width = STREAM_WIDTH - MODE2_PIP_WIDTH;
image_msg.height = STREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = (STREAM_WIDTH-MODE2_PIP_WIDTH)*3;
image_msg.data.clear();
image_msg.data.resize((STREAM_WIDTH - MODE2_PIP_WIDTH)*STREAM_HEIGHT*3);
_it = image_msg.data.begin();
for (int row = 0; row < STREAM_HEIGHT; row++)
{
int _b = (row * STREAM_WIDTH * 3) + (MODE2_PIP_WIDTH * 3);
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = STREAM_WIDTH - MODE2_PIP_WIDTH;
cinfo_msg.height = STREAM_HEIGHT;
hori_pub.publish(image_msg, cinfo_msg);
//Bottom
image_msg.width = MODE2_PIP_WIDTH;
image_msg.height = MODE2_PIP_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = MODE2_PIP_WIDTH * 3;
image_msg.data.clear();
image_msg.data.resize(MODE2_PIP_WIDTH * MODE2_PIP_HEIGHT * 3);
_it = image_msg.data.begin();
for (int row = 0; row < MODE2_PIP_HEIGHT; row++)
{
int _b = row * STREAM_WIDTH * 3;
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = MODE2_PIP_WIDTH;
cinfo_msg.height = MODE2_PIP_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI)
{
/*
* The Picture in Picture is activated with horizontal camera inside the vertical
* camera. Both /ardrone/front and /ardrone/bottom is being updated
*/
// Bottom (Cropping the first 58 columns)
image_msg.width = VERTSTREAM_WIDTH - MODE3_PIP_WIDTH;
image_msg.height = VERTSTREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = (VERTSTREAM_WIDTH - MODE3_PIP_WIDTH)*3;
image_msg.data.clear();
image_msg.data.resize((VERTSTREAM_WIDTH - MODE3_PIP_WIDTH)* VERTSTREAM_HEIGHT*3);
_it = image_msg.data.begin();
for (int row = 0; row < VERTSTREAM_HEIGHT; row++)
{
int _b = (row * (STREAM_WIDTH * 3)) + (MODE3_PIP_WIDTH * 3);
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = VERTSTREAM_WIDTH - MODE3_PIP_WIDTH;
cinfo_msg.height = VERTSTREAM_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
//Front
image_msg.width = MODE3_PIP_WIDTH;
image_msg.height = MODE3_PIP_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = MODE3_PIP_WIDTH * 3;
image_msg.data.clear();
image_msg.data.resize(MODE3_PIP_WIDTH * MODE3_PIP_HEIGHT * 3);
_it = image_msg.data.begin();
for (int row = 0; row < MODE3_PIP_HEIGHT; row++)
{
int _b = row * STREAM_WIDTH * 3;
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = MODE3_PIP_WIDTH;
cinfo_msg.height = MODE3_PIP_HEIGHT;
hori_pub.publish(image_msg, cinfo_msg);
}
// We only put the width and height in here.
cinfo_msg.width = D1_STREAM_WIDTH;
cinfo_msg.height = D1_STREAM_HEIGHT;
image_pub.publish(image_msg, cinfo_msg);
if (cam_state == ZAP_CHANNEL_HORI)
{
/*
* Horizontal camera is activated, only /ardrone/front/ is being updated
*/
hori_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_VERT)
{
/*
* Vertical camera is activated, only /ardrone/bottom/ is being updated
*/
image_msg.width = D1_VERTSTREAM_WIDTH;
image_msg.height = D1_VERTSTREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = D1_VERTSTREAM_WIDTH*3;
image_msg.data.clear();
image_msg.data.resize(D1_VERTSTREAM_WIDTH*D1_VERTSTREAM_HEIGHT*3);
_it = image_msg.data.begin();
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);
}
cinfo_msg.width = D1_VERTSTREAM_WIDTH;
cinfo_msg.height = D1_VERTSTREAM_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT)
{
/*
* The Picture in Picture is activated with vertical camera inside the horizontal
* camera. Both /ardrone/front and /ardrone/bottom is being updated
*/
// Front (Cropping the first 88 columns)
image_msg.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
image_msg.height = D1_STREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = (D1_STREAM_WIDTH-D1_MODE2_PIP_WIDTH)*3;
image_msg.data.clear();
image_msg.data.resize((D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH)*D1_STREAM_HEIGHT*3);
_it = image_msg.data.begin();
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);
}
cinfo_msg.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
cinfo_msg.height = D1_STREAM_HEIGHT;
hori_pub.publish(image_msg, cinfo_msg);
//Bottom
image_msg.width = D1_MODE2_PIP_WIDTH;
image_msg.height = D1_MODE2_PIP_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = D1_MODE2_PIP_WIDTH * 3;
image_msg.data.clear();
image_msg.data.resize(D1_MODE2_PIP_WIDTH * D1_MODE2_PIP_HEIGHT * 3);
_it = image_msg.data.begin();
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);
}
cinfo_msg.width = D1_MODE2_PIP_WIDTH;
cinfo_msg.height = D1_MODE2_PIP_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI)
{
/*
* The Picture in Picture is activated with horizontal camera inside the vertical
* camera. Both /ardrone/front and /ardrone/bottom is being updated
*/
// Bottom (Cropping the first 58 columns)
image_msg.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
image_msg.height = D1_VERTSTREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = (D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH)*3;
image_msg.data.clear();
image_msg.data.resize((D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH)* D1_VERTSTREAM_HEIGHT*3);
_it = image_msg.data.begin();
for (int row = 0; row < D1_VERTSTREAM_HEIGHT; row++)
{
int _b = (row * (D1_STREAM_WIDTH * 3)) + (D1_MODE3_PIP_WIDTH * 3);
int _e = _b + image_msg.step;
_it = std::copy(buffer + _b, buffer + _e, _it);
}
cinfo_msg.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
cinfo_msg.height = D1_VERTSTREAM_HEIGHT;
vert_pub.publish(image_msg, cinfo_msg);
//Front
image_msg.width = D1_MODE3_PIP_WIDTH;
image_msg.height = D1_MODE3_PIP_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = D1_MODE3_PIP_WIDTH * 3;
image_msg.data.clear();
image_msg.data.resize(D1_MODE3_PIP_WIDTH * D1_MODE3_PIP_HEIGHT * 3);
_it = image_msg.data.begin();
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);
}
cinfo_msg.width = D1_MODE3_PIP_WIDTH;
cinfo_msg.height = D1_MODE3_PIP_HEIGHT;
hori_pub.publish(image_msg, cinfo_msg);
}
}
/**
* For Drone 2 w/ SDK2. Both camera streams are 360p.
* No 720p support for now.
* SDK 2.0 Does not support PIP.
*/
if (IS_ARDRONE2)
{
sensor_msgs::Image image_msg;
sensor_msgs::CameraInfo cinfo_msg;
sensor_msgs::Image::_data_type::iterator _it;
image_msg.width = D2_STREAM_WIDTH;
image_msg.height = D2_STREAM_HEIGHT;
image_msg.encoding = "rgb8";
image_msg.is_bigendian = false;
image_msg.step = D2_STREAM_WIDTH*3;
image_msg.data.resize(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3);
std::copy(buffer, buffer+(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3), image_msg.data.begin());
// We only put the width and height in here.
cinfo_msg.width = D2_STREAM_WIDTH;
cinfo_msg.height = D2_STREAM_HEIGHT;
image_pub.publish(image_msg, cinfo_msg); // /ardrone
if (cam_state == ZAP_CHANNEL_HORI)
{
/*
* Horizontal camera is activated, only /ardrone/front/ is being updated
*/
hori_pub.publish(image_msg, cinfo_msg);
}
else if (cam_state == ZAP_CHANNEL_VERT)
{
/*
* Vertical camera is activated, only /ardrone/bottom/ is being updated
*/
vert_pub.publish(image_msg, cinfo_msg);
}
}
}
src/ardrone_sdk.cpp
+19 -9
Ver Arquivo
@@ -17,14 +17,26 @@ extern "C" {
// }
C_RESULT ardrone_tool_init_custom(void) {
int _w, _h;
if (IS_ARDRONE2)
{
ardrone_application_default_config.video_codec = H264_360P_CODEC;
_w = D2_STREAM_WIDTH;
_h = D2_STREAM_HEIGHT;
}
else if (IS_ARDRONE1)
{
ardrone_application_default_config.video_codec = UVLC_CODEC;
_w = D1_STREAM_WIDTH;
_h = D1_STREAM_HEIGHT;
}
else
{
ardrone_application_default_config.video_codec = UVLC_CODEC;
printf("Something must be really wrong with the SDK!");
}
ardrone_application_default_config.bitrate_ctrl_mode = 1;
ardrone_tool_input_add(&teleop);
uint8_t post_stages_index = 0;
@@ -36,19 +48,19 @@ extern "C" {
vp_api_picture_t * in_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
vp_api_picture_t * out_picture = (vp_api_picture_t*) vp_os_calloc(1, sizeof(vp_api_picture_t));
in_picture->width = STREAM_WIDTH;
in_picture->height = STREAM_HEIGHT;
in_picture->width = _w;
in_picture->height = _h;
out_picture->framerate = 30;
out_picture->format = PIX_FMT_RGB24;
out_picture->width = STREAM_WIDTH;
out_picture->height = STREAM_HEIGHT;
out_picture->width = _w;
out_picture->height = _h;
out_picture->y_buf = (uint8_t*) vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT * 3 );
out_picture->y_buf = (uint8_t*) vp_os_malloc( _w * _h * 3 );
out_picture->cr_buf = NULL;
out_picture->cb_buf = NULL;
out_picture->y_line_size = STREAM_WIDTH * 3;
out_picture->y_line_size = _w * 3;
out_picture->cb_line_size = 0;
out_picture->cr_line_size = 0;
@@ -98,8 +110,6 @@ extern "C" {
BEGIN_THREAD_TABLE
THREAD_TABLE_ENTRY(video_stage, 20)
//THREAD_TABLE_ENTRY(video_update_thread, 20)
//THREAD_TABLE_ENTRY(mani, 20)
THREAD_TABLE_ENTRY(navdata_update, 20)
THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 20)
THREAD_TABLE_ENTRY(ardrone_control, 20)
src/ardrone_sdk.h
-15
Ver Arquivo
@@ -19,21 +19,6 @@
#include <stdint.h>
extern "C" {
//#include <VP_Api/vp_api.h>
//#include <VP_Api/vp_api_error.h>
//#include <VP_Api/vp_api_stage.h>
//#include <VP_Api/vp_api_picture.h>
//#include <VP_Stages/vp_stages_io_file.h>
//#include <VP_Stages/vp_stages_i_camif.h>
//
//#include <VP_Os/vp_os_print.h>
//#include <VP_Os/vp_os_malloc.h>
//#include <VP_Os/vp_os_delay.h>
//#include <VP_Stages/vp_stages_yuv2rgb.h>
//#include <VP_Stages/vp_stages_buffer_to_picture.h>
//#include <VLIB/Stages/vlib_stage_decode.h>
#include <config.h>
src/teleop_twist.cpp
+3 -14
Ver Arquivo
@@ -30,30 +30,19 @@ bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empt
bool setCamChannelCallback(ardrone_brown::CamSelect::Request& request, ardrone_brown::CamSelect::Response& response)
{
cam_state = request.channel;
#ifdef _USING_SDK_1_7_
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
#else
ardrone_at_set_toy_configuration("video:video_channel",cam_state);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
#endif
response.result = true;
return true;
}
// ros service callback function for toggling Cam
bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
#ifdef _USING_SDK_1_7_
cam_state = (cam_state + 1) % 4;
int _modes = (IS_ARDRONE1) ? 4 : 2;
cam_state = (cam_state + 1) % _modes;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
#else
cam_state = (cam_state + 1) % 2;
ardrone_at_set_toy_configuration("video:video_channel",cam_state);
fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
#endif
return true;
return true;
}
src/teleop_twist.h
-2
Ver Arquivo
@@ -7,8 +7,6 @@
#include <std_srvs/Empty.h>
#include <ardrone_brown/CamSelect.h>
#define _USING_SDK_1_7_
extern input_device_t teleop;
void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg);
src/video.cpp
+6 -123
Ver Arquivo
@@ -3,11 +3,8 @@
#define NB_STAGES 10
#define CAMIF_H_CAMERA_USED CAMIF_CAMERA_OVTRULY
unsigned char buffer[STREAM_WIDTH * STREAM_HEIGHT * 3];
int current_frame_id = 0;
static uint8_t* pixbuf_data = NULL;
static vp_os_mutex_t video_update_lock = PTHREAD_MUTEX_INITIALIZER;
PIPELINE_HANDLE pipeline_handle;
unsigned char buffer[MAX_STREAM_WIDTH * MAX_STREAM_HEIGHT * 3];
long int current_frame_id = 0;
extern "C" C_RESULT export_stage_open( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
@@ -17,16 +14,16 @@ extern "C" C_RESULT export_stage_open( void *cfg, vp_api_io_data_t *in, vp_api_i
extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
// PRINT("In Transform before copy\n");
// printf("The size of buffer is %d\n", in->size);
printf("The size of buffer is %d\n", in->size);
// vp_os_mutex_lock(&video_update_lock);
// /* Get a reference to the last decoded picture */
// pixbuf_data = (uint8_t*)in->buffers[0];
// /* Copy the entire buffer, TODO: can we movet this to the thread to make the transform faster?*/
memcpy(buffer, in->buffers[0], STREAM_WIDTH * STREAM_HEIGHT * 3);
memcpy(buffer, in->buffers[0], in->size);
// //memcpy(buffer, in->buffers[0], in->size);
// vp_os_mutex_unlock(&video_update_lock);
// PRINT("In Transform after copy\n");
// current_frame_id++;
current_frame_id++;
return (SUCCESS);
}
@@ -41,118 +38,4 @@ const vp_api_stage_funcs_t vp_stages_export_funcs =
(vp_api_stage_open_t)export_stage_open,
(vp_api_stage_transform_t)export_stage_transform,
(vp_api_stage_close_t)export_stage_close
};
DEFINE_THREAD_ROUTINE(video_update_thread, data)
{
PRINT("***** Starting video capture thread ...\n");
// C_RESULT res;
//
// vp_api_io_pipeline_t pipeline;
// vp_api_io_data_t out;
// vp_api_io_stage_t stages[NB_STAGES];
//
// vp_api_picture_t picture;
//
// video_com_config_t icc;
// vlib_stage_decoding_config_t vec;
// vp_stages_yuv2rgb_config_t yuv2rgbconf;
//
// /// Picture configuration
// picture.format = PIX_FMT_YUV420P;
// picture.width = STREAM_WIDTH;
// picture.height = STREAM_HEIGHT;
// picture.framerate = 30;
//
// picture.y_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT );
// picture.cr_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT / 4 );
// picture.cb_buf = (uint8_t *)vp_os_malloc( STREAM_WIDTH * STREAM_HEIGHT / 4 );
//
// picture.y_line_size = STREAM_WIDTH;
// picture.cb_line_size = STREAM_WIDTH / 2;
// picture.cr_line_size = STREAM_WIDTH / 2;
//
// vp_os_memset(&icc, 0, sizeof( icc ));
// vp_os_memset(&vec, 0, sizeof( vec ));
// vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf ));
//
// PRINT("***** Before COM ...\n");
// icc.com = COM_VIDEO();
// icc.buffer_size = (1024*1024);
// icc.protocol = VP_COM_UDP;
// COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
//
// vec.width = STREAM_WIDTH;
// vec.height = STREAM_HEIGHT;
// vec.picture = &picture;
// vec.luma_only = FALSE;
// vec.block_mode_enable = TRUE;
//
// yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24;
//// if( CAMIF_H_CAMERA_USED == CAMIF_CAMERA_OVTRULY_UPSIDE_DOWN_ONE_BLOCKLINE_LESS )
//// yuv2rgbconf.mode = VP_STAGES_YUV2RGB_MODE_UPSIDE_DOWN;
//
// pipeline.nb_stages = 0;
//
// stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
// stages[pipeline.nb_stages].cfg = (void *)&icc;
// stages[pipeline.nb_stages].funcs = video_com_funcs;
//
// pipeline.nb_stages++;
//
// stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
// stages[pipeline.nb_stages].cfg = (void*)&vec;
// stages[pipeline.nb_stages].funcs = vlib_decoding_funcs;
//
// pipeline.nb_stages++;
//
// stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB;
// stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf;
// stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs;
//
// pipeline.nb_stages++;
//
// stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL;
// stages[pipeline.nb_stages].cfg = NULL;//(void *)&vec;
// stages[pipeline.nb_stages].funcs = vp_stages_export_funcs;
//
// pipeline.nb_stages++;
//
// pipeline.stages = &stages[0];
// //res = vp_api_open(&pipeline, &pipeline_handle);
//
// PRINT("***** Before Main Block ...\n");
// if( !ardrone_tool_exit() )
// {
// PRINT("\n Video stage thread initialisation\n\n");
//
// res = vp_api_open(&pipeline, &pipeline_handle);
// PRINT("***** API OPEN ... %d \n", res);
// if (SUCCEED(res)) {
// PRINT("API OPEN Successfull \n");
// int loop = SUCCESS;
// out.status = VP_API_STATUS_PROCESSING;
// while (!ardrone_tool_exit() && (loop == SUCCESS)) {
// if (SUCCEED(vp_api_run(&pipeline, &out))) {
// if ((out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING)) {
// loop = SUCCESS;
// }
// } else loop = -1; // Finish this thread
// }
//
// vp_api_close(&pipeline, &pipeline_handle);
// }
// }
//
// PRINT(" Video stage thread ended\n\n");
// return (THREAD_RET)0;
// if( SUCCEED(res) ) {
// while( SUCCEED(vp_api_run(&pipeline, &out)) ) {
// }
//
// vp_api_close(&pipeline, &pipeline_handle);
// }
//
return (THREAD_RET)0;
}
};
src/video.h
+23 -12
Ver Arquivo
@@ -3,28 +3,39 @@
#include "ardrone_sdk.h"
#define STREAM_WIDTH 640
#define STREAM_HEIGHT 360
// The maximum memory allocation
#define MAX_STREAM_WIDTH 1280//640
#define MAX_STREAM_HEIGHT 720//360
/** Drone 1 */
// Drone 1 Static Stream Size & PIP Stuff
// PIP is not supported in AR-Drone SDK 2.0
#define D1_STREAM_WIDTH 320
#define D1_STREAM_HEIGHT 240
//Vertical Camera standalone
#define VERTSTREAM_WIDTH 174
#define VERTSTREAM_HEIGHT 144
#define D1_VERTSTREAM_WIDTH 174
#define D1_VERTSTREAM_HEIGHT 144
// Vertical Camera in PIP
#define MODE2_PIP_WIDTH 87 //Huh?
#define MODE2_PIP_HEIGHT 72
#define D1_MODE2_PIP_WIDTH 87 //Huh?
#define D1_MODE2_PIP_HEIGHT 72
//Horizontal Camera in PIP
#define MODE3_PIP_WIDTH 58
#define MODE3_PIP_HEIGHT 42
#define D1_MODE3_PIP_WIDTH 58
#define D1_MODE3_PIP_HEIGHT 42
/** Drone 2 */
// NO PIP, Both camera streams provide the same reseloution: Simple!
#define D2_STREAM_WIDTH 640
#define D2_STREAM_HEIGHT 360
extern video_com_multisocket_config_t icc;
//extern parrot_video_encapsulation_codecs_t video_stage_decoder_lastDetectedCodec;
extern const vp_api_stage_funcs_t vp_stages_export_funcs;
extern unsigned char buffer[]; // size STREAM_WIDTH * STREAM_HEIGHT * 3
extern int current_frame_id; // this will be incremented for every frame
PROTO_THREAD_ROUTINE(video_update_thread, data);
extern long int current_frame_id; // this will be incremented for every frame
#endif