Navdata & Video handling callbacks from SDK are now being protected by
locks. Frequencies are now synchronized with actual data receive rate.
Esse commit está contido em:
Mani Monajjemi
@@ -12,6 +12,8 @@ ARDroneDriver::ARDroneDriver()
|
||||
: image_transport(node_handle)
|
||||
{
|
||||
inited = false;
|
||||
last_frame_id = -1;
|
||||
last_navdata_id = -1;
|
||||
cmd_vel_sub = node_handle.subscribe("cmd_vel", 100, &cmdVelCallback);
|
||||
takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
|
||||
reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
|
||||
@@ -124,12 +126,13 @@ void ARDroneDriver::run()
|
||||
if (((ros::Time::now() - startTime).toSec()) > 5.0)
|
||||
{
|
||||
inited = true;
|
||||
vp_os_mutex_lock(&navdata_lock);
|
||||
ROS_INFO("Successfully connected to '%s' (AR-Drone %d.0 - Firmware: %s) - Battery(\%): %d",
|
||||
ardrone_control_config.ardrone_name,
|
||||
(IS_ARDRONE1) ? 1 : 2,
|
||||
ardrone_control_config.num_version_soft,
|
||||
navdata.vbat_flying_percentage);
|
||||
|
||||
shared_navdata.vbat_flying_percentage);
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
if (ardrone_control_config.num_version_soft[0] == '0')
|
||||
{
|
||||
ROS_WARN("The AR-Drone has a suspicious Firmware number. It usually means the network link is unreliable.");
|
||||
@@ -141,9 +144,14 @@ void ARDroneDriver::run()
|
||||
publish_video();
|
||||
last_frame_id = current_frame_id;
|
||||
}
|
||||
if (current_navdata_id != last_navdata_id)
|
||||
{
|
||||
publish_navdata();
|
||||
last_navdata_id = current_navdata_id;
|
||||
}
|
||||
if (freq_dev == 0) publish_tf();
|
||||
publish_navdata();
|
||||
freq_dev = (freq_dev + 1) % 3; // ~10Hz TF publish
|
||||
|
||||
freq_dev = (freq_dev + 1) % 5; // ~10Hz TF publish
|
||||
}
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
@@ -266,7 +274,10 @@ void ARDroneDriver::publish_video()
|
||||
image_msg.is_bigendian = false;
|
||||
image_msg.step = D1_STREAM_WIDTH*3;
|
||||
image_msg.data.resize(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3);
|
||||
|
||||
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 (cam_state == ZAP_CHANNEL_HORI)
|
||||
{
|
||||
@@ -292,12 +303,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);
|
||||
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);
|
||||
|
||||
cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH;
|
||||
cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
|
||||
@@ -320,12 +333,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);
|
||||
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);
|
||||
|
||||
cinfo_msg_hori.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
|
||||
cinfo_msg_hori.height = D1_STREAM_HEIGHT;
|
||||
@@ -340,12 +355,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);
|
||||
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);
|
||||
|
||||
cinfo_msg_vert.width = D1_MODE2_PIP_WIDTH;
|
||||
cinfo_msg_vert.height = D1_MODE2_PIP_HEIGHT;
|
||||
@@ -371,7 +388,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);
|
||||
_it = std::copy(buffer + _b, buffer + _e, _it);
|
||||
vp_os_mutex_unlock(&video_lock);
|
||||
}
|
||||
|
||||
cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
|
||||
@@ -387,12 +406,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);
|
||||
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);
|
||||
|
||||
cinfo_msg_hori.width = D1_MODE3_PIP_WIDTH;
|
||||
cinfo_msg_hori.height = D1_MODE3_PIP_HEIGHT;
|
||||
@@ -433,8 +453,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);
|
||||
std::copy(buffer, buffer+(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3), image_msg.data.begin());
|
||||
|
||||
vp_os_mutex_unlock(&video_lock);
|
||||
// We only put the width and height in here.
|
||||
|
||||
|
||||
@@ -464,6 +485,21 @@ void ARDroneDriver::publish_video()
|
||||
|
||||
void ARDroneDriver::publish_navdata()
|
||||
{
|
||||
// Thread safe copy of interesting Navdata data
|
||||
vp_os_mutex_lock(&navdata_lock);
|
||||
navdata_detect = shared_navdata_detect;
|
||||
navdata_phys = shared_navdata_phys;
|
||||
navdata = shared_navdata;
|
||||
arnavtime = shared_arnavtime;
|
||||
if (IS_ARDRONE2)
|
||||
{ // This is neccessary
|
||||
navdata_pressure = shared_navdata_pressure;
|
||||
navdata_magneto = shared_navdata_magneto;
|
||||
navdata_wind = shared_navdata_wind;
|
||||
}
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
|
||||
|
||||
if (!caliberated)
|
||||
{
|
||||
acc_samples[0].push_back(navdata_phys.phys_accs[ACC_X]);
|
||||
|
||||
+10
-1
@@ -74,12 +74,21 @@ private:
|
||||
*/
|
||||
//ros::ServiceServer setHullType_service;
|
||||
|
||||
int last_frame_id;
|
||||
long int last_frame_id;
|
||||
long int last_navdata_id;
|
||||
int flying_state;
|
||||
|
||||
bool inited;
|
||||
std::string droneFrameId;
|
||||
|
||||
// Navdata copy
|
||||
navdata_demo_t navdata;
|
||||
navdata_phys_measures_t navdata_phys;
|
||||
navdata_vision_detect_t navdata_detect;
|
||||
navdata_pressure_raw_t navdata_pressure;
|
||||
navdata_magneto_t navdata_magneto;
|
||||
navdata_wind_speed_t navdata_wind;
|
||||
navdata_time_t arnavtime;
|
||||
|
||||
/*
|
||||
* TF Frames
|
||||
|
||||
+24
-15
@@ -3,15 +3,18 @@
|
||||
#include "teleop_twist.h"
|
||||
#include "ardrone_driver.h"
|
||||
|
||||
navdata_demo_t navdata;
|
||||
navdata_phys_measures_t navdata_phys;
|
||||
navdata_vision_detect_t navdata_detect;
|
||||
navdata_demo_t shared_navdata;
|
||||
navdata_phys_measures_t shared_navdata_phys;
|
||||
navdata_vision_detect_t shared_navdata_detect;
|
||||
navdata_pressure_raw_t shared_navdata_pressure;
|
||||
navdata_magneto_t shared_navdata_magneto;
|
||||
navdata_wind_speed_t shared_navdata_wind;
|
||||
navdata_time_t shared_arnavtime;
|
||||
|
||||
navdata_pressure_raw_t navdata_pressure;
|
||||
navdata_magneto_t navdata_magneto;
|
||||
navdata_wind_speed_t navdata_wind;
|
||||
vp_os_mutex_t navdata_lock;
|
||||
vp_os_mutex_t video_lock;
|
||||
|
||||
navdata_time_t arnavtime;
|
||||
long int current_navdata_id = 0;
|
||||
|
||||
ARDroneDriver* rosDriver;
|
||||
|
||||
@@ -32,6 +35,9 @@ extern "C" {
|
||||
C_RESULT ardrone_tool_init_custom(void)
|
||||
{
|
||||
should_exit = 0;
|
||||
vp_os_mutex_init(&navdata_lock);
|
||||
vp_os_mutex_init(&video_lock);
|
||||
|
||||
rosDriver = new ARDroneDriver();
|
||||
int _w, _h;
|
||||
|
||||
@@ -180,17 +186,20 @@ extern "C" {
|
||||
return C_OK;
|
||||
}
|
||||
|
||||
C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
|
||||
navdata_detect = pnd->navdata_vision_detect;
|
||||
navdata_phys = pnd->navdata_phys_measures;
|
||||
navdata = pnd->navdata_demo;
|
||||
arnavtime = pnd->navdata_time;
|
||||
C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
|
||||
vp_os_mutex_lock(&navdata_lock);
|
||||
shared_navdata_detect = pnd->navdata_vision_detect;
|
||||
shared_navdata_phys = pnd->navdata_phys_measures;
|
||||
shared_navdata = pnd->navdata_demo;
|
||||
shared_arnavtime = pnd->navdata_time;
|
||||
if (IS_ARDRONE2)
|
||||
{ // This is neccessary
|
||||
navdata_pressure = pnd->navdata_pressure_raw;
|
||||
navdata_magneto = pnd->navdata_magneto;
|
||||
navdata_wind = pnd->navdata_wind_speed;
|
||||
shared_navdata_pressure = pnd->navdata_pressure_raw;
|
||||
shared_navdata_magneto = pnd->navdata_magneto;
|
||||
shared_navdata_wind = pnd->navdata_wind_speed;
|
||||
}
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
current_navdata_id++;
|
||||
return C_OK;
|
||||
}
|
||||
|
||||
|
||||
+9
-7
@@ -39,14 +39,16 @@ extern video_decoder_config_t vec;
|
||||
|
||||
#define NB_DRIVER_POST_STAGES 10
|
||||
|
||||
extern navdata_vision_detect_t navdata_detect;
|
||||
extern navdata_phys_measures_t navdata_phys;
|
||||
extern navdata_demo_t navdata;
|
||||
extern navdata_time_t arnavtime;
|
||||
extern navdata_vision_detect_t shared_navdata_detect;
|
||||
extern navdata_phys_measures_t shared_navdata_phys;
|
||||
extern navdata_demo_t shared_navdata;
|
||||
extern navdata_time_t shared_arnavtime;
|
||||
extern navdata_pressure_raw_t shared_navdata_pressure;
|
||||
extern navdata_magneto_t shared_navdata_magneto;
|
||||
extern navdata_wind_speed_t shared_navdata_wind;
|
||||
|
||||
extern navdata_pressure_raw_t navdata_pressure;
|
||||
extern navdata_magneto_t navdata_magneto;
|
||||
extern navdata_wind_speed_t navdata_wind;
|
||||
extern vp_os_mutex_t navdata_lock;
|
||||
extern vp_os_mutex_t video_lock;
|
||||
|
||||
extern int32_t should_exit;
|
||||
|
||||
|
||||
+3
-1
@@ -15,7 +15,9 @@ extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_
|
||||
{
|
||||
// PRINT("In Transform before copy\n");
|
||||
// printf("The size of buffer is %d\n", in->size);
|
||||
vp_os_mutex_lock(&video_lock);
|
||||
memcpy(buffer, in->buffers[0], in->size);
|
||||
vp_os_mutex_unlock(&video_lock);
|
||||
// vp_os_mutex_unlock(&video_update_lock);
|
||||
current_frame_id++;
|
||||
return (SUCCESS);
|
||||
@@ -32,4 +34,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
|
||||
};
|
||||
};
|
||||
|
||||
@@ -36,6 +36,7 @@ extern video_com_multisocket_config_t icc;
|
||||
extern const vp_api_stage_funcs_t vp_stages_export_funcs;
|
||||
extern unsigned char buffer[]; // size STREAM_WIDTH * STREAM_HEIGHT * 3
|
||||
extern long int current_frame_id; // this will be incremented for every frame
|
||||
extern long int current_navdata_id;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
Referência em uma Nova Issue
Bloquear um usuário