【auto_stabilizer】 merge origin/master

CMakeLists.txt

@@ -13,6 +13,12 @@ include(FindPkgConfig)
 include(CheckIncludeFiles)
 
 option(ENABLE_INSTALL_RPATH "Enable RPATH setting for installed binary files" OFF)
+option(USE_HRPSYSUTIL "Build hrpsysUtil" ON)
+option(USE_IRRLICHT "Build Irrlicht components" OFF)
+option(NO_DIGITAL_INPUT "Disable readDigitalInput and lengthDigitalInput" OFF)
+option(USE_QPOASES "Build qpOASES" ON)
+option(ENABLE_DOXYGEN "Use Doxygen" ON)
+
 
 set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
@@ -142,51 +148,32 @@ include(CPack)
 
 find_package(LibXml2 REQUIRED)
 find_package(QuickHull REQUIRED)
-find_package(SDL)
-if ( NOT SDL_FOUND )
-  message(WARNING "Package SDL is not found!")
-endif( NOT SDL_FOUND )
-find_package(OpenGL)
-if ( NOT OPENGL_FOUND )
-  message(WARNING "Package OpenGL is not found!")
-endif( NOT OPENGL_FOUND )
-find_package(GLUT)
-if ( NOT GLUT_FOUND )
-  message(WARNING "Package GLUT is not found!")
-endif( NOT GLUT_FOUND )
-include(FindPkgConfig)
-pkg_check_modules(OpenCV opencv)
-if (NOT OpenCV_FOUND)
-  # pkg_check_modules(OpenCV opencv-2.3.1)
-  pkg_check_modules(OpenCV opencv-3.3.1-dev)
+
+if(USE_HRPSYSUTIL)
+  find_package(SDL REQUIRED)
+  find_package(OpenGL REQUIRED)
+  find_package(GLUT REQUIRED)
+  pkg_check_modules(OpenCV opencv)
   if (NOT OpenCV_FOUND)
-    message(WARNING "opencv not found")
-  endif()
-endif()
-pkg_check_modules(GLEW glew)
-if (NOT GLEW_FOUND)
-  message(WARNING "Package GLEW is not found")
-endif()
-find_package(PCL)
-if(OpenCV_FOUND AND OPENGL_FOUND AND GLEW_FOUND AND GLUT_FOUND AND SDL_FOUND)
-  if(APPLE)
-    if(PCL_FOUND) # qhull is included in pcl
-      set(USE_HRPSYSUTIL true)
-    else()
-      set(USE_HRPSYSUTIL false)
+    # pkg_check_modules(OpenCV opencv-2.3.1)
+    pkg_check_modules(OpenCV opencv-3.3.1-dev)
+    if (NOT OpenCV_FOUND)
+      pkg_check_modules(OpenCV opencv4)
+      if (NOT OpenCV_FOUND)
+        message(FATAL_ERROR "OpenCV is required to build hrpsysUtil")
+      endif()
     endif()
-  else()
-    set(USE_HRPSYSUTIL true)
   endif()
-else()
-  set(USE_HRPSYSUTIL false)
+  include_directories(${OpenCV_INCLUDE_DIRS})
+  link_directories(${OpenCV_LIBRARY_DIRS})
+  pkg_check_modules(GLEW glew)
+  if (NOT GLEW_FOUND)
+    message(FATAL_ERROR "GLEW is required to build hrpsysUtil")
+  endif()
 endif()
