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

Added fullspeed_video

Ver Código-Fonte 
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...
Esse commit está contido em:
Mike Hamer
2012-11-28 12:53:25 +01:00
pai 7fdf268969
commit 9297e938c7
7 arquivos alterados com 37 adições e 31 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
src/ardrone_driver.cpp
+25 -30
Ver Arquivo
@@ -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]);
src/ardrone_driver.h
+2 -1
Ver Arquivo
@@ -1,6 +1,7 @@
#ifndef _ARDRONE_DRIVER_H_
#define _ARDRONE_DRIVER_H_
class ARDroneDriver;
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <tf/transform_broadcaster.h>
@@ -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<double, 9> &cov_array);
src/ardrone_sdk.cpp
+2
Ver Arquivo
@@ -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)
src/ardrone_sdk.h
+2
Ver Arquivo
@@ -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;
src/video.cpp
+4
Ver Arquivo
@@ -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);
src/video.h
+1
Ver Arquivo
@@ -2,6 +2,7 @@
#define _VIDEO_H_
#include "ardrone_sdk.h"
#include "ardrone_driver.h"
// The maximum memory allocation
#define MAX_STREAM_WIDTH 640