Added project code

CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(ROBOT)
+
+set(CMAKE_MODULE_PATH
+    ${CMAKE_SOURCE_DIR}/Vision/cmake/modules
+    ${CMAKE_MODULE_PATH}
+    )
+
+################################################
+## Bring in dependent projects
+################################################
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(o3d3xx_camera REQUIRED)
+find_package(o3d3xx_framegrabber REQUIRED)
+find_package(o3d3xx_image REQUIRED)
+find_package(PCL 1.7.1 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+find_library(LIB_boost_system NAMES boost_system)
+
+################################################
+## Manage our compiler and linker flags
+################################################
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -std=c++11")
+
+include_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${CMAKE_SOURCE_DIR}/Robot/urlib
+  ${CMAKE_SOURCE_DIR}/Vision
+  )
+link_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS} 
+  ${CMAKE_SOURCE_DIR}/Robot/urlib
+  ${CMAKE_SOURCE_DIR}/Vision
+  )
+add_definitions(
+  ${PCL_DEFINITIONS}
+  ${OpenCV_DEFINITIONS}
+  )
+
+add_library (urlib 
+		${CMAKE_SOURCE_DIR}/Vision/AerobicVision.cpp
+		${CMAKE_SOURCE_DIR}/Robot/urlib/UR_Control.cpp
+		${CMAKE_SOURCE_DIR}/Robot/urlib/UR3_Lib.cpp
+ 		${CMAKE_SOURCE_DIR}/Robot/urlib/ur_communication.cpp
+		${CMAKE_SOURCE_DIR}/Robot/urlib/ur_realtime_communication.cpp
+		${CMAKE_SOURCE_DIR}/Robot/urlib/robot_state.cpp
+		${CMAKE_SOURCE_DIR}/Robot/urlib/robot_state_RT.cpp)
+
+target_link_libraries (urlib ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES}
+		      )
+
+
+## Build paths - add project files here
+
+## 2d depth image blob detector
+add_executable (binpicker binpicker.cpp)
+target_link_libraries (binpicker urlib)
+
+

CMakeLists.txt~

@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(aerobic_bin_picking)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  hardware_interface
+  actionlib
+  control_msgs
+  geometry_msgs
+  sensor_msgs
+  trajectory_msgs
+  ur_msgs
+  tf
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES aerobic_bin_picking
+  CATKIN_DEPENDS
+  roscpp
+  hardware_interface
+  actionlib
+  control_msgs
+  geometry_msgs
+  sensor_msgs
+  trajectory_msgs
+  ur_msgs
+  tf
+  std_msgs
+  DEPENDS hardware_interface
+)
+
+###########
+## Build ##
+###########
+
+# check c++11 / c++0x
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+    set(CMAKE_CXX_FLAGS "-std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+    set(CMAKE_CXX_FLAGS "-std=c++0x")
+else()
+    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ")
+endif()
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(ur_test src/ur_test.cpp)
+target_link_libraries(ur_test ${catkin_LIBRARIES})
+add_dependencies(ur_test aerobic_bin_picking_generate_messages_cpp)
+## Declare a C++ library
+# add_library(aerobic_bin_picking
+#   src/${PROJECT_NAME}/aerobic_bin_picking.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(aerobic_bin_picking ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(aerobic_bin_picking_node src/aerobic_bin_picking_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(aerobic_bin_picking_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(aerobic_bin_picking_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS aerobic_bin_picking aerobic_bin_picking_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}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_aerobic_bin_picking.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)

Robot/CMakeLists.txt

@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(ROBOT)
+
+set(CMAKE_MODULE_PATH
+    ${CMAKE_SOURCE_DIR}/../Vision/cmake/modules
+    ${CMAKE_MODULE_PATH}
+    )
+
+################################################
+## Bring in dependent projects
+################################################
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(o3d3xx_camera REQUIRED)
+find_package(o3d3xx_framegrabber REQUIRED)
+find_package(o3d3xx_image REQUIRED)
+find_package(PCL 1.7.1 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+find_library(LIB_boost_system NAMES boost_system)
+
+################################################
+## Manage our compiler and linker flags
+################################################
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -std=c++11")
+
+include_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${CMAKE_SOURCE_DIR}/urlib
+  )
+link_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS} 
+  ${CMAKE_SOURCE_DIR}/urlib
+  )
+add_definitions(
+  ${PCL_DEFINITIONS}
+  ${OpenCV_DEFINITIONS}
+  )
+
+add_library (urlib 
+		${CMAKE_SOURCE_DIR}/../Vision/AerobicVision.cpp
+		${CMAKE_SOURCE_DIR}/urlib/UR_Control.cpp
+		${CMAKE_SOURCE_DIR}/urlib/UR3_Lib.cpp
+ 		${CMAKE_SOURCE_DIR}/urlib/ur_communication.cpp
+		${CMAKE_SOURCE_DIR}/urlib/ur_realtime_communication.cpp
+		${CMAKE_SOURCE_DIR}/urlib/robot_state.cpp
+		${CMAKE_SOURCE_DIR}/urlib/robot_state_RT.cpp)
+
+target_link_libraries (urlib ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES}
+		      )
+
+
+## Build paths - add project files here
+
+## 2d depth image blob detector
+add_executable (main Main.cpp)
+target_link_libraries (main urlib)
+
+

Robot/Main.cpp

@@ -0,0 +1,37 @@
+//#include "UR3_Lib.h"
+#include "UR_Control.h"
+
+int main()
+{		
+	unsigned int usecs = 1000000; //1sec = 1.000.000 usec
+	//UR3* test = new UR3();
+	Binzone* zone = new Binzone();
+	Control* robot = new Control(0.0001,3.0, zone);
+	cout << "Starting in 2 seconds" << endl;
+	usleep(2000000);
+
+	/*
+	std::thread safety(&Control::safetyTest, robot_);
+	cout << "Safety Thread Started" << endl;
+	std::thread binpicking(&Control::Demo, robot_);
+	cout << "Started Bin Picking Thread" << endl;
+	safety.join();
+	binpicking.join();*/
+	
+	robot->Demo();
+	cout << "Done, shutting down" << endl;
+    return 0;
+}
+
+//Control* robot2_ = new Control(0.0001,3.0, zone);
+	/*while(true){	
+		cout << "Starting in 2 seconds" << endl;
+		usleep(2000000);
+		
+		//robot_->Demo(3.0); //ADD SLEEP!
+		robot_->Demo();
+		//robot_->robot_->IO_test();		
+	}*/
+
+//c++ UR3_Lib.cpp robot_state.cpp robot_state_RT.cpp ur_realtime_communication.cpp ur_communication.cpp UR_Control.cpp Main.cpp -o test -pthread -std=c++11
+

Robot/urlib/UR3_Lib.cpp

@@ -0,0 +1,296 @@
+//#include <boost/asio.hpp>
+//#include <boost/bind.hpp>
+//#include <boost/thread.hpp>
+//#include <boost/signals2.hpp>
+//#include <boost/array.hpp>
+//#include <boost/lexical_cast.hpp>
+#include "UR3_Lib.h"
+
+using namespace std;
+
+//TODO: COPY UR_DRIVER FUNCTIONS
+//TODO: IMPLEMENT DELAY FOR ALL GET FUNCTIONS BECAUSE OF REFRESH RATE DATA (+/- 128Hz)
+
+
+template <typename T> std::string UR3::to_string(T const& value){
+	stringstream sstr;
+	sstr << value;
+	return sstr.str();
+}
+
+UR3::UR3(){
+	condition_variable msg_cond;
+	unsigned int safety_count_max = 12;
+	rt_interface_ = new UrRealtimeCommunication(msg_cond, "192.168.1.11", safety_count_max);
+	sec_interface_ = new UrCommunication(msg_cond, "192.168.1.11");
+	if(!sec_interface_->start()){
+		cout << "Communication not started" << endl;
+	}
+	firmware_version_ = sec_interface_->robot_state_->getVersion();
+	cout << "Firmware Version: " << firmware_version_ << endl;	
+	rt_interface_->robot_state_->setVersion(firmware_version_);
+
+	if(!rt_interface_->start()){
+		cout << "RT Communication not started" << endl;
+	}else{
+		cout << "RT Communication started" << endl;
+	}
+
+}
+
+void UR3::_send(std::string message){
+	rt_interface_->addCommandToQueue(message);
+}
+
+void UR3::move(double x, double y, double z, double rx, double ry, double rz){
+	string xco, yco, zco, rxo, ryo,rzo, message;
+	xco = to_string(x);
+	yco = to_string(y);
+	zco = to_string(z);
+	rxo = to_string(rx);
+	ryo = to_string(ry);
+	rzo = to_string(rz);
+	
+	message = "movej(p[" +xco + ", " + yco + ", " + zco + ", " +rxo+", "+ryo+", "+rzo+"], a=1.0,v=1.0)\nend\n"; //a=1.0,v=1.0
+	//message = "movej(p[" +xco + ", " + yco + ", " + zco + "], a=1.0,v=1.0)\nend\n";
+	_send(message);
+}
+
+void UR3::moveLinear(double x, double y, double z, double rx, double ry, double rz){
+	string xco, yco, zco, rxo, ryo,rzo, message;
+	xco = to_string(x);
+	yco = to_string(y);
+	zco = to_string(z);
+	rxo = to_string(rx);
+	ryo = to_string(ry);
+	rzo = to_string(rz);
+	
+	message = "movel(p[" +xco + ", " + yco + ", " + zco + ", " +rxo+", "+ryo+", "+rzo+"], a=0.2,v=0.2)\nend\n";
+	_send(message);
+}
+
+void UR3::moveHome(){
+	string message = "movep(p[0.0,-getActualPosition90.0,0.0,-90.0,0.0,0.0], a=0.2,v=0.2)\nend\n"; //a=0.2,v=0.2
+	_send(message);
+}
+
+void UR3::moveGripper(bool close){
+	string message;
+	if(close)
+		message = "speedj([0.0,0.0,0.0,0.0,0.0,2.5], 1.0, 3.3)\nend\n"; //a=0.2,v=0.2
+	else{
+		message = "speedj([0.0,0.0,0.0,0.0,0.0,-2.5], 1.0, 3.3)\nend\n"; //a=0.2,v=0.2
+	}
+	_send(message);
+	usleep(3100000);
+	_send("stopj(1.0)\nend\n");
+}
+void UR3::openCloseRoboticGripper(bool close){
+	/*
+	string message = "rq_reset()\nrq_activate()\n";
+	//_send("rq_reset()\nend\n");
+	//usleep(500000);
+	//_send("rq_activate()\nend\n");
+	//usleep(500000);
+	if(close)
+		message += "rq_move(0)\nend\n"; 
+	else{
+		message += "rq_move(255)\nend\n"; 
+		//message = "rqc_open()\nend\n"; 
+	}
+	cout << message;
+	_send(message);
+	usleep(1000000);
+	*/
+
+	/*
+	string message = "socket_close(\"gripper_socket\")\nsync()\nsocket_open(\"127.0.0.1\", 63352, \"gripper_socket\")\nsync()\n";
+	message += "socket_set_var(\"GTO\", 1, \"gripper_socket\")\n";
+	message += "socket_set_var(\"FOR\", 255, \"gripper_socket\")\n";
+	message += "socket_set_var(\"SPE\", 150, \"gripper_socket\")\n";
+
+	if(close)
+		message += "socket_set_var(\"POS\", 0, \"gripper_socket\")\n";
+	else{
+		message += "socket_set_var(\"POS\", 255, \"gripper_socket\")\n";
+		//message = "rqc_open()\nend\n"; 
+	}
+
+	message += "end\n";
+
+	_send(message);
+	*/
+
+	_send("socket_close(\"gripper_socket\")\n");
+	usleep(100000);
+	_send("socket_open(\"127.0.0.1\", 63352, \"gripper_socket\")");
+	usleep(100000);
+	_send("socket_set_var(\"GTO\", 1, \"gripper_socket\")\n");
+	usleep(100000);
+	_send("sleep(1)\n");
+	usleep(1000000);
+	_send("socket_set_var(\"FOR\", 255, \"gripper_socket\")\n");
+	usleep(100000);
+	_send("socket_set_var(\"SPE\", 150, \"gripper_socket\")\n");
+	usleep(100000);
+
+	if(close)
+		_send("socket_set_var(\"POS\", 0, \"gripper_socket\")\n");
+	else{
+		_send("socket_set_var(\"POS\", 255, \"gripper_socket\")\n");
+		//message = "rqc_open()\nend\n"; 
+	}
+	usleep(100000);
+	_send("end\n");
+
+	usleep(3000000);
+}
+std::vector<bool> UR3::getDigitalInputs(){
+	usleep(1000);
+	//vector<bool> data = rt_interface_->robot_state_->getDigitalInputBits(); //RT dig in function - High speed
+	unsigned int data = sec_interface_->robot_state_->getDigitalInputBits();
+	unsigned int check = 1;
+	vector<bool> ret;
+	for(int i = 0; i < 18; i++){
+		if((data & check) == 1){ ret.push_back(true);}
+		else{ret.push_back(false);}
+		data = data >> 1;
+	}
+	return ret;
+}
+
+std::vector<bool> UR3::getDigitalOutputs(){
+	usleep(1000);
+	unsigned int data = sec_interface_->robot_state_->getDigitalOutputBits();
+	//cout << "Digital Outputs: " << data << endl;
+	unsigned int check = 1;
+	vector<bool> ret;
+	for(int i = 0; i < 18; i++){
+		if((data & check) == 1){ ret.push_back(true);}
+		else{ret.push_back(false);}
+		data = data >> 1;
+	}
+	return ret;
+}
+
+void UR3::setDigitalOut(unsigned int pin, bool state){
+	string p,s,message; 
+	p = to_string(pin);
+	s = to_string(state ? "True" : "False");
+	message = "sec setOut():\n\tset_standard_digital_out("+p+","+s+")\nend\n";
+	_send(message);
+}
+
+std::vector<float> UR3::getAnalogInputs(){
+	vector<float> ret;
+	ret.push_back(sec_interface_->robot_state_->getAnalogInput0());
+	ret.push_back(sec_interface_->robot_state_->getAnalogInput1());
+	return ret;
+}
+
+std::vector<float> UR3::getAnalogOutputs(){
+	vector<float> ret;
+	ret.push_back(sec_interface_->robot_state_->getAnalogOutput0());
+	ret.push_back(sec_interface_->robot_state_->getAnalogOutput1());
+	return ret;
+}
+
+void UR3::setAnalogOut(unsigned int pin, float value){
+	string p,v,message;
+	p = to_string(pin);
+	v = to_string(value);
+	message = "sec setOut():\n\tset_analog_out("+p+","+v+")\nend\n"; 
+	_send(message);
+}
+
+vector<double> UR3::getActualPosition(){
+	vector<double> ret = rt_interface_->robot_state_->getToolVectorActual();
+	return ret;
+}
+
+bool UR3::getSafety_mode(){
+	double ret = rt_interface_->robot_state_->getSafety_mode();
+	if(ret == 3.0){
+		cout << "Safety: Safety Stop" << endl;
+		return false;	
+	}else if(ret == 2.0){
+		cout << "Safety: Limited Operational" << endl;
+		return true;
+	}else{
+		return true;
+	}
+}
+
+void UR3::IO_test(){
+	vector<bool> dig_in = getDigitalInputs();
+	for(int i = 0; i < dig_in.size(); i++){
+		cout << "Digital Input Pin " << i << " - State: " << dig_in[i] << endl;
+	}
+	vector<bool> dig_out = getDigitalOutputs();
+	for(int i = 0; i < dig_out.size(); i++){
+		cout << "Digital Output Pin " << i << " - State: " << dig_out[i] << endl;
+	}
+	vector<float> analog_in = getAnalogInputs();
+	for(int i = 0; i < analog_in.size(); i++){
+		cout << "Analog Input Pin " << i << " - Value: " << analog_in[i] << endl;
+	}
+	vector<float> analog_out = getAnalogOutputs();
+	for(int i = 0; i < analog_out.size(); i++){
+		cout << "Analog Output Pin " << i << " - Value: " << analog_out[i] << endl;
+	}
+}
+
+void UR3::move_test(){
+		double rx, ry, rz;
+		rx = 0.0395;
+		ry = 3.1199;
+		rz = -0.0102;
+
+		setDigitalOut(0, true);
+		
+		move(0.43269,-0.05873,0.12940,rx,ry,rz);	//move above object at A	
+		move(0.43269,-0.05873,-0.08640,rx,ry,rz);	//touch object at A
+		setDigitalOut(0, false);			//vacuum at A
+		move(0.43269,-0.05873,0.12940,rx,ry,rz);	//remove object at A
+		//move(-0.04905,-0.33974,0.72268,0.4840,0.5834,-1.5299,time);
+		move(-0.30924,-0.26365,0.12940,rx,ry,rz);	//move object A to B
+		move(-0.30924,-0.26365,-0.08640,rx,ry,rz);	//touch object at B
+		setDigitalOut(0, true);				//release vacuum at B
+		move(-0.30924,-0.26365,0.12940,rx,ry,rz);	//remove object at B
+		move(-0.30924,-0.26365,-0.08640,rx,ry,rz);	//touch object at B
+		setDigitalOut(0, false);			//vacuum at B
+		move(-0.30924,-0.26365,0.12940,rx,ry,rz);	//remove object at B
+		//move(-0.04905,-0.33974,0.72268,0.4840,0.5834,-1.5299,time);
+		move(0.43269,-0.05873,0.12940,rx,ry,rz);	//move object B to A
+		move(0.43269,-0.05873,-0.08640,rx,ry,rz);	//touch object at A
+		setDigitalOut(0, true);				//vacuum at A
+		move(0.43269,-0.05873,0.12940,rx,ry,rz);	//remove object at A
+
+}
+
+void UR3::Halt(){
+	rt_interface_->halt();
+}
+
+void UR3::stop(){
+	string message = "sec setOut():\n\tstop()\nend\n";
+	_send(message);
+}
+
+bool UR3::getEmergencyStop(){
+	bool ret = sec_interface_->robot_state_->isEmergencyStopped();
+	if(ret){cout << "Emergency Stop" << endl;}
+	return ret;
+}
+
+bool UR3::getProtectiveStop(){
+	bool ret = sec_interface_->robot_state_->isEmergencyStopped();
+	if(ret){cout << "Protective Stop" << endl;}
+	return sec_interface_->robot_state_->isProtectiveStopped();
+}
+
+std::vector<double> UR3::getMotorTemperatures(){
+	return rt_interface_->robot_state_->getMotorTemperatures();
+}
+
+

Robot/urlib/UR3_Lib.h

@@ -0,0 +1,80 @@
+#ifndef UR_TEST_H_
+#define UR_TEST_H_
+
+#include <netdb.h>
+#include <unistd.h>
+#include <chrono>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <endian.h>
+#include <semaphore.h>
+
+#include <string>
+#include <queue>
+#include <stdexcept>
+#include <cstdarg>
+
+#include <iostream>
+#include <sstream>
+#include <unistd.h>
+#include <string>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+
+#include "robot_state.h"
+#include "robot_state_RT.h"
+#include "ur_realtime_communication.h" 
+#include "ur_communication.h"
+
+//TODO: MOVE GETBINSTATE TO UR_CONTROL
+
+class UR3{
+	private:
+		double firmware_version_;
+		string ip_addr_;
+	public:
+		UR3();
+
+		//Communication
+		UrRealtimeCommunication* rt_interface_;
+		UrCommunication* sec_interface_;
+		
+		void _send(std::string message);
+		void Halt();
+
+		//Support functions
+		template <typename T> std::string to_string(T const& value);
+	
+		//Setters
+		void stop();
+		void move(double x, double y, double z, double rx, double ry, double rz);
+		void moveLinear(double x, double y, double z, double rx, double ry, double rz);
+		void moveHome();
+		void moveGripper(bool dir);
+		void setDigitalOut(unsigned int pin, bool state);
+		void setAnalogOut(unsigned int pin, float value); //value range 0:1, maximum = 10V
+
+		//Getters
+		vector<double> getActualPosition();
+		std::vector<bool> getDigitalInputs();		//bit 0 - 7 = Digital Input, bit 8 - 15 = Configurable Digital Input, bit 16 - 17 = Tool Digital Input	
+		std::vector<bool> getDigitalOutputs();		//bit 0 - 7 = Digital Output, bit 8 - 15 = Configurable Digital Output, bit 16 - 17 = Tool Digital Output
+		std::vector<double> getMotorTemperatures();
+		std::vector<float> getAnalogInputs();
+		std::vector<float> getAnalogOutputs();
+		bool getSafety_mode();
+		bool getEmergencyStop();
+		bool getProtectiveStop();
+
+		//Test software
+		void move_test();
+		void IO_test();
+		void openCloseRoboticGripper(bool close);
+		void RotateGripper90Degrees(bool clockwise);
+		
+};
+
+#endif /* UR_TEST_H_ */

Robot/urlib/UR_Control.cpp

