Skip to content

Commit

Permalink
add swig C interfaces
Browse files Browse the repository at this point in the history
  • Loading branch information
k-okada committed Jul 20, 2016
1 parent 5717906 commit 6805fa3
Show file tree
Hide file tree
Showing 7 changed files with 3,206 additions and 0 deletions.
2 changes: 2 additions & 0 deletions hector_mapping/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -155,3 +155,5 @@ install(DIRECTORY launch/

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

#add_subdirectory(swig)
47 changes: 47 additions & 0 deletions hector_mapping/swig/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
cmake_minimum_required(VERSION 2.8)
find_package(SWIG REQUIRED)
find_package(Eigen3)

if(SWIG_FOUND)
include(${SWIG_USE_FILE})
include_directories(${CMAKE_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR})

# assuming you have installed https://svn.code.sf.net/p/swig/code/branches/gsoc2008-maciekd/
set(SWIG_EXECUTABLE ${CMAKE_CURRENT_SOURCE_DIR}/gsoc2008-maciekd/swig)
set(ENV{PATH} ${CMAKE_CURRENT_SOURCE_DIR}/gsoc2008-maciekd/:$ENV{PATH})
set(ENV{SWIG_LIB} ${CMAKE_CURRENT_SOURCE_DIR}/gsoc2008-maciekd/Lib)
message(WARNING "-- If you have trouble on using swig, try export SWIG_LIB=$ENV{SWIG_LIB}")

set(INTERFACE_FILES
hector_mapping.i)

set_source_files_properties(${INTERFACE_FILES} PROPERTIES
CPLUSPLUS ON
COMPILE_FLAGS "-fpermissive" # http://stackoverflow.com/questions/22411514/no-match-for-operator-error-in-c
)
set(CMAKE_SWIG_OUTDIR ${CMAKE_CURRENT_SOURCE_DIR})
set(CMAKE_CURRENT_BINARY_DIR_TMP ${CMAKE_CURRENT_BINARY_DIR}) ## to output wrap file
set(CMAKE_CURRENT_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
swig_add_module(swig_library c ${INTERFACE_FILES})
set(CMAKE_CURRENT_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR_TMP})

swig_link_libraries(swig_library)
set_target_properties(swig_library PROPERTIES OUTPUT_NAME hector_mapping)
add_dependencies(swig_library
${CMAKE_CURRENT_BINARY_DIR}/hector_mappingC_wrap.cxx
${CMAKE_CURRENT_SOURCE_DIR}/HectorSlam_proxy.c
${CMAKE_CURRENT_SOURCE_DIR}/HectorSlam_proxy.h
)
else()
include_directories(${CMAKE_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR})
add_library(swig_library hector_mappingC_wrap.cxx HectorSlam_proxy.c)
set_target_properties(swig_library PROPERTIES OUTPUT_NAME hector_mapping)
endif()

# example code
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${catkin_INCLUDE_DIRS})
link_directories(${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION})
add_executable(standalone standalone.cpp HectorSlam_proxy.c)
target_link_libraries(standalone libhector_mapping.so ${catkin_LIBRARIES})
add_dependencies(standalone swig_library)

437 changes: 437 additions & 0 deletions hector_mapping/swig/HectorSlam_proxy.c

Large diffs are not rendered by default.

215 changes: 215 additions & 0 deletions hector_mapping/swig/HectorSlam_proxy.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,215 @@
/* ----------------------------------------------------------------------------
* This file was automatically generated by SWIG (http://www.swig.org).
* Version 2.0.6
*
* This file is not intended to be easily readable and contains a number of
* coding conventions designed to improve portability and efficiency. Do not make
* changes to this file unless you know what you are doing--modify the SWIG
* interface file instead.
* ----------------------------------------------------------------------------- */
#ifndef _HectorSlam_proxy_H_
#define _HectorSlam_proxy_H_

/* -----------------------------------------------------------------------------
* cproxy.swg
*
* Definitions of C specific preprocessor symbols for proxies.
* ----------------------------------------------------------------------------- */