-
-include_directories(${OpenCV_INCLUDE_DIRS})
-link_directories(${OpenCV_LIBRARY_DIRS})
-if(NOT QNXNTO)
-  find_package(Eigen REQUIRED)
-  include_directories(${Eigen_INCLUDE_DIRS})
+find_package(PCL)
+if(USE_HRPSYSUTIL AND APPLE AND NOT PCL_FOUND)
+  message(FATAL_ERROR "PCL is required to build hrpsysUtil on Apple platform")
 endif()
 
 execute_process(
@@ -202,7 +189,6 @@ install(FILES
 
 add_definitions(-DHRPSYS_PACKAGE_VERSION=\"\\"${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}\\"\")
 
-option(NO_DIGITAL_INPUT "Disable readDigitalInput and lengthDigitalInput" OFF)
 if(NO_DIGITAL_INPUT)
   add_definitions(-DNO_DIGITAL_INPUT)
   message(STATUS "Disablng readDigitalInput and lengthDigitalInput")
@@ -219,19 +205,15 @@ add_subdirectory(python)
 add_subdirectory(idl)
 add_subdirectory(lib)
 add_subdirectory(ec)
-# option for 3rdparty
-option(USE_QPOASES "Build qpOASES" ON)
 add_subdirectory(3rdparty)
 add_subdirectory(rtc)
-option(ENABLE_DOXYGEN "Use Doxygen" ON)
 if(ENABLE_DOXYGEN)
   add_subdirectory(doc)
 endif()
 if (NOT QNXNTO)
   add_subdirectory(util)
 endif()
 add_subdirectory(sample)
-add_subdirectory(test)
 
 #if catkin environment
 string(REGEX MATCH "catkin" need_catkin "$ENV{_}")

ec/hrpEC/hrpEC-common.cpp

@@ -146,7 +146,7 @@ namespace RTC
 #else
                         RTC::RTObject_var rtc = list[i];
 #endif
-                        rtc_names.push_back(std::string(rtc->get_component_profile()->instance_name));
+                        rtc_names.push_back(std::string(RTC::ComponentProfile_var(rtc->get_component_profile())->instance_name));
                     }
                 }
                 printRTCProcessingTime(processes);

lib/util/GLsceneBase.h

@@ -6,8 +6,13 @@
 #include <map>
 #include <sys/time.h>
 //Open CV header
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#ifndef CV_VERSION_EPOCH
+  #if CV_VERSION_MAJOR > 3
+    #include <opencv2/videoio/videoio_c.h>
+  #endif
+#endif
 #include <SDL/SDL_thread.h>
 #include <hrpCorba/ModelLoader.hh>
 #include <hrpModel/ConstraintForceSolver.h>

rtc/CMakeLists.txt

@@ -63,9 +63,11 @@ if (NOT APPLE AND USE_HRPSYSUTIL)
   add_subdirectory(VideoCapture)
 endif()
 
-find_package(Irrlicht)
-if (IRRLICHT_FOUND AND USE_HRPSYSUTIL)
-  add_subdirectory(OGMap3DViewer)
+if (USE_IRRLICHT AND USE_HRPSYSUTIL)
+  find_package(Irrlicht)
+  if(IRRLICHT_FOUND)
+    add_subdirectory(OGMap3DViewer)
+  endif()
 endif()
 
 # Octomap

rtc/CameraImageLoader/CameraImageLoader.cpp

@@ -7,8 +7,14 @@
  * $Id$
  */
 
-#include <cv.h>
-#include <highgui.h>
+#ifndef CV_VERSION_EPOCH
+  #define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/imgcodecs.hpp>
+#else
+#include <opencv2/highgui/highgui.hpp>
+#endif
 #include "CameraImageLoader.h"
 
 // Module specification
@@ -116,26 +122,26 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id)
   std::string filename;
   std::cin >> filename;
 