@@ -0,0 +1,449 @@
+#include "UR_Control.h"
+
+//TODO: INSERT TIMEOUT TO MOVE_BASIC
+
+Control::Control(double offset, double speed, Binzone* zone){
+	robot = new UR3();
+	setOffset(offset);
+	setSpeed(speed);
+	zone_ = zone; //Zone is niet meer nodig
+	//RC0 = {0.0,0.0,0.0};
+	usleep(1000000);
+	//Move_Calibrate();
+	//readCalibration();	
+	usleep(1000000);
+	highestZ = 0;
+	safety = true;
+	cout << "Initialized Robot" << endl;
+}
+
+void Control::setSpeed(double speed){
+	speed_ = speed;
+}
+		
+double Control::getSpeed(){
+	return speed_;
+}
+
+void Control::setOffset(double offset){
+	offset_ = offset;
+}
+
+double Control::getOffset(){
+	return offset_;
+}
+
+bool Control::getBinState(){
+	vector<bool> inputs = robot->getDigitalInputs();
+	if(!inputs[INPUT_BIN]){
+		cout << "Bin not placed" << endl;
+	}
+	return inputs[INPUT_BIN];
+}
+
+bool Control::toolSafety(){
+	bool ret = true;
+	vector<bool> data = robot->getDigitalInputs();
+	if(data[TOOL_IO0] || data[TOOL_IO1]){
+		ret = false;	
+		cout << "Tool Safety Ring activated" << endl;
+	}
+	return ret;
+}
+
+bool Control::safetyCheck(){
+
+	if(!robot->getSafety_mode() || !getBinState() || robot->getEmergencyStop() || robot->getProtectiveStop() || !toolSafety()){
+		return false;
+	}else{
+		return true;	
+	}
+}
+
+bool Control::safetyTest(){	
+	while(safetyCheck()){
+	}
+	return false;
+}
+
+void Control::shutDown(){
+	robot->Halt();
+}
+
+void Control::moveGripper(bool dir)
+{
+	robot->moveGripper(dir);
+}
+
+void Control::Move_Basic(double x, double y, double z, bool linear, double rx, double ry, double rz){
+
+/*
+	if(rx == -1 || ry == -1 || rz == -1)
+	{
+		vector<double> pos = getRobotLocation();
+		rx = pos[3];
+		ry = pos[4];
+		rz = pos[5];
+	}
+*/
+	if(safety){	
+		if(!linear){
+			robot->move(x,y,z,rx,ry,rz);
+		}else{
+			robot->moveLinear(x,y,z,rx,ry,rz);
+		}
+
+		vector<double> posData = robot->getActualPosition();
+		double offset = getOffset();
+
+		while((posData[0] > x + offset) || (posData[0] < x - offset) 	|| (posData[1] > y + offset) || (posData[1] < y - offset) 	
+			|| (posData[2] > z + offset) || (posData[2] < z - offset) ||	(posData[3] > rx + 0.001) || (posData[3] < rx - 0.005) 	
+			|| (posData[4] > ry + 0.005) || (posData[4] < ry - 0.005) 	|| (posData[5] > rz + 0.005) || (posData[5] < rz - 0.005)){
+			posData = robot->getActualPosition();
+			if(!safetyCheck()){
+				safety = false;
+				robot->stop();
+				break;
+			}
+		}	
+	}	
+}
+
+void Control::Move_StaticZ(double x, double y){
+	double z;
+	z = 0.350;
+	vector<double> posData = robot->getActualPosition();
+	Move_Basic(posData[0], posData[1], z);				// misschien Move_StaticXY van maken
+	Move_Basic(x, y, z);
+}
+
+void Control::Move_Initial(){
+	double x, y;
+	x = 0.10500;
+	y = -0.37900;
+	Move_StaticZ(x,y);
+}
+
+void Control::Move_Box(){
+	double x, y;
+	x = -0.31400;
+	y = -0.12330;
+	Move_StaticZ(x,y);
+}
+
+void Control::Move_Export(){
+	double x, y;
+	x = 0.34905;
+	y = -0.01155;
+	Move_StaticZ(x,y);
+}
+
+void Control::Move_Approach(double z, double rx, double ry, double rz){
+	vector<double> posData  = robot->getActualPosition();
+	Move_Basic(posData[0], posData[1], z,true, rx, ry, rz);				// misschien Move_StaticXY van maken
+}
+
+void Control::moveToCalibratePosition(){
+	Move_Initial();
+	Move_Box();
+	double rx = 0.000;
+	double ry = 0.000;
+	double rz = 0.000;
+	double x = -0.28200; 	//-304mm
+	double y = -0.13900; 	//-135mm
+	double z = 0.11000;	//210mm
+	Move_Basic(x, y, z, false);
+	usleep(1000000);
+	Move_Basic(x,y,0.33000,false,rx,ry,rz);
+	cout << "Place Calibration Head and Press Enter" << endl;	
+	cin.get();
+	usleep(1000000);
+}
+
+void Control::Move_Calibrate(pcl::PointXYZ calibrationPoint){
+	double rx = 0.000;
+	double ry = 0.000;
+	double rz = 0.000;
+	double x = -0.28200; 	//-304mm
+	double y = -0.13900; 	//-135mm
+	double z = 0.11000;	//210mm
+	
+	if(safety){
+		vector<double> posData  = robot->getActualPosition();
+		cout << "calibrationPoint(XYZ): " << calibrationPoint.x << " , " << calibrationPoint.y << " , " << calibrationPoint.z << endl;
+
+		cout << "RobotPosition(XYZ): " << posData[0] << " , " << posData[1] << " , " << posData[2] << endl;
+
+		RC0[0] = posData[0] - (calibrationPoint.x * -1);	//x
+	 	RC0[1] = posData[1] - (calibrationPoint.y * -1);	//y
+		RC0[2] = posData[2] - (calibrationPoint.z * -1);	//z
+	
+		saveCalibration();
+		usleep(1000000);
+		cout << "Place Gripper Head and Press Enter" << endl;
+		cin.get();
+		usleep(1000000);
+		Move_Basic(-0.511,-0.169,0.275, false, 1.172, -1.222,-1.045);
+		Move_Basic(x, y, z, false);	
+		Move_Box();
+		Move_Initial();	
+	}
+}
+
+void Control::saveCalibration(){
+	cout << "Saving Calibration..." << endl;
+	cv::Mat coordinates = (cv::Mat_<double>(3,1) << RC0[0] , RC0[1], RC0[2]);
+	cv::FileStorage fs("calibration.yml", cv::FileStorage::WRITE);
+	fs << "CalibrationCoordinates" << coordinates;
+	fs.release();
+	cout << "Calibration Saved!" << endl;
+}
+bool Control::readCalibration(){
+	cout << "Reading Calibration... " << endl;
+	cv::FileStorage fs("calibration.yml", cv::FileStorage::READ);
+	if(!fs.isOpened()){ return false; }
+	cv::Mat coordinates;
+	fs["CalibrationCoordinates"] >> coordinates;
+	cout << "x: " << coordinates.at<double>(0,0)<< endl;
+	cout << "y: " << coordinates.at<double>(0,1)<< endl;
+	cout << "z: " << coordinates.at<double>(0,2)<< endl;
+	RC0[0] = coordinates.at<double>(0,0);
+	RC0[1] = coordinates.at<double>(0,1);
+	RC0[2] = coordinates.at<double>(0,2);
+	cout << "Calibration Loaded!"<< endl;
+	return true;
+}
+
+bool Control::Touch_Object(double cameraX, double cameraY, double cameraZ, double rx, double ry, double rz){ //BEFORE IMPLEMENTATION Z: implement offset
+	if(safety){
+		
+		Move_Initial();
+		Move_Box();
+		//openCloseRoboticGripper(false);
+		double z_offset = 0.005; //0.005 == 5mm
+		
+		double x = RC0[0] + (cameraX * -1);
+		double y = RC0[1] + (cameraY * -1);
+		double z = RC0[2] - z_offset + (cameraZ * -1) + 0.145; //0.145 voor normalen  en 0.225 voor de verlengde stukken
+		highestZ = z + 0.05;
+		Move_StaticZ(x,y);
+		//cout << "open Gripper";
+		//usleep(2500000);
+		Move_Approach(highestZ, rx, ry, rz);
+		//robot->setDigitalOut(0, false);
+		Move_Approach(z, rx, ry, rz);
+
+	//	cout << "close Gripper";
+
+		//robot->setDigitalOut(0, false);
+		usleep(2500000);
+		Move_Approach(highestZ, rx, ry, rz);
+		Move_Approach(0.32);
+		Move_Box();
+		Move_Initial();
+		Move_Export();
+		//cout << "open Gripper";
+		usleep(500000);
+		//moveGripper(true);
+		//usleep(2500000);
+
+		/*
+		if(getVacuum()){
+			Move_Export();
+			Move_Approach(0.25);
+			robot->setDigitalOut(0, true);
+			Move_Approach(0.30);
+			Move_Initial();
+		}else{
+			robot->setDigitalOut(0, true);
+		}
+		*/
+		return true;
+	}else{
+		return false;
+	}
+}
+
+void Control::Move_Position_Tool(double rx, double ry, double rz){
+	vector<double> posData = robot->getActualPosition();
+	Move_Basic(posData[0], posData[1], posData[2], false, rx, ry, rz);
+
+}
+
+bool Control::getVacuum(){
+	vector<bool> inputs = robot->getDigitalInputs();
+	if(!inputs[VACUUM_SEN]){
+		cout << "Missing Object" << endl;
+	}
+	return inputs[VACUUM_SEN];	
+}
+
+
+Binzone::Binzone(){
+	taken = false;
+	owner = "none";
+	key_ = 0;
+}
+
+bool Binzone::isTaken(){
+	return taken;
+}
+
+int Binzone::takeZone(string owner){
+	int ret;
+	if(!isTaken()){		//Zone is free
+		ret = rand();
+		owner = owner;	
+		taken = true;
+		key_ = ret;
+		cout << "Zone is taken by " << owner << ". Generated Key: " << ret << endl;
+	}else{			//Zone is taken
+		cout << "Zone is already taken by " << getOwner() << endl;
+		ret = 0;
+	}
+	return ret;
+}
+
+string Binzone::getOwner(){
+	return owner;
+}
+
+bool Binzone::releaseZone(string owner, int key){
+	bool ret;	
+	if(isTaken()){	
+		cout << "Original Key: " << key_ << " - Received Key: " << key << endl;
+		if(key == key_){
+			cout << "Zone released" << endl;
+			owner = "none";
+			key_ = 0;
+			taken = false;
+			ret = true;
+		}else{
+			cout << "Key not correct" << endl;
+			ret = false;
+		}	
+	}else{
+		cout << "Zone is free" << endl;
+		ret = false;
+	}
+	return ret;
+}
+
+void Control::printRobotLocation()
+{
+	vector<double> posData = robot->getActualPosition();
+	for(int i = 0; i < posData.size();i++)
+	{
+		cout<< posData[i] <<"  :" << i<<" ";
+	}
+}
+
+vector<double> Control::getRobotLocation()
+{
+	vector<double> posData = robot->getActualPosition();
+	return posData;
+}
+
+void Control::openCloseRoboticGripper(bool close)
+{
+	robot->openCloseRoboticGripper(close);
+}
+
+void Control::RotateGripper90Degrees(){
+	
+}
+pcl::PointXYZ Control::getRxRyRz(double rollval,double pitchval,double yawval)
+{
+	double yaw[3][3] =  {
+						{cos(yawval), -sin(yawval), 0.0},
+						{sin(yawval), cos(yawval), 0.0},
+						{0.0, 0.0, 1.0}
+						};
+	
+	double pitch[3][3] = {
+						 {cos(pitchval), 0.0, sin(pitchval)},
+						 {0.0, 1.0, 0.0},
+						 {-sin(pitchval), 0.0, cos(pitchval)}
+						 };
+	
+	double roll[3][3] = {
+						{1.0, 0.0, 0.0},
+						{0.0, cos(rollval), -sin(rollval)},
+						{0.0, sin(rollval), cos(rollval)}
+						};
+	
+	double R[3][3];
+	double temp[3][3];
+
+	int i = 0;
+	int j = 0;
+	int k = 0;
+
+	for(i = 0; i < 3; ++i)
+	{
+        for(j = 0; j < 3; ++j)
+        {
+			temp[i][j]=0.0;
+			R[i][j]=0.0;
+        }
+	}
+
+	for(i = 0; i < 3; ++i)
+	{
+        for(j = 0; j < 3; ++j)
+		{
+            for(k = 0; k < 3; ++k)
+            {
+                temp[i][j] += yaw[i][k] * pitch[k][j];
+            }
+		}
+	}
+
+	for(i = 0; i < 3; ++i)
+	{
+        for(j = 0; j < 3; ++j)
+		{
+				cout << "T " << i << "," << j << " is " << temp[i][j] << endl;
+		}
+	}
+
+	for(i = 0; i < 3; ++i)
+	{
+        for(j = 0; j < 3; ++j)
+		{
+            for(k = 0; k < 3; ++k)
+            {
+                R[i][j] += temp[i][k] * roll[k][j];
+
+            }
+		}
+	}
+
+	for(i = 0; i < 3; ++i)
+	{
+        for(j = 0; j < 3; ++j)
+		{
+				cout << "R " << i << "," << j << " is " << R[i][j] << endl;
+		}
+	}
+
+
+	double theta = acos(((R[0][0] + R[1][1] + R[2][2]) - 1.0) / 2.0);
+
+	cout << "Theta " << theta << endl;
+
+
+	double multi = 1.0 / (2.0 * sin(theta));
+
+	cout << "Multi " << multi << endl;
+
+	double rx = multi * (R[2][1] - R[1][2]) * theta;
+	double ry = multi * (R[0][2] - R[2][0]) * theta;
+	double rz = multi * (R[1][0] - R[0][1]) * theta;
+
+	cout << rx << "," << ry << "," << rz << endl;
+
+	pcl::PointXYZ p(rx, ry, rz);
+
+	return p;
+}

Robot/urlib/UR_Control.h

@@ -0,0 +1,85 @@
+#ifndef UR_CONTROL_H_
+#define UR_CONTROL_H_
+
+#include <stdio.h>
+#include <string>
+#include <fstream>
+#include <iostream>
+
+#include <pcl/point_types.h>
+#include <opencv2/opencv.hpp>
+
+#include "UR3_Lib.h"
+
+class Binzone{
+	private:
+		bool taken;
+		string owner;
+		int key_;
+	public:
+		Binzone();
+		bool isTaken();
+		int takeZone(string owner);
+		string getOwner();
+		bool releaseZone(string owner, int key);
+};
+
+class Control{
+	private:
+		double offset_;
+		double speed_;
+	public:
+		Control(double offset, double speed, Binzone* zone);
+		UR3* robot;
+		Binzone* zone_;
+		double RC0[3]; //Camera nulpunten gezien vanuit Robot
+		float highestZ;
+		bool safety;
+
+		void saveCalibration();
+		bool readCalibration();
+	
+		void shutDown();
+		void Move_Basic(double x, double y, double z, bool linear = false, double rx = 0.0, double ry = 3.1415, double rz = 0.0); // 0, 180, 0 3.1415
+		void setSpeed(double speed);
+		double getSpeed();
+		void setOffset(double offset);
+		double getOffset();
+		
+		//Safety
+		bool safetyCheck();
+		bool getBinState();
+		bool toolSafety();
+		bool getVacuum();
+
+		bool safetyTest();
+
+		//"Defines" 	
+		int INPUT_BIN	=	0;		//Input Bin is on Digital Input #0
+		int VACUUM_SEN	=	1;
+		int OUTPUT_BIN 	= 	2;	
+		int TOOL_IO0 	= 	16;	
+		int TOOL_IO1 	= 	17;	
+
+
+		//TO BE TESTED
+		
+	    bool Touch_Object(double cameraX, double cameraY, double cameraZ, double rx = 0.0, double ry = 3.1415, double rz = 0.0);
+		void Move_StaticZ(double x, double y);
+		void Move_Box();
+		void Move_Initial();
+		void Move_Export();
+		void moveGripper(bool dir);
+
+		void moveToCalibratePosition();
+		void Move_Calibrate(pcl::PointXYZ calibrationPoint);
+		void Move_Approach(double z, double rx = 0.0, double ry = 3.1415, double rz = 0.0);	
+		void Move_Position_Tool(double rx, double ry, double rz);
+		void printRobotLocation();
+		void RotateGripper90Degrees();
+		vector<double> getRobotLocation();
+		void openCloseRoboticGripper(bool close);
+		pcl::PointXYZ getRxRyRz(double roll,double pitch,double yaw);
+};
+
+#endif /* UR_CONTROL_H_ */

Robot/urlib/robot_state.cpp