#ifndef SWIGIMPORT
# ifndef __GNUC__
# define __DLL_IMPORT __declspec(dllimport)
# else
# define __DLL_IMPORT __attribute__((dllimport)) extern
# endif
# if !defined (__WIN32__)
# define SWIGIMPORT extern
# else
# define SWIGIMPORT __DLL_IMPORT
# endif
#endif

#include <stdio.h>

typedef struct {
void *obj;
const char **typenames;
} SwigObj;


// special value indicating any type of exception like 'catch(...)'
#define SWIG_AnyException "SWIG_AnyException"

#include <setjmp.h>

SWIGIMPORT jmp_buf SWIG_rt_env;

SWIGIMPORT struct SWIG_exc_struct {
int code;
char *msg;
SwigObj *klass;
} SWIG_exc;

SWIGIMPORT void SWIG_rt_try();
SWIGIMPORT int SWIG_rt_catch(const char *type);
SWIGIMPORT void SWIG_rt_throw(SwigObj *klass, const char * msg);
SWIGIMPORT int SWIG_rt_unhandled();
SWIGIMPORT void SWIG_rt_endtry();
SWIGIMPORT int SWIG_exit(int code);

#define SWIG_try \
SWIG_rt_try(); \
if ((SWIG_exc.code = setjmp(SWIG_rt_env)) == 0)
#define SWIG_catch(type) else if (SWIG_rt_catch(#type))
#define SWIG_throw(klass) SWIG_rt_throw((SwigObj *) klass, 0);
#define SWIG_throw_msg(klass, msg) SWIG_rt_throw((SwigObj *) klass, msg);
#define SWIG_endtry else SWIG_rt_unhandled(); SWIG_rt_endtry();



#include <stdarg.h>

#define SWIG_MAKE_DELETE(Name,Obj) void Name(Obj *op1, ...) {\
Obj *obj;\
va_list vl;\
va_start(vl, op1);\
do {\
obj = va_arg(vl, Obj *);\
delete_##Obj(obj);\
} while (obj);\
va_end(vl);\
}

/*aaaaaa*/SwigObj * Eigen_Vector2f(float carg1, float carg2);
float Eigen_Vector2f_x(SwigObj * carg1);
float Eigen_Vector2f_y(SwigObj * carg1);
float Eigen_Vector3f_x(SwigObj * carg1);
float Eigen_Vector3f_y(SwigObj * carg1);
float Eigen_Vector3f_z(SwigObj * carg1);
const char * SimdInstructionSetsInUse();
#define STAGE10_FULL_EIGEN2_API 10
#define STAGE20_RESOLVE_API_CONFLICTS 20
#define STAGE30_FULL_EIGEN3_API 30
#define STAGE40_FULL_EIGEN3_STRICTNESS 40
#define STAGE99_NO_EIGEN2_SUPPORT 99
#define EIGEN2_SUPPORT_STAGE 99

typedef SwigObj LogOddsCell;

void LogOddsCell_set(SwigObj * carg1, float carg2);
float LogOddsCell_getValue(SwigObj * carg1);
#include <stdbool.h>
bool LogOddsCell_isOccupied(SwigObj * carg1);
bool LogOddsCell_isFree(SwigObj * carg1);
void LogOddsCell_resetGridCell(SwigObj * carg1);
void LogOddsCell_logOddsVal_set(SwigObj * carg1, float carg2);
float LogOddsCell_logOddsVal_get(SwigObj * carg1);
void LogOddsCell_updateIndex_set(SwigObj * carg1, int carg2);
int LogOddsCell_updateIndex_get(SwigObj * carg1);
LogOddsCell * new_LogOddsCell();
void delete_LogOddsCell(LogOddsCell * carg1);

typedef SwigObj GridMapLogOddsFunctions;

GridMapLogOddsFunctions * new_GridMapLogOddsFunctions();
void GridMapLogOddsFunctions_updateSetOccupied(SwigObj * carg1, SwigObj * carg2);
void GridMapLogOddsFunctions_updateSetFree(SwigObj * carg1, SwigObj * carg2);
void GridMapLogOddsFunctions_updateUnsetFree(SwigObj * carg1, SwigObj * carg2);
float GridMapLogOddsFunctions_getGridProbability(SwigObj * carg1, SwigObj * carg2);
void GridMapLogOddsFunctions_setUpdateFreeFactor(SwigObj * carg1, float carg2);
void GridMapLogOddsFunctions_setUpdateOccupiedFactor(SwigObj * carg1, float carg2);
void delete_GridMapLogOddsFunctions(GridMapLogOddsFunctions * carg1);

