[Jderobot-admin] jderobot-r1046 - in trunk: . Deps Deps/kobuki src/stable/components src/stable/components/kobuki_driver src/stable/components/kobuki_driver/actuators src/stable/components/kobuki_driver/sensors

ahcorde en jderobot.org ahcorde en jderobot.org
Lun Oct 14 14:17:10 CEST 2013


Author: ahcorde
Date: 2013-10-14 14:17:10 +0200 (Mon, 14 Oct 2013)
New Revision: 1046

Added:
   trunk/Deps/kobuki/
   trunk/Deps/kobuki/CMakeLists.txt
   trunk/src/stable/components/kobuki_driver/
   trunk/src/stable/components/kobuki_driver/CMakeLists.txt
   trunk/src/stable/components/kobuki_driver/Robot/
   trunk/src/stable/components/kobuki_driver/actuators/
   trunk/src/stable/components/kobuki_driver/actuators/motors.cpp
   trunk/src/stable/components/kobuki_driver/actuators/motors.h
   trunk/src/stable/components/kobuki_driver/config.cfg
   trunk/src/stable/components/kobuki_driver/kobuki_driver.pro
   trunk/src/stable/components/kobuki_driver/kobukimanager.cpp
   trunk/src/stable/components/kobuki_driver/kobukimanager.h
   trunk/src/stable/components/kobuki_driver/main.cpp
   trunk/src/stable/components/kobuki_driver/sensors/
   trunk/src/stable/components/kobuki_driver/sensors/encoders.cpp
   trunk/src/stable/components/kobuki_driver/sensors/encoders.h
   trunk/src/stable/components/kobuki_driver/thread_control.cpp
   trunk/src/stable/components/kobuki_driver/thread_control.h
Modified:
   trunk/CMakeLists.txt
Log:
#70 kobuki driver integrate in JDErobot compilation


Modified: trunk/CMakeLists.txt
===================================================================
--- trunk/CMakeLists.txt	2013-10-13 10:00:59 UTC (rev 1045)
+++ trunk/CMakeLists.txt	2013-10-14 12:17:10 UTC (rev 1046)
@@ -90,7 +90,7 @@
 include(${DEPS_DIR}/gtk2/CMakeLists.txt)
 include(${DEPS_DIR}/qt/CMakeLists.txt)
 include(${DEPS_DIR}/gtk3/CMakeLists.txt)
-
+include(${DEPS_DIR}/kobuki/CMakeLists.txt)
 #include(${CMAKE_CURRENT_SOURCE_DIR}/../../../libs/CMakeLists.txt)
 
 