@@ -0,0 +1,399 @@
+/*
+ * robot_state.cpp
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "robot_state.h"
+
+
+RobotState::RobotState(std::condition_variable& msg_cond) {
+	version_msg_.major_version = 0;
+	version_msg_.minor_version = 0;
+	new_data_available_ = false;
+	pMsg_cond_ = &msg_cond;
+	RobotState::setDisconnected();
+	robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
+}
+double RobotState::ntohd(uint64_t nf) {
+	double x;
+	nf = be64toh(nf);
+	memcpy(&x, &nf, sizeof(x));
+	return x;
+}
+void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
+	/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
+	unsigned int offset = 0;
+	while (buf_length > offset) {
+		
+		/*for(int x = 0; x < sizeof(buf); x++){
+			cout << unsigned(buf[x]) << " ";
+		}*/
+		int len;
+		unsigned char message_type;
+		//cout << "Buffer size" << sizeof(buf) << " || Buf_length: " << buf_length << endl;
+	
+		memcpy(&len, &buf[offset], sizeof(len));
+		len = ntohl(len);
+		if (len + offset > buf_length) {
+			return;
+		}
+		//cout << "Len: " << len << endl;
+		memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
+		//cout << "Message Type: " << message_type << endl; 
+		switch (message_type) {
+		case messageType::ROBOT_MESSAGE:
+			RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
+			//cout << "Robot_Message" << endl;
+			break;
+		case messageType::ROBOT_STATE:
+			RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
+			//cout << "Robot_State" << endl;
+			break;
+		case messageType::PROGRAM_STATE_MESSAGE:
+			//Don't do anything atm...
+			//cout << "PROG_STATE" << endl;
+		default:
+			//cout << "Default" << endl;
+			break;
+		}
+		offset += len;
+
+	}
+	return;
+}
+
+void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
+		uint32_t len) {
+	offset += 5;
+	uint64_t timestamp;
+	int8_t source, robot_message_type;
+	memcpy(&timestamp, &buf[offset], sizeof(timestamp));
+	offset += sizeof(timestamp);
+	memcpy(&source, &buf[offset], sizeof(source));
+	offset += sizeof(source);
+	memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
+	offset += sizeof(robot_message_type);
+	switch (robot_message_type) {
+	case robotMessageType::ROBOT_MESSAGE_VERSION:
+		val_lock_.lock();
+		version_msg_.timestamp = timestamp;
+		version_msg_.source = source;
+		version_msg_.robot_message_type = robot_message_type;
+		RobotState::unpackRobotMessageVersion(buf, offset, len);
+		val_lock_.unlock();
+		break;
+	default:
+		break;
+	}
+
+}
+
+void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
+		uint32_t len) {
+	offset += 5;
+	while (offset < len) {
+		int32_t length;
+		uint8_t package_type;
+		memcpy(&length, &buf[offset], sizeof(length));
+		length = ntohl(length);
+		memcpy(&package_type, &buf[offset + sizeof(length)],
+				sizeof(package_type));
+		switch (package_type) {
+		case packageType::ROBOT_MODE_DATA:
+			val_lock_.lock();
+			RobotState::unpackRobotMode(buf, offset + 5);
+			val_lock_.unlock();
+			break;
+
+		case packageType::MASTERBOARD_DATA:
+			val_lock_.lock();
+			RobotState::unpackRobotStateMasterboard(buf, offset + 5);
+			val_lock_.unlock();
+			break;
+		default:
+			break;
+		}
+		offset += length;
+	}
+	new_data_available_ = true;
+	//pMsg_cond_->notify_all();
+}
+
+void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
+		uint32_t len) {
+	memcpy(&version_msg_.project_name_size, &buf[offset],
+			sizeof(version_msg_.project_name_size));
+	offset += sizeof(version_msg_.project_name_size);
+	memcpy(&version_msg_.project_name, &buf[offset],
+			sizeof(char) * version_msg_.project_name_size);
+	offset += version_msg_.project_name_size;
+	version_msg_.project_name[version_msg_.project_name_size] = '\0';
+	memcpy(&version_msg_.major_version, &buf[offset],
+			sizeof(version_msg_.major_version));
+	offset += sizeof(version_msg_.major_version);
+	memcpy(&version_msg_.minor_version, &buf[offset],
+			sizeof(version_msg_.minor_version));
+	offset += sizeof(version_msg_.minor_version);
+	memcpy(&version_msg_.svn_revision, &buf[offset],
+			sizeof(version_msg_.svn_revision));
+	offset += sizeof(version_msg_.svn_revision);
+	version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
+	memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
+	version_msg_.build_date[len - offset] = '\0';
+	if (version_msg_.major_version < 2) {
+		robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
+	}
+}
+
+void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
+	memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
+	offset += sizeof(robot_mode_.timestamp);
+	uint8_t tmp;
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isRobotConnected = true;
+	else
+		robot_mode_.isRobotConnected = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isRealRobotEnabled = true;
+	else
+		robot_mode_.isRealRobotEnabled = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	//printf("PowerOnRobot: %d\n", tmp);
+	if (tmp > 0)
+		robot_mode_.isPowerOnRobot = true;
+	else
+		robot_mode_.isPowerOnRobot = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isEmergencyStopped = true;
+	else
+		robot_mode_.isEmergencyStopped = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProtectiveStopped = true;
+	else
+		robot_mode_.isProtectiveStopped = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProgramRunning = true;
+	else
+		robot_mode_.isProgramRunning = false;
+	offset += sizeof(tmp);
+	memcpy(&tmp, &buf[offset], sizeof(tmp));
+	if (tmp > 0)
+		robot_mode_.isProgramPaused = true;
+	else
+		robot_mode_.isProgramPaused = false;
+	offset += sizeof(tmp);
+	memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
+	offset += sizeof(robot_mode_.robotMode);
+	uint64_t temp;
+	if (RobotState::getVersion() > 2.) {
+		memcpy(&robot_mode_.controlMode, &buf[offset],
+				sizeof(robot_mode_.controlMode));
+		offset += sizeof(robot_mode_.controlMode);
+		memcpy(&temp, &buf[offset], sizeof(temp));
+		offset += sizeof(temp);
+		robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
+	}
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	robot_mode_.speedScaling = RobotState::ntohd(temp);
+}
+
+void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
+		unsigned int offset) {
+	if (RobotState::getVersion() < 3.0) {
+		int16_t digital_input_bits, digital_output_bits;
+		memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+		offset += sizeof(digital_input_bits);
+		memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
+		offset += sizeof(digital_output_bits);
+		mb_data_.digitalInputBits = ntohs(digital_input_bits);
+		mb_data_.digitalOutputBits = ntohs(digital_output_bits);
+	} else {
+		memcpy(&mb_data_.digitalInputBits, &buf[offset],
+				sizeof(mb_data_.digitalInputBits));
+		offset += sizeof(mb_data_.digitalInputBits);
+		mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
+		memcpy(&mb_data_.digitalOutputBits, &buf[offset],
+				sizeof(mb_data_.digitalOutputBits));
+		offset += sizeof(mb_data_.digitalOutputBits);
+		mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
+	}
+
+	memcpy(&mb_data_.analogInputRange0, &buf[offset],
+			sizeof(mb_data_.analogInputRange0));
+	offset += sizeof(mb_data_.analogInputRange0);
+	memcpy(&mb_data_.analogInputRange1, &buf[offset],
+			sizeof(mb_data_.analogInputRange1));
+	offset += sizeof(mb_data_.analogInputRange1);
+	uint64_t temp;
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	mb_data_.analogInput0 = RobotState::ntohd(temp);
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	mb_data_.analogInput1 = RobotState::ntohd(temp);
+	memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
+			sizeof(mb_data_.analogOutputDomain0));
+	offset += sizeof(mb_data_.analogOutputDomain0);
+	memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
+			sizeof(mb_data_.analogOutputDomain1));
+	offset += sizeof(mb_data_.analogOutputDomain1);
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	mb_data_.analogOutput0 = RobotState::ntohd(temp);
+	memcpy(&temp, &buf[offset], sizeof(temp));
+	offset += sizeof(temp);
+	mb_data_.analogOutput1 = RobotState::ntohd(temp);
+
+	memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
+			sizeof(mb_data_.masterBoardTemperature));
+	offset += sizeof(mb_data_.masterBoardTemperature);
+	mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
+	memcpy(&mb_data_.robotVoltage48V, &buf[offset],
+			sizeof(mb_data_.robotVoltage48V));
+	offset += sizeof(mb_data_.robotVoltage48V);
+	mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
+	memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
+	offset += sizeof(mb_data_.robotCurrent);
+	mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
+	memcpy(&mb_data_.masterIOCurrent, &buf[offset],
+			sizeof(mb_data_.masterIOCurrent));
+	offset += sizeof(mb_data_.masterIOCurrent);
+	mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
+
+	memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
+	offset += sizeof(mb_data_.safetyMode);
+	memcpy(&mb_data_.masterOnOffState, &buf[offset],
+			sizeof(mb_data_.masterOnOffState));
+	offset += sizeof(mb_data_.masterOnOffState);
+
+	memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
+			sizeof(mb_data_.euromap67InterfaceInstalled));
+	offset += sizeof(mb_data_.euromap67InterfaceInstalled);
+	if (mb_data_.euromap67InterfaceInstalled != 0) {
+		memcpy(&mb_data_.euromapInputBits, &buf[offset],
+				sizeof(mb_data_.euromapInputBits));
+		offset += sizeof(mb_data_.euromapInputBits);
+		mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
+		memcpy(&mb_data_.euromapOutputBits, &buf[offset],
+				sizeof(mb_data_.euromapOutputBits));
+		offset += sizeof(mb_data_.euromapOutputBits);
+		mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
+		if (RobotState::getVersion() < 3.0) {
+			int16_t euromap_voltage, euromap_current;
+			memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
+			offset += sizeof(euromap_voltage);
+			memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
+			offset += sizeof(euromap_current);
+			mb_data_.euromapVoltage = ntohs(euromap_voltage);
+			mb_data_.euromapCurrent = ntohs(euromap_current);
+		} else {
+			memcpy(&mb_data_.euromapVoltage, &buf[offset],
+					sizeof(mb_data_.euromapVoltage));
+			offset += sizeof(mb_data_.euromapVoltage);
+			mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
+			memcpy(&mb_data_.euromapCurrent, &buf[offset],
+					sizeof(mb_data_.euromapCurrent));
+			offset += sizeof(mb_data_.euromapCurrent);
+			mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
+		}
+
+	}
+}
+
+double RobotState::getVersion() {
+	double ver;
+	val_lock_.lock();
+	ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+			+ .0000001 * version_msg_.svn_revision;
+	val_lock_.unlock();
+	return ver;
+
+}
+
+void RobotState::finishedReading() {
+	new_data_available_ = false;
+}
+
+bool RobotState::getNewDataAvailable() {
+	return new_data_available_;
+}
+
+int RobotState::getDigitalInputBits() {
+	return mb_data_.digitalInputBits;
+}
+int RobotState::getDigitalOutputBits() {
+	return mb_data_.digitalOutputBits;
+}
+double RobotState::getAnalogInput0() {
+	return mb_data_.analogInput0;
+}
+double RobotState::getAnalogInput1() {
+	return mb_data_.analogInput1;
+}
+double RobotState::getAnalogOutput0() {
+	return mb_data_.analogOutput0;
+
+}
+double RobotState::getAnalogOutput1() {
+	return mb_data_.analogOutput1;
+}
+bool RobotState::isRobotConnected() {
+	return robot_mode_.isRobotConnected;
+}
+bool RobotState::isRealRobotEnabled() {
+	return robot_mode_.isRealRobotEnabled;
+}
+bool RobotState::isPowerOnRobot() {
+	return robot_mode_.isPowerOnRobot;
+}
+bool RobotState::isEmergencyStopped() {
+	return robot_mode_.isEmergencyStopped;
+}
+bool RobotState::isProtectiveStopped() {
+	return robot_mode_.isProtectiveStopped;
+}
+bool RobotState::isProgramRunning() {
+	return robot_mode_.isProgramRunning;
+}
+bool RobotState::isProgramPaused() {
+	return robot_mode_.isProgramPaused;
+}
+unsigned char RobotState::getRobotMode() {
+	return robot_mode_.robotMode;
+}
+bool RobotState::isReady() {
+	if (robot_mode_.robotMode == robot_mode_running_) {
+		return true;
+	}
+	return false;
+}
+
+void RobotState::setDisconnected() {
+	robot_mode_.isRobotConnected = false;
+	robot_mode_.isRealRobotEnabled = false;
+	robot_mode_.isPowerOnRobot = false;
+}

Robot/urlib/robot_state.h

@@ -0,0 +1,221 @@
+/*
+ * robot_state.h
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ROBOT_STATE_H_
+#define ROBOT_STATE_H_
+
+#include <inttypes.h>
+#include <vector>
+#include <stdlib.h>
+#include <string.h>
+#include <mutex>
+#include <condition_variable>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <iostream>
+
+using namespace std;
+namespace message_types {
+enum message_type {
+	ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
+};
+}
+typedef message_types::message_type messageType;
+
+namespace package_types {
+enum package_type {
+	ROBOT_MODE_DATA = 0,
+	JOINT_DATA = 1,
+	TOOL_DATA = 2,
+	MASTERBOARD_DATA = 3,
+	CARTESIAN_INFO = 4,
+	KINEMATICS_INFO = 5,
+	CONFIGURATION_DATA = 6,
+	FORCE_MODE_DATA = 7,
+	ADDITIONAL_INFO = 8,
+	CALIBRATION_DATA = 9
+};
+}
+typedef package_types::package_type packageType;
+
+namespace robot_message_types {
+enum robot_message_type {
+	ROBOT_MESSAGE_TEXT = 0,
+	ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+	PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+	ROBOT_MESSAGE_VERSION = 3,
+	ROBOT_MESSAGE_SAFETY_MODE = 5,
+	ROBOT_MESSAGE_ERROR_CODE = 6,
+	ROBOT_MESSAGE_KEY = 7,
+	ROBOT_MESSAGE_REQUEST_VALUE = 9,
+	ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+};
+}
+typedef robot_message_types::robot_message_type robotMessageType;
+
+namespace robot_state_type_v18 {
+enum robot_state_type {
+	ROBOT_RUNNING_MODE = 0,
+	ROBOT_FREEDRIVE_MODE = 1,
+	ROBOT_READY_MODE = 2,
+	ROBOT_INITIALIZING_MODE = 3,
+	ROBOT_SECURITY_STOPPED_MODE = 4,
+	ROBOT_EMERGENCY_STOPPED_MODE = 5,
+	ROBOT_FATAL_ERROR_MODE = 6,
+	ROBOT_NO_POWER_MODE = 7,
+	ROBOT_NOT_CONNECTED_MODE = 8,
+	ROBOT_SHUTDOWN_MODE = 9,
+	ROBOT_SAFEGUARD_STOP_MODE = 10
+};
+}
+typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
+namespace robot_state_type_v30 {
+enum robot_state_type {
+	ROBOT_MODE_DISCONNECTED = 0,
+	ROBOT_MODE_CONFIRM_SAFETY = 1,
+	ROBOT_MODE_BOOTING = 2,
+	ROBOT_MODE_POWER_OFF = 3,
+	ROBOT_MODE_POWER_ON = 4,
+	ROBOT_MODE_IDLE = 5,
+	ROBOT_MODE_BACKDRIVE = 6,
+	ROBOT_MODE_RUNNING = 7,
+	ROBOT_MODE_UPDATING_FIRMWARE = 8
+};
+}
+
+typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
+
+struct version_message {
+	uint64_t timestamp;
+	int8_t source;
+	int8_t robot_message_type;
+	int8_t project_name_size;
+	char project_name[15];
+	uint8_t major_version;
+	uint8_t minor_version;
+	int svn_revision;
+	char build_date[25];
+};
+
+struct masterboard_data {
+	int digitalInputBits;
+	int digitalOutputBits;
+	char analogInputRange0;
+	char analogInputRange1;
+	double analogInput0;
+	double analogInput1;
+	char analogOutputDomain0;
+	char analogOutputDomain1;
+	double analogOutput0;
+	double analogOutput1;
+	float masterBoardTemperature;
+	float robotVoltage48V;
+	float robotCurrent;
+	float masterIOCurrent;
+	unsigned char safetyMode;
+	unsigned char masterOnOffState;
+	char euromap67InterfaceInstalled;
+	int euromapInputBits;
+	int euromapOutputBits;
+	float euromapVoltage;
+	float euromapCurrent;
+};
+
+struct robot_mode_data {
+	uint64_t timestamp;
+	bool isRobotConnected;
+	bool isRealRobotEnabled;
+	bool isPowerOnRobot;
+	bool isEmergencyStopped;
+	bool isProtectiveStopped;
+	bool isProgramRunning;
+	bool isProgramPaused;
+	unsigned char robotMode;
+	unsigned char controlMode;
+	double targetSpeedFraction;
+	double speedScaling;
+};
+
+class RobotState {
+private:
+	version_message version_msg_;
+	masterboard_data mb_data_;
+	robot_mode_data robot_mode_;
+
+	std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
+
+	std::condition_variable* pMsg_cond_; //Signals that new vars are available
+	bool new_data_available_; //to avoid spurious wakes
+	unsigned char robot_mode_running_;
+
+	double ntohd(uint64_t nf);
+
+public:
+	RobotState(std::condition_variable& msg_cond);
+	~RobotState();
+	double getVersion();
+	double getTime();
+	std::vector<double> getQTarget();
+	int getDigitalInputBits();
+	int getDigitalOutputBits();
+	char getAnalogInputRange0();
+	char getAnalogInputRange1();
+	double getAnalogInput0();
+	double getAnalogInput1();
+	char getAnalogOutputDomain0();
+	char getAnalogOutputDomain1();
+	double getAnalogOutput0();
+	double getAnalogOutput1();
+	std::vector<double> getVActual();
+	float getMasterBoardTemperature();
+	float getRobotVoltage48V();
+	float getRobotCurrent();
+	float getMasterIOCurrent();
+	unsigned char getSafetyMode();
+	unsigned char getInReducedMode();
+	char getEuromap67InterfaceInstalled();
+	int getEuromapInputBits();
+	int getEuromapOutputBits();
+	float getEuromapVoltage();
+	float getEuromapCurrent();
+
+	bool isRobotConnected();
+	bool isRealRobotEnabled();
+	bool isPowerOnRobot();
+	bool isEmergencyStopped();
+	bool isProtectiveStopped();
+	bool isProgramRunning();
+	bool isProgramPaused();
+	unsigned char getRobotMode();
+	bool isReady();
+
+	void setDisconnected();
+
+	bool getNewDataAvailable();
+	void finishedReading();
+
+	void unpack(uint8_t * buf, unsigned int buf_length);
+	void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
+	void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
+			uint32_t len);
+	void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
+	void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
+	void unpackRobotMode(uint8_t * buf, unsigned int offset);
+};
+
+#endif /* ROBOT_STATE_H_ */

Robot/urlib/robot_state_RT.cpp