typedef SwigObj HectorSlamProcessor;

HectorSlamProcessor * new_HectorSlamProcessor_f_i_i_rcEigen_Vector2f_i_pDrawInterface_pHectorDebugInfoInterface(float carg1, int carg2, int carg3, SwigObj * carg4, int carg5, SwigObj * carg6, SwigObj * carg7);
HectorSlamProcessor * new_HectorSlamProcessor_f_i_i_rcEigen_Vector2f_i_pDrawInterface(float carg1, int carg2, int carg3, SwigObj * carg4, int carg5, SwigObj * carg6);
HectorSlamProcessor * new_HectorSlamProcessor_f_i_i_rcEigen_Vector2f_i(float carg1, int carg2, int carg3, SwigObj * carg4, int carg5);
void delete_HectorSlamProcessor(HectorSlamProcessor * carg1);
void HectorSlamProcessor_update_phectorslam_HectorSlamProcessor_rchectorslam_DataPointContainer_Sl_Eigen_Vector2f_Sg__rcEigen_Vector3f_b(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3, bool carg4);
void HectorSlamProcessor_update_phectorslam_HectorSlamProcessor_rchectorslam_DataPointContainer_Sl_Eigen_Vector2f_Sg__rcEigen_Vector3f(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3);
void HectorSlamProcessor_reset(SwigObj * carg1);
SwigObj * HectorSlamProcessor_getLastScanMatchPose(SwigObj * carg1);
SwigObj * HectorSlamProcessor_getLastScanMatchCovariance(SwigObj * carg1);
float HectorSlamProcessor_getScaleToMap(SwigObj * carg1);
int HectorSlamProcessor_getMapLevels(SwigObj * carg1);
SwigObj * HectorSlamProcessor_getGridMap_pchectorslam_HectorSlamProcessor_i(SwigObj * carg1, int carg2);
SwigObj * HectorSlamProcessor_getGridMap_pchectorslam_HectorSlamProcessor(SwigObj * carg1);
void HectorSlamProcessor_addMapMutex(SwigObj * carg1, int carg2, SwigObj * carg3);
/*aaaaaa*/SwigObj * HectorSlamProcessor_getMapMutex(SwigObj * carg1, int carg2);
void HectorSlamProcessor_setUpdateFactorFree(SwigObj * carg1, float carg2);
void HectorSlamProcessor_setUpdateFactorOccupied(SwigObj * carg1, float carg2);
void HectorSlamProcessor_setMapUpdateMinDistDiff(SwigObj * carg1, float carg2);
void HectorSlamProcessor_setMapUpdateMinAngleDiff(SwigObj * carg1, float carg2);

typedef SwigObj DataPointContainer_Eigen_Vector2f;

DataPointContainer_Eigen_Vector2f * new_DataPointContainer_Eigen_Vector2f_i(int carg1);
DataPointContainer_Eigen_Vector2f * new_DataPointContainer_Eigen_Vector2f();
void DataPointContainer_Eigen_Vector2f_setFrom(SwigObj * carg1, SwigObj * carg2, float carg3);
void DataPointContainer_Eigen_Vector2f_add(SwigObj * carg1, SwigObj * carg2);
void DataPointContainer_Eigen_Vector2f_clear(SwigObj * carg1);
int DataPointContainer_Eigen_Vector2f_getSize(SwigObj * carg1);
SwigObj * DataPointContainer_Eigen_Vector2f_getVecEntry(SwigObj * carg1, int carg2);
SwigObj * DataPointContainer_Eigen_Vector2f_getOrigo(SwigObj * carg1);
void DataPointContainer_Eigen_Vector2f_setOrigo(SwigObj * carg1, SwigObj * carg2);
void delete_DataPointContainer_Eigen_Vector2f(DataPointContainer_Eigen_Vector2f * carg1);

