First commit before linking actual libraries

CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cosmotechs_driver)
+
+## Add support for C++11, supported in ROS Kinetic and newer
+# add_definitions(-std=c++11)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  actionlib
+  actionlib_msgs
+  message_generation
+)
+
+## Uncomment this if the package has a setup.py.
+catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+add_service_files(DIRECTORY srv)
+add_action_files(DIRECTORY action)
+
+generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(INCLUDE_DIRS include)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(pcio32ha_node src/pcio32ha_node.cpp)
+add_executable(pcpg23i_node src/pcpg23i_node.cpp)
+
+## Rename C++ executable without prefix
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+add_dependencies(pcio32ha_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(pcpg23i_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(pcio32ha_node ${catkin_LIBRARIES})
+target_link_libraries(pcpg23i_node ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+## Mark executable scripts (Python etc.) for installation
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS pcio32ha_node pcpg23i_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+#############
+## Testing ##
+#############
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/cosmotechs.test)
+endif()
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cosmotechs_driver.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

README.md

@@ -0,0 +1,27 @@
+# cosmotechs_driver
+
+This package contains driver nodes for control and mesurement board by
+[CosmoTechs](http://www.cosmotechs.co.jp/). 
+
+The supported products are:
+
+- Dual axis motor control board [PCPG-23I(F)](http://www.cosmotechs.co.jp/products/?id=1406335747-890867)
+- 32bit Digital I/O board [PCIO-32H/A](http://www.cosmotechs.co.jp/products/?id=1408345224-477949)
+
+# ROS Nodes
+
+## pcpg23i_node
+
+pcg23i_node is a driver for PCPG-23I(F). 
+
+### Actions
+
+- command (cosmotechs_driver/MultiJointPosition)
+
+## pcio32ha_node
+
+### Services
+
+- get_port (cosmotechs_driver/GetPort)
+- set_port (cosmotechs_driver/SetPort)
+

action/MultiJointPosition.action

@@ -0,0 +1,13 @@
+# Goal
+float64[] positions
+duration min_duration
+float64[] max_velocities
+---
+# Result
+float64[] positions
+---
+# Feedback
+Header header
+float64[] positions
+float64[] velocities
+float64[] errors

include/cosmotechs_driver/pcio32ha.h

@@ -0,0 +1,86 @@
+// header file of module pcidrv.o 
+#define OUT_P 	   1
+//#define IN_P  	   2
+#define IN_P  	   12
+#define MEMOUT_P   3
+#define MEMIN_P    4
+#define INTRSET_P  5
+#define INTRESET_P 6
+#define IN_P1  	   7
+#define MEMIN_P1   8
+#define WAIT_INTERRUPT 9
+#define GET_RESOURCE   13
+
+struct pmc_struct 
+{
+	unsigned int bsn;
+	unsigned int axis;
+	unsigned int adr;
+	unsigned int com;
+	unsigned long data;
+	unsigned char d;
+	unsigned int  i;
+};
+
+
+struct int_wait{
+	unsigned short timeout;
+	int err;
+
+};
+
+
+typedef struct{
+	unsigned char bus;		//PCIバス番号
+	unsigned char dev;		//PCIデバイス番号
+	unsigned char func;		//PCIファンクション番号
+	unsigned char baseclass;	//基本クラス
+	unsigned char subclass;		//サブクラス
+	unsigned char programif;
+	unsigned char revision;
+	unsigned char irq;		//Interrupt
+	unsigned char Bsn;		//Bsn
+	unsigned int  Mem_base;		//Memory Base Address
+	unsigned int  Io_base;		//IO Base Address
+} pciresource;
+
+
+//************PCIO32H ORIGINAL Define ************
+#define PCIO32H_MAX_SLOTS 16  //PCIO32H MAX SLOTS
+#define PCIO32H_MAX_PORT   7
+
+//IO map
+#define PCIO32H_PORT_1		0
+#define PCIO32H_PORT_2		1
+#define PCIO32H_PORT_3		2
+#define PCIO32H_PORT_4		3
+#define PCIO32H_BSN_SWITCH	4
+
+
+//ERROR CODE--------------------------------------------------------------------
+#define PCIO32H_SUCCESS			0  // SUCCESS
+#define PCIO32H_ERR_SYSTEM		1  // SYSTEM ERROR
+#define PCIO32H_ERR_NO_DEVICE		2  // NO DEVICE
+#define PCIO32H_ERR_IN_USE		3  // DEVICE USE
+#define PCIO32H_ERR_INVALID_BSN		4  // INVALID BSN
+#define PCIO32H_ERR_INVALID_PORT	6  // INVALID PORT
+#define PCIO32H_ERR_PARAMETER		7  // PARAMETER ERROR
+#define PCIO32H_ERR_PROC		8  // FUNC FAIL
+#define PCIO32H_ERR_USER_HANDLER	9  // USER HANDLER RUNNING
+
+
+void	ErrorSet( unsigned short Number, unsigned short Error );
+unsigned long Pcio32hwGetLastError( unsigned short wBsn );
+int Pcio32hwGetLibVersion( unsigned char *pbLibVersion);
+int Pcio32hwGetDrvVersion( unsigned char *pbDrvVersion);
+int Pcio32hwCreate( unsigned short wBsn );
+void Pcio32hwClose( void );
+int Pcio32hwGetResource( unsigned short wBsn, pciresource *pres );
+int Pcio32hwInPort( unsigned short wBsn,unsigned short wPort,unsigned char *pbData);
+int Pcio32hwOutPort( unsigned short wBsn,unsigned short wPort,unsigned char bData);
+int Pcio32hwInPortW( unsigned short wBsn,unsigned short wPort,unsigned short *pwData);
+int Pcio32hwOutPortW( unsigned short wBsn,unsigned short wPort,unsigned short wData);
+int Pcio32hwInPortDW( unsigned short wBsn,unsigned long *pdwData);
+int Pcio32hwOutPortDW( unsigned short wBsn,unsigned long dwData);
+int Pcio32hwInPortBit( unsigned short wBsn,unsigned char wBit,unsigned char *pbONOFF);
+int Pcio32hwOutPortBit( unsigned short wBsn,unsigned char wBit,unsigned char bONOFF);