@@ -0,0 +1,551 @@
+/*
+ * robotStateRT.cpp
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "robot_state_RT.h"
+
+RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
+	version_ = 0.0;
+	time_ = 0.0;
+	q_target_.assign(6, 0.0);
+	qd_target_.assign(6, 0.0);
+	qdd_target_.assign(6, 0.0);
+	i_target_.assign(6, 0.0);
+	m_target_.assign(6, 0.0);
+	q_actual_.assign(6, 0.0);
+	qd_actual_.assign(6, 0.0);
+	i_actual_.assign(6, 0.0);
+	i_control_.assign(6, 0.0);
+	tool_vector_actual_.assign(6, 0.0);
+	tcp_speed_actual_.assign(6, 0.0);
+	tcp_force_.assign(6, 0.0);
+	tool_vector_target_.assign(6, 0.0);
+	tcp_speed_target_.assign(6, 0.0);
+	//digital_input_bits_.assign(64, false);
+	digital_input_bits_ = 0;
+	motor_temperatures_.assign(6, 0.0);
+	controller_timer_ = 0.0;
+	robot_mode_ = 0.0;
+	joint_modes_.assign(6, 0.0);
+	safety_mode_ = 0.0;
+	tool_accelerometer_values_.assign(3, 0.0);
+	speed_scaling_ = 0.0;
+	linear_momentum_norm_ = 0.0;
+	v_main_ = 0.0;
+	v_robot_ = 0.0;
+	i_robot_ = 0.0;
+	v_actual_.assign(6, 0.0);
+	data_published_ = false;
+	controller_updated_ = false;
+	pMsg_cond_ = &msg_cond;
+}
+
+RobotStateRT::~RobotStateRT() {
+	/* Make sure nobody is waiting after this thread is destroyed */
+	data_published_ = true;
+	controller_updated_ = true;
+	pMsg_cond_->notify_all();
+}
+
+void RobotStateRT::setDataPublished() {
+	data_published_ = false;
+}
+bool RobotStateRT::getDataPublished() {
+	return data_published_;
+}
+
+void RobotStateRT::setControllerUpdated() {
+	controller_updated_ = false;
+}
+bool RobotStateRT::getControllerUpdated() {
+	return controller_updated_;
+}
+
+double RobotStateRT::ntohd(uint64_t nf) {
+	double x;
+	nf = be64toh(nf);
+	memcpy(&x, &nf, sizeof(x));
+	return x;
+}
+
+std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
+		int nr_of_vals) {
+	uint64_t q;
+	std::vector<double> ret;
+	for (int i = 0; i < nr_of_vals; i++) {
+		memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
+		ret.push_back(ntohd(q));
+	}
+	return ret;
+}
+
+/*std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
+	//cout << "Data: " << data << endl;
+	std::vector<bool> ret;
+	for (int i = 0; i < 64; i++) {
+		cout << "Pin: " << i << " state: " << ((data & (1 << i)) >> i) << endl;
+		ret.push_back((data & (1 << i)) >> i);
+	}
+	return ret;
+}*/
+
+int RobotStateRT::unpackDigitalInputBits(int64_t data){
+	int ret;
+	ret = ((data & (1 << 31)) >> 31);
+	return ret;
+}
+
+void RobotStateRT::setVersion(double ver) {
+	val_lock_.lock();
+	version_ = ver;
+	val_lock_.unlock();
+}
+
+double RobotStateRT::getVersion() {
+	double ret;
+	val_lock_.lock();
+	ret = version_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getTime() {
+	double ret;
+	val_lock_.lock();
+	ret = time_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = q_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQdTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = qd_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQddTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = qdd_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getITarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getMTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = m_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = q_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQdActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = qd_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getIActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getIControl() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_control_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolVectorActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_vector_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpSpeedActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_speed_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpForce() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_force_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolVectorTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_vector_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpSpeedTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_speed_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<bool> RobotStateRT::getDigitalInputBits() {
+	std::vector<bool> ret;
+	val_lock_.lock();
+	//ret = digital_input_bits_;
+	//cout << "Digital Bits: " << digital_input_bits_ << endl;
+	switch(digital_input_bits_){	//Pin 0	1 2 3		Binair 
+		case 2145386496:	//1000			0001
+			ret.push_back(true);
+			ret.push_back(false);
+			ret.push_back(false);	
+			ret.push_back(false);
+			break;
+		case 2147483648:	//0100			0010
+			ret.push_back(false);
+			ret.push_back(true);
+			ret.push_back(false);	
+			ret.push_back(false);
+			break;
+		case 2148532224:	//1100			0011
+			ret.push_back(true);
+			ret.push_back(true);
+			ret.push_back(false);	
+			ret.push_back(false);
+			break;
+		case 2149580800:	//1100			0011
+			ret.push_back(false);
+			ret.push_back(false);
+			ret.push_back(true);	
+			ret.push_back(false);
+			break;
+		case 2150105088:	//1010			0101
+			ret.push_back(true);
+			ret.push_back(false);
+			ret.push_back(true);	
+			ret.push_back(false);
+			break;
+		case 2150629376:	//0110			0110
+			ret.push_back(false);
+			ret.push_back(true);
+			ret.push_back(true);	
+			ret.push_back(false);
+			break;
+		case 2151153664:	//1110			0111
+			ret.push_back(true);
+			ret.push_back(true);
+			ret.push_back(true);	
+			ret.push_back(false);
+			break;
+		case 2151677952:	//0001			1000
+			ret.push_back(false);
+			ret.push_back(false);
+			ret.push_back(false);	
+			ret.push_back(true);
+			break;
+		case 2151940096:	//1001			1001
+			ret.push_back(true);
+			ret.push_back(false);
+			ret.push_back(false);	
+			ret.push_back(true);
+			break;
+		case 2152202240:	//0101			1010
+			ret.push_back(false);
+			ret.push_back(true);
+			ret.push_back(false);	
+			ret.push_back(true);
+			break;
+		case 2152464384:	//1101			1011
+			ret.push_back(true);
+			ret.push_back(true);
+			ret.push_back(false);	
+			ret.push_back(true);
+			break;
+		case 2152726528:	//0011			1100
+			ret.push_back(false);
+			ret.push_back(false);
+			ret.push_back(true);	
+			ret.push_back(true);
+			break;
+		case 2152988672:	//1011			1101
+			ret.push_back(true);
+			ret.push_back(false);
+			ret.push_back(true);	
+			ret.push_back(true);
+			break;
+		case 2153250816:	//0111			1110
+			ret.push_back(false);
+			ret.push_back(true);
+			ret.push_back(true);	
+			ret.push_back(true);
+			break;
+		case 2153512960:	//1111			1111
+			ret.push_back(true);
+			ret.push_back(true);
+			ret.push_back(true);	
+			ret.push_back(true);
+			break;
+		default :
+			ret.push_back(false);
+			ret.push_back(false);
+			ret.push_back(false);	
+			ret.push_back(false);
+			break;
+	}
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getMotorTemperatures() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = motor_temperatures_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getControllerTimer() {
+	double ret;
+	val_lock_.lock();
+	ret = controller_timer_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getRobotMode() {
+	double ret;
+	val_lock_.lock();
+	ret = robot_mode_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getJointModes() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = joint_modes_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getSafety_mode() {
+	double ret;
+	val_lock_.lock();
+	ret = safety_mode_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolAccelerometerValues() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_accelerometer_values_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getSpeedScaling() {
+	double ret;
+	val_lock_.lock();
+	ret = speed_scaling_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getLinearMomentumNorm() {
+	double ret;
+	val_lock_.lock();
+	ret = linear_momentum_norm_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getVMain() {
+	double ret;
+	val_lock_.lock();
+	ret = v_main_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getVRobot() {
+	double ret;
+	val_lock_.lock();
+	ret = v_robot_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getIRobot() {
+	double ret;
+	val_lock_.lock();
+	ret = i_robot_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getVActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = v_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+void RobotStateRT::unpack(uint8_t * buf) {	
+	//cout << "UNPAAAACKKK" << endl;
+	int64_t digital_input_bits;
+	uint64_t unpack_to;
+	uint16_t offset = 0;
+	val_lock_.lock();
+	int len;
+	memcpy(&len, &buf[offset], sizeof(len));
+
+	offset += sizeof(len);
+	len = ntohl(len);
+	//cout << "PACK 1 " << endl;
+	//Check the correct message length is received
+	bool len_good = true;
+	if (version_ >= 1.6 && version_ < 1.7) { //v1.6
+		if (len != 756)
+			len_good = false;
+	} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
+		if (len != 764)
+			len_good = false;
+	} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
+		if (len != 812)
+			len_good = false;
+	} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
+		if (len != 1044)
+			len_good = false;
+	} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
+		if (len != 1060)
+			len_good = false;
+	}
+	//cout << "PACK 2 " << endl;
+	if (!len_good) {
+		printf("Wrong length of message on RT interface: %i\n", len);
+		val_lock_.unlock();
+		return;
+	}
+	//cout << "PACK 3 " << endl;
+	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+	time_ = RobotStateRT::ntohd(unpack_to);
+	offset += sizeof(double);
+	q_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qd_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qdd_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	i_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	m_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	q_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qd_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	i_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	//cout << "PACK 4 " << endl;
+	if (version_ <= 1.9) {
+		if (version_ > 1.6)
+			tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+		offset += sizeof(double) * (3 + 15);
+		tcp_force_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_actual_ = unpackVector(buf, offset, 6);
+	} else {
+		i_control_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_force_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_target_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_target_ = unpackVector(buf, offset, 6);
+	}
+	offset += sizeof(double) * 6;
+	//cout << "PACK 5 " << endl;
+	memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+	digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
+	offset += sizeof(double);
+	motor_temperatures_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+	controller_timer_ = ntohd(unpack_to);
+	if (version_ > 1.6) {
+		offset += sizeof(double) * 2;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		robot_mode_ = ntohd(unpack_to);
+		if (version_ > 1.7) {
+			offset += sizeof(double);
+			joint_modes_ = unpackVector(buf, offset, 6);
+		}
+	}
+	//cout << "PACK 6 " << endl;
+	if (version_ > 1.8) {
+		offset += sizeof(double) * 6;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		safety_mode_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+		offset += sizeof(double) * 3;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		speed_scaling_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		linear_momentum_norm_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		v_main_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		v_robot_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		i_robot_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		v_actual_ = unpackVector(buf, offset, 6);
+	}
+
+	//cout << "PACK 7 " << endl;
+	val_lock_.unlock();
+	controller_updated_ = true;
+	data_published_ = true;
+	//cout << "PACK 8 " << endl;
+	//pMsg_cond_->notify_all();
+	//cout << "PACK DONE " << endl;
+
+}

Robot/urlib/robot_state_RT.h

@@ -0,0 +1,121 @@
+/*
+ * robotStateRT.h
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ROBOT_STATE_RT_H_
+#define ROBOT_STATE_RT_H_
+
+#include <inttypes.h>
+#include <vector>
+#include <stdlib.h>
+#include <string.h>
+#include <mutex>
+#include <netinet/in.h>
+#include <condition_variable>
+#include <stdio.h>
+#include <iostream>
+using namespace std;
+
+class RobotStateRT {
+private:
+	double version_; //protocol version
+
+	double time_; //Time elapsed since the controller was started
+	std::vector<double> q_target_; //Target joint positions
+	std::vector<double> qd_target_; //Target joint velocities
+	std::vector<double> qdd_target_; //Target joint accelerations
+	std::vector<double> i_target_; //Target joint currents
+	std::vector<double> m_target_; //Target joint moments (torques)
+	std::vector<double> q_actual_; //Actual joint positions
+	std::vector<double> qd_actual_; //Actual joint velocities
+	std::vector<double> i_actual_; //Actual joint currents
+	std::vector<double> i_control_; //Joint control currents
+	std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+	std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
+	std::vector<double> tcp_force_; //Generalised forces in the TC
+	std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+	std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
+	//std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
+	unsigned int digital_input_bits_;	
+	std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
+	double controller_timer_; //Controller realtime thread execution time
+	double robot_mode_; //Robot mode
+	std::vector<double> joint_modes_; //Joint control modes
+	double safety_mode_; //Safety mode
+	std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
+	double speed_scaling_; //Speed scaling of the trajectory limiter
+	double linear_momentum_norm_; //Norm of Cartesian linear momentum
+	double v_main_; //Masterboard: Main voltage
+	double v_robot_; //Matorborad: Robot voltage (48V)
+	double i_robot_; //Masterboard: Robot current
+	std::vector<double> v_actual_; //Actual joint voltages
+
+	std::mutex val_lock_; // Locks the variables while unpack parses data;
+
+	std::condition_variable* pMsg_cond_; //Signals that new vars are available
+	bool data_published_; //to avoid spurious wakes
+	bool controller_updated_; //to avoid spurious wakes
+
+	std::vector<double> unpackVector(uint8_t * buf, int start_index,
+			int nr_of_vals);
+	//std::vector<bool> unpackDigitalInputBits(int64_t data);
+	int unpackDigitalInputBits(int64_t data);
+	double ntohd(uint64_t nf);
+
+public:
+	RobotStateRT(std::condition_variable& msg_cond);
+	~RobotStateRT();
+	double getVersion();
+	double getTime();
+	std::vector<double> getQTarget();
+	std::vector<double> getQdTarget();
+	std::vector<double> getQddTarget();
+	std::vector<double> getITarget();
+	std::vector<double> getMTarget();
+	std::vector<double> getQActual();
+	std::vector<double> getQdActual();
+	std::vector<double> getIActual();
+	std::vector<double> getIControl();
+	std::vector<double> getToolVectorActual();
+	std::vector<double> getTcpSpeedActual();
+	std::vector<double> getTcpForce();
+	std::vector<double> getToolVectorTarget();
+	std::vector<double> getTcpSpeedTarget();
+	std::vector<bool> getDigitalInputBits();
+	std::vector<double> getMotorTemperatures();
+	double getControllerTimer();
+	double getRobotMode();
+	std::vector<double> getJointModes();
+	double getSafety_mode();
+	std::vector<double> getToolAccelerometerValues();
+	double getSpeedScaling();
+	double getLinearMomentumNorm();
+	double getVMain();
+	double getVRobot();
+	double getIRobot();
+
+	void setVersion(double ver);
+
+	void setDataPublished();
+	bool getDataPublished();
+	bool getControllerUpdated();
+	void setControllerUpdated();
+	std::vector<double> getVActual();
+	void unpack(uint8_t * buf);
+};
+
+#endif /* ROBOT_STATE_RT_H_ */

Robot/urlib/ur_communication.cpp

@@ -0,0 +1,179 @@
+/*
+ * ur_communication.cpp
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_communication.h"
+
+UrCommunication::UrCommunication(std::condition_variable& msg_cond,
+		std::string host) {
+	robot_state_ = new RobotState(msg_cond);
+	bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
+	bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
+	pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (pri_sockfd_ < 0) {
+		cout << "ERROR opening socket pri_sockfd" << endl;
+	}
+	sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (sec_sockfd_ < 0) {
+		cout << "ERROR opening socket sec_sockfd" << endl;
+	}
+	server_ = gethostbyname(host.c_str());
+	if (server_ == NULL) {
+		cout << "ERROR, unknown host" << endl;
+	}
+	pri_serv_addr_.sin_family = AF_INET;
+	sec_serv_addr_.sin_family = AF_INET;
+	bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
+	bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
+	pri_serv_addr_.sin_port = htons(30001);
+	sec_serv_addr_.sin_port = htons(30002);
+	flag_ = 1;
+	setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+			sizeof(int));
+	setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
+			sizeof(int));
+	setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
+			sizeof(int));
+	setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+			sizeof(int));
+	setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
+			sizeof(int));
+	setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
+			sizeof(int));
+	fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
+	connected_ = false;
+	keepalive_ = false;
+}
+
+bool UrCommunication::start() {
+	keepalive_ = true;
+	uint8_t buf[512];
+	unsigned int bytes_read;
+	std::string cmd;
+	bzero(buf, 512);
+	cout << "Acquire firmware version: Connecting..." << endl;
+	if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
+			sizeof(pri_serv_addr_)) < 0) {
+		cout << "Error connecting to get firmware version" << endl;
+		return false;
+	}
+	cout << "Acquire firmware version: Got connection" << endl;
+	bytes_read = read(pri_sockfd_, buf, 512);
+	setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
+			sizeof(int));
+	robot_state_->unpack(buf, bytes_read);
+	//wait for some traffic so the UR socket doesn't die in version 3.1.
+	std::this_thread::sleep_for(std::chrono::milliseconds(500));
+	char tmp[64];
+	sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
+	cout << tmp << endl;
+	close(pri_sockfd_);
+
+	cout << "Switching to secondary interface for masterboard data: Connecting..." << endl;
+
+	fd_set writefds;
+	struct timeval timeout;
+
+	connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
+			sizeof(sec_serv_addr_));
+	FD_ZERO(&writefds);
+	FD_SET(sec_sockfd_, &writefds);
+	timeout.tv_sec = 10;
+	timeout.tv_usec = 0;
+	select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+	unsigned int flag_len;
+	getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+	if (flag_ < 0) {
+		cout << "Error connecting to secondary interface" << endl;
+		return false;
+	}
+	cout << "Secondary interface: Got connection" << endl;
+	comThread_ = std::thread(&UrCommunication::run, this);
+	return true;
+}
+
+void UrCommunication::halt() {
+	keepalive_ = false;
+	comThread_.join();
+}
+
+void UrCommunication::run() {
+	uint8_t buf[2048];
+	int bytes_read;
+	bzero(buf, 2048);
+	struct timeval timeout;
+	fd_set readfds;
+	FD_ZERO(&readfds);
+	FD_SET(sec_sockfd_, &readfds);
+	connected_ = true;
+	while (keepalive_) {
+		while (connected_ && keepalive_) {
+			timeout.tv_sec = 0; //do this each loop as selects modifies timeout
+			timeout.tv_usec = 500000; // timeout of 0.5 sec
+			select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+			bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
+			if (bytes_read > 0) {
+				setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
+						(char *) &flag_, sizeof(int));
+				robot_state_->unpack(buf, bytes_read);
+			} else {
+				connected_ = false;
+				robot_state_->setDisconnected();
+				close(sec_sockfd_);
+			}
+		}
+		if (keepalive_) {
+			//reconnect
+			cout << "Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..." << endl;
+			sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+			if (sec_sockfd_ < 0) {
+				cout << "ERROR opening secondary socket" << endl;
+			}
+			flag_ = 1;
+			setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+					sizeof(int));
+			setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
+					sizeof(int));
+			setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
+					sizeof(int));
+			fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
+			while (keepalive_ && !connected_) {
+				std::this_thread::sleep_for(std::chrono::seconds(10));
+				fd_set writefds;
+
+				connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
+						sizeof(sec_serv_addr_));
+				FD_ZERO(&writefds);
+				FD_SET(sec_sockfd_, &writefds);
+				select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
+				unsigned int flag_len;
+				getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
+						&flag_len);
+				if (flag_ < 0) {
+					cout << "Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..." << endl;
+				} else {
+					connected_ = true;
+					cout << "Secondary port: Reconnected" << endl;
+				}
+			}
+		}
+	}
+
+	//wait for some traffic so the UR socket doesn't die in version 3.1.
+	std::this_thread::sleep_for(std::chrono::milliseconds(500));
+	close(sec_sockfd_);
+}

Robot/urlib/ur_communication.h

@@ -0,0 +1,63 @@
+/*
+ * ur_communication.h
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef UR_COMMUNICATION_H_
+#define UR_COMMUNICATION_H_
+
+#include "robot_state.h"
+#include <vector>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/time.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <netdb.h>
+#include <iostream>
+#include <unistd.h>
+#include <chrono>
+#include <fcntl.h>
+#include <sys/types.h>
+
+
+class UrCommunication {
+private:
+	int pri_sockfd_, sec_sockfd_;
+	struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
+	struct hostent *server_;
+	bool keepalive_;
+	std::thread comThread_;
+	int flag_;
+	void run();
+
+public:
+	bool connected_;
+	RobotState* robot_state_;
+
+	UrCommunication(std::condition_variable& msg_cond, std::string host);
+	bool start();
+	void halt();
+
+};
+
+#endif /* UR_COMMUNICATION_H_ */

Robot/urlib/ur_realtime_communication.cpp

@@ -0,0 +1,205 @@
+/*
+ * ur_realtime_communication.cpp
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_realtime_communication.h"
+
+UrRealtimeCommunication::UrRealtimeCommunication(
+		std::condition_variable& msg_cond, std::string host,
+		unsigned int safety_count_max) {
+	robot_state_ = new RobotStateRT(msg_cond);
+	bzero((char *) &serv_addr_, sizeof(serv_addr_));
+	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+	if (sockfd_ < 0) {
+		cout << "ERROR opening socket" << endl;
+	}
+	server_ = gethostbyname(host.c_str());
+	if (server_ == NULL) {
+		cout <<"ERROR, no such host"<< endl;
+	}
+	serv_addr_.sin_family = AF_INET;
+	bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
+	serv_addr_.sin_port = htons(30003);
+	flag_ = 1;
+	setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
+	setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int));
+	setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
+	fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+	connected_ = false;
+	keepalive_ = false;
+	safety_count_ = safety_count_max + 1;
+	safety_count_max_ = safety_count_max;
+}
+
+bool UrRealtimeCommunication::start() {
+	fd_set writefds;
+	struct timeval timeout;
+
+	keepalive_ = true;
+	cout <<"Realtime port: Connecting..." << endl;
+
+	connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
+	FD_ZERO(&writefds);
+	FD_SET(sockfd_, &writefds);
+	timeout.tv_sec = 10;
+	timeout.tv_usec = 0;
+	select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
+	unsigned int flag_len;
+	getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+	if (flag_ < 0) {
+		cout <<"Error connecting to RT port 30003" << endl;
+		return false;
+	}
+	sockaddr_in name;
+	socklen_t namelen = sizeof(name);
+	int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
+	if (err < 0) {
+		cout << "Could not get local IP" << endl;
+		close(sockfd_);
+		return false;
+	}
+	char str[18];
+	inet_ntop(AF_INET, &name.sin_addr, str, 18);
+	local_ip_ = str;
+	cout << "Starting Threads" << endl;
+	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
+	return true;
+}
+
+void UrRealtimeCommunication::halt() {
+	keepalive_ = false;
+	comThread_.join();
+}
+
+void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
+	int bytes_written;
+	if (inp.back() != '\n') {
+		inp.append("\n");
+	}
+	if (connected_)
+		bytes_written = write(sockfd_, inp.c_str(), inp.length());
+	else
+		cout << "Could not send command "  << inp <<  ". The robot is not connected! Command is discarded" << endl;
+}
+
+void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
+		double q3, double q4, double q5, double acc) {
+	char cmd[1024];
+	if( robot_state_->getVersion() >= 3.1 ) {
+		sprintf(cmd,
+				"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
+				q0, q1, q2, q3, q4, q5, acc);
+	}
+	else {
+		sprintf(cmd,
+				"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
+				q0, q1, q2, q3, q4, q5, acc);		
+	}
+	addCommandToQueue((std::string) (cmd));
+	if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
+		//If a joint speed is set, make sure we stop it again after some time if the user doesn't
+		safety_count_ = 0;
+	}
+}
+
+void UrRealtimeCommunication::run() {
+	uint8_t buf[2048];
+	int bytes_read;
+	bzero(buf, 2048);
+	struct timeval timeout;
+	fd_set readfds;
+	FD_ZERO(&readfds);
+	FD_SET(sockfd_, &readfds);
+	cout << "Realtime port: Got connection" << endl;
+	connected_ = true;
+	//cout << "Keepaliiiive: " << keepalive_ << endl;
+	while (keepalive_) {
+		//cout << "Connected: " << connected_ << " - Keepalive: " << keepalive_ << endl;
+		while (connected_ && keepalive_) {
+			//cout << "A" << endl;
+			timeout.tv_sec = 0; //do this each loop as selects modifies timeout
+			timeout.tv_usec = 500000; // timeout of 0.5 sec
+			//cout << "B" << endl;
+			select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
+			//cout << "C" << endl;
+			bytes_read = read(sockfd_, buf, 2048);
+			//cout << "Bytes Read: " << bytes_read << endl;
+			if (bytes_read > 0) {
+				setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, 
+						sizeof(int));
+				robot_state_->unpack(buf);
+				//cout << "Done UNPACK" << endl;
+				/*for(int x = 0; x < sizeof(buf); x++){
+					cout << (int)buf << " ";
+				}*/
+				if (safety_count_ == safety_count_max_) {
+					setSpeed(0., 0., 0., 0., 0., 0.);
+					//cout << "Set Speed" << endl;
+				}
+				safety_count_ += 1;
+			} else {
+				connected_ = false;
+				close(sockfd_);
+			}
+		}
+		if (keepalive_) {
+			//reconnect
+			cout << "Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..." << endl;
+			sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+			if (sockfd_ < 0) {
+				cout << "ERROR opening socket" << endl;
+			}
+			flag_ = 1;
+			setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
+					sizeof(int));
+			setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, 
+					sizeof(int));
+	
+			setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
+					sizeof(int));
+			fcntl(sockfd_, F_SETFL, O_NONBLOCK);
+			while (keepalive_ && !connected_) {
+				std::this_thread::sleep_for(std::chrono::seconds(10));
+				fd_set writefds;
+
+				connect(sockfd_, (struct sockaddr *) &serv_addr_,
+						sizeof(serv_addr_));
+				FD_ZERO(&writefds);
+				FD_SET(sockfd_, &writefds);
+				select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
+				unsigned int flag_len;
+				getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
+				if (flag_ < 0) {
+					cout << "Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..." << endl;
+				} else {
+					connected_ = true;
+					cout << "Realtime port: Reconnected" << endl;
+				}
+			}
+		}
+	}
+	setSpeed(0., 0., 0., 0., 0., 0.);
+	close(sockfd_);
+}
+
+void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
+	safety_count_max_ = inp;
+}
+
+std::string UrRealtimeCommunication::getLocalIp() {
+	return local_ip_;
+}

Robot/urlib/ur_realtime_communication.h

@@ -0,0 +1,75 @@
+/*
+ * ur_realtime_communication.h
+ *
+ * Copyright 2015 Thomas Timm Andersen
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef UR_REALTIME_COMMUNICATION_H_
+#define UR_REALTIME_COMMUNICATION_H_
+
+#include "robot_state_RT.h"
+#include <vector>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/time.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <netdb.h>
+#include <iostream>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/types.h>
+
+class UrRealtimeCommunication {
+private:
+	unsigned int safety_count_max_;
+	int sockfd_;
+	struct sockaddr_in serv_addr_;
+	struct hostent *server_;
+	std::string local_ip_;
+	bool keepalive_;
+	std::thread comThread_;
+	int flag_;
+	std::recursive_mutex command_string_lock_;
+	std::string command_;
+	unsigned int safety_count_;
+	void run();
+
+
+public:
+	bool connected_;
+	RobotStateRT* robot_state_;
+
+	UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
+			unsigned int safety_count_max = 12);
+	bool start();
+	void halt();
+	void setSpeed(double q0, double q1, double q2, double q3, double q4,
+			double q5, double acc = 100.);
+	void addCommandToQueue(std::string inp);
+	void setSafetyCountMax(uint inp);
+	std::string getLocalIp();
+
+};
+
+#endif /* UR_REALTIME_COMMUNICATION_H_ */

Vision/2d_depth_detector.cpp