typedef SwigObj GridMap;

GridMap * new_GridMap(float carg1, SwigObj * carg2, SwigObj * carg3);
void delete_GridMap(GridMap * carg1);
void GridMap_updateSetOccupied(SwigObj * carg1, int carg2);
void GridMap_updateSetFree(SwigObj * carg1, int carg2);
void GridMap_updateUnsetFree(SwigObj * carg1, int carg2);
float GridMap_getGridProbabilityMap(SwigObj * carg1, int carg2);
bool GridMap_isOccupied_pchectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__i_i(SwigObj * carg1, int carg2, int carg3);
bool GridMap_isFree_pchectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__i_i(SwigObj * carg1, int carg2, int carg3);
bool GridMap_isOccupied_pchectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__i(SwigObj * carg1, int carg2);
bool GridMap_isFree_pchectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__i(SwigObj * carg1, int carg2);
float GridMap_getObstacleThreshold(SwigObj * carg1);
void GridMap_setUpdateFreeFactor(SwigObj * carg1, float carg2);
void GridMap_setUpdateOccupiedFactor(SwigObj * carg1, float carg2);
void GridMap_updateByScan(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3);
void GridMap_updateLineBresenhami_phectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__rcEigen_Vector2i_rcEigen_Vector2i_unsigned_SS_int(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3, unsigned int carg4);
void GridMap_updateLineBresenhami_phectorslam_OccGridMapBase_Sl_LogOddsCell_Sc_GridMapLogOddsFunctions_Sg__rcEigen_Vector2i_rcEigen_Vector2i(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3);
void GridMap_bresenhamCellFree(SwigObj * carg1, unsigned int carg2);
void GridMap_bresenhamCellOcc(SwigObj * carg1, unsigned int carg2);
void GridMap_bresenham2D(SwigObj * carg1, unsigned int carg2, unsigned int carg3, int carg4, int carg5, int carg6, unsigned int carg7);

typedef SwigObj GridMapBase_LogOddsCell;