-  IplImage *image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR);
-  if (!image) {
+  cv::Mat image = cv::imread(filename.c_str(), cv::IMREAD_COLOR);
+  if (image.empty()) {
       std::cerr << m_profile.instance_name << ": failed to load("
                 << filename << ")" << std::endl;
       return RTC::RTC_OK;
   }
 
-  m_image.data.image.width = image->width;
-  m_image.data.image.height = image->height;
-  m_image.data.image.raw_data.length(image->imageSize);
-  switch(image->nChannels){
+  m_image.data.image.width = image.size().width;
+  m_image.data.image.height = image.size().height;
+  m_image.data.image.raw_data.length(image.size().area());
+  switch(image.channels()){
   case 3:
       m_image.data.image.format = Img::CF_RGB;
       {
           // BGR -> RGB
-          char *src;
+          unsigned char *src;
           unsigned char *dst = m_image.data.image.raw_data.get_buffer();
-          for (int i=0; i<image->height; i++){
-              for (int j=0; j<image->width; j++){
-                  src = image->imageData + image->widthStep*i + j*3;
+          for (int i=0; i<image.size().height; i++){
+              for (int j=0; j<image.size().width; j++){
+                  src = image.data + image.step * i + j * 3;
                   dst[2] = src[0];
                   dst[1] = src[1];
                   dst[0] = src[2];
@@ -147,15 +153,13 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id)
   case 1:
       m_image.data.image.format = Img::CF_GRAY;
       memcpy(m_image.data.image.raw_data.get_buffer(),
-             image->imageData,
+             image.data,
              m_image.data.image.raw_data.length());
       break;
   default:
       break;
   }
 
-  cvReleaseImage (&image);
-
   m_imageOut.write();
 
   return RTC::RTC_OK;

rtc/CameraImageSaver/CameraImageSaver.cpp

@@ -7,8 +7,16 @@
  * $Id$
  */
 
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#ifndef CV_VERSION_EPOCH
+  #define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/imgcodecs.hpp>
+#else
+#include <opencv2/highgui/highgui.hpp>
+#endif
 #include "CameraImageSaver.h"
 
 // Module specification
@@ -158,7 +166,8 @@ RTC::ReturnCode_t CameraImageSaver::onExecute(RTC::UniqueId ec_id)
 
     char fname[256];
     sprintf(fname, "%s%04d.png", m_basename.c_str(), m_count++);
-    cvSaveImage(fname, cvImage);
+    cv::Mat image = cv::cvarrToMat(cvImage);
+    cv::imwrite(fname, image);
 
     cvReleaseImage(&cvImage);
   }

rtc/CameraImageViewer/CameraImageViewer.h

@@ -21,8 +21,8 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include <rtm/idl/InterfaceDataTypesSkel.h>
-#include <cv.h>
-#include <highgui.h>
+#include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">

rtc/ColorExtractor/ColorExtractor.cpp

@@ -7,7 +7,11 @@
  * $Id$
  */
 
-#include <highgui.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
+#ifndef CV_RGB
+#define CV_RGB( r, g, b )  cvScalar( (b), (g), (r), 0 )
+#endif
 #include "ColorExtractor.h"
 #include "hrpsys/util/VectorConvert.h"
 

rtc/ColorExtractor/ColorExtractor.h

@@ -19,7 +19,7 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
-#include <cv.h>
+#include <opencv2/core/core_c.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">

rtc/ImpedanceController/ImpedanceController.cpp

@@ -535,7 +535,7 @@ void ImpedanceController::calcForceMoment ()
           if ( sensor ) {
             // real force sensor
             sensorR = sensor->link->R * sensor->localR;
-            sensorPos = sensor->link->p + sensorR * sensor->localPos;
+            sensorPos = sensor->link->p + sensor->link->R * sensor->localPos;
           } else if ( m_vfs.find(sensor_name) !=  m_vfs.end()) {
             // virtual force sensor
             if ( DEBUGP ) {

rtc/JpegDecoder/JpegDecoder.cpp

@@ -9,6 +9,12 @@
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
+#ifndef CV_VERSION_EPOCH
+#define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/legacy/constants_c.h>
+#endif
 #include "JpegDecoder.h"
 
 // Module specification

rtc/JpegEncoder/JpegEncoder.cpp

@@ -10,6 +10,12 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/core/core.hpp>
+#ifndef CV_VERSION_EPOCH
+#define CV_VERSION_EPOCH CV_VERSION_MAJOR
+#endif
+#if CV_VERSION_EPOCH > 3
+#include <opencv2/imgcodecs/legacy/constants_c.h>
+#endif
 #include "JpegEncoder.h"
 
 // Module specification