@@ -0,0 +1,94 @@
+#include "2d_depth_detector.h"
+
+using namespace cv;
+using namespace std;
+
+DepthDetector::DepthDetector() {
+	//positions = new vector<Point>;
+}
+
+vector<Point2d> DepthDetector::getPositions() {
+  //TO DO: use main camera class
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address, using default one
+  o3d3xx::Camera::Ptr cam = make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =
+    make_shared<o3d3xx::FrameGrabber>(
+      cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+  //get frame from camera
+  if (! fg->WaitForFrame(img.get(), 2000))
+    {
+      //TO DO: error handling
+      cerr << "Timeout waiting for camera!" << endl;
+      //return positions;	
+    }
+  //2D images
+  double min, max;
+  ////create depth image
+  Mat depthImageRaw = Mat(img->DepthImage()); 
+  //Mat xyzImageRaw = Mat(img->XYZImage());
+  //resize image for better visibility when debugging
+  resize(depthImageRaw,depthImageRaw, Size(depthImageRaw.cols*2,depthImageRaw.rows*2), 2, 2, INTER_CUBIC);
+  
+  //resize(xyzImageRaw,xyzImageRaw, Size(xyzImageRaw.cols*2,xyzImageRaw.rows*2), 2, 2, INTER_CUBIC);
+  //apply region of interest to only view inside of bin
+  //Xmin = -120, Xmax = 130, Ymin = -800, Ymax = 250
+  Rect roi = Rect(120,45,110,145);
+  Mat depthImage = depthImageRaw(roi);
+  //Mat xyzImage = xyzImageRaw(roi);
+  //minMaxIdx(xyzImage, &min, &max);
+  //convertScaleAbs(xyzImage, xyzImage, 255 / max);
+  //applyColorMap(xyzImage, xyzImage, COLORMAP_JET);
+  //Mat channel[3];
+  //split(xyzImage, channel);
+  minMaxIdx(depthImage, &min, &max);
+  convertScaleAbs(depthImage, depthImage, 255 / max);
+  applyColorMap(depthImage, depthImage, COLORMAP_JET);
+  //convert to gray scale
+  Mat depthImageGray;
+  cvtColor(depthImage, depthImageGray, CV_BGR2GRAY);
+  //create new mat objects to hold binary data
+  Mat depthImageBinary;
+  Mat depthImageBinary16S;
+  Mat depthImageBinary16SLabeled;
+  //create variables for blob detection
+  vector<Point2d*> firstPixelvec;
+  vector<Point2d*> posVec;
+  vector<int> areaVec;
+  double x,y;
+  //detect blobs going from high to low threshold
+  for(int i = 12; i > -1; i--){
+    //dynamically apply thresholds
+    threshold(depthImageGray, depthImageBinary, 20*i, 1, CV_THRESH_BINARY);
+    //use 16S image
+    depthImageBinary.convertTo(depthImageBinary16S, CV_16S);
+    //call blob detection
+    labelBLOBsInfo(depthImageBinary16S,depthImageBinary16SLabeled,firstPixelvec, posVec,areaVec, 200, 10000);
+    for(int j = 0; j < firstPixelvec.size(); j++){
+       //calculate in meters the x and y position
+       x = ((posVec[j]->x * 1.65) - 430)/1000; 
+       y = ((posVec[j]->y*1.8) - 250)/1000;
+       positions.push_back(Point2d(x,y));
+       
+       //get depth value (z value?)
+       /*uchar val;
+       int posy = posVec[j]->y;
+       int posx = posVec[j]->x;
+       for(int k = 0; k < 3; k++){
+        val = channel[k].at<uchar>(posy,posx);
+        cout << "Value Channel"<<k<<": " << (double)val << " thresh: " << to_string(20*i) << " at("<<posy<<","<<posx<<")"<< endl;
+       }
+       circle(depthImageBinary16S, Point(posy,posx), 5, Scalar(10));*/
+       show16SImageStretch(depthImageBinary16S, "16s binary");
+    }
+    //clear vectors to search next threshold
+    firstPixelvec.clear(); 
+    areaVec.clear(); 
+    posVec.clear();
+  }
+  return positions;
+}

Vision/2d_depth_detector.h

@@ -0,0 +1,16 @@
+#include <opencv2/opencv.hpp>
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+#include "avansvisionlib.h"
+
+using namespace cv;
+using namespace std;
+
+class DepthDetector {
+	public:
+	DepthDetector();
+	vector<Point2d> getPositions();
+	private:
+	vector<Point2d> positions;
+};

Vision/AerobicVision.cpp

@@ -0,0 +1,938 @@
+#include "AerobicVision.h"
+#include <math.h>
+//TODO: Implement Filterfunction in calibration function
+//TODO: SORT GROWING ALGORITHM OUTPUT VECTOR ON Z AXIS HIGH TO LOW
+
+
+AerobicVision::AerobicVision(std::string IP){
+	  //logging method
+	  o3d3xx::Logging::Init();
+	  //initialise camera constructor expects IP address, using default one
+	  cam = std::make_shared<o3d3xx::Camera>(IP);
+	  //create buffer to fetch image
+	  img = std::make_shared<o3d3xx::ImageBuffer>();
+	  //framegrabber
+	  fg = std::make_shared<o3d3xx::FrameGrabber>(cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+}
+
+bool wayToSort(objectData i, objectData j){ // point x = real z
+	return i.xyz.x < j.xyz.x;
+}
+
+
+pcl::PointCloud<pcl::PointXYZI>::Ptr AerobicVision::PassFilterPCD(pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax){
+	  
+	pcl::PointCloud<pcl::PointXYZI>::Ptr zfilt (new pcl::PointCloud<pcl::PointXYZI>), yfilt (new pcl::PointCloud<pcl::PointXYZI>);
+
+	//passthrough filter
+	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZI>);
+	pcl::PassThrough<pcl::PointXYZI> passX, passY, passZ;
+
+	
+	passZ.setInputCloud(cloudIn);
+	passZ.setFilterFieldName("z"); 		// blauw = z			REAL = X
+	passZ.setFilterLimits(xmin,xmax);	
+	//passZ.setFilterLimits(-0.05,0.05);	//calibration
+	passZ.filter(*zfilt);
+	
+	passY.setInputCloud(zfilt);
+	passY.setFilterFieldName("y"); 		// y= groen			REAL = Y
+	passY.setFilterLimits(ymin,ymax);	//
+	//passY.setFilterLimits(-0.05,0.05);	//calibratiom
+	passY.filter(*yfilt);
+
+	passX.setInputCloud(yfilt);
+	passX.setFilterFieldName("x"); 		// rood = x			REAL = Z
+	passX.setFilterLimits(zmin,zmax);
+	//passX.setFilterLimits(0.35,0.38);	//calibration	
+	passX.filter(*cloud_filtered);
+
+	return cloud_filtered;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr AerobicVision::convertCloudXYZItoXYZ(pcl::PointCloud<pcl::PointXYZI> cloudIn){
+	pcl::PCDWriter writer;
+	pcl::PCDReader reader;
+	writer.write<pcl::PointXYZI>("tmp_pcd.pcd", cloudIn, false);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZ>);
+	reader.read("tmp_pcd.pcd", *cloudOut);
+	return cloudOut;
+}
+
+pcl::PointXYZ AerobicVision::Calibrate(int samples){
+	std::vector<pcl::PointXYZ> average;
+ 	cout << "Start Calibration with " << samples <<  " samples" << endl;
+  	//get frame from camera (could be looped to create an actual live feed)
+ 	for(int cnt = 0; cnt < samples; cnt++){
+	  if (! fg->WaitForFrame(img.get(), 2000))
+	    {
+	      std::cerr << "Timeout waiting for camera!" << std::endl;
+	    }
+	  
+
+	  //3D pointcloud
+	  
+	  //convert o3d3xx cloud to PointXYZI  
+	  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn = img->Cloud();
+	  pcl::PointCloud<pcl::PointXYZI>::Ptr zfilt (new pcl::PointCloud<pcl::PointXYZI>), yfilt (new pcl::PointCloud<pcl::PointXYZI>);
+
+	  //passthrough filter
+	  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZI>);
+	  pcl::PassThrough<pcl::PointXYZI> passX, passY, passZ;
+
+
+	  passZ.setInputCloud(cloudIn);
+	  passZ.setFilterFieldName("z"); 	// blauw = z			REAL = X
+	  passZ.setFilterLimits(-0.125,0.105);	//calibration
+	  passZ.filter(*zfilt);
+
+	  passY.setInputCloud(zfilt);
+	  passY.setFilterFieldName("y"); 	// y= groen			REAL = Y
+	  //passY.setFilterLimits(-0.11,0.09);	//ops
+	  passY.setFilterLimits(-0.1,0.075);	//calibratiom
+	  passY.filter(*yfilt);
+
+	  passX.setInputCloud(yfilt);
+	  passX.setFilterFieldName("x"); 	// rood = x			REAL = Z
+	  //passX.setFilterLimits(0.0,0.60);
+	  passX.setFilterLimits(0.27,0.33);	//calibration
+	  passX.filter(*cloud_filtered2);
+	
+	  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (convertCloudXYZItoXYZ(*cloud_filtered2));
+	
+	  pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
+	  feature_extractor.setInputCloud (cloud);
+	  feature_extractor.compute();
+
+	  pcl::PointXYZ min_point_AABB;
+	  pcl::PointXYZ max_point_AABB;
+	  
+	  feature_extractor.getAABB (min_point_AABB, max_point_AABB);
+	  
+	  pcl::PointXYZ middle_point;
+	  middle_point.x = min_point_AABB.x + ((max_point_AABB.x - min_point_AABB.x) / 2);
+	  middle_point.y = min_point_AABB.y + ((max_point_AABB.y - min_point_AABB.y) / 2);
+	  middle_point.z = min_point_AABB.z + ((max_point_AABB.z - min_point_AABB.z) / 2);
+	  average.push_back(middle_point);
+
+  }
+  double x = 0;
+  double y = 0;
+  double z = 0;
+  int cnt;
+  pcl::PointXYZ ret;
+  for(cnt = 0; cnt < samples; cnt++){
+  	x = x + average[cnt].x; 
+	y = y + average[cnt].y; 
+	z = z + average[cnt].z; 
+  }
+  //set correct XYZ format
+  
+  z = (z / (double)samples); 
+  y = (y / (double)samples); 
+  x = (x / (double)samples); 
+
+  ret.z = x;
+  ret.y = y;
+  ret.x = z;
+  cout << "Finished Vision Calibration" << endl;
+  return ret;
+}
+
+std::vector<objectData> AerobicVision::RegionGrowing(){
+	pcl::console::TicToc tt;
+	// Load the input point cloud
+
+	std::cerr << "Loading...\n", tt.tic ();
+	if (! fg->WaitForFrame(img.get(), 2000))
+	{
+	std::cerr << "Timeout waiting for camera!" << std::endl;
+	}
+
+	pcl::PCDWriter writer;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn = img->Cloud();
+	writer.write<pcl::PointXYZI>("tmp_pcd.pcd", *cloudIn, false);	
+	pcl::io::loadPCDFile ("tmp_pcd.pcd", *cloud);
+	//std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud->points.size () << " points\n";
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr zfilt (new pcl::PointCloud<pcl::PointXYZ>), yfilt (new pcl::PointCloud<pcl::PointXYZ>);
+
+	//passthrough filter
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PassThrough<pcl::PointXYZ> passX, passY, passZ;
+
+	passZ.setInputCloud(cloud);
+	passZ.setFilterFieldName("y"); 	// blauw = z			REAL = X
+	passZ.setFilterLimits(-0.1,0.1);	//calibration
+	passZ.filter(*zfilt);
+
+	passY.setInputCloud(zfilt);
+	passY.setFilterFieldName("z"); 	// y= groen			REAL = Y
+	//passY.setFilterLimits(-0.11,0.09);	//ops
+	passY.setFilterLimits(-0.12,0.12);	//calibratiom
+	passY.filter(*yfilt);
+
+	passX.setInputCloud(yfilt);
+	passX.setFilterFieldName("x"); 	// rood = x			REAL = Z
+	//passX.setFilterLimits(0.0,0.60);
+	passX.setFilterLimits(0.0,0.6);	//calibration
+	passX.filter(*cloud_filtered);
+
+	std::vector<int> temp;
+	pcl::removeNaNFromPointCloud(*cloud_filtered, *cloud_filtered, temp); 
+
+	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new   pcl::search::KdTree<pcl::PointXYZ>);
+	pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
+	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+	normal_estimator.setSearchMethod (tree);
+	normal_estimator.setInputCloud (cloud_filtered);
+	normal_estimator.setKSearch (100);
+	normal_estimator.compute (*normals);
+
+	pcl::IndicesPtr indices (new std::vector <int>);
+	pcl::PassThrough<pcl::PointXYZ> pass;
+	pass.setInputCloud (cloud_filtered);
+	pass.setFilterFieldName ("z");
+	pass.setFilterLimits (0.0, 1.0);
+	pass.filter (*indices);
+
+	pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+	reg.setMinClusterSize (50); //minimum points to find before assigning a cluster
+	reg.setMaxClusterSize (1000); //maximum points to find
+	reg.setSearchMethod (tree); //search using the KdTree
+	reg.setNumberOfNeighbours (15); //number of neighbours??
+	reg.setInputCloud (cloud_filtered); //input cloud is filtered on xyz axis
+	//reg.setIndices (indices);
+	reg.setInputNormals (normals); //input normals used to find the region
+	reg.setSmoothnessThreshold (5.0 / 180.0 * M_PI); //how smooth has the surface to be in degrees
+	reg.setCurvatureThreshold (1.0); //how much curvature can there be??
+
+	std::vector <pcl::PointIndices> clusters;
+	reg.extract (clusters);
+	
+
+	std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters_with_points;
+	std::vector<objectData> cluster_middle_points;
+	for(int cnt = 0; cnt < clusters.size(); cnt++){
+		//Create the filtering object
+		pcl::ExtractIndices<pcl::PointXYZ> extract;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tmpcloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);;
+		pcl::PointIndices::Ptr inliers (new pcl::PointIndices (clusters[cnt]));
+		extract.setInputCloud (cloud_filtered);
+		extract.setIndices (inliers);
+		extract.setNegative (false);
+		extract.filter (*tmpcloud);
+		clusters_with_points.push_back(*tmpcloud);
+
+		pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
+		feature_extractor.setInputCloud (tmpcloud);
+		feature_extractor.compute();
+	
+		normal_estimator.setSearchMethod (tree);
+		normal_estimator.setInputCloud (tmpcloud);
+		normal_estimator.setKSearch (100);
+		normal_estimator.compute (*normals);
+
+		pcl::PointXYZ min_point_AABB;
+		pcl::PointXYZ max_point_AABB;
+		  
+		feature_extractor.getAABB (min_point_AABB, max_point_AABB);
+		 
+		objectData middle_point;
+		middle_point.xyz.x = min_point_AABB.x + ((max_point_AABB.x - min_point_AABB.x) / 2);
+		middle_point.xyz.y = min_point_AABB.y + ((max_point_AABB.y - min_point_AABB.y) / 2);
+		middle_point.xyz.z = min_point_AABB.z + ((max_point_AABB.z - min_point_AABB.z) / 2);
+
+		//calculate single normal vector		
+		float normal_x = 0.0f;
+		float normal_y = 0.0f;
+		float normal_z = 0.0f;
+		for (int i = 0; i < normals->size();i++){
+			normal_x = normal_x + normals->points[i].normal_x;
+			normal_y = normal_y + normals->points[i].normal_y;
+			normal_z = normal_z + normals->points[i].normal_z;
+		}
+		normal_x = (normal_x/normals->size());
+		normal_y = (normal_y/normals->size());
+		normal_z = (normal_z/normals->size());
+		if(normal_x > 1 || normal_y > 1 || normal_z > 1 || normal_x < -1 || 
+		normal_y < -1 || normal_z < -1) {
+			cout << "Normals not correct, try again." << endl;
+		} else {
+			pcl::Normal normal = pcl::Normal(-normal_x,-normal_y,-normal_z);
+					
+			float pitch = asin(-normal_y);
+			float yaw = atan2(normal_z,normal_x);
+			float roll = 0.0;
+			pcl::PointXYZ rpy = pcl::PointXYZ(roll,pitch,yaw);
+			middle_point.pitch = pitch;
+			middle_point.yaw = yaw;		
+	
+			
+			Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
+			Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
+			Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
+
+			Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;
+			Eigen::Matrix3d rotationMatrix = q.matrix();
+
+			double rotSum = rotationMatrix(0,0) + rotationMatrix(1,1) + rotationMatrix(2,2) - 1;
+		    	double alpha = acos(rotSum / 2);
+		    	double theta = 0;
+		    	if (roll >= 0)
+				theta = alpha;
+		   	else
+				theta = 2 * 3.1415926535897 - alpha;
+		    	double my = 1.0 / (2 * sin(theta));
+		 
+		    	double rx = my * (rotationMatrix(2,1) - rotationMatrix(1,2)) * theta;
+		    	double ry = my * (rotationMatrix(0,2) - rotationMatrix(2,0)) * theta;
+		    	double rz = my * (rotationMatrix(1,0) - rotationMatrix(0,1)) * theta;
+		 
+		    	pcl::PointXYZ rotationVector = pcl::PointXYZ();
+		    	rotationVector.x = (float)rx;
+		    	rotationVector.y = (float)ry;
+		    	rotationVector.z = (float)rz;
+			middle_point.rx = rotationVector.x;
+			middle_point.ry = rotationVector.y;
+			middle_point.rz = rotationVector.z;
+		}
+	
+		//add found point to vector		
+		cluster_middle_points.push_back(middle_point);
+	}
+	
+	//Sort found clusters from closest to farest	
+	sort(cluster_middle_points.begin(), cluster_middle_points.end(), wayToSort);
+
+	return cluster_middle_points;
+}
+
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////
+//Projectgroep 2 test pogingen
+///////////////////////////////////////////////////////
+
+pcl::PointCloud<pcl::PointXYZI> AerobicVision::AverageCloud(std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> totalList)
+{ /*
+	pcl::PointCloud<pcl::PointXYZI>::Ptr toReturn = totalList[0]->makeShared();
+	
+	for(int i = 0; i< totalList[0]->points.size(); i++)
+	{
+		pcl::PointXYZI averagePoint = pcl::PointXYZI(0.0f);
+		for(int k = 0; k< totalList.size();k++)
+		{
+			toReturn[i]->x += totalList[k]->points[i].x;
+			toReturn[i]->y += totalList[k]->points[i].y;
+			toReturn[i]->z += totalList[k]->points[i].z;
+			toReturn[i]->intensity += totalList[k]->points[i].intensity;
+		}
+
+		toReturn[i]->x /= totalList.size();
+		toReturn[i]->y /= totalList.size();
+		toReturn[i]->z /= totalList.size();
+		toReturn[i]->intensity /= totalList.size();
+	}
+	return toReturn;*/
+	pcl::PointCloud<pcl::PointXYZI> toReturn;
+	
+	for(int i = 0; i< totalList[0]->points.size(); i++)
+	{
+		if(isnan(totalList[0]->points[i].x ))
+		{
+			toReturn.push_back(totalList[0]->points[i]);
+		}
+		else{
+
+		
+			pcl::PointXYZI averagePoint = pcl::PointXYZI(0.0f);
+			for(int k = 0; k< totalList.size();k++)
+			{
+				averagePoint.x += totalList[k]->points[i].x;
+				averagePoint.y += totalList[k]->points[i].y;
+				averagePoint.z += totalList[k]->points[i].z;
+				averagePoint.intensity += totalList[k]->points[i].intensity;
+			}
+
+			averagePoint.x /= totalList.size();
+			averagePoint.y /= totalList.size();
+			averagePoint.z /= totalList.size();
+			averagePoint.intensity /= totalList.size();
+			toReturn.push_back(averagePoint);
+		}
+	}
+	return toReturn;
+}
+
+void AerobicVision::PrintClouds(std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> totalList)
+{
+   for(int i = 0; i< totalList[0]->points.size(); i++)
+	{
+		cout << "[" << i << "] ";
+
+		for(int k = 0; k< totalList.size();k++)
+		{
+			cout << std::setprecision(8) << "(" << totalList[k]->points[i].x << "," << totalList[k]->points[i].y << "," << totalList[k]->points[i].z << ")";
+		}
+
+		cout << endl;
+	}
+}
+
+void AerobicVision::PrintCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
+{
+   for(int i = 0; i< cloud->points.size(); i++)
+	{
+		cout << "[" << i << "] ";
+		cout << std::setprecision(5) << "(" << cloud->points[i].x << ", " << cloud->points[i].y << ", " << cloud->points[i].z << ")";
+		cout << endl;
+	}
+}
+
+
+void AerobicVision::Test(){
+	pcl::console::TicToc tt;
+	// Load the input point cloud
+	std::cerr << "Loading...\n", tt.tic ();
+
+	usleep(2000000);
+
+	std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> allClouds;
+
+	pcl::PointCloud<pcl::PointXYZI> cloudAverage;
+
+	for(int i = 0; i< 4; i++)
+	{
+
+		usleep(500000);
+		if (! fg->WaitForFrame(img.get(), 2000))
+		{
+			std::cerr << "Timeout waiting for camera!" << std::endl;
+		}
+		usleep(2000000);
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn = img->Cloud()->makeShared();
+
+		allClouds.push_back(cloudIn);
+
+		std::cout << "Stored pic " << i << endl;
+	}
+
+	//cloudAverage = AverageCloud(allClouds);
+	PrintClouds(allClouds);
+	/*
+	PrintCloud(cloudAverage);
+	
+	pcl::PointCloud<pcl::PointXYZI>::Ptr m_ptrCloud(&cloudAverage);
+
+	pcl::visualization::CloudViewer viewer("Cloud Viewer", false);
+    
+  //blocks until the cloud is actually rendered
+	viewer.showCloud(m_ptrCloud);
+	*/
+
+	cout << "Press Enter" << endl;
+    cin.get();
+
+	return;
+}
+
+
+void AerobicVision::AverageCloudsKdTree(pcl::PointCloud<pcl::PointXYZI>::Ptr &average, std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> allClouds)
+{
+	vector<pcl::KdTreeFLANN<pcl::PointXYZI>> kdtree;
+
+	if(allClouds.size() <= 2)
+		return;
+	for(int g= 0; g< allClouds.size();g++)
+	{
+		kdtree.push_back(pcl::KdTreeFLANN<pcl::PointXYZI>());
+		kdtree[g].setInputCloud(allClouds[g]);
+	}
+	for(int i = 0; i< allClouds[0]->points.size(); i++)
+	{
+		pcl::PointXYZI averagePoint = pcl::PointXYZI(0.0f);
+		
+		if(isnan(allClouds[0]->points[i].x))
+		{
+			averagePoint.x = 0;
+			averagePoint.y = 0;
+			averagePoint.z = 0;
+			averagePoint.intensity =0;
+			average->push_back(averagePoint);
+			continue;
+		}
+
+		pcl::PointXYZI searchPoint;
+		searchPoint.x = allClouds[0]->points[i].x;
+		searchPoint.y = allClouds[0]->points[i].y;
+		searchPoint.z = allClouds[0]->points[i].y;
+		searchPoint.intensity = allClouds[0]->points[i].intensity;
+
+		averagePoint.x = searchPoint.x;
+		averagePoint.y = searchPoint.y;
+		averagePoint.z = searchPoint.z;
+		averagePoint.intensity = searchPoint.intensity;
+
+		cout << "[" << i << "] (" << searchPoint.x << "," << searchPoint.y << "," << searchPoint.z << ") > ";
+
+		for(int k = 1; k< allClouds.size();k++)
+		{
+			std::vector<int> pointIndex(1);
+			std::vector<float> pointDistance(1);
+
+			if(kdtree[k].nearestKSearch(searchPoint, 1, pointIndex, pointDistance) > 0)
+			{
+				cout << pointDistance[0] << ", ";
+				averagePoint.x += allClouds[k]->points[pointIndex[0]].x;
+				averagePoint.y += allClouds[k]->points[pointIndex[0]].y;
+				averagePoint.z += allClouds[k]->points[pointIndex[0]].z;
+				averagePoint.intensity += allClouds[k]->points[pointIndex[0]].intensity;
+			}
+			else
+			{
+				averagePoint.x += searchPoint.x;
+				averagePoint.y += searchPoint.y;
+				averagePoint.z += searchPoint.z;
+				averagePoint.intensity += searchPoint.intensity;
+			}
+		}
+
+		averagePoint.x /= allClouds.size();
+		averagePoint.y /= allClouds.size();
+		averagePoint.z /= allClouds.size();
+		averagePoint.intensity /= allClouds.size();
+		average->push_back(averagePoint);
+
+		cout << " < (" << averagePoint.x << "," << averagePoint.y << "," << averagePoint.z << ")" << endl;
+	}
+}
+
+void AerobicVision::Test2()
+{
+	cout << "Starting..." << endl;
+
+	usleep(500000);
+
+	std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> allClouds;
+
+	for(int i = 0; i< 4; i++)
+	{
+		usleep(200000);
+		if (! fg->WaitForFrame(img.get(), 2000))
+		{
+			std::cerr << "Timeout waiting for camera!" << std::endl;
+		}
+		usleep(500000);
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn = img->Cloud()->makeShared();
+		allClouds.push_back(cloudIn);
+		std::cout << "Taken pic " << i << endl;
+	}
+
+	//New cloud
+	pcl::PointCloud<pcl::PointXYZI>::Ptr average (new pcl::PointCloud<pcl::PointXYZI>);
+
+	average->width = allClouds[0]->width;
+	average->height = allClouds[0]->height;
+	average->points.resize(allClouds[0]->points.size());
+
+	//Average all clouds
+	AverageCloudsKdTree(average, allClouds);
+
+	//PrintCloud(average);
+
+	
+	//Visualize cloud
+	pcl::visualization::CloudViewer viewer("Cloud Viewer");
+	viewer.showCloud(average);
+	
+
+	cout << "Press Enter" << endl;
+    cin.get();
+
+	return;
+}
+
+
+
+///////////////////////////////////////////////////////
+//Projectgroep 2 implementatie
+///////////////////////////////////////////////////////
+
+pcl::PointXYZ AerobicVision::runAlgorithm(double &rotation, pcl::PointCloud<pcl::PointXYZ>::Ptr &showcloud)
+{
+	
+//logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address, using default one
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =std::make_shared<o3d3xx::FrameGrabber>(cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  if (! fg->WaitForFrame(img.get(), 2000))
+  {
+      std::cerr << "Timeout waiting for camera!" << std::endl;
+      return pcl::PointXYZ(0,0,0);	
+  }
+
+  //3D pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr xfilt (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr yfilt (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr zfilt (new pcl::PointCloud<pcl::PointXYZ>);
+
+  //convert o3d3xx cloud to PointXYZ  
+  pcl::PointCloud<pcl::PointXYZI>::Ptr temp_cloud = img->Cloud();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr segment (new pcl::PointCloud<pcl::PointXYZ>);
+  copyCloud(temp_cloud,cloud);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr vox_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ 
+   // cut the z axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passz;
+  passz.setInputCloud (cloud);
+  passz.setFilterFieldName ("z");
+  passz.setFilterLimits (-0.125,0.105);
+  passz.filter (*zfilt);
+
+  // cut the y axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passy;
+  passy.setInputCloud (zfilt);
+  passy.setFilterFieldName ("y");
+  passy.setFilterLimits (-0.1,0.075);
+  passy.filter (*yfilt);
+   
+  // Create the filtering object
+  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+  sor.setInputCloud (yfilt);
+  sor.setMeanK (100);
+  sor.setStddevMulThresh (0.5);
+  sor.filter (*cloud_filtered);
+
+   // Create the filtering object
+  pcl::VoxelGrid<pcl::PointXYZ> vox;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  vox.setInputCloud (cloud_filtered);
+  vox.setLeafSize (0.001f, 0.001f, 0.001f);
+  vox.filter (*vox_cloud);
+  
+  // cut the x axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passx;
+  passx.setInputCloud (vox_cloud);
+  passx.setFilterFieldName ("x");
+  passx.setFilterLimits (0.3, .63);//0.3,0.63
+  passx.filter (*xfilt);
+  /*
+    pcl::visualization::CloudViewer viewer("Filtered cloud");
+   viewer.showCloud(xfilt);
+
+  while (!viewer.wasStopped ())
+	{
+	
+	}*/
+
+  vector<pcl::PointXYZ> points;
+  if(xfilt->points.size()>0)
+  {
+	GetObject(xfilt, points);
+  }
+  else
+  {
+	  return pcl::PointXYZ(1000,1000,1000);
+  }
+
+  showcloud = xfilt;
+
+
+  rotation = GetRotation(xfilt,points);
+/*
+  std::vector <pcl::PointIndices> indices;
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> filteredCloud;
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr color (new pcl::PointCloud<pcl::PointXYZRGB>);
+  //Eigen::Vector4f min_pt,max_pt;
+  //pcl::getMinMax3D(xfilt,indices,min_pt,max_pt);
+  getClusters(xfilt, indices, color);
+  
+  for(int i = 0; i < indices.size();i++)
+  {
+	  pcl::PointCloud<pcl::PointXYZ>::Ptr cld (new pcl::PointCloud<pcl::PointXYZ>);
+	  filteredCloud.push_back(cld); 
+
+	  for(int k = 0; k < indices[i].indices.size(); k++)
+	  {
+	  	filteredCloud[i]->push_back(xfilt->points[ indices[i].indices[k] ]);
+	  }
+  }
+
+  FeatureExtractor(filteredCloud);
+
+
+  /*
+  for(int i = 0; i < filteredCloud.size(); i++)
+  {
+	cout << "Opening viewer for cloud " << to_string(i) << "/" << filteredCloud.size() << endl;
+
+	pcl::visualization::CloudViewer viewer("Filtered cloud #" + to_string(i));
+	viewer.showCloud(filteredCloud[i]);
+
+	usleep(1000000);
+
+	cout << "Press enter to continue" << endl;
+
+	while (!viewer.wasStopped ()){}
+
+	//pcl::visualization::PCLVisualizer viewer ("Filtered cloud #" + to_string(i));
+	//viewer.addPointCloud<pcl::PointXYZ>(filteredCloud[i],"cloud");
+
+	//while (!viewer.wasStopped ())
+	//{
+	//	viewer.spinOnce(100);
+	//}
+  }
+  
+  cin.ignore();
+  */
+
+  return points[2];
+}
+double AerobicVision::GetRotation(pcl::PointCloud<pcl::PointXYZ>::Ptr &in,vector<pcl::PointXYZ> &points)
+{
+	
+	Eigen::Vector3f major,middle,minor;
+	double rotation;
+	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
+	feature_extractor.setInputCloud(in);
+	feature_extractor.compute();
+	feature_extractor.getEigenVectors (major,middle,minor);
+	float length,height;
+	length = points[1].y - points[0].y;
+	height = points[1].z - points[0].z;
+	bool useMajorVector = false;
+	if(height > length)
+	{
+		if(height < 0.04)
+		{
+			useMajorVector = true;
+		}
+	}
+	else
+	{
+		if(length > height)
+		{
+			if(length < 0.04)
+			{
+				useMajorVector = true;
+			}
+		}
+	}
+	if(useMajorVector)
+	{
+		
+		rotation = acos(major[2]);
+	}
+	else
+	{
+		
+		rotation = acos(middle[2]);
+	}
+	cout << setprecision(15) << rotation << endl;
+	//cout << major[0] << "," << major[1] << "," << major[2] << endl;
+	//cout << middle[0] << "," << middle[1] << "," << middle[2] << endl;
+	//cout<< rotation.x <<"," << rotation.y << "," << rotation.z << endl;
+	
+	return rotation;
+}
+bool AerobicVision::GetObject(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, vector<pcl::PointXYZ> &out)
+{
+	float treshold = 0.01f;
+	pcl::PointXYZ highPoint = pcl::PointXYZ(1000,1000,1000);
+	pcl::PointXYZ AA,BB;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr toFilter(new pcl::PointCloud<pcl::PointXYZ>);
+	for(int i = 0; i< in->points.size();i++)
+	{
+		if(in->points[i].x < highPoint.x)
+		{
+			highPoint = in->points[i];
+		}
+	}
+	  // cut the z axis of pcl
+
+   
+  // cut the x axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passx;
+  passx.setInputCloud (in);
+  passx.setFilterFieldName ("x");
+  passx.setFilterLimits (highPoint.x-treshold, highPoint.x +treshold);//0.3,0.63
+  passx.filter (*toFilter);
+
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr color (new pcl::PointCloud<pcl::PointXYZRGB>);
+  getClusters(toFilter, clusters, color); 
+
+  AA = clusters[0]->points[0];
+  BB = clusters[0]->points[0];
+
+/*
+	   pcl::visualization::CloudViewer viewer("Filtered cloud #");
+   viewer.showCloud(clusters[0]);
+
+  while (!viewer.wasStopped ())
+	{
+	
+	}
+*/
+
+  for(pcl::PointXYZ point : clusters[0]->points)
+  {
+	if(point.y < AA.y)
+	{
+		AA.y = point.y;
+	}
+	if(point.y > BB.y)
+	{
+		BB.y = point.y;
+	}
+	if(point.z < AA.z)
+	{
+		AA.z = point.z;
+	}
+	if(point.z> BB.z)
+	{
+		BB.z = point.z;
+	}
+  }
+
+  out.push_back(AA);
+  out.push_back(BB);
+  highPoint.y = (AA.y+BB.y)/2;
+  highPoint.z = (AA.z+BB.z)/2;
+  out.push_back(highPoint);
+
+  std::cout << "Found AA " << AA.x << ", " << AA.y << ", " << AA.z << endl;
+  std::cout << "Found BB " << BB.x << ", " << BB.y << ", " << BB.z << endl;
+  std::cout << "Found CC " << highPoint.x << ", " <<  highPoint.y << ", " <<  highPoint.z << endl;
+
+  return true;
+}
+
+void AerobicVision::copyCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out)
+{
+  for(int i = 0; i< in->points.size(); i++)
+  {
+    pcl::PointXYZ p = pcl::PointXYZ(in->points[i].x, in->points[i].y, in->points[i].z);
+    out->push_back(p);
+  }
+}
+
+void AerobicVision::getClusters(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &out, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &colorout)
+{
+  // find the normals
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+  tree->setInputCloud(in);
+
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setSearchMethod (tree);
+  normal_estimator.setInputCloud (in);
+  //normal_estimator.setKSearch (50);
+  normal_estimator.setRadiusSearch (0.001);
+  normal_estimator.compute (*normals);
+    
+  // region growing
+  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+  std::vector <pcl::PointIndices> indices;
+
+  reg.setMinClusterSize (25);
+  reg.setMaxClusterSize (2000);
+  reg.setSearchMethod (tree);
+  reg.setNumberOfNeighbours (15);
+  reg.setInputCloud (in);
+  //reg.setIndices (indices);
+  reg.setInputNormals (normals);
+  reg.setSmoothnessThreshold (2.0 / 180.0 * M_PI);// graden naar radial
+  reg.setCurvatureThreshold (1.0);
+  reg.extract (indices);
+
+  colorout = reg.getColoredCloud();
+
+  for(int i = 0; i < indices.size();i++)
+  {
+	  pcl::PointCloud<pcl::PointXYZ>::Ptr cld (new pcl::PointCloud<pcl::PointXYZ>);
+	  out.push_back(cld); 
+
+	  for(int k = 0; k < indices[i].indices.size(); k++)
+	  {
+	  	out[i]->push_back(in->points[ indices[i].indices[k] ]);
+	  }
+  }
+}
+
+bool AerobicVision::FeatureExtractor(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &in)
+{
+  for(int i = 0; i < in.size()-1; i++)
+  {
+
+	cout << "Showing object " << i << endl;
+
+	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
+	feature_extractor.setInputCloud (in[i]);
+	feature_extractor.compute ();
+
+	std::vector <float> moment_of_inertia;
+	std::vector <float> eccentricity;
+	pcl::PointXYZ min_point_AABB;
+	pcl::PointXYZ max_point_AABB;
+	pcl::PointXYZ min_point_OBB;
+	pcl::PointXYZ max_point_OBB;
+	pcl::PointXYZ position_OBB;
+	Eigen::Matrix3f rotational_matrix_OBB;
+	float major_value, middle_value, minor_value;
+	Eigen::Vector3f major_vector, middle_vector, minor_vector;
+	Eigen::Vector3f mass_center;
+
+	feature_extractor.getMomentOfInertia (moment_of_inertia);
+	feature_extractor.getEccentricity (eccentricity);
+	feature_extractor.getAABB (min_point_AABB, max_point_AABB);
+	feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
+	feature_extractor.getEigenValues (major_value, middle_value, minor_value);
+	feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
+	feature_extractor.getMassCenter (mass_center);
+
+	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+	viewer->setBackgroundColor (0, 0, 0);
+	viewer->addCoordinateSystem (1.0);
+	viewer->initCameraParameters ();
+	viewer->addPointCloud<pcl::PointXYZ> (in[i], "sample cloud");
+	viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
+
+	Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
+	Eigen::Quaternionf quat (rotational_matrix_OBB);
+	viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
+
+	pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
+	pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
+	pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
+	pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
+	viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
+	viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
+	viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
+
+	while(!viewer->wasStopped())
+	{
+		viewer->spinOnce (100);
+		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+	}
+  }
+
+  return true;
+}

