Skip to content

Commit

Permalink
Changes for ROS kinetic and formatting.
Browse files Browse the repository at this point in the history
  • Loading branch information
jgoppert committed May 7, 2016
1 parent dc03d8e commit 2b9b130
Show file tree
Hide file tree
Showing 5 changed files with 360 additions and 337 deletions.
50 changes: 31 additions & 19 deletions CMakeLists.txt
Expand Up @@ -8,40 +8,52 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs
geometry_msgs
)
find_package(OpenCV REQUIRED)
find_package(OpenCV 2 REQUIRED core highgui)

catkin_package(
)

include_directories(
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
include
)

MESSAGE("System is: " ${CMAKE_SYSTEM_PROCESSOR})
message("System is: " ${CMAKE_SYSTEM_PROCESSOR})
if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
if (CMAKE_SIZEOF_VOID_P EQUAL 4)
MESSAGE("-- 32bit detected")
link_directories(${PROJECT_SOURCE_DIR}/lib/x86)
elseif (CMAKE_SIZEOF_VOID_P EQUAL 8)
MESSAGE("-- 64bit detected")
link_directories(${PROJECT_SOURCE_DIR}/lib/x64)
endif ()
if (CMAKE_SIZEOF_VOID_P EQUAL 4)
message("-- 32bit detected")
link_directories(lib/x86)
elseif (CMAKE_SIZEOF_VOID_P EQUAL 8)
message("-- 64bit detected")
link_directories(lib/x64)
endif ()
else()
MESSAGE("-- Non-linux platform detected but sorry we do not support :D")
message("-- Non-linux platform detected but sorry we do not support :D")
endif ()

if (${CMAKE_SYSTEM_PROCESSOR} MATCHES "armv7l" )
MESSAGE("-- " ${CMAKE_SYSTEM_PROCESSOR} " detected")
link_directories(${PROJECT_SOURCE_DIR}/lib/XU3)
message("-- " ${CMAKE_SYSTEM_PROCESSOR} " detected")
link_directories(lib/XU3)
else ()
endif ()

include_directories(${PROJECT_SOURCE_DIR}/include)
link_libraries(
${OpenCV_LIBS}
${catkin_LIBRARIES}
DJI_guidance
usb-1.0
yaml-cpp)

add_executable(guidanceNode ${PROJECT_SOURCE_DIR}/src/GuidanceNode.cpp ${PROJECT_SOURCE_DIR}/src/DJI_utility.cpp)
add_executable(guidanceNodeTest ${PROJECT_SOURCE_DIR}/src/GuidanceNodeTest.cpp)
add_executable(guidanceNodeCalibration ${PROJECT_SOURCE_DIR}/src/GuidanceNodeCalibration.cpp ${PROJECT_SOURCE_DIR}/src/DJI_utility.cpp)
add_executable(guidanceNode
src/GuidanceNode.cpp
src/DJI_utility.cpp)