bool GridMapBase_LogOddsCell_hasGridValue(SwigObj * carg1, int carg2, int carg3);
SwigObj * GridMapBase_LogOddsCell_getMapDimensions(SwigObj * carg1);
int GridMapBase_LogOddsCell_getSizeX(SwigObj * carg1);
int GridMapBase_LogOddsCell_getSizeY(SwigObj * carg1);
bool GridMapBase_LogOddsCell_pointOutOfMapBounds(SwigObj * carg1, SwigObj * carg2);
void GridMapBase_LogOddsCell_reset(GridMapBase_LogOddsCell * carg1);
void GridMapBase_LogOddsCell_clear(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getMapDimProperties(SwigObj * carg1);
GridMapBase_LogOddsCell * new_GridMapBase_LogOddsCell_f_rcEigen_Vector2i_rcEigen_Vector2f(float carg1, SwigObj * carg2, SwigObj * carg3);
void delete_GridMapBase_LogOddsCell(GridMapBase_LogOddsCell * carg1);
void GridMapBase_LogOddsCell_allocateArray(SwigObj * carg1, SwigObj * carg2);
void GridMapBase_LogOddsCell_deleteArray(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getCell_phectorslam_GridMapBase_Sl_LogOddsCell_Sg__i_i(SwigObj * carg1, int carg2, int carg3);
SwigObj * GridMapBase_LogOddsCell_getCell_pchectorslam_GridMapBase_Sl_LogOddsCell_Sg__i_i(SwigObj * carg1, int carg2, int carg3);
SwigObj * GridMapBase_LogOddsCell_getCell_phectorslam_GridMapBase_Sl_LogOddsCell_Sg__i(SwigObj * carg1, int carg2);
SwigObj * GridMapBase_LogOddsCell_getCell_pchectorslam_GridMapBase_Sl_LogOddsCell_Sg__i(SwigObj * carg1, int carg2);
void GridMapBase_LogOddsCell_setMapGridSize(SwigObj * carg1, SwigObj * carg2);
GridMapBase_LogOddsCell * copy_GridMapBase_LogOddsCell(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getWorldCoords(SwigObj * carg1, SwigObj * carg2);
SwigObj * GridMapBase_LogOddsCell_getMapCoords(SwigObj * carg1, SwigObj * carg2);
SwigObj * GridMapBase_LogOddsCell_getWorldCoordsPose(SwigObj * carg1, SwigObj * carg2);
SwigObj * GridMapBase_LogOddsCell_getMapCoordsPose(SwigObj * carg1, SwigObj * carg2);
void GridMapBase_LogOddsCell_setDimensionProperties_phectorslam_GridMapBase_Sl_LogOddsCell_Sg__rcEigen_Vector2f_rcEigen_Vector2i_f(SwigObj * carg1, SwigObj * carg2, SwigObj * carg3, float carg4);
void GridMapBase_LogOddsCell_setDimensionProperties_phectorslam_GridMapBase_Sl_LogOddsCell_Sg__rcMapDimensionProperties(SwigObj * carg1, SwigObj * carg2);
void GridMapBase_LogOddsCell_setMapTransformation(SwigObj * carg1, SwigObj * carg2, float carg3);
float GridMapBase_LogOddsCell_getScaleToMap(SwigObj * carg1);
float GridMapBase_LogOddsCell_getCellLength(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getWorldTmap(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getWorldTmap3D(SwigObj * carg1);
SwigObj * GridMapBase_LogOddsCell_getMapTworld(SwigObj * carg1);
void GridMapBase_LogOddsCell_setUpdated(SwigObj * carg1);
int GridMapBase_LogOddsCell_getUpdateIndex(SwigObj * carg1);

#endif /* _HectorSlam_proxy_H_ */
55 changes: 55 additions & 0 deletions hector_mapping/swig/hector_mapping.i
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
%module HectorSlam
%{
#define SWIG_FILE_WITH_INIT
%}
%{
#include <iostream>
#include <Eigen/Core>

Eigen::Vector2f * Eigen_Vector2f(float x, float y){
return new Eigen::Vector2f(x, y);
}
float Eigen_Vector2f_x(Eigen::Vector2f * v){ return v->x(); }
float Eigen_Vector2f_y(Eigen::Vector2f * v){ return v->y(); }

float Eigen_Vector3f_x(Eigen::Vector3f * v){ return v->x(); }
float Eigen_Vector3f_y(Eigen::Vector3f * v){ return v->y(); }
float Eigen_Vector3f_z(Eigen::Vector3f * v){ return v->z(); }

#include "include/hector_slam_lib/slam_main/HectorSlamProcessor.h"
#include "include/hector_slam_lib/scan/DataPointContainer.h"
#include "include/hector_slam_lib/map/GridMap.h"
%}

Eigen::Vector2f * Eigen_Vector2f(float x, float y);
float Eigen_Vector2f_x(Eigen::Vector2f * v);
float Eigen_Vector2f_y(Eigen::Vector2f * v);

float Eigen_Vector3f_x(Eigen::Vector3f * v);
float Eigen_Vector3f_y(Eigen::Vector3f * v);
float Eigen_Vector3f_z(Eigen::Vector3f * v);


/*************************************************************/
%include <Eigen/Core>
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW // dummy defined to pass swig
// IGNORE : GridMapBase.h:349:30: error: no match for ‘operator[]’ (operand types are ‘LogOddsCell’ and ‘int’)
%ignore hectorslam::GridMapBase::getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const;
%include "include/hector_slam_lib/map/GridMapLogOdds.h"
%include "include/hector_slam_lib/map/GridMapBase.h"
%include "include/hector_slam_lib/map/OccGridMapBase.h"
%include "include/hector_slam_lib/scan/DataPointContainer.h"
%include "include/hector_slam_lib/slam_main/HectorSlamProcessor.h"

%template(DataPointContainer_Eigen_Vector2f) hectorslam::DataPointContainer<Eigen::Vector2f>;
namespace hectorslam {
%template(GridMap) OccGridMapBase<LogOddsCell, GridMapLogOddsFunctions>;
%template(GridMapBase_LogOddsCell) GridMapBase<LogOddsCell>;

}






Loading

0 comments on commit 6805fa3

Please sign in to comment.