Vision/AerobicVision.h

@@ -0,0 +1,69 @@
+#include <iostream>
+#include <algorithm>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/gp3.h>
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+#include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/conditional_euclidean_clustering.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <vector>
+#include <pcl/filters/extract_indices.h>
+#include <math.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/segmentation/region_growing.h>
+#include <boost/thread/thread.hpp>
+#include <pcl/console/time.h>
+#include <stdio.h>
+#include <string>
+
+using namespace std;
+
+typedef pcl::PointXYZI PointTypeIO;
+typedef pcl::PointXYZINormal PointTypeFull;
+
+struct objectData {
+	pcl::PointXYZ xyz;
+	float pitch;
+	float yaw;
+	float rx, ry, rz;		
+};
+
+class AerobicVision{
+	private:
+		o3d3xx::Camera::Ptr cam;
+		o3d3xx::ImageBuffer::Ptr img;
+		o3d3xx::FrameGrabber::Ptr fg;
+	public:
+		AerobicVision(std::string IP);
+		pcl::PointCloud<pcl::PointXYZI>::Ptr PassFilterPCD(pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr convertCloudXYZItoXYZ(pcl::PointCloud<pcl::PointXYZI> cloudIn);		
+		pcl::PointXYZ Calibrate(int samples);
+ 		std::vector<objectData> RegionGrowing();
+		pcl::PointXYZ ToRotVector(pcl::PointXYZ rpy);
+		//bool wayToSort(pcl::PointXYZ i, pcl::PointXYZ j);
+		pcl::PointCloud<pcl::PointXYZI> AverageCloud(std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> totalList);
+		void PrintClouds(std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> totalList);
+		void PrintCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud);
+		void Test();
+		void AverageCloudsKdTree(pcl::PointCloud<pcl::PointXYZI>::Ptr &average, std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> allClouds);
+		void Test2();
+		pcl::PointXYZ runAlgorithm(double&, pcl::PointCloud<pcl::PointXYZ>::Ptr &showcloud);
+		void copyCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+		void getClusters(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &out, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &colorout);
+		bool GetObject(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, vector<pcl::PointXYZ> &out);
+		bool FeatureExtractor(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &in);
+		double GetRotation(pcl::PointCloud<pcl::PointXYZ>::Ptr &in,vector<pcl::PointXYZ> &points);
+};

Vision/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(cloud_viewer)
+
+set(CMAKE_MODULE_PATH
+    ${CMAKE_SOURCE_DIR}/cmake/modules
+    ${CMAKE_MODULE_PATH}
+    )
+
+################################################
+## Bring in dependent projects
+################################################
+find_package(o3d3xx_camera REQUIRED)
+find_package(o3d3xx_framegrabber REQUIRED)
+find_package(o3d3xx_image REQUIRED)
+find_package(PCL 1.7.1 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+find_library(LIB_boost_system NAMES boost_system)
+
+################################################
+## Manage our compiler and linker flags
+################################################
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+include_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  )
+link_directories(
+  ${O3D3XX_IMAGE_LIBRARY_DIR}
+  ${O3D3XX_CAMERA_LIBRARY_DIR}
+  ${O3D3XX_FRAMEGRABBER_LIBRARY_DIR}
+  ${PCL_LIBRARY_DIRS}
+  ${OpenCV_LIBRARY_DIRS}
+  )
+add_definitions(
+  ${PCL_DEFINITIONS}
+  ${OpenCV_DEFINITIONS}
+  )
+add_library(visionlib
+	${CMAKE_SOURCE_DIR}/AerobicVision.cpp
+)
+target_link_libraries (visionlib ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES}
+		      )
+
+## Build paths - add example files here
+
+## example segmentation 
+add_executable (main main.cpp)
+target_link_libraries (main ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES}
+		      visionlib)
+
+##add_executable (AerobicVision AerobicVision.cpp)
+##target_link_libraries (AerobicVision ${O3D3XX_CAMERA_LIBRARIES}
+##                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+##                      ${O3D3XX_IMAGE_LIBRARIES}
+##                      ${PCL_LIBRARIES}
+##                      ${OpenCV_LIBRARIES})
+
+
+
+

Vision/examples/3d_test.cpp

