diff --git a/ReadMe.md b/ReadMe.md
index 6b2012d38cfdb5bc4df3146edf505fa8af0b84aa..bbd3b8913b8b3da79ebdc305cc1943d89a4ee35d 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -7,16 +7,14 @@ Darwin localization and navigation using QR codes {#mainp
This package requires of the following system libraries and packages
-* [cmake](https://www.cmake.org "CMake's Homepage"), a cross-platform build system.
-* [doxygen](http://www.doxygen.org "Doxygen's Homepage") and [graphviz](http://www.graphviz.org "Graphviz's Homepage") to generate the documentation.
-* stdc++ and pthread libraries.
-
-Under linux all of these utilities are available in ready-to-use packages.
-
-Under MacOS most of the packages are available via [fink](http://www.finkproject.org/ "Fink's Homepage")
+* [socat](http://www.dest-unreach.org/socat/ "SOCAT Homepage"), a command line based utility that establishes two bidirectional byte streams and transfers data between them. Use the apt-get utility to install it.
+* [v4l2loopback](https://github.com/umlaeute/v4l2loopback "V4L2 loopback utility"), system module to create virtual video devices. Follow the instructions on the README file
+for information on how to build and use it.
This package also requires of the following IRI libraries:
+*
+
## Compilation and installation
Download this repository and create a build folder inside:
@@ -39,39 +37,12 @@ The release mode will be kept until next time cmake is executed.
In case no errors are reported, the generated libraries (if any) will be located at the
_lib_ folder and the executables (if any) will be located at the _bin_ folder.
-In order to be able to use the library, it it necessary to copy it into the system.
-To do that, execute
-
-``` make install ```
-
-as root and the shared libraries will be copied to */usr/local/lib/iridrivers* directory
-and the header files will be copied to */usr/local/include/iridrivers* dierctory. At
-this point, the library may be used by any user.
-
-To remove the library from the system, exceute
-
-``` make uninstall ```
-
-as root, and all the associated files will be removed from the system.
-
To generate the documentation execute the following command:
``` make doc ```
## How to use it
-To use this library in an other library or application, in the CMakeLists.txt file, first it is necessary to locate if the library has been installed or not using the following command
-
-``` FIND_PACKAGE(Darwin localization and navigation using QR codes) ```
-
-In the case that the package is present, it is necessary to add the header files directory to the include directory path by using
-
-``` INCLUDE_DIRECTORIES(${Darwin localization and navigation using QR codes_INCLUDE_DIR}) ```
-
-and it is also necessary to link with the desired libraries by using the following command
-
-``` TARGET_LINK_LIBRARIES(<executable name> ${Darwin localization and navigation using QR codes_LIBRARY}) ```
-
## Disclaimer
Copyright (C) 2009-2018 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index debb1da6f8e57535879e870bf9437ff00d5d2aeb..77eff4cf6000856cff18625d57945195a958dfe0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,6 +1,22 @@
# locate the necessary dependencies
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+FIND_PACKAGE(dynamixel REQUIRED)
+FIND_PACKAGE(darwin_robot REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(detectqrcode REQUIRED)
+
+ADD_DEFINITIONS(-D_HAVE_XSD)
+ADD_DEFINITIONS(-D_SIM)
+
# add the necessary include directories
INCLUDE_DIRECTORIES(../include)
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${darwin_robot_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${detectqrcode_INCLUDE_DIR})
# application source files
SET(sources darwin_loc_nav.cpp)
# application header files
@@ -8,3 +24,9 @@ SET(headers ../include/darwin_loc_nav.h)
# create the executable file
ADD_EXECUTABLE(darwin_loc_nav ${sources})
# link necessary libraries
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${dynamixel_LIBRARY})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${darwin_robot_LIBRARY})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${OpenCV_LIBRARIES})
+TARGET_LINK_LIBRARIES(darwin_loc_nav ${detectqrcode_LIBRARY})
diff --git a/src/darwin_loc_nav.cpp b/src/darwin_loc_nav.cpp
index 8f611b2f67a4cfcca3a3c3facb01f29f493c8533..3f126c5450dfcab99d30ca5fa21b1d52687a8202 100755
--- a/src/darwin_loc_nav.cpp
+++ b/src/darwin_loc_nav.cpp
@@ -1,5 +1,90 @@
-int main(int argc,char *argv[])
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+#include "action_id.h"
+#include "detectqrcode.h"
+
+#include <iostream>
+
+#ifdef _SIM
+std::string robot_device="/dev/pts/20";
+#else
+std::string robot_device="A603LOBS";
+#endif
+
+int main(int argc, char *argv[])
{
+ cv::Mat frame;
+ cv::Mat gray;
+ int i=0,num_servos;
+ unsigned int present_servos;
+ std::vector<std::vector<cv::Point> > squares;
+
+ try{
+ #ifdef _SIM
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
+ #else
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,false);
+ #endif
+
+ CDetectqrcode detector;
+
+ /* initialize QR detector */
+ detector.init(721.977446f,717.128980f,334.800583f,242.691968f,0.0475f,0.0475f);
+
+ cv::VideoCapture cap(0); // open the default camera
+ if(!cap.isOpened()) // check if we succeeded
+ {
+ std::cout << "Error: Device is not open" << std::endl;
+ return 0;
+ }
+
+ cv::namedWindow("QRcode",1);
+ /* initialize robot */
+ num_servos=darwin.mm_get_num_servos();
+ present_servos=darwin.mm_get_present_servos();
+ std::cout << "Found " << num_servos << " servos " << std::endl;
+ std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
+ // enable all servos and assign them to the action module
+ darwin.mm_enable_power();
+ sleep(1);
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ {
+ darwin.mm_enable_servo(i);
+ darwin.mm_assign_module(i,DARWIN_MM_ACTION);
+ }
+ darwin.mm_start();
+ // execute an action
+ darwin.action_load_page(WALK_READY);
+ darwin.action_start();
+ while(darwin.action_is_page_running())
+ {
+ std::cout << "action running ... " << std::endl;
+ usleep(100000);
+ }
+ while(1)
+ {
+ cap.read(frame);
+ std::cout << frame.cols << "," << frame.rows << "," << frame.size << "," << frame.step << std::endl;
+ std::vector<TQRInfo> tags;
+ cv::cvtColor(frame,gray,CV_BGR2GRAY);
+ detector.findSquares(gray, squares);
+ detector.findQR(gray,tags);
+ for(unsigned int i=0;i<tags.size();i++)
+ {
+ std::cout << "Tag ID: " << tags[i].tag_id << std::endl;
+ std::cout << "Tag position: X:" << tags[i].tvec[0] << " Y: " << tags[i].tvec[1] << " Z: " << tags[i].tvec[2] << std::endl;
+ }
+ detector.drawSquares(frame, squares);
+ cv::imshow("QRcode",frame);
+ if(cv::waitKey(30)>=0) break;
+ }
+
+ // disable robot
+ darwin.mm_stop();
+ darwin.mm_disable_power();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
}