Added: trunk/Deps/kobuki/CMakeLists.txt
===================================================================
--- trunk/Deps/kobuki/CMakeLists.txt	                        (rev 0)
+++ trunk/Deps/kobuki/CMakeLists.txt	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,45 @@
+FIND_PATH( kobuki_INCLUDE_DIR NAMES kobuki_driver/kobuki.hpp  PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/include)
+
+MESSAGE(" kobuki_LIBRARIES ${kobuki_LIBRARIES}")
+#MESSAGE(" kobuki_INCLUDE_DIR ${kobuki_INCLUDE_DIR}")
+
+IF (kobuki_INCLUDE_DIR)
+
+	MESSAGE("kobuki was found in: ${kobuki_INCLUDE_DIR}")
+    FIND_LIBRARY( kobuki_LIBRARY1 NAMES kobuki PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY2 NAMES kobuki_dock_drive PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    
+    FIND_LIBRARY( kobuki_LIBRARY3 NAMES ecl_devices PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY4 NAMES ecl_errors PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY5 NAMES ecl_exceptions PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY6 NAMES ecl_formatters PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY7 NAMES ecl_geometry PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    
+    FIND_LIBRARY( kobuki_LIBRARY8 NAMES ecl_io PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY9 NAMES ecl_ipc PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY10 NAMES ecl_mobile_robot PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY11 NAMES ecl_statistics PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY12 NAMES ecl_streams PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY13 NAMES ecl_threads PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY14 NAMES ecl_time PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    
+    FIND_LIBRARY( kobuki_LIBRARY15 NAMES ecl_time_lite PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+    FIND_LIBRARY( kobuki_LIBRARY16 NAMES ecl_type_traits PATHS ENV C++LIB ENV PATH PATH_SUFFIXES kobuki/lib)
+  #MESSAGE("PATH_SUFFIXES: ${PATH_SUFFIXES}")
+    
+    SET (kobuki_LIBRARIES ${kobuki_LIBRARY1} ${kobuki_LIBRARY2} ${kobuki_LIBRARY3} ${kobuki_LIBRARY4} ${kobuki_LIBRARY5} ${kobuki_LIBRARY6}
+                          ${kobuki_LIBRARY7} ${kobuki_LIBRARY8} ${kobuki_LIBRARY9} ${kobuki_LIBRARY10} ${kobuki_LIBRARY11} ${kobuki_LIBRARY12}
+                          ${kobuki_LIBRARY13} ${kobuki_LIBRARY14} ${kobuki_LIBRARY15} ${kobuki_LIBRARY16})
+    
+    IF( OpenCV_LIBRARIES )
+		MESSAGE("kobuki LIBRARY FOUND IN ${kobuki_LIBRARIES}")
+		SET(kobuki_COMPILE TRUE)
+    else()
+		MESSAGE("kobuki LIBRARY NOT FOUND IN ${kobuki_LIBRARIES}")
+		SET(kobuki_COMPILE FALSE)
+    ENDIF(  )
+
+else() 
+	MESSAGE("*** kobuki not found")
+	SET(kobuki_COMPILE FALSE)
+ENDIF()

Added: trunk/src/stable/components/kobuki_driver/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/kobuki_driver/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/CMakeLists.txt	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,25 @@
+if (${kobuki_COMPILE})
+
+
+    include_directories(
+        ${INTERFACES_CPP_DIR}
+        ${LIBS_DIR}/
+        ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+
+    add_executable( kobuki_driver  main.cpp kobukimanager.cpp kobukimanager.h thread_control.cpp thread_control.h
+                                   actuators/motors.cpp actuators/motors.h
+                                   sensors/encoders.h sensors/encoders.cpp
+    )
+			       
+    link_directories(${kobuki_LIBRARIES})
+    include_directories(${kobuki_INCLUDE_DIR})
+			       
+    target_link_libraries(kobuki_driver
+        ${CMAKE_THREAD_LIBS_INIT}
+        ${kobuki_LIBRARIES}
+        JderobotInterfaces
+        jderobotutil
+        ${ZeroCIce_LIBRARIES}
+    )
+ENDIF()

Added: trunk/src/stable/components/kobuki_driver/actuators/motors.cpp
===================================================================
--- trunk/src/stable/components/kobuki_driver/actuators/motors.cpp	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/actuators/motors.cpp	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,42 @@
+#include "motors.h"
+
+Motors::Motors(KobukiManager *kobuki_manager)
+{
+    this->kobuki_manager = kobuki_manager;
+}
+
+Motors::~Motors()
+{
+
+}
+
+float Motors::getV(const Ice::Current&)
+{
+
+}
+
+float Motors::getW(const Ice::Current&)
+{
+
+}
+
+float Motors::getL(const Ice::Current&)
+{
+    return 0.;
+}
+
+Ice::Int Motors::setV(Ice::Float v, const Ice::Current&)
+{
+    kobuki_manager->setV(v);
+}
+
+Ice::Int Motors::setW(Ice::Float _w, const Ice::Current&)
+{
+    kobuki_manager->setW(_w);
+}
+
+Ice::Int Motors::setL(Ice::Float l, const Ice::Current&)
+{
+    l = 0;
+    return 0;
+}

Added: trunk/src/stable/components/kobuki_driver/actuators/motors.h
===================================================================
--- trunk/src/stable/components/kobuki_driver/actuators/motors.h	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/actuators/motors.h	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,67 @@
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include <jderobot/motors.h>
+
+#include "../kobukimanager.h"
+#include <iostream>
+
+class Motors : virtual public jderobot::Motors {
+ public:
+
+    /** Construtor
+      * @brief MotorsI
+      * @param propertyPrefix name of the server
+      * @param sharer Pointer to share memory
+      */
+     Motors(KobukiManager* kobuki_manager);
+
+     /** Destructor
+      * @brief ~MotorsI Destructor
+      */
+     virtual ~Motors();
+
+     /**
+      * @brief getV
+      * @return Return the velocity of the robot
+      */
+     virtual float getV(const Ice::Current&);
+
+     /**
+      * @brief getW
+      * @return return the angle of the wheel
+      */
+     virtual float getW(const Ice::Current&);
+
+     /**
+      * @brief getL
+      * @return Not used
+      */
+     virtual float getL(const Ice::Current&);
+
+     /**
+      * @brief setV
+      * @param v velocity in m/s
+      * @return return the velocity
+      */
+     virtual Ice::Int setV(Ice::Float v, const Ice::Current&);
+
+     /**
+      * @brief setW
+      * @param _w angle of the wheel in radians
+      * @return return the angle of the robot
+      */
+     virtual Ice::Int setW(Ice::Float _w, const Ice::Current&) ;
+
+     /**
+      * @brief setL
+      * @param l Not used
+      * @return Not used
+      */
+     virtual Ice::Int setL(Ice::Float l, const Ice::Current&);
+
+private:
+     KobukiManager* kobuki_manager;
+ };
+
+#endif // MOTORS_H

Added: trunk/src/stable/components/kobuki_driver/config.cfg
===================================================================
--- trunk/src/stable/components/kobuki_driver/config.cfg	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/config.cfg	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,2 @@
+kobuki.Motors.Endpoints=default -h 0.0.0.0 -p 9999
+kobuki.Encoders.Endpoints=default -h 0.0.0.0 -p 9997

Added: trunk/src/stable/components/kobuki_driver/kobuki_driver.pro
===================================================================
--- trunk/src/stable/components/kobuki_driver/kobuki_driver.pro	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/kobuki_driver.pro	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,35 @@
+SOURCES += \
+    main.cpp \
+    kobukimanager.cpp \
+    sensors/encoders.cpp \
+    actuators/motors.cpp \
+    thread_control.cpp
+HEADERS += \
+    kobukimanager.h \
+    sensors/encoders.h \
+    actuators/motors.h \
+    thread_control.h
+
+INCLUDEPATH+=/usr/local/kobuki/include/ \
+    /usr/include/eigen3/
+
+LIBS+=-L/usr/local/kobuki/install/lib \
+    -lecl_devices     -lecl_ipc           -lecl_time_lite \
+    -lecl_errors      -lecl_mobile_robot  -lecl_type_traits \
+    -lecl_exceptions  -lecl_statistics    -lkobuki \
+    -lecl_formatters  -lecl_streams       -lkobuki_dock_drive\
+    -lecl_geometry    -lecl_threads \
+    -lecl_io          -lecl_time
+
+#jderobot
+INCLUDEPATH +=  /usr/local/include/jderobot
+LIBS += -L/usr/local/lib/jderobot \
+        -lJderobotInterfaces
+
+#ICE
+LIBS += -L/usr/lib \
+    -lIce -lIceUtil
+
+OTHER_FILES += \
+    config.cfg
+

Added: trunk/src/stable/components/kobuki_driver/kobukimanager.cpp
===================================================================
--- trunk/src/stable/components/kobuki_driver/kobukimanager.cpp	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/kobukimanager.cpp	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,114 @@
+#include "kobukimanager.h"
+
+KobukiManager::KobukiManager() :
+    dx(0.0), dth(0.0),
+    slot_stream_data(&KobukiManager::update, *this)
+{
+    kobuki::Parameters parameters;
+    parameters.sigslots_namespace = "/kobuki";
+    parameters.device_port = "/dev/ttyUSB0";
+    parameters.enable_acceleration_limiter = false;
+    kobuki.init(parameters);
+    kobuki.enable();
+    slot_stream_data.connect("/kobuki/stream_data");
+
+    v = 0;
+    w = 0;
+
+}
+
+KobukiManager::~KobukiManager()
+{
+  kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
+  kobuki.disable();
+}
+
+void KobukiManager::setV(float v)
+{
+    mutex.lock();
+    std::cout << "recieved v: " << v  << std::endl;
+    this->v = v;
+    mutex.unlock();
+}
+
+double KobukiManager::getRobotX()
+{
+    double result;
+    mutex.lock();
+    result = pose.x();
+    mutex.unlock();
+    return result;
+}
+
+double KobukiManager::getRobotY()
+{
+    double result;
+    mutex.lock();
+    result = pose.y();
+    mutex.unlock();
+    return result;
+}
+
+double KobukiManager::getRobotTheta()
+{
+    double result;
+    mutex.lock();
+    result = pose.heading();
+    mutex.unlock();
+    return result;
+}
+
+void KobukiManager::setW(float w)
+{
+    mutex.lock();
+    std::cout << "recieved w: " << w  << std::endl;
+    this->w = w;
+    mutex.unlock();
+}
+
+void KobukiManager::processMotion()
+{
+    mutex.lock();
+    kobuki.setBaseControl(v, w);
+    mutex.unlock();
+}
+
+
+void KobukiManager::update()
+{
+    mutex.lock();
+    ecl::Pose2D<double> pose_update;
+    ecl::linear_algebra::Vector3d pose_update_rates;
+    kobuki.updateOdometry(pose_update, pose_update_rates);
+    pose *= pose_update;
+    dx += pose_update.x();
+    dth += pose_update.heading();
+    mutex.unlock();
+    processMotion();
+    kobuki::Battery batery = kobuki.batteryStatus();
+    kobuki::CoreSensors::Data d = kobuki.getCoreSensorData();
+
+    kobuki::DockIR::Data ir = kobuki.getDockIRData();
+
+    std::cout << "ir: " << ir.docking.size()<< std::endl;
+
+    for(int  i =0; i < ir.docking.size(); i++){
+        std::cout << (unsigned char)ir.docking[i] << ", ";
+    }
+
+    printf("--[%03d | %03d | %03d]\n", ir.docking[2], ir.docking[1], ir.docking[0] );
+    std::cout << std::endl;
+
+    std::cout << "Bumper: " << (int)d.bumper << std::endl;
+    std::cout << "Bateria: " << batery.percent() << " " << batery.capacity << std::endl;
+}
+
+ecl::Pose2D<double> KobukiManager::getPose()
+{
+    ecl::Pose2D<double> pose_result;
+    mutex.lock();
+    pose_result = pose;
+    mutex.unlock();
+    return pose_result;
+
+}

Added: trunk/src/stable/components/kobuki_driver/kobukimanager.h
===================================================================
--- trunk/src/stable/components/kobuki_driver/kobukimanager.h	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/kobukimanager.h	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,52 @@
+#ifndef KOBUKIMANAGER_H
+#define KOBUKIMANAGER_H
+/*****************************************************************************
+ Includes
+ ****************************************************************************/
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <csignal>
+#include <ecl/time.hpp>
+#include <ecl/sigslots.hpp>
+#include <ecl/geometry/pose2d.hpp>
+#include <ecl/linear_algebra.hpp>
+#include "kobuki_driver/kobuki.hpp"
+
+//boost
+#include <boost/signals2/mutex.hpp>
+
+/*****************************************************************************
+ Classes
+*****************************************************************************/
+
+class KobukiManager {
+public:
+  KobukiManager();
+
+  ~KobukiManager();
+
+  void update();
+
+  void setV(float v);
+  void setW(float w);
+
+  double getRobotX();
+  double getRobotY();
+  double getRobotTheta();
+
+  void processMotion();
+
+  ecl::Pose2D<double> getPose();
+
+private:
+    double dx, dth;
+    ecl::Pose2D<double> pose;
+    kobuki::Kobuki kobuki;
+    ecl::Slot<> slot_stream_data;
+    private: boost::signals2::mutex mutex; ///< Mutex for thread-safe access to internal data.
+
+    float v;
+    float w;
+
+};
+#endif // KOBUKIMANAGER_H

Added: trunk/src/stable/components/kobuki_driver/main.cpp
===================================================================
--- trunk/src/stable/components/kobuki_driver/main.cpp	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/main.cpp	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,59 @@
+/*****************************************************************************
+ Includes
+ ****************************************************************************/
+#include "thread_control.h"
+
+/*****************************************************************************
+ Signal Handler
+*****************************************************************************/
+
+bool shutdown_req = false;
+void signalHandler(int signum) {
+  shutdown_req = true;
+}
+
+/*****************************************************************************
+ Main
+*****************************************************************************/
+
+
+int main(int argc, char** argv)
+{
+
+    KobukiManager* kobuki_manager = new KobukiManager();
+
+    Ice::CommunicatorPtr ic;
+
+    try {
+        //-----------------ICE----------------//
+        ic = Ice::initialize(argc, argv);
+
+        Thread_control* thread = new Thread_control(ic, kobuki_manager);
+        thread->run();
+
+    } catch (const Ice::Exception& ex) {
+        std::cerr << ex << std::endl;
+        exit(-1);
+    } catch (const char* msg) {
+        std::cerr << msg << std::endl;
+        exit(-1);
+    }
+
+//  signal(SIGINT, signalHandler);
+
+//  std::cout << "Demo : Example of simple control loop." << std::endl;
+//  KobukiManager kobuki_manager;
+
+//  ecl::Sleep sleep(1);
+//  ecl::Pose2D<double> pose;
+//  try {
+//    while (!shutdown_req){
+//      sleep();
+//      pose = kobuki_manager.getPose();
+//      std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
+//    }
+//  } catch ( ecl::StandardException &e ) {
+//    std::cout << e.what();
+//  }
+//  return 0;
+}


Property changes on: trunk/src/stable/components/kobuki_driver/main.cpp
___________________________________________________________________
Added: svn:executable
   + *

Added: trunk/src/stable/components/kobuki_driver/sensors/encoders.cpp
===================================================================
--- trunk/src/stable/components/kobuki_driver/sensors/encoders.cpp	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/sensors/encoders.cpp	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,26 @@
+#include "encoders.h"
+
+Encoders::Encoders(KobukiManager *kobuki)
+{
+    this->kobuki = kobuki;
+}
+
+void Encoders::setEncodersData(const jderobot::EncodersDataPtr&  encodersData,
+                             const Ice::Current&)
+{
+
+}
+
+
+jderobot::EncodersDataPtr Encoders::getEncodersData(const Ice::Current&)
+{
+
+    mutex.lock();
+    jderobot::EncodersDataPtr encodersData(new jderobot::EncodersData());
+    encodersData->robotx = kobuki->getRobotX();
+    encodersData->roboty = kobuki->getRobotY();
+    encodersData->robottheta = kobuki->getRobotTheta();
+    mutex.unlock();
+    return encodersData;
+}
+

Added: trunk/src/stable/components/kobuki_driver/sensors/encoders.h
===================================================================
--- trunk/src/stable/components/kobuki_driver/sensors/encoders.h	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/sensors/encoders.h	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,34 @@
+#ifndef ENCODERS_H
+#define ENCODERS_H
+
+//boost
+#include <boost/signals2/mutex.hpp>
+
+//Encoders
+#include <jderobot/encoders.h>
+
+//ICE
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include "../kobukimanager.h"
+
+class Encoders: virtual public jderobot::Encoders
+{
+public:
+    Encoders(KobukiManager* kobuki);
+
+    virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&);
+
+    virtual void setEncodersData(const jderobot::EncodersDataPtr&  encodersData,
+                                 const Ice::Current&);
+
+    private: boost::signals2::mutex mutex; ///< Mutex for thread-safe access to internal data.
+
+    private: ;
+
+    private: KobukiManager* kobuki;
+
+};
+
+#endif // ENCODERS_H

Added: trunk/src/stable/components/kobuki_driver/thread_control.cpp
===================================================================
--- trunk/src/stable/components/kobuki_driver/thread_control.cpp	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/thread_control.cpp	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,69 @@
+#include "thread_control.h"
+
+Thread_control::Thread_control(Ice::CommunicatorPtr ic, KobukiManager* kobuki_manager)
+{
+    this->ic = ic;
+    prop = ic->getProperties();
+
+    this->kobuki_manager = kobuki_manager;
+    initMotors();
+    initEncoders();
+}
+
+void Thread_control::initEncoders()
+{
+    std::string motorsControl_string = "Encoders";
+    encoders = new Encoders(kobuki_manager);
+
+    std::string Endpoints = prop->getProperty("kobuki.Encoders.Endpoints");
+
+    Ice::ObjectAdapterPtr adapter =
+        ic->createObjectAdapterWithEndpoints(motorsControl_string, Endpoints);
+
+    adapter->add(encoders, ic->stringToIdentity(std::string(motorsControl_string)));
+    adapter->activate();
+}
+
+
+void Thread_control::initMotors()
+{
+    std::string motorsControl_string = "Motors";
+    motors = new Motors(kobuki_manager);
+
+    std::string Endpoints = prop->getProperty("kobuki.Motors.Endpoints");
+
+    Ice::ObjectAdapterPtr adapter =
+        ic->createObjectAdapterWithEndpoints(motorsControl_string, Endpoints);
+
+    adapter->add(motors, ic->stringToIdentity(std::string(motorsControl_string)));
+    adapter->activate();
+}
+
+void Thread_control::run()
+{
+    struct timeval a, b;
+    long totalb, totala;
+    long diff;
+
+    while (true) {
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        this->kobuki_manager->update();
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+        diff = (totalb - totala) / 1000;
+
+        if (diff < 0 || diff > cycle_control)
+            diff = cycle_control;
+        else
+            diff = cycle_control - diff;
+
+
+        /*Sleep Algorithm*/
+        usleep(diff * 1000);
+        if (diff < 33)
+            usleep(33 * 1000);
+    }
+}

Added: trunk/src/stable/components/kobuki_driver/thread_control.h
===================================================================
--- trunk/src/stable/components/kobuki_driver/thread_control.h	                        (rev 0)
+++ trunk/src/stable/components/kobuki_driver/thread_control.h	2013-10-14 12:17:10 UTC (rev 1046)
@@ -0,0 +1,34 @@
+#ifndef THREAD_CONTROL_H
+#define THREAD_CONTROL_H
+
+#include <iostream>
+#include <sys/time.h>
+
+#include "kobukimanager.h"
+
+#include "actuators/motors.h"
+#include "sensors/encoders.h"
+
+#include <Ice/Ice.h>
+
+
+#define cycle_control 50 //miliseconds
+
+class Thread_control
+{
+public:
+    Thread_control(Ice::CommunicatorPtr ic, KobukiManager* kobuki_manager);
+
+public: void run();
+
+private: void initMotors();
+private: void initEncoders();
+private: Ice::CommunicatorPtr ic;
+private: Ice::PropertiesPtr prop;
+private: KobukiManager* kobuki_manager;
+private: Motors* motors;
+private: Encoders* encoders;
+
+};
+
+#endif // THREAD_CONTROL_H



More information about the Jderobot-admin mailing list