Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Integrated ardrone cameras with camera stack. It's all over the place…

…... Sorry for the big commit.
  • Loading branch information...
commit 709dfa7f7d43fda45bc5bb109ef6ab3daa073723 1 parent 4b35a6d
@bmcdorman bmcdorman authored
View
2  CMakeLists.txt
@@ -35,7 +35,7 @@ ELSEIF(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
ENDIF()
-add_definitions(-O3)
+# add_definitions(-O2)
ADD_LIBRARY(kovan SHARED ${SOURCES})
IF(NOT WIN32)
View
36 include/kovan/ardrone.hpp
@@ -22,10 +22,11 @@
#define _ARDRONE_HPP_
#include <opencv2/core/core.hpp>
+#include "kovan/camera.hpp"
class DroneController;
-class ARDrone
+class EXPORT_SYM ARDrone
{
public:
enum State
@@ -35,11 +36,21 @@ class ARDrone
Flying
};
+ enum Camera
+ {
+ None = 0,
+ Front,
+ Bottom
+ };
+
~ARDrone();
- bool connect(const char *const ip = "192.168.1.1");
+ bool connect(const char *const ip = "192.168.1.1", const double timeout = 2.0);
void disconnect();
+ void setActiveCamera(const Camera activeCamera);
+ Camera activeCamera() const;
+
void flatTrim();
void takeoff();
void land();
@@ -47,7 +58,7 @@ class ARDrone
void hover();
void move(const float x, const float y, const float z, const float yaw);
- cv::Mat video() const;
+ const cv::Mat &rawImage() const;
ARDrone::State state() const;
@@ -57,8 +68,25 @@ class ARDrone
ARDrone();
DroneController *m_controller;
+ Camera m_activeCamera;
};
-
+namespace Camera
+{
+ class EXPORT_SYM ARDroneInputProvider : public InputProvider
+ {
+ public:
+ ARDroneInputProvider();
+ virtual bool open(const int number);
+ virtual bool isOpen() const;
+ virtual void setWidth(const unsigned width);
+ virtual void setHeight(const unsigned height);
+ virtual bool next(cv::Mat &image);
+ virtual bool close();
+
+ private:
+ bool m_opened;
+ };
+}
#endif
View
4 include/kovan/camera.h
@@ -63,10 +63,6 @@ EXPORT_SYM int camera_open(enum Resolution res);
*/
EXPORT_SYM int camera_open_device(int number);
-EXPORT_SYM void set_camera_grab_count(int grabs);
-
-EXPORT_SYM int get_camera_grab_count();
-
/**
* Loads the config file specified by name.
* \param name The configuration to load. Configuration file names are case sensitive.
View
46 include/kovan/camera.hpp
@@ -160,25 +160,52 @@ namespace Camera
static std::string s_path;
};
+ class EXPORT_SYM InputProvider
+ {
+ public:
+ virtual ~InputProvider();
+ virtual bool open(const int number) = 0;
+ virtual bool isOpen() const = 0;
+ virtual void setWidth(const unsigned width) = 0;
+ virtual void setHeight(const unsigned height) = 0;
+ virtual bool next(cv::Mat &image) = 0;
+ virtual bool close() = 0;
+ };
+
+ class EXPORT_SYM UsbInputProvider : public InputProvider
+ {
+ public:
+ UsbInputProvider();
+ ~UsbInputProvider();
+
+ virtual bool open(const int number);
+ virtual bool isOpen() const;
+ virtual void setWidth(const unsigned width);
+ virtual void setHeight(const unsigned height);
+ virtual bool next(cv::Mat &image);
+ virtual bool close();
+
+ private:
+ cv::VideoCapture *m_capture;
+ };
+
class EXPORT_SYM Device
{
public:
- Device();
+ Device(InputProvider *const inputProvider);
~Device();
- bool open(const int &number = 0);
+ bool open(const int number = 0);
bool isOpen() const;
- void close();
+ bool close();
bool update();
- void setWidth(const unsigned &width);
- void setHeight(const unsigned &height);
- void setGrabCount(unsigned char grabs);
- unsigned char grabCount() const;
+ void setWidth(const unsigned width);
+ void setHeight(const unsigned height);
const ChannelPtrVector &channels() const;
- cv::VideoCapture *videoCapture() const;
+ InputProvider *inputProvider() const;
const cv::Mat &rawImage() const;
void setConfig(const Config &config);
@@ -190,11 +217,10 @@ namespace Camera
private:
void updateConfig();
+ InputProvider *const m_inputProvider;
Config m_config;
- cv::VideoCapture *m_capture;
ChannelPtrVector m_channels;
ChannelImplManager *m_channelImplManager;
- unsigned char m_grabCount;
cv::Mat m_image;
timeval m_lastUpdate;
};
View
4 include/kovan/socket.hpp
@@ -41,8 +41,8 @@ class Socket
bool bind(const unsigned short port);
bool close();
- ssize_t recv(void *buffer, const size_t length, int flags = 0);
- ssize_t recvfrom(void *buffer, const size_t length, Address &address, int flags = 0);
+ ssize_t recv(void *const buffer, const size_t length, int flags = 0);
+ ssize_t recvfrom(void *const buffer, const size_t length, Address &address, int flags = 0);
ssize_t send(const void *const buffer, const size_t length, int flags = 0);
ssize_t sendto(const void *const buffer, const size_t length, const Address &dest, int flags = 0);
View
332 src/ardrone.cpp
@@ -217,6 +217,12 @@ DroneCommand::DroneCommand()
class DroneController : public Thread
{
public:
+ enum DoReturn {
+ Success = 0,
+ Error,
+ NotReady
+ };
+
DroneController();
~DroneController();
@@ -235,6 +241,7 @@ class DroneController : public Thread
void setVideoCodec(const unsigned id);
void configure(const char *const cmd, const char *const value);
+ void configure(const char *const cmd, unsigned value);
void run();
@@ -254,6 +261,8 @@ class DroneController : public Thread
void resetControlWatchdog();
void sendCommWatchdog();
+ void setActiveCamera(const ARDrone::Camera camera);
+
const navdata_t *latestNavdata() const;
const navdata_demo_t *latestNavdataDemo() const;
const navdata_raw_t *latestNavdataRawMeasures() const;
@@ -268,7 +277,12 @@ class DroneController : public Thread
const Address &navdataAddress() const;
const Address &videoAddress() const;
- const Private::Image &image() const;
+ bool isValid() const;
+ bool isAtSocketValid() const;
+ bool isNavdataSocketValid() const;
+ bool isVideoSocketValid() const;
+
+ const cv::Mat &image() const;
private:
void pushCommand(const char *const command);
@@ -279,9 +293,11 @@ class DroneController : public Thread
bool fetchNavdata();
bool fetchVideo();
- bool wakeupStream(Socket &socket, const Address &address);
+ DoReturn doAt(const size_t ticks);
+ DoReturn doNavdata(const size_t ticks);
+ DoReturn doVideo(const size_t ticks);
- void __enumerateOptions();
+ bool wakeupStream(Socket &socket, const Address &address);
bool setupSocket(Socket &socket, const unsigned short bindTo = 0) const;
@@ -303,10 +319,14 @@ class DroneController : public Thread
const navdata_demo_t *m_navdataDemo;
const navdata_raw_t *m_navdataRawMeasures;
- Private::Image m_image;
+ bool m_cameraActivated;
+
+ cv::Mat m_image;
};
DroneController::DroneController()
+ : m_stop(true),
+ m_cameraActivated(false)
{
memset(m_navdata, 0, sizeof(m_navdata));
}
@@ -317,11 +337,12 @@ DroneController::~DroneController()
void DroneController::invalidate()
{
- m_mutex.lock();
- m_atSocket.close();
- m_navdataSocket.close();
- m_videoSocket.close();
- m_mutex.unlock();
+ setAtAddress(Address());
+ setNavdataAddress(Address());
+ setVideoAddress(Address());
+
+ // Invalidate navdata by clearing header
+ memset(m_navdata, 0, 8);
}
void DroneController::flatTrim()
@@ -378,9 +399,7 @@ bool DroneController::requestVideo()
void DroneController::setVideoCodec(const unsigned id)
{
- char idbuf[128];
- sprintf(idbuf, "%d", id);
- configure(ARDRONE_VIDEO_CODEC, idbuf);
+ configure(ARDRONE_VIDEO_CODEC, id);
}
void DroneController::configure(const char *const cmd, const char *const value)
@@ -393,32 +412,31 @@ void DroneController::configure(const char *const cmd, const char *const value)
sprintf(command, "%s%%u,\"%s\",\"%s\"\r", ARDRONE_AT_CONFIG, cmd, value);
oneTimeCommand(command);
}
+
+void DroneController::configure(const char *const cmd, unsigned value)
+{
+ char vbuf[128];
+ sprintf(vbuf, "%u", value);
+ configure(cmd, vbuf);
+}
void DroneController::run()
{
unsigned long ticks = 0;
+ m_stop = false;
while(!m_stop) {
- if(ticks % 100 == 0) {
- // sendCommWatchdog();
- requestVideo();
- requestNavdata();
- }
-
m_mutex.lock();
- bool success = true;
+ DroneController::DoReturn at = doAt(ticks);
+ DroneController::DoReturn navdata = DroneController::NotReady; // doNavdata(ticks);
+ DroneController::DoReturn video = doVideo(ticks);
+ m_mutex.unlock();
- // Slower update sub-loop
- if(++ticks % 11 == 0) {
- __enumerateOptions();
- success &= sendCurrentCommand();
- success &= fetchVideo();
- if(isControlWatchdog()) std::cout << "control watchdog!" << std::endl;
- }
+ if(at == DroneController::Error
+ || navdata == DroneController::Error
+ || video == DroneController::Error) break;
- success &= fetchNavdata();
- m_mutex.unlock();
- if(!success) break;
msleep(3);
+ ++ticks;
}
m_stop = false;
m_seq.reset();
@@ -495,6 +513,27 @@ void DroneController::sendCommWatchdog()
oneTimeCommand(command);
}
+void DroneController::setActiveCamera(const ARDrone::Camera camera)
+{
+ m_mutex.lock();
+ m_cameraActivated = camera != ARDrone::None;
+ m_mutex.unlock();
+ if(!m_cameraActivated) return;
+
+ /*unsigned value = 0;
+ switch(camera) {
+ case ARDrone::Front:
+ value = 1;
+ break;
+ case ARDrone::Bottom:
+ value = 2;
+ break;
+ default: break;
+ }
+
+ configure(ARDRONE_VIDEO_CHANNEL, value);*/
+}
+
const navdata_t *DroneController::latestNavdata() const
{
const navdata_t *const ret = reinterpret_cast<const navdata_t *>(m_navdata);
@@ -516,7 +555,9 @@ void DroneController::setAtAddress(const Address &atAddress)
m_mutex.lock();
m_atAddress = atAddress;
- setupSocket(m_atSocket);
+ if(m_atAddress.isValid()) setupSocket(m_atSocket);
+ else m_atSocket.close();
+
m_mutex.unlock();
}
@@ -525,7 +566,9 @@ void DroneController::setNavdataAddress(const Address &navdataAddress)
m_mutex.lock();
m_navdataAddress = navdataAddress;
- setupSocket(m_navdataSocket, m_navdataAddress.port());
+ if(m_navdataAddress.isValid()) setupSocket(m_navdataSocket, m_navdataAddress.port());
+ else m_navdataSocket.close();
+
m_mutex.unlock();
}
@@ -534,7 +577,9 @@ void DroneController::setVideoAddress(const Address &videoAddress)
m_mutex.lock();
m_videoAddress = videoAddress;
- setupSocket(m_videoSocket, m_videoAddress.port());
+ if(m_videoAddress.isValid()) setupSocket(m_videoSocket, m_videoAddress.port());
+ else m_videoSocket.close();
+
m_mutex.unlock();
}
@@ -553,7 +598,28 @@ const Address &DroneController::videoAddress() const
return m_videoAddress;
}
-const Private::Image &DroneController::image() const
+bool DroneController::isValid() const
+{
+ return isAtSocketValid() && isNavdataSocketValid()
+ && isVideoSocketValid();
+}
+
+bool DroneController::isAtSocketValid() const
+{
+ return m_atSocket.isOpen();
+}
+
+bool DroneController::isNavdataSocketValid() const
+{
+ return m_navdataSocket.isOpen();
+}
+
+bool DroneController::isVideoSocketValid() const
+{
+ return m_videoSocket.isOpen();
+}
+
+const cv::Mat &DroneController::image() const
{
return m_image;
}
@@ -584,12 +650,10 @@ void DroneController::oneTimeCommand(const char *const command)
bool DroneController::fetchNavdata()
{
- if(!m_navdataSocket.isOpen()) return true;
-
ssize_t readLength = 0;
char data[sizeof(m_navdata)];
if((readLength = m_navdataSocket.recv(data, sizeof(data))) < 0 && errno != EAGAIN) {
- perror("recvfrom");
+ perror("DroneController::fetchNavdata");
return false;
}
if(readLength < 0) return true;
@@ -602,32 +666,70 @@ bool DroneController::fetchNavdata()
bool DroneController::fetchVideo()
{
- if(!m_videoSocket.isOpen()) {
-#ifdef ARDRONE_DEBUG
- std::cout << "Video socket is not open. fetchVideo will silently fail." << std::endl;
-#endif
- return true;
- }
-
ssize_t readLength = 0;
- unsigned char data[UVLC_MAX_SIZE];
- if((readLength = m_videoSocket.recv(data, sizeof(data))) < 0 && errno != EAGAIN) {
- perror("recvfrom");
+ unsigned char data[40000];
+ if((readLength = m_videoSocket.recv(data, 40000)) < 0 && errno != EAGAIN) {
+ perror("DroneController::fetchVideo");
return false;
}
+ // std::cout << std::hex << data << std::endl;
if(readLength < 0) {
#ifdef ARDRONE_DEBUG
std::cout << "Didn't read any data from video stream." << std::endl;
#endif
return true;
}
-#ifdef ARDRONE_DEBUG
+ // #ifdef ARDRONE_DEBUG
std::cout << "Read " << readLength << " bytes from video stream" << std::endl;
-#endif
+ // #endif
Private::UvlcVideoDecoder().decode(data, readLength, m_image);
return true;
}
+DroneController::DoReturn DroneController::doAt(const size_t it)
+{
+ if(!m_atSocket.isOpen()) return DroneController::NotReady;
+
+ // 30 Hz
+ if(it % 11) {
+ if(!sendCurrentCommand()) return DroneController::Error;
+ }
+
+ return DroneController::Success;
+}
+
+DroneController::DoReturn DroneController::doNavdata(const size_t it)
+{
+ if(!m_navdataSocket.isOpen()) return DroneController::NotReady;
+
+ // Frequently
+ if(!fetchNavdata()) return DroneController::Error;
+
+ // Periodicly
+ if(it % 100) {
+ if(!requestNavdata()) return DroneController::Error;
+ }
+
+ return DroneController::Success;
+}
+
+DroneController::DoReturn DroneController::doVideo(const size_t it)
+{
+ if(!m_videoSocket.isOpen()) return DroneController::NotReady;
+
+ // 30 Hz
+ if(it % 11) {
+ if(!fetchVideo()) return DroneController::Error;
+ }
+
+ // Periodically
+ if(it % 100) {
+ if(!requestVideo()) return DroneController::Error;
+ }
+
+ return DroneController::Success;
+}
+
bool DroneController::wakeupStream(Socket &socket, const Address &address)
{
if(!socket.isOpen()) {
@@ -646,33 +748,12 @@ bool DroneController::wakeupStream(Socket &socket, const Address &address)
return true;
}
-void DroneController::__enumerateOptions()
-{
- const navdata_t *const navdata = latestNavdata();
- if(!navdata || navdata->header != ARDRONE_NAVDATA_HEADER) return;
-
- const char *offset = reinterpret_cast<const char *>(navdata->options);
- for(;;) {
- const navdata_option_t *const option = reinterpret_cast<const navdata_option_t *const>(offset);
- if(option->tag == ARDRONE_NAVDATA_CKS_TAG) break;
- offset += option->size;
-
- switch(option->tag) {
- case ARDRONE_NAVDATA_DEMO_TAG:
- m_navdataDemo = reinterpret_cast<const navdata_demo_t *>(option->data);
- break;
- }
- // std::cout << "Got tag: " << (int)option->tag << std::endl;
- // std::cout << "\tsize: " << (int)option->size << " (navdata_raw_t size: " << sizeof(navdata_raw_t) << ")" << std::endl;
- }
-}
-
bool DroneController::sendCurrentCommand()
{
// Nothing to do.
- if(m_commandStack.empty() || !m_atSocket.isOpen()) {
+ if(m_commandStack.empty()) {
#ifdef ARDRONE_DEBUG
- std::cout << "Waiting on valid socket and non-empty command stack" << std::endl;
+ std::cout << "Waiting on non-empty command stack" << std::endl;
#endif
return true;
}
@@ -714,17 +795,36 @@ ARDrone::~ARDrone()
disconnect();
}
-bool ARDrone::connect(const char *const ip)
+bool ARDrone::connect(const char *const ip, const double timeout)
{
m_controller->setAtAddress(Address(ip, ARDRONE_AT_PORT));
m_controller->setNavdataAddress(Address(ip, ARDRONE_NAVDATA_PORT));
m_controller->setVideoAddress(Address(ip, ARDRONE_VIDEO_PORT));
+ if(!m_controller->isValid()) return false;
+
+ // We know we're connected once we start
+ // receiving navdata back
+
m_controller->sendMagic();
m_controller->setNavdataDemo(true);
m_controller->setVideoCodec(ARDRONE_VIDEO_UVLC_CODEC);
+ // m_controller->setActiveCamera(m_activeCamera);
m_controller->control();
+ /* bool gotComm = false;
+ double startWait = seconds();
+ while(seconds() - startWait < timeout) {
+ if(m_controller->latestNavdata()) {
+ gotComm = true;
+ break;
+ }
+ }
+ if(!gotComm) {
+ m_controller->invalidate();
+ return false;
+ } */
+
// Set ourself as the owner
m_controller->setSsid("Braden's Drone");
// m_controller->setOwner("48:5d:60:a3:af:c4");
@@ -741,6 +841,17 @@ void ARDrone::disconnect()
m_controller->invalidate();
}
+void ARDrone::setActiveCamera(const Camera activeCamera)
+{
+ m_activeCamera = activeCamera;
+ m_controller->setActiveCamera(m_activeCamera);
+}
+
+ARDrone::Camera ARDrone::activeCamera() const
+{
+ return m_activeCamera;
+}
+
void ARDrone::flatTrim()
{
m_controller->flatTrim();
@@ -766,29 +877,19 @@ void ARDrone::move(const float x, const float y, const float z, const float yaw)
m_controller->move(x, y, z, yaw);
}
-cv::Mat ARDrone::video() const
+const cv::Mat &ARDrone::rawImage() const
{
- const Private::Image &image = m_controller->image();
- if(image.width <= 0 || image.height <= 0) return cv::Mat();
- cv::Mat mat(image.height, image.width, CV_8UC3);
- memcpy(mat.ptr(), image.data, image.height * image.width * 3);
- cv::Mat bgr = mat;
- cv::cvtColor(bgr, mat, CV_RGB2BGR);
- cv::imshow("AR.Drone Camera", mat);
+ const cv::Mat &mat = m_controller->image();
+ if(!mat.empty()) cv::imshow("AR.Drone Camera", mat);
return mat;
}
ARDrone::State ARDrone::state() const
{
- // TODO: Stub
- const navdata_t *const navdata = m_controller->latestNavdata();
- if(!navdata) return ARDrone::Disconnected;
-
- static int i = 0;
- char buffer[32];
- sprintf(buffer, "navdata_%d.bin", i++);
- m_controller->writeNavdata(buffer);
+ if(!m_controller->isAtSocketValid()) return ARDrone::Disconnected;
+ const navdata_t *const navdata = m_controller->latestNavdata();
+ if(!navdata) return ARDrone::Landed;
if(navdata->ardrone_state & ARDRONE_FLY_MASK) return ARDrone::Flying;
return ARDrone::Landed;
}
@@ -800,7 +901,60 @@ ARDrone *ARDrone::instance()
}
ARDrone::ARDrone()
- : m_controller(new DroneController)
+ : m_controller(new DroneController),
+ m_activeCamera(ARDrone::None)
{
m_controller->start();
+}
+
+Camera::ARDroneInputProvider::ARDroneInputProvider()
+ : m_opened(false)
+{
+}
+
+bool Camera::ARDroneInputProvider::open(const int number)
+{
+ if(ARDrone::instance()->state() == ARDrone::Disconnected) {
+ std::cout << "Failed to open the ARDrone's camera (disconnected)" << std::endl;
+ return false;
+ }
+
+ ARDrone::instance()->setActiveCamera(number == 0
+ ? ARDrone::Front : ARDrone::Bottom);
+
+ m_opened = true;
+ return true;
+}
+
+bool Camera::ARDroneInputProvider::isOpen() const
+{
+ return m_opened;
+}
+
+void Camera::ARDroneInputProvider::setWidth(const unsigned width)
+{
+ // NYI
+}
+
+void Camera::ARDroneInputProvider::setHeight(const unsigned height)
+{
+ // NYI
+}
+
+bool Camera::ARDroneInputProvider::next(cv::Mat &image)
+{
+ ARDrone *const drone = ARDrone::instance();
+ if(!m_opened || drone->state() == ARDrone::Disconnected)
+ return false;
+
+ image = drone->rawImage();
+ return true;
+}
+
+bool Camera::ARDroneInputProvider::close()
+{
+ if(!m_opened) return false;
+ ARDrone::instance()->setActiveCamera(ARDrone::None);
+ m_opened = false;
+ return true;
}
View
5 src/ardrone_constants_p.hpp
@@ -38,6 +38,7 @@
#define ARDRONE_NETWORK_OWNER_MAC ("network:owner_mac")
#define ARDRONE_NETWORK_SSID ("network:ssid_single_player")
#define ARDRONE_VIDEO_CODEC ("video:video_codec")
+#define ARDRONE_VIDEO_CHANNEL ("video:video_channel")
// Navdata
#define ARDRONE_NAVDATA_HEADER (0x55667788)
@@ -115,6 +116,10 @@ typedef enum {
#define ARDRONE_VIDEO_UVLC_CODEC (0x20)
#define ARDRONE_VIDEO_P264_CODEC (0x40)
+// Used in conjunction with video:video_channel
+#define ARDRONE_FRONT_CAMERA (0)
+#define ARDRONE_BOTTOM_CAMERA (1)
+
// Application IDs
#define ARDRONE_APPLICATION_ID ("d87f7e0c")
#define ARDRONE_USER_ID ("be27e2e4")
View
105 src/camera.cpp
@@ -1,4 +1,5 @@
#include "kovan/camera.hpp"
+#include "kovan/ardrone.hpp"
#include "channel_p.hpp"
#include "warn.hpp"
@@ -232,75 +233,105 @@ void Camera::ConfigPath::setDefaultConfigPath(const std::string &name)
file.close();
}
-// Device //
+// Input Providers //
-Camera::Device::Device()
- : m_capture(new cv::VideoCapture),
- m_channelImplManager(new DefaultChannelImplManager),
- m_grabCount(1)
+InputProvider::~InputProvider()
{
- Config *config = Config::load(Camera::ConfigPath::defaultConfigPath());
- if(!config) return;
- setConfig(*config);
- delete config;
}
-Camera::Device::~Device()
+UsbInputProvider::UsbInputProvider()
+ : m_capture(new cv::VideoCapture)
+{
+}
+
+UsbInputProvider::~UsbInputProvider()
{
- ChannelPtrVector::iterator it = m_channels.begin();
- for(; it != m_channels.end(); ++it) delete *it;
delete m_capture;
}
-bool Camera::Device::open(const int &number)
+bool UsbInputProvider::open(const int number)
{
- if(m_capture->isOpened()) return true;
+ if(m_capture->isOpened()) return false;
return m_capture->open(number);
}
-bool Camera::Device::isOpen() const
+bool UsbInputProvider::isOpen() const
{
return m_capture->isOpened();
}
-void Camera::Device::setWidth(const unsigned &width)
+void UsbInputProvider::setWidth(const unsigned width)
{
m_capture->set(CV_CAP_PROP_FRAME_WIDTH, width);
}
-void Camera::Device::setHeight(const unsigned &height)
+void UsbInputProvider::setHeight(const unsigned height)
{
m_capture->set(CV_CAP_PROP_FRAME_HEIGHT, height);
}
-void Camera::Device::setGrabCount(unsigned char grabs)
+bool UsbInputProvider::next(cv::Mat &image)
{
- if(grabs < 1) grabs = 1;
- else if(grabs > 5) grabs = 5;
- m_grabCount = grabs;
+ bool success = true;
+ success &= m_capture->grab();
+ success &= m_capture->retrieve(image);
+ return success;
}
-unsigned char Camera::Device::grabCount() const
+bool UsbInputProvider::close()
{
- return m_grabCount;
+ if(!m_capture->isOpened()) return false;
+ m_capture->release();
+ return true;
}
-void Camera::Device::close()
+// Device //
+
+Camera::Device::Device(InputProvider *const inputProvider)
+ : m_inputProvider(inputProvider),
+ m_channelImplManager(new DefaultChannelImplManager)
{
- if(!m_capture->isOpened()) return;
- m_capture->release();
+ Config *config = Config::load(Camera::ConfigPath::defaultConfigPath());
+ if(!config) return;
+ setConfig(*config);
+ delete config;
+}
+
+Camera::Device::~Device()
+{
+ ChannelPtrVector::iterator it = m_channels.begin();
+ for(; it != m_channels.end(); ++it) delete *it;
+}
+
+bool Camera::Device::open(const int number)
+{
+ return m_inputProvider->open(number);
+}
+
+bool Camera::Device::isOpen() const
+{
+ return m_inputProvider->isOpen();
+}
+
+void Camera::Device::setWidth(const unsigned width)
+{
+ m_inputProvider->setWidth(width);
+}
+
+void Camera::Device::setHeight(const unsigned height)
+{
+ m_inputProvider->setHeight(height);
+}
+
+bool Camera::Device::close()
+{
+ return m_inputProvider->close();
}
bool Camera::Device::update()
{
// Get new image
- bool success = true;
- for(unsigned char i = 0; i < m_grabCount; ++i) {
- success &= m_capture->grab();
- }
- success &= m_capture->retrieve(m_image);
-
- if(!success) return false;
+ if(!m_inputProvider->next(m_image)) return false;
// No need to update channels if there are none.
if(m_channels.empty()) return true;
@@ -309,7 +340,7 @@ bool Camera::Device::update()
m_channelImplManager->setImage(m_image);
// Invalidate all channels
- ChannelPtrVector::iterator it = m_channels.begin();
+ ChannelPtrVector::const_iterator it = m_channels.begin();
for(; it != m_channels.end(); ++it) (*it)->invalidate();
return true;
}
@@ -319,9 +350,9 @@ const ChannelPtrVector &Camera::Device::channels() const
return m_channels;
}
-cv::VideoCapture *Camera::Device::videoCapture() const
+InputProvider *Camera::Device::inputProvider() const
{
- return m_capture;
+ return m_inputProvider;
}
const cv::Mat &Camera::Device::rawImage() const
@@ -353,7 +384,7 @@ ChannelImplManager *Camera::Device::channelImplManager() const
void Camera::Device::updateConfig()
{
- ChannelPtrVector::iterator it = m_channels.begin();
+ ChannelPtrVector::const_iterator it = m_channels.begin();
for(; it != m_channels.end(); ++it) delete *it;
m_channels.clear();
View
12 src/camera_c.cpp
@@ -10,7 +10,7 @@ class DeviceSingleton
public:
static Camera::Device *instance()
{
- static Camera::Device s_device;
+ static Camera::Device s_device(new Camera::UsbInputProvider);
return &s_device;
}
};
@@ -45,16 +45,6 @@ int camera_open_device(int number)
return DeviceSingleton::instance()->open(number) ? 1 : 0;
}
-void set_camera_grab_count(int grabs)
-{
- DeviceSingleton::instance()->setGrabCount(grabs);
-}
-
-int get_camera_grab_count()
-{
- return DeviceSingleton::instance()->grabCount();
-}
-
int camera_load_config(const char *name)
{
Config *config = Config::load(Camera::ConfigPath::path(name));
View
14 src/channel_p.cpp
@@ -14,11 +14,18 @@ HsvChannelImpl::HsvChannelImpl()
void HsvChannelImpl::update(const cv::Mat &image)
{
+ if(image.empty()) {
+ m_image = cv::Mat();
+ return;
+ }
cv::cvtColor(image, m_image, CV_BGR2HSV);
}
Camera::ObjectVector HsvChannelImpl::findObjects(const Config &config)
{
+ if(m_image.empty()) return ::Camera::ObjectVector();
+
+
// TODO: This lookup is really slow compared to the rest of
// the algorithm.
cv::Vec3b top(config.intValue("th"),
@@ -78,6 +85,11 @@ BarcodeChannelImpl::BarcodeChannelImpl()
void BarcodeChannelImpl::update(const cv::Mat &image)
{
+ if(image.empty()) {
+ m_gray = cv::Mat();
+ return;
+ }
+
cv::cvtColor(image, m_gray, CV_BGR2GRAY);
m_image.set_data(m_gray.data, m_gray.cols * m_gray.rows);
m_image.set_size(m_gray.cols, m_gray.rows);
@@ -85,6 +97,8 @@ void BarcodeChannelImpl::update(const cv::Mat &image)
::Camera::ObjectVector BarcodeChannelImpl::findObjects(const Config &config)
{
+ if(m_gray.empty()) return ::Camera::ObjectVector();
+
m_scanner.scan(m_image);
zbar::SymbolSet symbols = m_scanner.get_results();
::Camera::ObjectVector ret;
View
4 src/socket.cpp
@@ -134,12 +134,12 @@ bool Socket::close()
return ret;
}
-ssize_t Socket::recv(void *buffer, const size_t length, int flags)
+ssize_t Socket::recv(void *const buffer, const size_t length, int flags)
{
return ::recv(m_fd, buffer, length, flags);
}
-ssize_t Socket::recvfrom(void *buffer, const size_t length, Address &address, int flags)
+ssize_t Socket::recvfrom(void *const buffer, const size_t length, Address &address, int flags)
{
sockaddr_in rawAddress;
socklen_t rawAddressLength;
View
48 src/uvlc_video_decoder_p.cpp
@@ -657,46 +657,44 @@ void ProcessStream()
}
}
-bool decodeImage(const unsigned char *const stream, const size_t streamLength, Private::Image &resultImage)
+Private::UvlcVideoDecoder::UvlcVideoDecoder()
+{
+}
+
+Private::UvlcVideoDecoder::~UvlcVideoDecoder()
+{
+}
+
+bool Private::UvlcVideoDecoder::decode(const unsigned char *const buffer, const size_t length, cv::Mat &image)
{
Width = Height = -1;
- ImageStream = stream;
- ImageStreamLength = streamLength;
+ ImageStream = buffer;
+ ImageStreamLength = length;
ProcessStream();
- resultImage.width = Width;
- resultImage.height = Height;
+ ImageStream = 0;
if(Width < 0 || Height < 0) {
std::cout << "image decoding FAIL!!" << std::endl;
return false;
}
+ image = cv::Mat(Height, Width, CV_8UC3);
+
const static unsigned short red_mask = 0xF800;
const static unsigned short green_mask = 0x07E0;
const static unsigned short blue_mask = 0x001F;
- size_t length = Width * Height;
- for(int i = 0, j = 0; i < length; i++, j += 3) {
+ size_t imageLength = Width * Height;
+ for(int i = 0, j = 0; i < imageLength; i++, j += 3) {
unsigned char red = (PixelData[i] & red_mask) >> 11;
unsigned char green = (PixelData[i] & green_mask) >> 5;
- unsigned char blue = (PixelData[i] & blue_mask);
+ unsigned char blue = (PixelData[i] & blue_mask) >> 0;
- resultImage.data[j + 0] = red << 3;
- resultImage.data[j + 1] = green << 2;
- resultImage.data[j + 2] = blue << 3;
- }
+ // OpenCV expects BGR
+ image.at<unsigned char>(j + 2) = red << 3;
+ image.at<unsigned char>(j + 1) = green << 2;
+ image.at<unsigned char>(j + 0) = blue << 3;
+ }
+
return true;
-}
-
-Private::UvlcVideoDecoder::UvlcVideoDecoder()
-{
-}
-
-Private::UvlcVideoDecoder::~UvlcVideoDecoder()
-{
-}
-
-void Private::UvlcVideoDecoder::decode(const unsigned char *const buffer, const size_t length, Private::Image &image)
-{
- if(!decodeImage(buffer, length, image)) return;
}
View
10 src/uvlc_video_decoder_p.hpp
@@ -2,25 +2,19 @@
#define _UVLC_VIDEO_DECODER_P_HPP_
#include <cstdlib>
+#include <opencv2/core/core.hpp>
#define UVLC_MAX_SIZE (921600)
namespace Private
{
- struct Image
- {
- unsigned char data[UVLC_MAX_SIZE];
- int width;
- int height;
- };
-
class UvlcVideoDecoder
{
public:
UvlcVideoDecoder();
~UvlcVideoDecoder();
- void decode(const unsigned char *const buffer, const size_t length, Image &image);
+ bool decode(const unsigned char *const buffer, const size_t length, cv::Mat &image);
private:
};
}
View
8 test/ardrone/ardrone.cpp
@@ -9,7 +9,7 @@ int main(int argc, char *argv[])
if(!drone->connect()) {
return EXIT_FAILURE;
}
-
+ drone->setActiveCamera(ARDrone::Front);
drone->flatTrim();
msleep(1000);
@@ -19,11 +19,9 @@ int main(int argc, char *argv[])
const double start = seconds();
while(seconds() - start < 20.0) {
- cv::Mat mat = drone->video();
- if(mat.empty()) std::cout << ".";
- else cv::imshow("test", mat);
+ // drone->rawImage();
cv::waitKey(1);
- msleep(10);
+ msleep(30);
}
drone->land();
View
41 test/camera/camera.cpp
@@ -12,16 +12,22 @@
int main(int argc, char *argv[])
{
- Camera::Device device;
- if(!device.open(0)) {
+
+ ARDrone *drone = ARDrone::instance();
+ if(!drone->connect()) {
+ std::cout << "Failed to connect to drone" << std::endl;
return 1;
}
- device.setWidth(320);
- device.setHeight(240);
+
+ Camera::Device device(new Camera::ARDroneInputProvider);
+ if(!device.open()) return 1;
+
+ // device.setWidth(320);
+ // device.setHeight(240);
cv::namedWindow("Blobs");
Config config;
- config.setValue("camera/num_channels", 2);
+ config.setValue("camera/num_channels", 1);
config.setValue("camera/channel_0/type", "hsv");
config.setValue("camera/channel_0/th", 10);
@@ -32,8 +38,6 @@ int main(int argc, char *argv[])
config.setValue("camera/channel_0/bs", 150);
config.setValue("camera/channel_0/bv", 100);
- config.setValue("camera/channel_1/type", "barcode");
-
device.setConfig(config);
time_t start, end;
@@ -42,11 +46,15 @@ int main(int argc, char *argv[])
time(&start);
+ drone->takeoff();
+ msleep(10000);
+
cv::Mat image;
while(cv::waitKey(1) == -1) {
device.update();
- device.videoCapture()->retrieve(image);
- const Camera::ObjectVector *objects = device.channels()[1]->objects();
+ image = device.rawImage();
+ if(image.empty()) continue;
+ const Camera::ObjectVector *objects = device.channels()[0]->objects();
if(!objects) continue;
Camera::ObjectVector::const_iterator it = objects->begin();
for(; it != objects->end(); ++it) {
@@ -55,6 +63,17 @@ int main(int argc, char *argv[])
object.boundingBox().width(), object.boundingBox().height()), cv::Scalar(0, 255, 0));
}
+ if(objects->size() > 0) {
+ Camera::Object obj = objects->at(0);
+ if(obj.boundingBox().center().x() < image.size().width / 2) {
+ drone->move(0.0f, 0.0f, 0.0f, -0.3f);
+ std::cout << "left" << std::endl;
+ } else {
+ drone->move(0.0f, 0.0f, 0.0f, 0.3f);
+ std::cout << "right" << std::endl;
+ }
+ }
+
time(&end);
++counter;
@@ -69,7 +88,11 @@ int main(int argc, char *argv[])
cv::imshow("Blobs", image);
}
+ drone->land();
+ msleep(3000);
+
device.close();
+ drone->disconnect();
cv::destroyAllWindows();
}
Please sign in to comment.
Something went wrong with that request. Please try again.