@@ -0,0 +1,131 @@
+#include <iostream>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/gp3.h>
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr convertCloud(pcl::PointCloud<o3d3xx::PointT>::Ptr inputCloud);
+
+int main(int argc, const char **argv)
+{
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =
+    std::make_shared<o3d3xx::FrameGrabber>(
+      cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  //get frame from camera
+  if (! fg->WaitForFrame(img.get(), 2000))
+    {
+      std::cerr << "Timeout waiting for camera!" << std::endl;
+      return -1;	
+    }
+
+  //3D pointcloud
+  
+  //convert o3d3xx cloud to PointXYZI  
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = img->Cloud();
+
+  //init cloud viewer
+  //pcl::visualization::CloudViewer cloudViewer("Cloud Viewer");
+  //show in cloud viewer
+  //cloudViewer.showCloud(cloud);
+  
+  
+  // ----------------------------------------------------------------
+  // --------Create PCL visualizer with paramaters-------------------
+  // ----------------------------------------------------------------
+  pcl::visualization::PCLVisualizer viewer("3D Viewer");
+  viewer.setCameraPosition(0,0,0,-1,0,0);
+  viewer.setBackgroundColor(0,0,0);
+  //viewer.addPointCloud<pcl::PointXYZI>(cloud,"Sample cloud");
+  //viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Sample cloud");
+  viewer.addCoordinateSystem(1.0,0,0,0,0);
+  viewer.initCameraParameters();
+  // ----------------------------------------------------------------
+  // -----Calculate surface normals with a search radius of 0.05-----
+  // ----------------------------------------------------------------
+  pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;
+  ne.setInputCloud (cloud);
+  pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI> ());
+  ne.setSearchMethod (tree);
+  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
+  ne.setRadiusSearch (0.05);
+  ne.compute (*cloud_normals);
+  cout << "Found normals: " << cloud_normals->size() << endl;
+  //display normals
+  //viewer.addPointCloudNormals<pcl::PointXYZI, pcl::Normal> (cloud, cloud_normals, 10, 0.05, "normals1", 0);
+
+
+  //--------rough surface calculation
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
+  cloudXYZ = convertCloud(img->Cloud());
+  cout << "Size: " << cloudXYZ->size() << endl;
+  pcl::PointCloud<pcl::Normal>::Ptr surface_normals(new pcl::PointCloud<pcl::Normal>);
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr treeXYZ(new pcl::search::KdTree<pcl::PointXYZ> ());
+  treeXYZ->setInputCloud(cloudXYZ);
+  n.setInputCloud(cloudXYZ);
+  n.setSearchMethod(treeXYZ);
+  n.setRadiusSearch(0.05);
+  n.compute(*surface_normals);
+  //cloud concatenate
+  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_surface ( new pcl::PointCloud<pcl::PointNormal>);
+  pcl::concatenateFields(*cloudXYZ, *surface_normals, *cloud_surface);
+  //search tree
+  pcl::search::KdTree<pcl::PointNormal>::Ptr normaltree (new pcl::search::KdTree<pcl::PointNormal>);
+  normaltree->setInputCloud(cloud_surface);
+  //triangulate polygons
+  pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
+  pcl::PolygonMesh triangles;
+  gp3.setSearchRadius(0.5);
+  gp3.setMu(2.5);
+  gp3.setMaximumNearestNeighbors(100);
+  gp3.setMaximumSurfaceAngle(M_PI/4);
+  gp3.setMinimumAngle(M_PI/18);
+  gp3.setMaximumAngle(2*M_PI/3);
+  gp3.setNormalConsistency(false);
+  //calculate
+  gp3.setInputCloud(cloud_surface);
+  gp3.setSearchMethod(normaltree);
+  gp3.reconstruct(triangles);
+  //display
+  viewer.addPolygonMesh(triangles,"surfaces", 0);
+  //wait to display images/pointcloud
+  while (!viewer.wasStopped ())
+  {
+    viewer.spinOnce (100);
+  }
+  cv::waitKey(0);
+  return 0;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr convertCloud(pcl::PointCloud<o3d3xx::PointT>::Ptr inputCloud){
+  pcl::PointCloud<o3d3xx::PointT>::Ptr cloud = inputCloud;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr newcloud(new pcl::PointCloud<pcl::PointXYZ>);
+  
+  newcloud->width = cloud->width;
+  newcloud->height = cloud->height;
+  newcloud->points.resize(cloud->points.size());
+
+  for(std::size_t i = 0; i < cloud->points.size(); i++) {
+    newcloud->points[i].x = cloud->points[i].x;
+    newcloud->points[i].y = cloud->points[i].y;
+    newcloud->points[i].z = cloud->points[i].z;
+  }
+
+  return newcloud;
+}

Vision/examples/3d_viewer.cpp

@@ -0,0 +1,357 @@
+
+#include <memory>
+#include <vector>
+#include <iostream>
+#include <pcl/common/io.h>
+#include <opencv2/opencv.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/gp3.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/search/search.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/segmentation/region_growing.h>
+#include <pcl/segmentation/segment_differences.h>
+
+#include <pcl/ModelCoefficients.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+
+#include <pcl/filters/extract_indices.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+
+#include <pcl/io/pcd_io.h>
+
+void copyCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+void testPlanarSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &in);
+void testRegionGrowing(pcl::PointCloud<pcl::PointXYZ>::Ptr &in);
+void normal_est(pcl::PointCloud<pcl::PointXYZ>::Ptr &in);
+pcl::PointXYZ findHighestPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr &in);
+//void substract_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+
+int main(int argc, const char **argv)
+{
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address, using default one
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =std::make_shared<o3d3xx::FrameGrabber>(cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  if (! fg->WaitForFrame(img.get(), 2000))
+  {
+      std::cerr << "Timeout waiting for camera!" << std::endl;
+      return -1;	
+  }
+
+  //3D pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr xfilt (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr yfilt (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr zfilt (new pcl::PointCloud<pcl::PointXYZ>);
+
+  //convert o3d3xx cloud to PointXYZ  
+  pcl::PointCloud<pcl::PointXYZI>::Ptr temp_cloud = img->Cloud();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr segment (new pcl::PointCloud<pcl::PointXYZ>);
+  copyCloud(temp_cloud,cloud);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr vox_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ 
+  
+  
+   // cut the z axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passz;
+  passz.setInputCloud (cloud);
+  passz.setFilterFieldName ("z");
+  passz.setFilterLimits (-0.125,0.105);
+  passz.filter (*zfilt);
+
+  // cut the y axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passy;
+  passy.setInputCloud (zfilt);
+  passy.setFilterFieldName ("y");
+  passy.setFilterLimits (-0.1,0.075);
+  passy.filter (*yfilt);
+   
+  // Create the filtering object
+  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+  sor.setInputCloud (yfilt);
+  sor.setMeanK (100);
+  sor.setStddevMulThresh (0.5);
+  sor.filter (*cloud_filtered);
+
+   // Create the filtering object
+  pcl::VoxelGrid<pcl::PointXYZ> vox;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  vox.setInputCloud (cloud_filtered);
+  vox.setLeafSize (0.001f, 0.001f, 0.001f);
+  vox.filter (*vox_cloud);
+  
+  //pcl::PCDWriter writer;
+	pcl::PCDReader reader;
+	//writer.write<pcl::PointXYZ>("bottom.pcd", *vox_cloud, false); // 1x 
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+	reader.read("bottom.pcd", *read_cloud);
+
+  pcl::PointXYZ point = findHighestPoint(read_cloud);
+  cout <<"point x: "<< point.x <<" point y: " <<point.y <<" point z: " <<point.z << endl;
+  // cut the x axis of pcl
+  pcl::PassThrough<pcl::PointXYZ> passx;
+  passx.setInputCloud (vox_cloud);
+  passx.setFilterFieldName ("x");
+  passx.setFilterLimits (0.3, .63);//0.3,0.63
+  passx.filter (*xfilt);
+
+  normal_est(xfilt);
+  //testPlanarSegmentation(vox_cloud);
+  //substract_cloud(vox_cloud,read_cloud);
+
+  //pcl::visualization::CloudViewer viewer("Cloud Viewer");
+  //viewer.showCloud(xfilt);
+
+  cv::waitKey(0);
+  return 0;
+}
+
+void copyCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out)
+{
+  for(int i = 0; i< in->points.size(); i++)
+  {
+    pcl::PointXYZ p = pcl::PointXYZ(in->points[i].x, in->points[i].y, in->points[i].z);
+    out->push_back(p);
+  }
+}
+
+pcl::PointXYZ findHighestPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr &in)
+{
+  pcl::PointXYZ fp;
+
+  for(pcl::PointXYZ p : in->points)
+  {
+    if(p.x > fp.x)
+      fp = pcl::PointXYZ(p.x, p.y, p.z);
+  }
+
+  return fp;
+}
+
+//void substract_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &in, pcl::PointCloud<pcl::PointXYZ>::Ptr &out)
+//{
+//  return 0;
+//}
+void testPlanarSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &in)
+{ 
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
+
+  // PLANAR SEGMENTATION
+  // Create the segmentation object
+  pcl::SACSegmentation<pcl::PointXYZ> seg;
+  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
+
+  // Optional
+  seg.setOptimizeCoefficients (true);
+  // Mandatory
+  seg.setModelType (pcl::SACMODEL_PLANE);
+  seg.setMethodType (pcl::SAC_RANSAC);
+  seg.setMaxIterations (100);
+  seg.setDistanceThreshold (0.001);
+
+  /*
+  seg.setInputCloud (in);
+  seg.segment (*inliers, *coefficients);
+  cout << inliers << " - " << coefficients <<endl;
+  */
+  int i=0, nr_points = (int) in->points.size ();
+  while (in->points.size () > 0.3 * nr_points)
+  {
+    // Segment the largest planar component from the remaining cloud
+    seg.setInputCloud (in);
+    seg.segment (*inliers, *coefficients);
+    if (inliers->indices.size () == 0)
+    {
+      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
+      break;
+    }
+
+    // Extract the planar inliers from the input cloud
+    pcl::ExtractIndices<pcl::PointXYZ> extract;
+    extract.setInputCloud (in);
+    extract.setIndices (inliers);
+    extract.setNegative (false);
+
+    // Get the points associated with the planar surface
+    extract.filter (*cloud_plane);
+    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
+
+    // Remove the planar inliers, extract the rest
+    extract.setNegative (false);
+    extract.filter (*cloud_f);
+    *in = *cloud_f;
+  }
+
+  *cloud_plane = *in;
+
+  // Creating the KdTree object for the search method of the extraction
+  cout << " Clustersize: " << cloud_plane->points.size() << endl;
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr boom (new pcl::search::KdTree<pcl::PointXYZ>);
+  //pcl::search::Search<pcl::PointXYZ>::Ptr boom = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
+  boom->setInputCloud(cloud_plane);
+
+  std::vector<pcl::PointIndices> cluster_indices;
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+  ec.setClusterTolerance (0.2); // 1cm
+  ec.setMinClusterSize (10);
+  ec.setMaxClusterSize (1000);
+  ec.setSearchMethod (boom);
+  ec.setInputCloud (cloud_plane);
+  ec.extract (cluster_indices);
+  
+  /*
+  //SUBSTRACT PCL METHOD
+  pcl::PointIndices::Ptr fInliers (new pcl::PointIndices);
+ 
+  //Extract fInliers from the input cloud
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud(cloud_plane);
+  extract.setIndices (fInliers);
+  //extract.setNegative (false); //Removes part_of_cloud but retain the original full_cloud
+  extract.setNegative (false); // Removes part_of_cloud from full cloud  and keep the rest
+  extract.filter (*in);
+  */
+
+  //viewer.addPointCloud<pcl::PointXYZ>(in,"cloud");
+  /*
+  int j = 0;
+  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+  {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
+      cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
+    cloud_cluster->width = cloud_cluster->points.size ();
+    cloud_cluster->height = 1;
+    cloud_cluster->is_dense = true;
+  }*/
+
+  /*
+  
+  
+
+  //pcl::visualization::PCLVisualizer viewer ("Cluster viewer");
+
+  while (!viewer.wasStopped ())
+  {
+      viewer.spinOnce(100);
+  }
+  */
+  cv::waitKey(0);
+  return;
+}
+
+void normal_est(pcl::PointCloud<pcl::PointXYZ>::Ptr &in)
+{
+  // find the normals
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+  tree->setInputCloud(in);
+
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setSearchMethod (tree);
+  normal_estimator.setInputCloud (in);
+  //normal_estimator.setKSearch (50);
+  normal_estimator.setRadiusSearch (0.02);
+  normal_estimator.compute (*normals);
+    
+  // region growing
+  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+  std::vector <pcl::PointIndices> clusters;
+
+  reg.setMinClusterSize (10);
+  reg.setMaxClusterSize (1000);
+  reg.setSearchMethod (tree);
+  reg.setNumberOfNeighbours (50);
+  reg.setInputCloud (in);
+  //reg.setIndices (indices);
+  reg.setInputNormals (normals);
+  reg.setSmoothnessThreshold (2.0 / 180.0 * M_PI);// graden naar radial
+  reg.setCurvatureThreshold (5.0);
+  reg.extract (clusters);
+
+  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
+  pcl::visualization::PCLVisualizer viewer ("Cloud viewer");
+  viewer.addPointCloud<pcl::PointXYZRGB>(colored_cloud,"cloud");
+  viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(in, normals, 100, 0.01, "normals",0);
+
+  while (!viewer.wasStopped ())
+  {
+      viewer.spinOnce(100);
+  }
+
+
+}
+
+void testRegionGrowing(pcl::PointCloud<pcl::PointXYZ>::Ptr &in)
+{
+   //pcl::visualization::CloudViewer cviewer("Cloud Viewer");
+  //cviewer.showCloud(vox_cloud);
+
+  /*
+  // find the normals
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setSearchMethod (tree);
+  normal_estimator.setInputCloud (vox_cloud);
+  normal_estimator.setKSearch (50);
+  normal_estimator.compute (*normals);
+    
+  // region growing
+  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+  std::vector <pcl::PointIndices> clusters;
+
+  reg.setMinClusterSize (10);
+  reg.setMaxClusterSize (1000);
+  reg.setSearchMethod (tree);
+  reg.setNumberOfNeighbours (50);
+  reg.setInputCloud (vox_cloud);
+  //reg.setIndices (indices);
+  reg.setInputNormals (normals);
+  reg.setSmoothnessThreshold (2.0 / 180.0 * M_PI);// graden naar radial
+  reg.setCurvatureThreshold (5.0);
+  reg.extract (clusters);
+
+
+  */
+  //pcl::visualization::CloudViewer viewer("Cloud Viewer");
+  //viewer.showCloud(cloud_filtered);
+  
+  //pcl::PointCloud <pcl::PointXYZRGB>::Ptr vox_cloud = seg.getInputNormals ();
+  //pcl::PointCloud <pcl::PointXYZRGB>::Ptr vox_cloud = ec.getInputCloud();
+  pcl::visualization::PCLVisualizer viewer ("Cluster viewer");
+  //viewer.addPointCloud<pcl::PointXYZ>(vox_cloud,"cloud");
+  //viewer.addPointCloud<pcl::PointXYZRGB>(segview,"cloud");
+  
+  //viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(colored_cloud, normals, 10, 0.05, "normals",0);
+
+  while (!viewer.wasStopped ())
+  {
+      viewer.spinOnce(100);
+  }
+}

Vision/examples/CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(cloud_viewer)
+
+set(CMAKE_MODULE_PATH
+    ${CMAKE_SOURCE_DIR}/../cmake/modules
+    ${CMAKE_MODULE_PATH}
+    )
+
+################################################
+## Bring in dependent projects
+################################################
+find_package(o3d3xx_camera REQUIRED)
+find_package(o3d3xx_framegrabber REQUIRED)
+find_package(o3d3xx_image REQUIRED)
+find_package(PCL 1.7.1 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+find_library(LIB_boost_system NAMES boost_system)
+
+################################################
+## Manage our compiler and linker flags
+################################################
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+include_directories(
+  ${O3D3XX_IMAGE_INCLUDE_DIRS}
+  ${O3D3XX_CAMERA_INCLUDE_DIRS}
+  ${O3D3XX_FRAMEGRABBER_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  )
+link_directories(
+  ${O3D3XX_IMAGE_LIBRARY_DIR}
+  ${O3D3XX_CAMERA_LIBRARY_DIR}
+  ${O3D3XX_FRAMEGRABBER_LIBRARY_DIR}
+  ${PCL_LIBRARY_DIRS}
+  ${OpenCV_LIBRARY_DIRS}
+  )
+add_definitions(
+  ${PCL_DEFINITIONS}
+  ${OpenCV_DEFINITIONS}
+  )
+
+## Build paths - add example files here
+
+## example image viewer
+add_executable (DisplayImage DisplayImage.cpp )
+target_link_libraries( DisplayImage ${OpenCV_LIBS} )
+## example cloud viewer
+add_executable (cloud_viewer_ex cloud_viewer_example.cpp)
+target_link_libraries (cloud_viewer_ex ${PCL_LIBRARIES})
+## example pclvisualizer
+
+add_executable (pclvis pclvisualizer.cpp)
+target_link_libraries (pclvis ${PCL_LIBRARIES})
+## example live viewer 
+add_executable (live_viewer live_viewer.cpp)
+target_link_libraries (live_viewer ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES})
+## 3d viewer
+add_executable (3d_viewer 3d_viewer.cpp)
+target_link_libraries (3d_viewer ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES})
+## example segmentation 
+add_executable (segmentation segmentation.cpp)
+target_link_libraries (segmentation  ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES})
+## example segmentation 
+add_executable (3d_test 3d_test.cpp)
+target_link_libraries (3d_test  ${O3D3XX_CAMERA_LIBRARIES}
+                      ${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      ${O3D3XX_IMAGE_LIBRARIES}
+                      ${PCL_LIBRARIES}
+                      ${OpenCV_LIBRARIES})
+## cec
+add_executable (conditional_euclidean_clustering conditional_euclidean_clustering.cpp)
+target_link_libraries (conditional_euclidean_clustering ${PCL_LIBRARIES})
+
+#rgs
+add_executable (region_growing_segmentation region_growing_segmentation.cpp)
+target_link_libraries (region_growing_segmentation  ${O3D3XX_CAMERA_LIBRARIES}
+                      	${O3D3XX_FRAMEGRABBER_LIBRARIES}
+                      	${O3D3XX_IMAGE_LIBRARIES}
+			${PCL_LIBRARIES})
+
+
+

Vision/examples/DisplayImage.cpp

@@ -0,0 +1,28 @@
+#include <stdio.h>
+#include <opencv2/opencv.hpp>
+
+using namespace cv;
+
+int main(int argc, char** argv )
+{
+    if ( argc != 2 )
+    {
+        printf("usage: DisplayImage.out <Image_Path>\n");
+        return -1;
+    }
+
+    Mat image;
+    image = imread( argv[1], 1 );
+
+    if ( !image.data )
+    {
+        printf("No image data \n");
+        return -1;
+    }
+    namedWindow("Display Image", WINDOW_AUTOSIZE );
+    imshow("Display Image", image);
+
+    waitKey(0);
+
+    return 0;
+}

Vision/examples/cloud_viewer_example.cpp

@@ -0,0 +1,69 @@
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/parse.h>
+    
+int user_data;
+    
+void 
+viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
+{
+    viewer.setBackgroundColor (1.0, 0.5, 1.0);
+    pcl::PointXYZ o;
+    o.x = 1.0;
+    o.y = 0;
+    o.z = 0;
+    viewer.addSphere (o, 0.25, "sphere", 0);
+    std::cout << "i only run once" << std::endl;
+    
+}
+    
+void 
+viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
+{
+    static unsigned count = 0;
+    std::stringstream ss;
+    ss << "Once per viewer loop: " << count++;
+    viewer.removeShape ("text", 0);
+    viewer.addText (ss.str(), 200, 300, "text", 0);
+    
+    //FIXME: possible race condition here:
+    user_data++;
+}
+    
+int 
+main (int argc, char** argv)
+{
+    if (pcl::console::find_argument (argc, argv, "-f") >= 0)
+    {
+	cout << "File: " << argv[2] << endl;	
+    } else {
+        cout << "Please input file name using -f example.pcd" << endl;
+        return 0;
+    }
+    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
+    pcl::io::loadPCDFile (argv[2], *cloud);
+    
+    pcl::visualization::CloudViewer viewer("Cloud Viewer");
+    
+    //blocks until the cloud is actually rendered
+    viewer.showCloud(cloud);
+    
+    //use the following functions to get access to the underlying more advanced/powerful
+    //PCLVisualizer
+    
+    //This will only get called once
+    viewer.runOnVisualizationThreadOnce (viewerOneOff);
+    
+    //This will get called once per visualization iteration
+    viewer.runOnVisualizationThread (viewerPsycho);
+    while (!viewer.wasStopped ())
+    {
+    //you can also do cool processing here
+    //FIXME: Note that this is running in a separate thread from viewerPsycho
+    //and you should guard against race conditions yourself...
+    user_data++;
+    }
+    return 0;
+}

Vision/examples/conditional_euclidean_clustering.cpp

@@ -0,0 +1,120 @@
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/time.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/segmentation/conditional_euclidean_clustering.h>
+
+typedef pcl::PointXYZI PointTypeIO;
+typedef pcl::PointXYZINormal PointTypeFull;
+
+bool
+enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
+{
+  if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
+    return (true);
+  else
+    return (false);
+}
+
+bool
+enforceCurvatureOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
+{
+  Eigen::Map<const Eigen::Vector3f> point_a_normal(point_a.normal);
+  Eigen::Map<const Eigen::Vector3f> point_b_normal(point_b.normal);
+  if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
+    return (true);
+  if (fabs (point_a_normal.dot (point_b_normal)) < 0.05)
+    return (true);
+  return (false);
+}
+
+bool
+customRegionGrowing (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
+{
+  Eigen::Map<const Eigen::Vector3f> point_a_normal(point_a.normal);
+  Eigen::Map<const Eigen::Vector3f> point_b_normal(point_b.normal);
+  if (squared_distance < 10000)
+  {
+    if (fabs (point_a.intensity - point_b.intensity) < 8.0f)
+      return (true);
+    if (fabs (point_a_normal.dot (point_b_normal)) < 0.06)
+      return (true);
+  }
+  else
+  {
+    if (fabs (point_a.intensity - point_b.intensity) < 3.0f)
+      return (true);
+  }
+  return (false);
+}
+
+int
+main (int argc, char** argv)
+{
+  // Data containers used
+  pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
+  pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
+  pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
+  pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
+  pcl::console::TicToc tt;
+
+  // Load the input point cloud
+  std::cerr << "Loading...\n", tt.tic ();
+  pcl::io::loadPCDFile ("Statues_4.pcd", *cloud_in);
+  std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";
+
+  // Downsample the cloud using a Voxel Grid class
+  std::cerr << "Downsampling...\n", tt.tic ();
+  pcl::VoxelGrid<PointTypeIO> vg;
+  vg.setInputCloud (cloud_in);
+  vg.setLeafSize (80.0, 80.0, 80.0);
+  vg.setDownsampleAllData (true);
+  vg.filter (*cloud_out);
+  std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";
+
+  // Set up a Normal Estimation class and merge data in cloud_with_normals
+  std::cerr << "Computing normals...\n", tt.tic ();
+  pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
+  pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
+  ne.setInputCloud (cloud_out);
+  ne.setSearchMethod (search_tree);
+  ne.setRadiusSearch (300.0);
+  ne.compute (*cloud_with_normals);
+  std::cerr << ">> Done: " << tt.toc () << " ms\n";
+
+  // Set up a Conditional Euclidean Clustering class
+  std::cerr << "Segmenting to clusters...\n", tt.tic ();
+  pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);
+  cec.setInputCloud (cloud_with_normals);
+  cec.setConditionFunction (&customRegionGrowing);
+  cec.setClusterTolerance (500.0);
+  cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);
+  cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);
+  cec.segment (*clusters);
+  cec.getRemovedClusters (small_clusters, large_clusters);
+  std::cerr << ">> Done: " << tt.toc () << " ms\n";
+
+  // Using the intensity channel for lazy visualization of the output
+  for (int i = 0; i < small_clusters->size (); ++i)
+    for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
+      cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
+  for (int i = 0; i < large_clusters->size (); ++i)
+    for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
+      cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;
+  for (int i = 0; i < clusters->size (); ++i)
+  {
+    int label = rand () % 8;
+    for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
+      cloud_out->points[(*clusters)[i].indices[j]].intensity = label;
+  }
+
+  // Save the output point cloud
+  std::cerr << "Saving...\n", tt.tic ();
+  pcl::io::savePCDFile ("output.pcd", *cloud_out);
+  std::cerr << ">> Done: " << tt.toc () << " ms\n";
+
+  return (0);
+}
+

Vision/examples/live_viewer.cpp

