[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