target_link_libraries(guidanceNode DJI_guidance usb-1.0 ${catkin_LIBRARIES} ${OpenCV_LIBS})
target_link_libraries(guidanceNodeTest ${catkin_LIBRARIES} ${OpenCV_LIBS})
target_link_libraries(guidanceNodeCalibration DJI_guidance usb-1.0 yaml-cpp ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_executable(guidanceNodeTest
src/GuidanceNodeTest.cpp)

add_executable(guidanceNodeCalibration
src/GuidanceNodeCalibration.cpp
src/DJI_utility.cpp)

# vim: set et fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :
202 changes: 101 additions & 101 deletions src/DJI_utility.cpp
@@ -1,101 +1,101 @@
#include "DJI_utility.h"

#ifdef WIN32

DJI_lock::DJI_lock()
{
InitializeCriticalSection( &m_critical_section );
}

DJI_lock::~DJI_lock()
{
}

void DJI_lock::enter()
{
EnterCriticalSection( &m_critical_section );
}

void DJI_lock::leave()
{
LeaveCriticalSection( &m_critical_section );
}

void sleep( int microsecond )
{
Sleep( ( microsecond + 999 ) / 1000 );
}
DJI_event::DJI_event()
{
SECURITY_ATTRIBUTES sa;
sa.nLength = sizeof(sa);
sa.lpSecurityDescriptor = NULL;
sa.bInheritHandle = TRUE;
CreatePipe( &m_pipe_read, &m_pipe_write, &sa, 0 );
}

DJI_event::~DJI_event()
{
CloseHandle( m_pipe_read );
CloseHandle( m_pipe_write );
}

int DJI_event::set_event()
{
char buffer[1] = {0};
int count = 0;
int ret = WriteFile( m_pipe_write, (LPWORD)buffer, 1, (LPDWORD)&count, NULL );
return ret;
}

int DJI_event::wait_event()
{
char buffer[1] = {0};
int count = 0;
int ret = ReadFile( m_pipe_read, (LPWORD)buffer, 1, (LPDWORD)&count, NULL );
return ret;
}

#else

DJI_lock::DJI_lock()
{
pthread_mutex_init( &m_lock, NULL );
}

DJI_lock::~DJI_lock()
{
}

void DJI_lock::enter()
{
pthread_mutex_lock( &m_lock );
}

void DJI_lock::leave()
{
pthread_mutex_unlock( &m_lock );
}

DJI_event::DJI_event()
{
sem_init( &m_sem, 0, 0 );
}

DJI_event::~DJI_event()
{
}

int DJI_event::set_event()
{
int ret = sem_post( &m_sem );
return ret;
}

int DJI_event::wait_event()
{
int ret = sem_wait( &m_sem );
return ret;
}

#endif
#include "DJI_utility.h"

#ifdef WIN32

DJI_lock::DJI_lock()
{
InitializeCriticalSection( &m_critical_section );
}

DJI_lock::~DJI_lock()
{
}

void DJI_lock::enter()
{
EnterCriticalSection( &m_critical_section );
}

void DJI_lock::leave()
{
LeaveCriticalSection( &m_critical_section );
}

void sleep( int microsecond )
{
Sleep( ( microsecond + 999 ) / 1000 );
}
DJI_event::DJI_event()
{
SECURITY_ATTRIBUTES sa;
sa.nLength = sizeof(sa);
sa.lpSecurityDescriptor = NULL;
sa.bInheritHandle = TRUE;
CreatePipe( &m_pipe_read, &m_pipe_write, &sa, 0 );
}

DJI_event::~DJI_event()
{
CloseHandle( m_pipe_read );
CloseHandle( m_pipe_write );
}

int DJI_event::set_event()
{
char buffer[1] = {0};
int count = 0;
int ret = WriteFile( m_pipe_write, (LPWORD)buffer, 1, (LPDWORD)&count, NULL );
return ret;
}

int DJI_event::wait_event()
{
char buffer[1] = {0};
int count = 0;
int ret = ReadFile( m_pipe_read, (LPWORD)buffer, 1, (LPDWORD)&count, NULL );
return ret;
}

#else

DJI_lock::DJI_lock()
{
pthread_mutex_init( &m_lock, NULL );
}

DJI_lock::~DJI_lock()
{
}

void DJI_lock::enter()
{
pthread_mutex_lock( &m_lock );
}

void DJI_lock::leave()
{
pthread_mutex_unlock( &m_lock );
}

DJI_event::DJI_event()
{
sem_init( &m_sem, 0, 0 );
}

DJI_event::~DJI_event()
{
}

int DJI_event::set_event()
{
int ret = sem_post( &m_sem );
return ret;
}

int DJI_event::wait_event()
{
int ret = sem_wait( &m_sem );
return ret;
}

#endif
5 changes: 3 additions & 2 deletions src/GuidanceNode.cpp
Expand Up @@ -12,8 +12,7 @@
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv2/opencv.hpp>

#include "DJI_guidance.h"
#include "DJI_utility.h"
Expand Down Expand Up @@ -347,3 +346,5 @@ int main(int argc, char** argv)

return 0;
}

/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

0 comments on commit 2b9b130

Please sign in to comment.