@@ -0,0 +1,82 @@
+#include <iostream>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/gp3.h>
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+
+int main(int argc, const char **argv)
+{
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address, using default one
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =
+    std::make_shared<o3d3xx::FrameGrabber>(
+      cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  //get frame from camera (could be looped to create an actual live feed)
+  if (! fg->WaitForFrame(img.get(), 2000))
+    {
+      std::cerr << "Timeout waiting for camera!" << std::endl;
+      return -1;	
+    }
+  
+  //2D images
+  double min, max;
+  //depth image
+  cv::Mat depthImage = cv::Mat(img->DepthImage());
+  cv::minMaxIdx(depthImage, &min, &max);
+  cout << "Depth image" << endl; 
+  cout << "Min: " << min << " Max: " << max << endl;
+  cout << "Type: " << depthImage.type() << endl;
+  cv::convertScaleAbs(depthImage, depthImage, 255 / max);
+  cv::applyColorMap(depthImage, depthImage, cv::COLORMAP_JET);
+  cv::imshow("Depth image", depthImage);
+  //xyz image
+  cv::Mat xyzImage = cv::Mat(img->XYZImage());
+  cv::minMaxIdx(xyzImage, &min, &max);
+  cout << "XYZ image" << endl; 
+  cout << "Min: " << min << " Max: " << max << endl;
+  cout << "Type: " << xyzImage.type() << endl;
+  cv::convertScaleAbs(xyzImage, xyzImage, 255 / max);
+  cv::applyColorMap(xyzImage, xyzImage, cv::COLORMAP_JET);
+  cv::imshow("XYZ image", xyzImage);
+  //amplitude image
+  cv::imshow("Raw amplitude image", img->AmplitudeImage());
+  //confidence image
+  cv::imshow("Raw confidence image", img->ConfidenceImage());
+  cv::waitKey(0);
+  //3D pointcloud
+  
+  //convert o3d3xx cloud to PointXYZI  
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = img->Cloud();
+
+  // ----------------------------------------------------------------
+  // --------Create PCL visualizer with paramaters-------------------
+  // ----------------------------------------------------------------
+  pcl::visualization::PCLVisualizer viewer("3D Viewer");
+  viewer.setCameraPosition(0,0,0,-1,0,0);
+  viewer.setBackgroundColor(0,0,0);
+  viewer.addPointCloud<pcl::PointXYZI>(cloud,"Sample cloud");
+  //viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Sample cloud");
+  viewer.addCoordinateSystem(1.0,0,0,0,0);
+  viewer.initCameraParameters();
+  //wait to display images/pointcloud
+  while (!viewer.wasStopped ())
+  {
+    viewer.spinOnce (100);
+  }
+  cv::waitKey(0);
+  return 0;
+}

Vision/examples/pclvisualizer.cpp

@@ -0,0 +1,381 @@
+/*author Geoffrey Biggs */
+
+
+#include <iostream>
+
+#include <boost/thread/thread.hpp>
+#include <pcl/common/common_headers.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/console/parse.h>
+
+// --------------
+// -----Help-----
+// --------------
+void
+printUsage (const char* progName)
+{
+  std::cout << "\n\nUsage: "<<progName<<" [options]\n\n"
+            << "Options:\n"
+            << "-------------------------------------------\n"
+            << "-h           this help\n"
+            << "-s           Simple visualisation example\n"
+            << "-r           RGB colour visualisation example\n"
+            << "-c           Custom colour visualisation example\n"
+            << "-n           Normals visualisation example\n"
+            << "-a           Shapes visualisation example\n"
+            << "-v           Viewports example\n"
+            << "-i           Interaction Customization example\n"
+            << "\n\n";
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
+{
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+  viewer->addCoordinateSystem (1.0);
+  viewer->initCameraParameters ();
+  return (viewer);
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
+{
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+  viewer->addCoordinateSystem (1.0);
+  viewer->initCameraParameters ();
+  return (viewer);
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> customColourVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
+{
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
+  viewer->addPointCloud<pcl::PointXYZ> (cloud, single_color, "sample cloud");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+  viewer->addCoordinateSystem (1.0);
+  viewer->initCameraParameters ();
+  return (viewer);
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> normalsVis (
+    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::ConstPtr normals)
+{
+  // --------------------------------------------------------
+  // -----Open 3D viewer and add point cloud and normals-----
+  // --------------------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+  viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals, 10, 0.05, "normals");
+  viewer->addCoordinateSystem (1.0);
+  viewer->initCameraParameters ();
+  return (viewer);
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> shapesVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
+{
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+  viewer->addCoordinateSystem (1.0);
+  viewer->initCameraParameters ();
+
+  //------------------------------------
+  //-----Add shapes at cloud points-----
+  //------------------------------------
+  viewer->addLine<pcl::PointXYZRGB> (cloud->points[0],
+                                     cloud->points[cloud->size() - 1], "line");
+  viewer->addSphere (cloud->points[0], 0.2, 0.5, 0.5, 0.0, "sphere");
+
+  //---------------------------------------
+  //-----Add shapes at other locations-----
+  //---------------------------------------
+  pcl::ModelCoefficients coeffs;
+  coeffs.values.push_back (0.0);
+  coeffs.values.push_back (0.0);
+  coeffs.values.push_back (1.0);
+  coeffs.values.push_back (0.0);
+  viewer->addPlane (coeffs, "plane");
+  coeffs.values.clear ();
+  coeffs.values.push_back (0.3);
+  coeffs.values.push_back (0.3);
+  coeffs.values.push_back (0.0);
+  coeffs.values.push_back (0.0);
+  coeffs.values.push_back (1.0);
+  coeffs.values.push_back (0.0);
+  coeffs.values.push_back (5.0);
+  viewer->addCone (coeffs, "cone");
+
+  return (viewer);
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> viewportsVis (
+    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::ConstPtr normals1, pcl::PointCloud<pcl::Normal>::ConstPtr normals2)
+{
+  // --------------------------------------------------------
+  // -----Open 3D viewer and add point cloud and normals-----
+  // --------------------------------------------------------
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->initCameraParameters ();
+
+  int v1(0);
+  viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
+  viewer->setBackgroundColor (0, 0, 0, v1);
+  viewer->addText("Radius: 0.01", 10, 10, "v1 text", v1);
+  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud1", v1);
+
+  int v2(0);
+  viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
+  viewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
+  viewer->addText("Radius: 0.1", 10, 10, "v2 text", v2);
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(cloud, 0, 255, 0);
+  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, single_color, "sample cloud2", v2);
+
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud1");
+  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud2");
+  viewer->addCoordinateSystem (1.0);
+
+  viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals1, 10, 0.05, "normals1", v1);
+  viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals2, 10, 0.05, "normals2", v2);
+
+  return (viewer);
+}
+
+
+unsigned int text_id = 0;
+void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event,
+                            void* viewer_void)
+{
+  pcl::visualization::PCLVisualizer *viewer = static_cast<pcl::visualization::PCLVisualizer *> (viewer_void);
+  if (event.getKeySym () == "r" && event.keyDown ())
+  {
+    std::cout << "r was pressed => removing all text" << std::endl;
+
+    char str[512];
+    for (unsigned int i = 0; i < text_id; ++i)
+    {
+      sprintf (str, "text#%03d", i);
+      viewer->removeShape (str);
+    }
+    text_id = 0;
+  }
+}
+
+void mouseEventOccurred (const pcl::visualization::MouseEvent &event,
+                         void* viewer_void)
+{
+  pcl::visualization::PCLVisualizer *viewer = static_cast<pcl::visualization::PCLVisualizer *> (viewer_void);
+  if (event.getButton () == pcl::visualization::MouseEvent::LeftButton &&
+      event.getType () == pcl::visualization::MouseEvent::MouseButtonRelease)
+  {
+    std::cout << "Left mouse button released at position (" << event.getX () << ", " << event.getY () << ")" << std::endl;
+
+    char str[512];
+    sprintf (str, "text#%03d", text_id ++);
+    viewer->addText ("clicked here", event.getX (), event.getY (), str);
+  }
+}
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> interactionCustomizationVis ()
+{
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+  viewer->setBackgroundColor (0, 0, 0);
+  viewer->addCoordinateSystem (1.0);
+
+  viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)viewer.get ());
+  viewer->registerMouseCallback (mouseEventOccurred, (void*)viewer.get ());
+
+  return (viewer);
+}
+
+
+// --------------
+// -----Main-----
+// --------------
+int
+main (int argc, char** argv)
+{
+  // --------------------------------------
+  // -----Parse Command Line Arguments-----
+  // --------------------------------------
+  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
+  {
+    printUsage (argv[0]);
+    return 0;
+  }
+  bool simple(false), rgb(false), custom_c(false), normals(false),
+    shapes(false), viewports(false), interaction_customization(false);
+  if (pcl::console::find_argument (argc, argv, "-s") >= 0)
+  {
+    simple = true;
+    std::cout << "Simple visualisation example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-c") >= 0)
+  {
+    custom_c = true;
+    std::cout << "Custom colour visualisation example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-r") >= 0)
+  {
+    rgb = true;
+    std::cout << "RGB colour visualisation example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-n") >= 0)
+  {
+    normals = true;
+    std::cout << "Normals visualisation example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
+  {
+    shapes = true;
+    std::cout << "Shapes visualisation example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
+  {
+    viewports = true;
+    std::cout << "Viewports example\n";
+  }
+  else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
+  {
+    interaction_customization = true;
+    std::cout << "Interaction Customization example\n";
+  }
+  else
+  {
+    printUsage (argv[0]);
+    return 0;
+  }
+
+  // ------------------------------------
+  // -----Create example point cloud-----
+  // ------------------------------------
+  pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
+  std::cout << "Genarating example point clouds.\n\n";
+  // We're going to make an ellipse extruded along the z-axis. The colour for
+  // the XYZRGB cloud will gradually go from red to green to blue.
+  uint8_t r(255), g(15), b(15);
+  for (float z(-1.0); z <= 1.0; z += 0.05)
+  {
+    for (float angle(0.0); angle <= 360.0; angle += 5.0)
+    {
+      pcl::PointXYZ basic_point;
+      basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
+      basic_point.y = sinf (pcl::deg2rad(angle));
+      basic_point.z = z;
+      basic_cloud_ptr->points.push_back(basic_point);
+
+      pcl::PointXYZRGB point;
+      point.x = basic_point.x;
+      point.y = basic_point.y;
+      point.z = basic_point.z;
+      uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
+              static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
+      point.rgb = *reinterpret_cast<float*>(&rgb);
+      point_cloud_ptr->points.push_back (point);
+    }
+    if (z < 0.0)
+    {
+      r -= 12;
+      g += 12;
+    }
+    else
+    {
+      g -= 12;
+      b += 12;
+    }
+  }
+  basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size ();
+  basic_cloud_ptr->height = 1;
+  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
+  point_cloud_ptr->height = 1;
+
+  // ----------------------------------------------------------------
+  // -----Calculate surface normals with a search radius of 0.05-----
+  // ----------------------------------------------------------------
+  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
+  ne.setInputCloud (point_cloud_ptr);
+  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
+  ne.setSearchMethod (tree);
+  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
+  ne.setRadiusSearch (0.05);
+  ne.compute (*cloud_normals1);
+
+  // ---------------------------------------------------------------
+  // -----Calculate surface normals with a search radius of 0.1-----
+  // ---------------------------------------------------------------
+  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
+  ne.setRadiusSearch (0.1);
+  ne.compute (*cloud_normals2);
+
+  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+  if (simple)
+  {
+    viewer = simpleVis(basic_cloud_ptr);
+  }
+  else if (rgb)
+  {
+    viewer = rgbVis(point_cloud_ptr);
+  }
+  else if (custom_c)
+  {
+    viewer = customColourVis(basic_cloud_ptr);
+  }
+  else if (normals)
+  {
+    viewer = normalsVis(point_cloud_ptr, cloud_normals2);
+  }
+  else if (shapes)
+  {
+    viewer = shapesVis(point_cloud_ptr);
+  }
+  else if (viewports)
+  {
+    viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
+  }
+  else if (interaction_customization)
+  {
+    viewer = interactionCustomizationVis();
+  }
+
+  //--------------------
+  // -----Main loop-----
+  //--------------------
+  while (!viewer->wasStopped ())
+  {
+    viewer->spinOnce (100);
+    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+  }
+}
+

Vision/examples/readme.md

@@ -0,0 +1,7 @@
+Vision examples
+
+Cloud Viewer - Load a PCD file and show in the cloud viewer.
+
+Live Viewer - Show pointcloud and 2D images from live camera feed once.
+
+Display Image - Simple script to show OpenCV image.

Vision/examples/region_growing_segmentation.cpp

@@ -0,0 +1,107 @@
+#include <iostream>
+#include <vector>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/search/search.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/region_growing.h>
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+
+using namespace std;
+
+int
+main (int argc, char** argv)
+{
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =
+    std::make_shared<o3d3xx::FrameGrabber>(
+      cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  //get frame from camera
+  if (! fg->WaitForFrame(img.get(), 2000))
+    {
+      std::cerr << "Timeout waiting for camera!" << std::endl;
+      return -1;	
+    }
+  pcl::PCDWriter writer;
+  
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZI>);
+  cloudIn = img->Cloud();
+  writer.write<pcl::PointXYZI>("region_growing_tutorial.pcd", *cloudIn, false);
+ 
+  if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("region_growing_tutorial.pcd", *cloud) == -1)
+  {
+    std::cout << "Cloud reading failed." << std::endl;
+    return (-1);
+  }
+  vector<int> temp;
+  pcl::removeNaNFromPointCloud(*cloud,*cloud, temp);
+  cout << temp.size() << endl;
+  cout << cloud->size() << endl;
+
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
+  pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setSearchMethod (tree);
+  normal_estimator.setInputCloud (cloud);
+  normal_estimator.setKSearch (50);
+  normal_estimator.compute (*normals);
+  
+  pcl::IndicesPtr indices (new std::vector <int>);
+  pcl::PassThrough<pcl::PointXYZ> pass;
+  pass.setInputCloud (cloud);
+  pass.setFilterFieldName ("z");
+  pass.setFilterLimits (0.0, 1.0);
+  pass.filter (*indices);
+
+  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+  reg.setMinClusterSize (50);
+  reg.setMaxClusterSize (1000000);
+  reg.setSearchMethod (tree);
+  reg.setNumberOfNeighbours (30);
+  reg.setInputCloud (cloud);
+  //reg.setIndices (indices);
+  reg.setInputNormals (normals);
+  reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);// graden naar radial
+  reg.setCurvatureThreshold (1.0);
+
+  std::vector <pcl::PointIndices> clusters;
+  reg.extract (clusters);
+
+  std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
+  std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
+  std::cout << "These are the indices of the points of the initial" <<
+    std::endl << "cloud that belong to the first cluster:" << std::endl;
+  int counter = 0;
+  while (counter < clusters[0].indices.size ())
+  {
+    std::cout << clusters[0].indices[counter] << ", ";
+    counter++;
+    if (counter % 10 == 0)
+      std::cout << std::endl;
+  }
+  std::cout << std::endl;
+
+  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
+  pcl::visualization::PCLVisualizer viewer ("Cluster viewer");
+  viewer.addPointCloud<pcl::PointXYZRGB>(colored_cloud,"cloud");
+  //viewer.addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(colored_cloud, normals, 10, 0.05, "normals",0);
+  while (!viewer.wasStopped ())
+  {
+      viewer.spinOnce(100);
+  }
+
+  return (0);
+}

Vision/examples/segmentation.cpp

@@ -0,0 +1,127 @@
+#include <iostream>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/extract_indices.h>
+//for accessing camera
+#include "o3d3xx_camera.h"
+#include "o3d3xx_framegrabber.h"
+#include "o3d3xx_image.h"
+
+pcl::PointCloud<pcl::PointXYZ> convertCloud(pcl::PointCloud<o3d3xx::PointT>::Ptr inputCloud){
+  pcl::PointCloud<o3d3xx::PointT>::Ptr cloud = inputCloud;
+  pcl::PointCloud<pcl::PointXYZ> newcloud;
+  
+  newcloud.width = cloud->width;
+  newcloud.height = cloud->height;
+  newcloud.points.resize(cloud->points.size());
+
+  for(std::size_t i = 0; i < cloud->points.size(); i++) {
+    newcloud.points[i].x = cloud->points[i].x;
+    newcloud.points[i].y = cloud->points[i].y;
+    newcloud.points[i].z = cloud->points[i].z;
+  }
+
+  return newcloud;
+}
+
+int
+main (int argc, char** argv)
+{
+  //logging method
+  o3d3xx::Logging::Init();
+  //initialise camera constructor expects IP address, using default one
+  o3d3xx::Camera::Ptr cam = std::make_shared<o3d3xx::Camera>("192.168.1.69");
+  //create buffer to fetch image
+  o3d3xx::ImageBuffer::Ptr img = std::make_shared<o3d3xx::ImageBuffer>();
+  //framegrabber
+  o3d3xx::FrameGrabber::Ptr fg =
+    std::make_shared<o3d3xx::FrameGrabber>(
+      cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
+
+  //get frame from camera (could be looped to create an actual live feed)
+  if (! fg->WaitForFrame(img.get(), 2000))
+  {
+    std::cerr << "Timeout waiting for camera!" << std::endl;
+    return -1;	
+  }
+  pcl::PointCloud<pcl::PointXYZ> cloud(convertCloud(img->Cloud()));
+  pcl::io::savePCDFileASCII("temp.pcd", cloud);
+  std::cout << "Saved PCD" << std::endl;
+ 
+  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new    pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
+
+  // Fill in the cloud data
+  pcl::PCDReader reader;
+  reader.read ("temp.pcd", *cloud_blob);
+
+  std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
+
+  // Create the filtering object: downsample the dataset using a leaf size of 1cm
+  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+  sor.setInputCloud (cloud_blob);
+  sor.setLeafSize (0.01f, 0.01f, 0.01f);
+  sor.filter (*cloud_filtered_blob);
+
+  // Convert to the templated PointCloud
+  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
+
+  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
+
+  // Write the downsampled version to disk
+  pcl::PCDWriter writer;
+  writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
+
+  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
+  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
+  // Create the segmentation object
+  pcl::SACSegmentation<pcl::PointXYZ> seg;
+  // Optional
+  seg.setOptimizeCoefficients (true);
+  // Mandatory
+  seg.setModelType (pcl::SACMODEL_PLANE);
+  seg.setMethodType (pcl::SAC_RANSAC);
+  seg.setMaxIterations (1000);
+  seg.setDistanceThreshold (0.01);
+
+  // Create the filtering object
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+
+  int i = 0, nr_points = (int) cloud_filtered->points.size ();
+  // While 30% of the original cloud is still there
+  while (cloud_filtered->points.size () > 0.3 * nr_points)
+  {
+    // Segment the largest planar component from the remaining cloud
+    seg.setInputCloud (cloud_filtered);
+    seg.segment (*inliers, *coefficients);
+    if (inliers->indices.size () == 0)
+    {
+      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
+      break;
+    }
+
+    // Extract the inliers
+    extract.setInputCloud (cloud_filtered);
+    extract.setIndices (inliers);
+    extract.setNegative (false);
+    extract.filter (*cloud_p);
+    std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
+
+    std::stringstream ss;
+    ss << "table_scene_lms400_plane_" << i << ".pcd";
+    writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
+
+    // Create the filtering object
+    extract.setNegative (true);
+    extract.filter (*cloud_f);
+    cloud_filtered.swap (cloud_f);
+    i++;
+  }
+
+  return (0);
+}

Vision/main.cpp

@@ -0,0 +1,15 @@
+#include "AerobicVision.h"
+
+//TODO: TWO DIMENSIONAL FILTER TO FILTER OUTSIDE OF BIN
+//TODO: Z-AXIS FILTER TO FILTER FROM 1 CM ABOVE BOTTOM AND HIGHER
+//TODO: CHECK EUCLIDEAN CLUSTER EXTRACTION
+
+
+int main()
+{
+  AerobicVision* test = new AerobicVision("192.168.1.69");
+  test->RegionGrowing();
+  //pcl::PointXYZ data = test->Calibrate(100);
+  //cout << "Calibration Result (XYZ): "<< data.x << " , " << data.y << " , " << data.z << endl;
+  return 0;
+}

binpicker.cpp

@@ -0,0 +1,89 @@
+#include "AerobicVision.h"
+#include "UR_Control.h"
+#include <iostream>
+#include <stdio.h>
+#include <string>
+
+int main()
+{	
+	std::cout << "Version 1.0 team Awesome" << endl;
+
+	unsigned int usecs = 1000000; //1sec = 1.000.000 usec
+	//UR3* test = new UR3();
+	Binzone* zone = new Binzone();
+	Control* control = new Control(0.0001,3.0, zone);
+	AerobicVision* vision = new AerobicVision("192.168.1.69");
+
+	std::cout << "Starting in 1 seconds" << endl;
+	usleep(1000000);
+
+	cout << "Program starting" << endl;
+	
+	//control->printRobotLocation();
+
+	if(!control->readCalibration()){
+		cout << "No calibration file found, starting calibration.." << endl;
+		control->moveToCalibratePosition();
+		pcl::PointXYZ calibrationPoint = vision->Calibrate(100);
+		control->Move_Calibrate(calibrationPoint);
+	}
+	//control->openCloseRoboticGripper(false);
+	//control->openCloseRoboticGripper(true);
+
+	pcl::visualization::CloudViewer viewer("Filtered cloud");
+
+	bool run = true;
+	while(run)
+	{
+		double rotation =0;
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr showcloud;
+
+		pcl::PointXYZ p = vision->runAlgorithm(rotation, showcloud);
+		viewer.showCloud(showcloud);
+		if(p.x != 1000 && p.y != 1000 && p.z != 1000)
+		{
+			vector<double> pos = control->getRobotLocation();
+			pcl::PointXYZ rotations = control->getRxRyRz(3.1415926535897932384626433832795, 0, rotation);
+			control->Touch_Object(p.z, p.y, p.x ); //, rotations.x, rotations.y, rotations.z);
+
+			cout << "Sleeping 1 seconds" << endl;
+			usleep(1000000);
+			cout << "Running again.. yippie!" << endl;
+		}
+		else
+		{
+			run = false;
+		}
+		
+
+		
+	}
+
+	std::cout << "Done!" << endl;
+	/*
+	
+	bool run = true;
+	int objects = 0;
+	while(run){
+		std::vector<objectData> data = vision_->RegionGrowing();
+		objects = data.size();
+		cout << "Found objects: " << objects << endl;
+		if(objects == 0){ run = false;}
+		else{
+			//take highest (first) object and make new picture
+			run = control_->Touch_Object(data[0].xyz.z, data[0].xyz.y, data[0].xyz.x,
+			data[0].rx,data[0].ry,data[0].rz); 
+		}
+	}
+
+	cout << "Bin Empty, Finished Bin Picking" << endl;
+    	return 0;
+	
+	*/
+
+	return 0;
+}
+
+
+