diff --git a/Deps/kobuki/CMakeLists.txt b/Deps/kobuki/CMakeLists.txt deleted file mode 100644 index 7cbed9eeb..000000000 --- a/Deps/kobuki/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -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( kobuki_LIBRARIES ) - MESSAGE("kobuki LIBRARY FOUND IN ${kobuki_LIBRARIES}") - SET(kobuki_COMPILE TRUE) - list(APPEND DEPS kobukilib) - else() - MESSAGE("kobuki LIBRARY NOT FOUND IN ${kobuki_LIBRARIES}") - SET(kobuki_COMPILE FALSE) - ENDIF( ) - -else() - MESSAGE("*** kobuki not found") - SET(kobuki_COMPILE FALSE) -ENDIF() diff --git a/Deps/ros/CMakeLists.txt b/Deps/ros/CMakeLists.txt index f5a23308b..d60380d0d 100644 --- a/Deps/ros/CMakeLists.txt +++ b/Deps/ros/CMakeLists.txt @@ -1,7 +1,7 @@ OPTION(ENABLE_ROS "Enable ROS compatibility modules" ON) if (ENABLE_ROS) - find_package(roscpp QUIET) + find_package(roscpp REQUIRED) if(roscpp_FOUND) @@ -11,7 +11,7 @@ if (ENABLE_ROS) usePython(2) set (CATKIN_BUILD_BINARY_PACKAGE 1) #doesn't add ROS environment files to package - + find_package(catkin REQUIRED COMPONENTS roscpp std_msgs @@ -19,10 +19,14 @@ if (ENABLE_ROS) image_transport nav_msgs geometry_msgs - kobuki_gazebo ) - list(APPEND DEPS ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ros-kinetic-kobuki-gazebo) + list(APPEND DEPS ros-kinetic-roscpp ros-kinetic-std-msgs ros-kinetic-cv-bridge ros-kinetic-image-transport ros-kinetic-roscpp-core ros-kinetic-rospy ros-kinetic-nav-msgs ros-kinetic-geometry-msgs ) + + ADD_SUBDIRECTORY (${CMAKE_CURRENT_SOURCE_DIR}/Deps/ros/laser) + ADD_SUBDIRECTORY (${CMAKE_CURRENT_SOURCE_DIR}/Deps/ros/kobuki-real) + ADD_SUBDIRECTORY (${CMAKE_CURRENT_SOURCE_DIR}/Deps/ros/kobuki-sim) + # list(APPEND DEPS_DEV ) else() diff --git a/Deps/ros/kobuki-real/CMakeLists.txt b/Deps/ros/kobuki-real/CMakeLists.txt new file mode 100644 index 000000000..4fd79363b --- /dev/null +++ b/Deps/ros/kobuki-real/CMakeLists.txt @@ -0,0 +1,20 @@ + +#[[find_package(catkin QUIET COMPONENTS + kobuki + kobuki_core + ) + +messge(kobuki_FOUND) +if(kobuki_FOUND) + message("*** kobuki FOUND") +else(kobuki_FOUND) + message("*** kobuki NOT FOUND") +endif(kobuki_FOUND) + +if(kobuki_core_FOUND) + message("*** kobuki_core FOUND") +else(kobuki_core_FOUND) + message("*** kobuki_core NOT FOUND") +endif(kobuki_core_FOUND) +]] +list(APPEND DEPS ros-kinetic-kobuki ros-kinetic-kobuki-core) \ No newline at end of file diff --git a/Deps/ros/kobuki-sim/CMakeLists.txt b/Deps/ros/kobuki-sim/CMakeLists.txt new file mode 100644 index 000000000..a96f7a00b --- /dev/null +++ b/Deps/ros/kobuki-sim/CMakeLists.txt @@ -0,0 +1,11 @@ +#[[find_package(catkin QUIET COMPONENTS + kobuki_gazebo + ) + +if(kobuki_gazebo_FOUND) + message("*** kobuki_gazebo FOUND") +else() + message("*** kobuki_gazebo NOT FOUND") +endif() +]] +list(APPEND DEPS ros-kinetic-kobuki-gazebo) \ No newline at end of file diff --git a/Deps/ros/laser/CMakeLists.txt b/Deps/ros/laser/CMakeLists.txt new file mode 100644 index 000000000..b429d80a0 --- /dev/null +++ b/Deps/ros/laser/CMakeLists.txt @@ -0,0 +1,19 @@ + +#[[find_package(catkin QUIET COMPONENTS + rplidar_ros + urg_node + ) + +if(rplidar_ros_FOUND) + message("*** rplidar FOUND") +else() + message("*** RPLIDAR NOT FOUND") +endif() + +if(urg_node_FOUND) + message("*** HOKUYO FOUND") +else() + message("*** HOKUYO NOT FOUND") +endif() +]] +list(APPEND DEPS ros-kinetic-rplidar-ros ros-kinetic-urg-node) \ No newline at end of file diff --git a/buildpresets.cmake b/buildpresets.cmake index 0d2c78049..175b5d1de 100644 --- a/buildpresets.cmake +++ b/buildpresets.cmake @@ -158,7 +158,6 @@ if (build_drivers) build_component(turtlebot ON) build_component(giraffeServer ON) build_component(kinect2server ON) - build_component(kobuki_driver ON) build_component(naoserver ON) build_component(openni1Server ON) build_component(openniServer ON) diff --git a/cmake/cpack_metainfo/drivers.cmake b/cmake/cpack_metainfo/drivers.cmake index bfa07480f..005749c7e 100644 --- a/cmake/cpack_metainfo/drivers.cmake +++ b/cmake/cpack_metainfo/drivers.cmake @@ -14,6 +14,12 @@ SET(CPACK_COMPONENT_CAMERASERVER_DESCRIPTION "Generic server for RGB cameras. Manual Page http://jderobot.org/index.php/Drivers#cameraserver Home page https://jderobot.org") + + SET(CPACK_DEBIAN_CAMERASERVER-PYTHON_PACKAGE_DEPENDS "jderobot-config-python, jderobot-interfaces") +SET(CPACK_COMPONENT_BASIC-COMPONENT-PYTHON_DESCRIPTION +"Example of tool Python + Manual Page http://jderobot.org/ + Home page https://jderobot.org") SET(CPACK_DEBIAN_EMSENSORDRIVER_PACKAGE_DEPENDS " jderobot-interfaces") SET(CPACK_COMPONENT_EMSENSORDRIVER_DESCRIPTION @@ -33,12 +39,6 @@ SET(CPACK_COMPONENT_GAZEBOSERVER_DESCRIPTION Manual Page http://jderobot.org/index.php/Drivers (sections 3-9) Home page https://jderobot.org") -SET(CPACK_DEBIAN_LASER-SERVER_PACKAGE_DEPENDS "jderobot-easyice, jderobot-viewer, jderobot-util, jderobot-interfaces, jderobot-logger, jderobot-ns, jderobot-colorspaces") -SET(CPACK_COMPONENT_LASER-SERVER_DESCRIPTION -"Server for Hokuyo and RPLidar lasers. - Manual Page http://jderobot.org/index.php/Drivers#Laser_Server - Home page https://jderobot.org") - SET(CPACK_DEBIAN_OPENNISERVER_PACKAGE_DEPENDS "jderobot-easyice, jderobot-geometry, jderobot-util, jderobot-interfaces, jderobot-logger, jderobot-ns, jderobot-colorspaces") SET(CPACK_COMPONENT_OPENNISERVER_DESCRIPTION "Server for RGBD cameras (ASUS Xtion, Microsoft Kinect, Orbecc, ...) @@ -51,12 +51,6 @@ SET(CPACK_COMPONENT_PCLRGBDSERVER_DESCRIPTION Manual Page Home page https://jderobot.org") -SET(CPACK_DEBIAN_KOBUKI-DRIVER_PACKAGE_DEPENDS "jderobot-easyice, jderobot-interfaces, jderobot-colorspaces") -SET(CPACK_COMPONENT_KOBUKI-DRIVER_DESCRIPTION -"Driver for the Yujin Robot Kobuki - Manual page http://jderobot.org/index.php/Drivers#kobuki_driver - Home page https://jderobot.org") - SET(CPACK_DEBIAN_MAVLINKSERVER_PACKAGE_DEPENDS "jderobot-easyice, jderobot-interfaces, jderobot-colorspaces") SET(CPACK_COMPONENT_MAVLINKSERVER_DESCRIPTION "Driver for MAVLink based air drones and planes diff --git a/cmake/cpackit.cmake b/cmake/cpackit.cmake index 45807c904..c05544fc5 100644 --- a/cmake/cpackit.cmake +++ b/cmake/cpackit.cmake @@ -64,7 +64,6 @@ SET (CPACK_MONOLITHIC_INSTALL OFF) #include(cmake/cpack_metainfo/gazeboserver.cmake) #include(cmake/cpack_metainfo/giraffeclient.cmake) #include(cmake/cpack_metainfo/kobukiviewer.cmake) -#include(cmake/cpack_metainfo/laser-server.cmake) #include(cmake/cpack_metainfo/namingservice.cmake) #include(cmake/cpack_metainfo/navigatorcamera.cmake) #include(cmake/cpack_metainfo/openniserver.cmake) diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh index fd20ce1f3..9c9e92a58 100644 --- a/scripts/install_deps.sh +++ b/scripts/install_deps.sh @@ -65,7 +65,7 @@ sudo apt-get install -y nodejs sudo apt install -y sophus -sudo apt-get install -y kobukilib ardronelib libgoogle-glog-dev +sudo apt-get install -y ardronelib libgoogle-glog-dev diff --git a/scripts/metapackages/jderobot-drivers.info.in b/scripts/metapackages/jderobot-drivers.info.in index 79afeb923..76343da16 100644 --- a/scripts/metapackages/jderobot-drivers.info.in +++ b/scripts/metapackages/jderobot-drivers.info.in @@ -8,7 +8,7 @@ Section: net Priority: extra Size: 0 Installed-Size: 0 -Depends: jderobot-ardrone-server, jderobot-cameraserver, jderobot-gazeboserver, jderobot-laser-server, jderobot-mavlinkserver, jderobot-openni1server, jderobot-openniserver, jderobot-pclrgbdserver,jderobot-evicamdriver +Depends: jderobot-ardrone-server, jderobot-cameraserver, jderobot-gazeboserver, jderobot-mavlinkserver, jderobot-openni1server, jderobot-openniserver, jderobot-pclrgbdserver,jderobot-evicamdriver, jderobot-cameraserver-python Maintainer: Francisco Perez Homepage: http://jderobot.org Description: Metapackage that gathers all the drivers of JdeRobot framework. diff --git a/src/drivers/kobuki_driver/CMakeLists.txt b/src/drivers/kobuki_driver/CMakeLists.txt deleted file mode 100644 index f731833cd..000000000 --- a/src/drivers/kobuki_driver/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -if (${kobuki_COMPILE}) - - - include_directories( - ${INTERFACES_CPP_DIR} - ${LIBS_DIR}/ - ${CMAKE_CURRENT_SOURCE_DIR} - ${easyiceconfig_INCLUDE_DIRS} - ) - - add_executable( kobuki_driver main.cpp kobukimanager.cpp kobukimanager.h thread_control.cpp thread_control.h - actuators/motors.cpp actuators/motors.h - sensors/pose3d.h sensors/pose3d.cpp sensors/quaternion.h - ) - - link_directories( - ${kobuki_LIBRARIES} - ${easyiceconfig_LIBRARY_DIRS} - ) - include_directories(${kobuki_INCLUDE_DIR}) - - target_link_libraries(kobuki_driver - ${CMAKE_THREAD_LIBS_INIT} - ${kobuki_LIBRARIES} - JderobotInterfaces - jderobotutil - ${ZeroCIce_LIBRARIES} - ${easyiceconfig_LIBRARIES} - ) - - install(TARGETS kobuki_driver - DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ - COMPONENT kobuki-driver - ) - - INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/kobuki_driver.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT kobuki-driver) -ENDIF() \ No newline at end of file diff --git a/src/drivers/kobuki_driver/actuators/motors.cpp b/src/drivers/kobuki_driver/actuators/motors.cpp deleted file mode 100644 index 29a9d88ef..000000000 --- a/src/drivers/kobuki_driver/actuators/motors.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "motors.h" - -Motors::Motors(KobukiManager *kobuki_manager) -{ - this->kobuki_manager = kobuki_manager; -} - -Motors::~Motors() -{ - -} - -float Motors::getV(const Ice::Current&) -{ - return 0.; -} - -float Motors::getW(const Ice::Current&) -{ - return 0.; -} - -float Motors::getL(const Ice::Current&) -{ - return 0.; -} - -Ice::Int Motors::setV(Ice::Float v, const Ice::Current&) -{ - kobuki_manager->setV(v); - return 0; -} - -Ice::Int Motors::setW(Ice::Float _w, const Ice::Current&) -{ - kobuki_manager->setW(_w); - return 0; -} - -Ice::Int Motors::setL(Ice::Float l, const Ice::Current&) -{ - l = 0; - return 0; -} diff --git a/src/drivers/kobuki_driver/actuators/motors.h b/src/drivers/kobuki_driver/actuators/motors.h deleted file mode 100644 index 830aebbc1..000000000 --- a/src/drivers/kobuki_driver/actuators/motors.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef MOTORS_H -#define MOTORS_H - -#include - -#include "../kobukimanager.h" -#include - -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 diff --git a/src/drivers/kobuki_driver/kobuki_driver.cfg b/src/drivers/kobuki_driver/kobuki_driver.cfg deleted file mode 100644 index c2990afac..000000000 --- a/src/drivers/kobuki_driver/kobuki_driver.cfg +++ /dev/null @@ -1,2 +0,0 @@ -kobuki.Motors.Endpoints=default -h 0.0.0.0 -p 9999:ws -h 0.0.0.0 -p 11002 -kobuki.Pose3D.Endpoints=default -h 0.0.0.0 -p 9997:ws -h 0.0.0.0 -p 11003 diff --git a/src/drivers/kobuki_driver/kobuki_driver.pro b/src/drivers/kobuki_driver/kobuki_driver.pro deleted file mode 100644 index c4867ba52..000000000 --- a/src/drivers/kobuki_driver/kobuki_driver.pro +++ /dev/null @@ -1,35 +0,0 @@ -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 - diff --git a/src/drivers/kobuki_driver/kobukimanager.cpp b/src/drivers/kobuki_driver/kobukimanager.cpp deleted file mode 100644 index 7629f7091..000000000 --- a/src/drivers/kobuki_driver/kobukimanager.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#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(); - std::cout << "set X: " << pose.x() << std::endl; - mutex.unlock(); - return result; -} - -double KobukiManager::getRobotY() -{ - double result; - mutex.lock(); - result = pose.y(); - std::cout << "set Y: " << pose.y() << std::endl; - 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::LegacyPose2D 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(unsigned 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::LegacyPose2D KobukiManager::getPose() -{ - ecl::LegacyPose2D pose_result; - mutex.lock(); - pose_result = pose; - mutex.unlock(); - return pose_result; - -} diff --git a/src/drivers/kobuki_driver/kobukimanager.h b/src/drivers/kobuki_driver/kobukimanager.h deleted file mode 100644 index 8b0f03e46..000000000 --- a/src/drivers/kobuki_driver/kobukimanager.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef KOBUKIMANAGER_H -#define KOBUKIMANAGER_H -/***************************************************************************** - Includes - ****************************************************************************/ -//#define EIGEN_DONT_ALIGN_STATICALLY False - -#include -#include -#include -#include -#include -#include "kobuki_driver/kobuki.hpp" - -//boost -#include - -/***************************************************************************** - 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::LegacyPose2D getPose(); - -private: - double dx, dth; - ecl::LegacyPose2D 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 diff --git a/src/drivers/kobuki_driver/main.cpp b/src/drivers/kobuki_driver/main.cpp deleted file mode 100755 index e79ee80b2..000000000 --- a/src/drivers/kobuki_driver/main.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/***************************************************************************** - Includes - ****************************************************************************/ -#include "thread_control.h" -#include "easyiceconfig/EasyIce.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 = EasyIce::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 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; -} diff --git a/src/drivers/kobuki_driver/sensors/pose3d.cpp b/src/drivers/kobuki_driver/sensors/pose3d.cpp deleted file mode 100644 index ce56f5a27..000000000 --- a/src/drivers/kobuki_driver/sensors/pose3d.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "pose3d.h" - -Pose3D::Pose3D(KobukiManager *kobuki) -{ - this->kobuki = kobuki; -} - -int Pose3D::setPose3DData(const jderobot::Pose3DDataPtr& pose3dData, - const Ice::Current&) -{ - return 0; -} - - -jderobot::Pose3DDataPtr Pose3D::getPose3DData(const Ice::Current&) -{ - - mutex.lock(); - jderobot::Pose3DDataPtr pose3dData(new jderobot::Pose3DData()); - pose3dData->x = kobuki->getRobotX(); - pose3dData->y = kobuki->getRobotY(); - - Eigen::Matrix m; - m(0,0) = 0.0; - m(1,0) = 0.0; - m(2,0) = kobuki->getRobotTheta(); - - jderobot::math::Quaternion q = jderobot::math::Quaternion::FromEuler(m); - - pose3dData->q0 = q.w(); - pose3dData->q1 = q.x(); - pose3dData->q2 = q.y(); - pose3dData->q3 = q.z(); - - mutex.unlock(); - return pose3dData; -} - diff --git a/src/drivers/kobuki_driver/sensors/pose3d.h b/src/drivers/kobuki_driver/sensors/pose3d.h deleted file mode 100644 index cdd050c52..000000000 --- a/src/drivers/kobuki_driver/sensors/pose3d.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef POSE3D_H -#define POSE3D_H - -//boost -#include - -//Pose3D -#include - -//ICE -#include -#include - -#include "../kobukimanager.h" -#include "quaternion.h" - -class Pose3D: virtual public jderobot::Pose3D -{ -public: - Pose3D(KobukiManager* kobuki); - - virtual jderobot::Pose3DDataPtr getPose3DData(const Ice::Current&); - - virtual int setPose3DData(const jderobot::Pose3DDataPtr& pose3dData, - const Ice::Current&); - - private: boost::signals2::mutex mutex; ///< Mutex for thread-safe access to internal data. - - - private: KobukiManager* kobuki; - -}; - -#endif // POSE3D_H diff --git a/src/drivers/kobuki_driver/sensors/quaternion.h b/src/drivers/kobuki_driver/sensors/quaternion.h deleted file mode 100644 index 8f27d8bd8..000000000 --- a/src/drivers/kobuki_driver/sensors/quaternion.h +++ /dev/null @@ -1,125 +0,0 @@ -/** @file quaternion.h - * - * @par License - * - * @par - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * @par - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Library General Public License for more details. - * - * @par - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * @date 24/07/2014 - * @author L. Roberto Morales Iglesias - * - */ -#ifndef JDEROBOT_QUATERNION_H_ -#define JDEROBOT_QUATERNION_H_ - -#include -#include - -namespace jderobot { - - namespace math { - - template - class Quaternion: public Eigen::Quaternion { - public: - - - typedef Eigen::Quaternion Base; - typedef typename Base::AngleAxisType AngleAxisType; - - /// Default constructor leaving the quaternion uninitialized. - Quaternion(void):Eigen::Quaternion() {} - - /** Constructs and initializes the quaternion $w+xi+yj+zk$ - * from its four coefficients w, x, y and z. - * - * @warning Note the order of the arguments: - * the real w coefficient first, while internally - * the coefficients are stored in the following - * order: [x, y, z, w] - */ - Quaternion(const Scalar & w, const Scalar & x, const Scalar &y, - const Scalar & z) - :Eigen::Quaternion(w,x,y,z){} - - /// Copy constructor - template - Quaternion(const Eigen::QuaternionBase& other) - : Eigen::Quaternion(other) { } - - /// Constructs and initializes a quaternion from the angle-axis aa - Quaternion(const AngleAxisType& aa) - : Eigen::Quaternion(aa) {} - - static Eigen::Quaternion FromEuler(const Eigen::Matrix& v){ - double w,x,y,z; - double phi, theta, psi; - - phi = v.x() / 2.0; - theta = v.y() / 2.0; - psi = v.z() / 2.0; - w = std::cos(phi) * std::cos(theta) * std::cos(psi) - + std::sin(phi) * std::sin(theta) * std::sin(psi); - x = std::sin(phi) * std::cos(theta) * std::cos(psi) - - std::cos(phi) * std::sin(theta) * std::sin(psi); - y = std::cos(phi) * std::sin(theta) * std::cos(psi) - + std::sin(phi) * std::cos(theta) * std::sin(psi); - z = std::cos(phi) * std::cos(theta) * std::sin(psi) - - std::sin(phi) * std::sin(theta) * std::cos(psi); - - - return Eigen::Quaternion(w,x,y,z).normalized(); - } - - Eigen::Matrix getEulerVector() const{ - Scalar x,y,z; - Scalar ww,xx,yy,zz; - - ww = this->w() * this->w(); - xx = this->x() * this->x(); - yy = this->y() * this->y(); - zz = this->z() * this->z(); - - x = std::atan2(2 * (this->y()*this->z() + this->w()*this->x()), ww - xx - yy + zz); - - Scalar sarg = -2 * (this->x()*this->z() - this->w() * this->y()); - y = (sarg <= -1.0) ? -0.5*M_PI : ((sarg >= 1.0) ? 0.5*M_PI : std::asin(sarg)); - - z = std::atan2(2 * (this->x()*this->y() + this->w()*this->z()), ww + xx - yy - zz); - - - return Eigen::Matrix(x,y,z); - } - - - - template - Quaternion & operator= (const Eigen::QuaternionBase & other) { - this->Base::operator=(other); - return *this; - } - }; - - /// Floating point with simple precision based quaternion - typedef Quaternion Quaternionf; - /// Floating point with double precision based quaternion - typedef Quaternion Quaterniond; - - } -} - -#endif /* JDEROBOT_QUATERNION_H_ */ diff --git a/src/drivers/kobuki_driver/thread_control.cpp b/src/drivers/kobuki_driver/thread_control.cpp deleted file mode 100644 index ae7bb1756..000000000 --- a/src/drivers/kobuki_driver/thread_control.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#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(); - initPose3D(); -} - -void Thread_control::initPose3D() -{ - std::string motorsControl_string = "Pose3D"; - pose3d = new Pose3D(kobuki_manager); - - std::string Endpoints = prop->getProperty("kobuki.Pose3D.Endpoints"); - - Ice::ObjectAdapterPtr adapter = - ic->createObjectAdapterWithEndpoints(motorsControl_string, Endpoints); - - adapter->add(pose3d, 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); - } -} diff --git a/src/drivers/kobuki_driver/thread_control.h b/src/drivers/kobuki_driver/thread_control.h deleted file mode 100644 index 3a4e855dc..000000000 --- a/src/drivers/kobuki_driver/thread_control.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef THREAD_CONTROL_H -#define THREAD_CONTROL_H - -#include -#include - -#include "kobukimanager.h" - -#include "actuators/motors.h" -#include "sensors/pose3d.h" - -#include - - -#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 initPose3D(); -private: Ice::CommunicatorPtr ic; -private: Ice::PropertiesPtr prop; -private: KobukiManager* kobuki_manager; -private: Motors* motors; -private: Pose3D* pose3d; - -}; - -#endif // THREAD_CONTROL_H diff --git a/src/drivers/laser_server/CMakeLists.txt b/src/drivers/laser_server/CMakeLists.txt deleted file mode 100644 index 0ff04a8d8..000000000 --- a/src/drivers/laser_server/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -set (NAME laser_server) -cmake_minimum_required(VERSION 2.8) -set (SRC - main.cpp - hokuyo/hokuyo.cpp - hokuyo/hokuyomanager.cpp - laseri.cpp) - -include_directories(${INTERFACES_CPP_DIR} - ${LIBS_DIR}/ - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/hokuyo - ${easyiceconfig_INCLUDE_DIRS} -) - -link_directories( - ${easyiceconfig_LIBRARY_DIRS} -) - - -add_executable(${NAME} ${SRC}) -TARGET_LINK_LIBRARIES(${NAME} - ${ZeroCIce_LIBRARIES} - JderobotInterfaces - ${easyiceconfig_LIBRARIES} -) - -INSTALL(TARGETS ${NAME} - DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ - COMPONENT laser-server -) - -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/laser_server.cfg - DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf - COMPONENT laser-server -) diff --git a/src/drivers/laser_server/hokuyo/hokuyo.cpp b/src/drivers/laser_server/hokuyo/hokuyo.cpp deleted file mode 100644 index d53efa5d6..000000000 --- a/src/drivers/laser_server/hokuyo/hokuyo.cpp +++ /dev/null @@ -1,1176 +0,0 @@ -/* - * Player - One Hell of a Robot Server - * Copyright (C) 2008-2010 Willow Garage - * - * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "hokuyo.h" - -#include - -#include - -#include - -//#include "ros/console.h" - -#if POSIX_TIMERS <= 0 -#include -#endif - - -//! Macro for throwing an exception with a message, passing args -#define HOKUYO_EXCEPT(except, msg, ...) \ - { \ - char __buf[1000]; \ - snprintf(__buf, 1000, msg " (in hokuyo::laser::%s) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting" , ##__VA_ARGS__, __FUNCTION__); \ - throw except(__buf); \ - } - - -//! Helper function for querying the system time -static uint64_t timeHelper() -{ -#if POSIX_TIMERS > 0 - struct timespec curtime; - clock_gettime(CLOCK_REALTIME, &curtime); - return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec); -#else - struct timeval timeofday; - gettimeofday(&timeofday,NULL); - return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000; -#endif -} - - -#ifdef USE_LOG_FILE -FILE *logfile; -#endif - -/////////////////////////////////////////////////////////////////////////////// -hokuyo::Laser::Laser() : - dmin_(0), dmax_(0), ares_(0), amin_(0), amax_(0), afrt_(0), rate_(0), - wrapped_(0), last_time_(0), time_repeat_count_(0), offset_(0), - laser_fd_(-1) -{ -#ifdef USE_LOG_FILE - if (!logfile) - logfile = fopen("hokuyo.log", "w"); -#endif -} - - -/////////////////////////////////////////////////////////////////////////////// -hokuyo::Laser::~Laser () -{ - if (portOpen()) - close(); -} - - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::open(const char * port_name) -{ - if (portOpen()) - close(); - - // Make IO non blocking. This way there are no race conditions that - // cause blocking when a badly behaving process does a read at the same - // time as us. Will need to switch to blocking for writes or errors - // occur just after a replug event. - laser_fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY); - read_buf_start = read_buf_end = 0; - - if (laser_fd_ == -1) - { - const char *extra_msg = ""; - switch (errno) - { - case EACCES: - extra_msg = "You probably don't have premission to open the port for reading and writing."; - break; - case ENOENT: - extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?"; - break; - } - - HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg); - } - try - { - struct flock fl; - fl.l_type = F_WRLCK; - fl.l_whence = SEEK_SET; - fl.l_start = 0; - fl.l_len = 0; - fl.l_pid = getpid(); - - if (fcntl(laser_fd_, F_SETLK, &fl) != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name); - - // Settings for USB? - struct termios newtio; - tcgetattr(laser_fd_, &newtio); - memset (&newtio.c_cc, 0, sizeof (newtio.c_cc)); - newtio.c_cflag = CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - - // activate new settings - tcflush (laser_fd_, TCIFLUSH); - if (tcsetattr (laser_fd_, TCSANOW, &newtio) < 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); /// @todo tcsetattr returns true if at least one attribute was set. Hence, we might not have set everything on success. - usleep (200000); - - // Some models (04LX) need to be told to go into SCIP2 mode... - laserFlush(); - // Just in case a previous failure mode has left our Hokuyo - // spewing data, we send reset the laser to be safe. - try { - reset(); - } - catch (hokuyo::Exception &e) - { - // This might be a device that needs to be explicitely placed in - // SCIP2 mode. - // Note: Not tested: a device that is currently scanning in SCIP1.1 - // mode might not manage to switch to SCIP2.0. - - setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2. - reset(); // If this one fails, it really is an error. - } - - querySensorConfig(); - - queryVersionInformation(); // In preparation for calls to get various parts of the version info. - } - catch (hokuyo::Exception& e) - { - // These exceptions mean something failed on open and we should close - if (laser_fd_ != -1) - ::close(laser_fd_); - laser_fd_ = -1; - throw e; - } -} - - -/////////////////////////////////////////////////////////////////////////////// -void hokuyo::Laser::reset () -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - laserFlush(); - try - { - sendCmd("TM2", 1000); - } - catch (hokuyo::Exception &e) - {} // Ignore. If the laser was scanning TM2 would fail - try - { - sendCmd("RS", 1000); - last_time_ = 0; // RS resets the hokuyo clock. - wrapped_ = 0; // RS resets the hokuyo clock. - } - catch (hokuyo::Exception &e) - {} // Ignore. If the command coincided with a scan we might get garbage. - laserFlush(); - sendCmd("RS", 1000); // This one should just work. -} - - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::close () -{ - int retval = 0; - - if (portOpen()) { - //Try to be a good citizen and completely shut down the laser before we shutdown communication - try - { - reset(); - } - catch (hokuyo::Exception& e) { - //Exceptions here can be safely ignored since we are closing the port anyways - } - - retval = ::close(laser_fd_); // Automatically releases the lock. - } - - laser_fd_ = -1; - - if (retval != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno)); -} - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::setToSCIP2() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - const char * cmd = "SCIP2.0"; - char buf[100]; - laserWrite(cmd); - laserWrite("\n"); - - laserReadline(buf, 100, 1000); - //ROS_DEBUG("Laser comm protocol changed to %s \n", buf); - printf ("Laser comm protocol changed to %s \n", buf); -} - - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::sendCmd(const char* cmd, int timeout) -{ -if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - char buf[100]; - - laserWrite(cmd); - laserWrite("\n"); - - laserReadlineAfter(buf, 100, cmd, timeout); - laserReadline(buf,100,timeout); - - if (!checkSum(buf,4)) - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code."); - - buf[2] = 0; - - if (buf[0] - '0' >= 0 && buf[0] - '0' <= 9 && buf[1] - '0' >= 0 && buf[1] - '0' <= 9) - return (buf[0] - '0')*10 + (buf[1] - '0'); - else - HOKUYO_EXCEPT(hokuyo::Exception, "Hokuyo error code returned. Cmd: %s -- Error: %s", cmd, buf); -} - - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::getConfig(LaserConfig& config) -{ - config.min_angle = (amin_ - afrt_) * (2.0*M_PI)/(ares_); - config.max_angle = (amax_ - afrt_) * (2.0*M_PI)/(ares_); - config.ang_increment = (2.0*M_PI)/(ares_); - config.time_increment = (60.0)/(double)(rate_ * ares_); - config.scan_time = 60.0/((double)(rate_)); - config.min_range = dmin_ / 1000.0; - config.max_range = dmax_ / 1000.0; -} - - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::laserWrite(const char* msg) -{ - // IO is currently non-blocking. This is what we want for the more common read case. - int origflags = fcntl(laser_fd_,F_GETFL,0); - fcntl(laser_fd_, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking? - ssize_t len = strlen(msg); - ssize_t retval = write(laser_fd_, msg, len); - int fputserrno = errno; - fcntl(laser_fd_, F_SETFL, origflags | O_NONBLOCK); - errno = fputserrno; // Don't want to see the fcntl errno below. - - if (retval != -1) - { -#ifdef USE_LOG_FILE - if (strlen(msg) > 1) - { - long long outtime = timeHelper(); - fprintf(logfile, "Out: %lli.%09lli %s\n", outtime / 1000000000L, outtime % 1000000000L, msg); - } -#endif - return retval; - } - else - HOKUYO_EXCEPT(hokuyo::Exception, "fputs failed -- Error = %d: %s", errno, strerror(errno)); -} - - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::laserFlush() -{ - int retval = tcflush(laser_fd_, TCIOFLUSH); - if (retval != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed"); - read_buf_start = 0; - read_buf_end = 0; - - return retval; -} - - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::laserReadline(char *buf, int len, int timeout) -{ - int current=0; - - struct pollfd ufd[1]; - int retval; - ufd[0].fd = laser_fd_; - ufd[0].events = POLLIN; - - if (timeout == 0) - timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout. - - while (true) - { - if (read_buf_start == read_buf_end) // Need to read? - { - if ((retval = poll(ufd, 1, timeout)) < 0) - HOKUYO_EXCEPT(hokuyo::Exception, "poll failed -- error = %d: %s", errno, strerror(errno)); - - if (retval == 0) - HOKUYO_EXCEPT(hokuyo::TimeoutException, "timeout reached"); - - if (ufd[0].revents & POLLERR) - HOKUYO_EXCEPT(hokuyo::Exception, "error on socket, possibly unplugged"); - - int bytes = read(laser_fd_, read_buf, sizeof(read_buf)); - if (bytes == -1 && errno != EAGAIN && errno != EWOULDBLOCK) - HOKUYO_EXCEPT(hokuyo::Exception, "read failed"); - read_buf_start = 0; - read_buf_end = bytes; - } - - while (read_buf_end != read_buf_start) - { - if (current == len - 1) - { - buf[current] = 0; - HOKUYO_EXCEPT(hokuyo::Exception, "buffer filled without end of line being found"); - } - - buf[current] = read_buf[read_buf_start++]; - if (buf[current++] == '\n') - { - buf[current] = 0; - return current; - } - } - -#ifdef USE_LOG_FILE - long long outtime = timeHelper(); - fprintf(logfile, "In: %lli.%09lli %s", outtime / 1000000000L, outtime % 1000000000L, buf); -#endif - } -} - - -char* -hokuyo::Laser::laserReadlineAfter(char* buf, int len, const char* str, int timeout) -{ - buf[0] = 0; - char* ind = &buf[0]; - - int bytes_read = 0; - int skipped = 0; - - while ((strncmp(buf, str, strlen(str))) != 0) { - bytes_read = laserReadline(buf,len,timeout); - - if ((skipped += bytes_read) > MAX_SKIPPED) - HOKUYO_EXCEPT(hokuyo::Exception, "too many bytes skipped while searching for match"); - } - - return ind += strlen(str); -} - - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::querySensorConfig() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - if (sendCmd("PP",1000) != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting configuration information"); - - char buf[100]; - char* ind; - - ind = laserReadlineAfter(buf,100,"DMIN:",-1); - sscanf(ind, "%d", &dmin_); - - ind = laserReadlineAfter(buf,100,"DMAX:",-1); - sscanf(ind, "%d", &dmax_); - - ind = laserReadlineAfter(buf,100,"ARES:",-1); - sscanf(ind, "%d", &ares_); - - ind = laserReadlineAfter(buf,100,"AMIN:",-1); - sscanf(ind, "%d", &amin_); - - ind = laserReadlineAfter(buf,100,"AMAX:",-1); - sscanf(ind, "%d", &amax_); - - ind = laserReadlineAfter(buf,100,"AFRT:",-1); - sscanf(ind, "%d", &afrt_); - - ind = laserReadlineAfter(buf,100,"SCAN:",-1); - sscanf(ind, "%d", &rate_); - - return; -} - - -/////////////////////////////////////////////////////////////////////////////// -bool -hokuyo::Laser::checkSum(const char* buf, int buf_len) -{ - char sum = 0; - for (int i = 0; i < buf_len - 2; i++) - sum += (unsigned char)(buf[i]); - - if ((sum & 63) + 0x30 == buf[buf_len - 2]) - return true; - else - return false; -} - - -/////////////////////////////////////////////////////////////////////////////// -uint64_t -hokuyo::Laser::readTime(int timeout) -{ - char buf[100]; - - laserReadline(buf, 100, timeout); - if (!checkSum(buf, 6)) - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on time stamp."); - - unsigned int laser_time = ((buf[0]-0x30) << 18) | ((buf[1]-0x30) << 12) | ((buf[2]-0x30) << 6) | (buf[3] - 0x30); - - if (laser_time == last_time_) - { - if (++time_repeat_count_ > 2) - { - HOKUYO_EXCEPT(hokuyo::RepeatedTimeException, "The timestamp has not changed for %d reads", time_repeat_count_); - } - else if (time_repeat_count_ > 0) - //ROS_DEBUG("The timestamp has not changed for %d reads. Ignoring for now.", time_repeat_count_); - printf ("The timestamp has not changed for %d reads. Ignoring for now.", time_repeat_count_); - } - else - { - time_repeat_count_ = 0; - } - - if (laser_time < last_time_) - wrapped_++; - - last_time_ = laser_time; - - return (uint64_t)((wrapped_ << 24) | laser_time)*(uint64_t)(1000000); -} - - -/////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout) -{ - scan.ranges.clear(); - scan.intensities.clear(); - - int data_size = 3; - if (has_intensity) - data_size = 6; - - char buf[100]; - - int ind = 0; - - scan.self_time_stamp = readTime(timeout); - - int bytes; - - int range; - float intensity; - - for (;;) - { - bytes = laserReadline(&buf[ind], 100 - ind, timeout); - - if (bytes == 1) // This is \n\n so we should be done - return; - - if (!checkSum(&buf[ind], bytes)) - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read."); - - bytes += ind - 2; - - // Read as many ranges as we can get - if(dmax_ > 20000){ // Check error codes for the UTM 30LX (it is the only one with the long max range and has different error codes) - for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) - { - if (scan.ranges.size() < MAX_READINGS) - { - range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); - - switch (range) // See the SCIP2.0 reference on page 12, Table 4 - { - case 1: // No Object in Range - scan.ranges.push_back(std::numeric_limits::infinity()); - break; - case 2: // Object is too near (Internal Error) - scan.ranges.push_back(-std::numeric_limits::infinity()); - break; - case 3: // Measurement Error (May be due to interference) - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 4: // Object out of range (at the near end) - ///< @todo, Should this be an Infinity Instead? - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 5: // Other errors - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - default: - scan.ranges.push_back(((float)range)/1000.0); - } - - if (has_intensity) - { - intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); - scan.intensities.push_back(intensity); - } - } - else - { - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected"); - } - } - } else { // Check error codes for all other lasers (URG-04LX UBG-04LX-F01 UHG-08LX) - for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) - { - if (scan.ranges.size() < MAX_READINGS) - { - range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); - - switch (range) // See the SCIP2.0 reference on page 12, Table 3 - { - case 0: // Detected object is possibly at 22m - scan.ranges.push_back(std::numeric_limits::infinity()); - break; - case 1: // Reflected light has low intensity - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 2: // Reflected light has low intensity - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 6: // Detected object is possibly at 5.7m - scan.ranges.push_back(std::numeric_limits::infinity()); - break; - case 7: // Distance data on the preceding and succeeding steps have errors - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 8: // Intensity difference of two waves - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 9: // The same step had error in the last two scan - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 10: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 11: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 12: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 13: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 14: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 15: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 16: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 17: // Others - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 18: // Error reading due to strong reflective object - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - case 19: // Non-Measurable step - scan.ranges.push_back(std::numeric_limits::quiet_NaN()); - break; - default: - scan.ranges.push_back(((float)range)/1000.0); - } - - if (has_intensity) - { - intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); - scan.intensities.push_back(intensity); - } - } - else - { - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected"); - } - } - } - // Shuffle remaining bytes to front of buffer to get them on the next loop - ind = 0; - for (int j = bytes - (bytes % data_size); j < bytes ; j++) - buf[ind++] = buf[j]; - } -} - - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::pollScan(hokuyo::LaserScan& scan, double min_ang, double max_ang, int cluster, int timeout) -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - int status; - - // Always clear ranges/intensities so we can return easily in case of erro - scan.ranges.clear(); - scan.intensities.clear(); - - // clustering of 0 and 1 are actually the same - if (cluster == 0) - cluster = 1; - - int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI)); - int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI)); - - char cmdbuf[MAX_CMD_LEN]; - - sprintf(cmdbuf,"GD%.4d%.4d%.2d", min_i, max_i, cluster); - - status = sendCmd(cmdbuf, timeout); - - scan.system_time_stamp = timeHelper() + offset_; - - if (status != 0) - return status; - - // Populate configuration - scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); - scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); - scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); - scan.config.time_increment = (60.0)/(double)(rate_ * ares_); - scan.config.scan_time = 0.0; - scan.config.min_range = dmin_ / 1000.0; - scan.config.max_range = dmax_ / 1000.0; - - readData(scan, false, timeout); - - long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); - - scan.system_time_stamp += inc; - scan.self_time_stamp += inc; - - return 0; -} - -int -hokuyo::Laser::laserOn() { - int res = sendCmd("BM",1000); - if (res == 1) - HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction."); - return res; -} - -int -hokuyo::Laser::laserOff() { - return sendCmd("QT",1000); -} - -int -hokuyo::Laser::stopScanning() { - try { - return laserOff(); - } - catch (hokuyo::Exception &e) - { - // Ignore exception because we might have gotten part of a scan - // instead of the expected response, which shows up as a bad checksum. - laserFlush(); - } - return laserOff(); // This one should work because the scan is stopped. -} - -/////////////////////////////////////////////////////////////////////////////// -int -hokuyo::Laser::requestScans(bool intensity, double min_ang, double max_ang, int cluster, int skip, int count, int timeout) -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - //! @todo check that values are within range? - - int status; - - if (cluster == 0) - cluster = 1; - - int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI)); - int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI)); - - char cmdbuf[MAX_CMD_LEN]; - - char intensity_char = 'D'; - if (intensity) - intensity_char = 'E'; - - sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count); - - status = sendCmd(cmdbuf, timeout); - - return status; -} - -bool -hokuyo::Laser::isIntensitySupported() -{ - hokuyo::LaserScan scan; - - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - // Try an intensity command. - try - { - requestScans(1, 0, 0, 0, 0, 1); - serviceScan(scan, 1000); - return true; - } - catch (hokuyo::Exception &e) - {} - - // Try a non intensity command. - try - { - requestScans(0, 0, 0, 0, 0, 1); - serviceScan(scan, 1000); - return false; - } - catch (hokuyo::Exception &e) - { - HOKUYO_EXCEPT(hokuyo::Exception, "Exception whil trying to determine if intensity scans are supported.") - } -} - -int -hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout) -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - // Always clear ranges/intensities so we can return easily in case of erro - scan.ranges.clear(); - scan.intensities.clear(); - - char buf[100]; - - bool intensity = false; - int min_i; - int max_i; - int cluster; - int skip; - int left; - - char* ind; - - int status = -1; - - do { - ind = laserReadlineAfter(buf, 100, "M",timeout); - scan.system_time_stamp = timeHelper() + offset_; - - if (ind[0] == 'D') - intensity = false; - else if (ind[0] == 'E') - intensity = true; - else - continue; - - ind++; - - sscanf(ind, "%4d%4d%2d%1d%2d", &min_i, &max_i, &cluster, &skip, &left); - laserReadline(buf,100,timeout); - - buf[4] = 0; - - if (!checkSum(buf, 4)) - HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code: %s", buf); - - sscanf(buf, "%2d", &status); - - if (status != 99) - return status; - - } while(status != 99); - - scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); - scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); - scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); - scan.config.time_increment = (60.0)/(double)(rate_ * ares_); - scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_)); - scan.config.min_range = dmin_ / 1000.0; - scan.config.max_range = dmax_ / 1000.0; - - readData(scan, intensity, timeout); - - long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); - - scan.system_time_stamp += inc; - scan.self_time_stamp += inc; - -// printf("Scan took %lli.\n", -scan.system_time_stamp + timeHelper() + offset_); - - return 0; -} - -////////////////////////////////////////////////////////////////////////////// -void -hokuyo::Laser::queryVersionInformation() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - if (sendCmd("VV",1000) != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting version information"); - - char buf[100]; - vendor_name_ = laserReadlineAfter(buf, 100, "VEND:"); - vendor_name_ = vendor_name_.substr(0,vendor_name_.length() - 3); - - product_name_ = laserReadlineAfter(buf, 100, "PROD:"); - product_name_ = product_name_.substr(0,product_name_.length() - 3); - - firmware_version_ = laserReadlineAfter(buf, 100, "FIRM:"); - firmware_version_ = firmware_version_.substr(0,firmware_version_.length() - 3); - - protocol_version_ = laserReadlineAfter(buf, 100, "PROT:"); - protocol_version_ = protocol_version_.substr(0,protocol_version_.length() - 3); - - // This crazy naming scheme is for backward compatibility. Initially - // the serial number always started with an H. Then it got changed to a - // zero. For a while the driver was removing the leading zero in the - // serial number. This is fine as long as it is indeed a zero in front. - // The current behavior is backward compatible but will accomodate full - // length serial numbers. - serial_number_ = laserReadlineAfter(buf, 100, "SERI:"); - serial_number_ = serial_number_.substr(0,serial_number_.length() - 3); - if (serial_number_[0] == '0') - serial_number_[0] = 'H'; - else if (serial_number_[0] != 'H') - serial_number_ = 'H' + serial_number_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getID() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - return serial_number_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getFirmwareVersion() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - return firmware_version_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getProtocolVersion() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - return protocol_version_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getVendorName() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - return vendor_name_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getProductName() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - return product_name_; -} - - -////////////////////////////////////////////////////////////////////////////// -std::string -hokuyo::Laser::getStatus() -{ - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - if (sendCmd("II",1000) != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting device information information"); - - char buf[100]; - char* stat = laserReadlineAfter(buf, 100, "STAT:"); - - std::string statstr(stat); - statstr = statstr.substr(0,statstr.length() - 3); - - return statstr; -} - -template -C median(std::vector &v) -{ - std::vector::iterator start = v.begin(); - std::vector::iterator end = v.end(); - std::vector::iterator median = start + (end - start) / 2; - //std::vector::iterator quarter1 = median - (end - start) / 4; - //std::vector::iterator quarter2 = median + (end - start) / 4; - std::nth_element(start, median, end); - //long long int medianval = *median; - //std::nth_element(start, quarter1, end); - //long long int quarter1val = *quarter1; - //std::nth_element(start, quarter2, end); - //long long int quarter2val = *quarter2; - return *median; -} - -#if 1 -long long int hokuyo::Laser::getHokuyoClockOffset(int reps, int timeout) -{ - std::vector offset(reps); - - sendCmd("TM0",timeout); - for (int i = 0; i < reps; i++) - { - long long int prestamp = timeHelper(); - sendCmd("TM1",timeout); - long long int hokuyostamp = readTime(); - long long int poststamp = timeHelper(); - offset[i] = hokuyostamp - (prestamp + poststamp) / 2; - //printf("%lli %lli %lli", hokuyostamp, prestamp, poststamp); - } - sendCmd("TM2",timeout); - - long long out = median(offset); - - return out; -} - -long long int hokuyo::Laser::getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout) -{ - if (reps < 1) - reps = 1; - else if (reps > 99) - reps = 99; - - std::vector offset(reps); - - if (requestScans(intensity, min_ang, max_ang, clustering, skip, reps, timeout) != 0) - { - HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scan while caliblating time."); - return 1; - } - - hokuyo::LaserScan scan; - for (int i = 0; i < reps; i++) - { - serviceScan(scan, timeout); - //printf("%lli %lli\n", scan.self_time_stamp, scan.system_time_stamp); - offset[i] = scan.self_time_stamp - scan.system_time_stamp; - } - - return median(offset); -} - -////////////////////////////////////////////////////////////////////////////// -long long -hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout) -{ - offset_ = 0; - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - if (num <= 0) - num = 10; - - int ckreps = 1; - int scanreps = 1; - long long int start = getHokuyoClockOffset(ckreps, timeout); - long long int pre = 0; - std::vector samples(num); - for (int i = 0; i < num; i++) - { - long long int scan = getHokuyoScanStampToSystemStampOffset(intensity, min_ang, max_ang, clustering, skip, scanreps, timeout) - start; - long long int post = getHokuyoClockOffset(ckreps, timeout) - start; - samples[i] = scan - (post+pre)/2; - //printf("%lli %lli %lli %lli %lli\n", samples[i], post, pre, scan, pre - post); - //fflush(stdout); - pre = post; - } - - offset_ = median(samples); - //printf("%lli\n", median(samples)); - return offset_; -} - -#else -////////////////////////////////////////////////////////////////////////////// -long long -hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout) -{ - //ROS_DEBUG("Entering calcLatency."); - printf ("Entering calcLatency."); - if (!portOpen()) - HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); - - static const std::string buggy_version = "1.16.02(19/Jan./2010)"; - if (firmware_version_ == buggy_version) - { - //ROS_INFO("Hokuyo firmware version %s detected. Using hard-coded time offset of -23 ms.", - // buggy_version.c_str()); - printf ("Hokuyo firmware version %s detected. Using hard-coded time offset of -23 ms.", buggy_version.c_str()); - offset_ = -23000000; - } - else - { - offset_ = 0; - - uint64_t comp_time = 0; - uint64_t laser_time = 0; - long long diff_time = 0; - long long drift_time = 0; - long long tmp_offset1 = 0; - long long tmp_offset2 = 0; - - int count = 0; - - sendCmd("TM0",timeout); - count = 100; - - for (int i = 0; i < count;i++) - { - usleep(1000); - sendCmd("TM1",timeout); - comp_time = timeHelper(); - try - { - laser_time = readTime(); - - diff_time = comp_time - laser_time; - - tmp_offset1 += diff_time / count; - } catch (hokuyo::RepeatedTimeException &e) - { - // We expect to get Repeated Time's when hammering on the time server - continue; - } - } - - uint64_t start_time = timeHelper(); - usleep(5000000); - sendCmd("TM1;a",timeout); - sendCmd("TM1;b",timeout); - comp_time = timeHelper(); - drift_time = comp_time - start_time; - laser_time = readTime() + tmp_offset1; - diff_time = comp_time - laser_time; - double drift_rate = double(diff_time) / double(drift_time); - - sendCmd("TM2",timeout); - - if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0) - HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation"); - - hokuyo::LaserScan scan; - - count = 200; - for (int i = 0; i < count;i++) - { - try - { - serviceScan(scan, 1000); - } catch (hokuyo::CorruptedDataException &e) { - continue; - } - - comp_time = scan.system_time_stamp; - drift_time = comp_time - start_time; - laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate); - diff_time = laser_time - comp_time; - - tmp_offset2 += diff_time / count; - } - - offset_ = tmp_offset2; - - stopScanning(); - } - - //ROS_DEBUG("Leaving calcLatency."); - printf ("Leaving calcLatency."); - return offset_; -} -#endif diff --git a/src/drivers/laser_server/hokuyo/hokuyo.h b/src/drivers/laser_server/hokuyo/hokuyo.h deleted file mode 100644 index 43e000aea..000000000 --- a/src/drivers/laser_server/hokuyo/hokuyo.h +++ /dev/null @@ -1,398 +0,0 @@ -/* - * Player - One Hell of a Robot Server - * Copyright (C) 2008-2010 Willow Garage - * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ - -/* \mainpage hokuyo_node - * \htmlinclude manifest.html - */ - -#ifndef HOKUYO_HH -#define HOKUYO_HH - -#include -#include -#include -#include -#include -#include - -//! A namespace containing the hokuyo device driver -namespace hokuyo -{ - //! The maximum number of readings that can be returned from a hokuyo - const uint32_t MAX_READINGS = 1128; - - //! The maximum length a command will ever be - const int MAX_CMD_LEN = 100; - - //! The maximum number of bytes that should be skipped when looking for a response - const int MAX_SKIPPED = 1000000; - - //! Macro for defining an exception with a given parent (std::runtime_error should be top parent) -#define DEF_EXCEPTION(name, parent) \ - class name : public parent { \ - public: \ - name(const char* msg) : parent(msg) {} \ - }\ - - //! A standard Hokuyo exception - DEF_EXCEPTION(Exception, std::runtime_error); - - //! An exception for use when a timeout is exceeded - DEF_EXCEPTION(TimeoutException, Exception); - - //! An exception for use when data is corrupted - DEF_EXCEPTION(CorruptedDataException, Exception); - - //! An exception for use when the timestamp on the data is repeating (a good indicator something has gone wrong) - DEF_EXCEPTION(RepeatedTimeException, Exception); - -#undef DEF_EXCEPTION - - //! A struct for returning configuration from the Hokuyo - struct LaserConfig - { - //! Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. - float min_angle; - //! Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. - float max_angle; - //! Scan resolution [rad]. - float ang_increment; - //! Scan resoltuion [s] - float time_increment; - //! Time between scans - float scan_time; - //! Minimum range [m] - float min_range; - //! Maximum range [m] - float max_range; - //! Range Resolution [m] - float range_res; - }; - - - //! A struct for returning laser readings from the Hokuyo - struct LaserScan - { - //! Array of ranges - std::vector ranges; - //! Array of intensities - std::vector intensities; - //! Self reported time stamp in nanoseconds - uint64_t self_time_stamp; - //! System time when first range was measured in nanoseconds - uint64_t system_time_stamp; - //! Configuration of scan - LaserConfig config; - }; - - - //! A class for interfacing to an SCIP2.0-compliant Hokuyo laser device - /*! - * Almost all methods of this class may throw an exception of type - * hokuyo::Exception in the event of an error when communicating - * with the device. This primarily include read/write problems - * (hokuyo::Exception), communication time-outs - * (hokuyo:TimeoutException), and corrupted data - * (hokuyo::CorruptedDataException). However, many methods which - * wrap commands that are sent to the hokuyo will return a - * hokuyo-supplied status code as well. These are not - * "exceptional," in that the device is operating correctly from a - * communication standpoint, although the return code may still - * indicate that the device has undergone some sort of failure. In - * these cases, return codes of 0 are normal operation. For more - * information on the possible device error codes, consult the - * hokuyo manual. - */ - class Laser - { - public: - //! Constructor - Laser(); - - //! Destructor - ~Laser(); - - //! Open the port - /*! - * This must be done before the hokuyo can be used. This call essentially - * wraps fopen, with some additional calls to tcsetattr. - * - * \param port_name A character array containing the name of the port - * - */ - void open(const char * port_name); - - //! Close the port - /*! - * This call essentiall wraps fclose. - */ - void close(); - - //! Reset the laser to its power on state. - /*! - * This calls TM2 and RS. Exceptions raised by TM2 are ignored as they - * are normal in the case where the laser is scanning. Exceptions - * raised by RS are passed back to the caller. - */ - void reset(); - - //! Check whether the port is open - bool portOpen() { return laser_fd_ != -1; } - - //! Sends an SCIP2.0 command to the hokuyo device - // sets up model 04LX to work in SCIP 2.0 mode - void setToSCIP2(); - - /*! - * This function allows commands to be sent directly to the hokuyo. - * Possible commands can be found in the hokuyo documentation. It - * will only read up until the end of the status code. Any - * additional bytes will be left on the line for parsing. - * - * \param cmd Command and arguments in a character array - * \param timeout Timeout in milliseconds. - * - * \return Status code returned from hokuyo device. - */ - int sendCmd(const char* cmd, int timeout = -1); - - //! Retrieve the native hokuyo configuration. - /*! - * This function will retrieve the native configuration for the laser. - * In particular, this specifies the smallest possible min, and largest - * possible maximum values that can be used. - * - * \param config A reference to the LaserConfig which will be populated - * - * \return Status code returned from hokuyo device. - */ - void getConfig(LaserConfig& config); - - - //! Retrieve a single scan from the hokuyo - /*! - * \param scan A reference to the LaserScan which will be populated - * \param min_ang Minimal angle of the scan - * \param max_ang Maximum angle of the scan - * \param clustering Number of adjascent ranges to be clustered into a single measurement. - * \param timeout Timeout in milliseconds. - * - * \return Status code returned from hokuyo device. - */ - int pollScan(LaserScan& scan, double min_ang, double max_ang, int clustering = 0, int timeout = -1); - - //! Request a sequence of scans from the hokuyo - /*! - * This function will request a sequence of scans from the hokuyo. If - * indefinite scans are requested, stop_scanning() must be called - * to terminate scanning. Service_scan() must be called repeatedly - * to receive scans as they come in. - * - * \param intensity Whether or not intensity data should be provided - * \param min_ang Minimal angle of the scan (radians) - * \param max_ang Maximum angle of the scan (radians) - * \param clustering Number of adjascent ranges to be clustered into a single measurement - * \param skip Number of scans to skip between returning measurements - * \param num Number of scans to return (0 for indefinite) - * \param timeout Timeout in milliseconds. - * - * \return Status code returned from hokuyo device. - */ - int requestScans(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); - - //! Retrieve a scan if they have been requested - /*! - * \param scan A reference to the LaserScan which will be populated. - * \param timeout Timeout in milliseconds. - * - * \return 0 on succes, Status code returned from hokuyo device on failure. (Normally 99 is used for success) - */ - int serviceScan(LaserScan& scan, int timeout = -1); - - //! Turn the laser off - /*! - * \return Status code returned from hokuyo device. - */ - int laserOff(); - - //! Turn the laser on - /*! - * \return Status code returned from hokuyo device. - */ - int laserOn(); - - //! Stop retrieving scans - /*! - * \return Status code returned from hokuyo device. - */ - int stopScanning(); - - //! Return hokuyo serial number - /*! - * \return Serial number of hokuyo. - */ - std::string getID(); - - //! Get status message from the hokuyo - /*! - * \return Status message - */ - std::string getStatus(); - - //! Compute latency between actual hokuyo readings and system time - /*! - * This function will estimate the latency between when the data - * is actually acquired by the hokuyo and a read call returns from - * the hokuyo with the data. This latency is stored in an offset - * variable and applied to the measurement of system time which - * occurs immediately after a read. This means that the - * system_time_stamp variable on LaserScan struct is corrected - * and accurately reflects the exact time of the hokuyo data in - * computer system time. This function takes the same arguments - * as a call to request scans, since this latency may be - * parameter dependent. NOTE: This method will take - * approximately 10 seconds to return. - * - * \param intensity Whether or not intensity data should be provided - * \param min_ang Minimal angle of the scan (radians) - * \param max_ang Maximum angle of the scan (radians) - * \param clustering Number of adjascent ranges to be clustered into a single measurement - * \param skip Number of scans to skip between returning measurements - * \param num Number of times to repeat the measurement 0 for auto. - * \param timeout Timeout in milliseconds. - * - * \return Median latency in nanoseconds - */ - long long calcLatency(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); - - //! This method clears the offset that was computed with calcLatency. - void clearLatency() - { - offset_ = 0; - } - - //! Returns the latency that is in use. - //! \return Latency in nanoseconds. - long long getLatency() - { - return offset_; - } - - //! Return firmware version - /*! - * \return Firmware version of hokuyo. - */ - std::string getFirmwareVersion(); - - //! Return hokuyo vendor name - /*! - * \return Vendor name of hokuyo. - */ - std::string getVendorName(); - - //! Return hokuyo product name - /*! - * \return Product name of hokuyo. - */ - std::string getProductName(); - - //! Return hokuyo protocol version - /*! - * \return Protocol version of hokuyo. - */ - std::string getProtocolVersion(); - - //! Tries to determine if intensity mode is supported - /*! - * \return True if intensity mode is supported, false if it is not. - * hokuyo::Exception if no scan could be read even out of intensity - * mode. - */ - bool isIntensitySupported(); - - //! Call VV and store the returned version information - //! Use other methods to read the information. - void queryVersionInformation(); - - //! Wrapper around fputs - int laserWrite(const char* msg); - - //! Read a full line from the hokuyo using fgets - int laserReadline(char *buf, int len, int timeout = -1); - - //! Wrapper around tcflush - int laserFlush(); - - //! Read in a time value - uint64_t readTime(int timeout = -1); - - private: - //! Use the TM command to determine the offset between the PC clock and - //the Hokuyo clock. Returns the median of reps measurements. - long long int getHokuyoClockOffset(int reps, int timeout = -1); - - //! Get reps scans and returns the median difference between the system - // timestamp (PC) and the scan timestamp (Hokuyo). - long long int getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout = -1); - - //! Query the sensor configuration of the hokuyo - void querySensorConfig(); - - //! Search for a particular sequence and then read the rest of the line - char* laserReadlineAfter(char *buf, int len, const char *str, int timeout = -1); - - //! Compute the checksum of a given buffer - bool checkSum(const char* buf, int buf_len); - - //! Read in a scan - void readData(LaserScan& scan, bool has_intensity, int timout = -1); - - int dmin_; - int dmax_; - int ares_; - int amin_; - int amax_; - int afrt_; - int rate_; - - int wrapped_; - - unsigned int last_time_; - - unsigned int time_repeat_count_; - - long long offset_; - - int laser_fd_; - - std::string vendor_name_; - std::string product_name_; - std::string serial_number_; - std::string protocol_version_; - std::string firmware_version_; - - char read_buf[256]; - int read_buf_start; - int read_buf_end; - }; - -} - -#endif diff --git a/src/drivers/laser_server/hokuyo/hokuyomanager.cpp b/src/drivers/laser_server/hokuyo/hokuyomanager.cpp deleted file mode 100644 index 65330b5c9..000000000 --- a/src/drivers/laser_server/hokuyo/hokuyomanager.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "hokuyomanager.h" - -namespace hokuyo{ - HokuyoManager::HokuyoManager(std::string deviceId, double min_ang, double max_ang, int clustering, int timeout, int faceup) - { - std::string port = DEVICEPORTPRE + deviceId; - - laser_.open(port.c_str()); - laser_.laserOn(); - laser_.setToSCIP2(); - this->min_ang = min_ang; - this->max_ang = max_ang; - this->clustering = clustering; - this->timeout = timeout; - this->faceup = faceup; -std::cout << "open: " << port << std::endl; - LaserConfig config; - laser_.getConfig(config); -std::cout<< "min_range: " << config.min_range << std::endl; -std::cout<< "max_range: " << config.max_range << std::endl; -std::cout<< "range_res: " << config.range_res << std::endl; - } - - HokuyoManager::~HokuyoManager() - { - laser_.laserOff(); - laser_.close(); - } - - void - HokuyoManager::laserScan2LaserData(hokuyo::LaserScan scan, jderobot::LaserData *data) - { - data->numLaser = scan.ranges.size(); - data->distanceData.resize(sizeof(int)*data->numLaser); - data->maxRange = scan.config.max_range*1000; - data->minRange = 0; - if (this->faceup!=0){ - for(int i = 0 ; i < data->numLaser; i++){ - if (std::numeric_limits::infinity() == scan.ranges[i]){ - data->distanceData[i] = scan.config.max_range*1000; - }else if (scan.ranges[i] < 0){ - data->distanceData[i] = 0; - }else{ - data->distanceData[i] = scan.ranges[i]*1000; - } - } - }else{ - for(int i = 0 ; i < data->numLaser; i++){ - if (std::numeric_limits::infinity() == scan.ranges[i]){ - data->distanceData[data->numLaser-1-i] = scan.config.max_range*1000; - }else if (scan.ranges[i] <0){ - data->distanceData[data->numLaser-1-i] = 0; - }else{ - data->distanceData[data->numLaser-1-i] = scan.ranges[i]*1000; - } - } - } - } - - jderobot::LaserData - *HokuyoManager::getLaserData() - { - jderobot::LaserData *data = new jderobot::LaserData(); - data->minAngle = this->min_ang; - data->maxAngle = this->max_ang; - - hokuyo::LaserScan scan; - - mutex.lock(); - //res = laser_.pollScan(scan,this->min_ang,this->max_ang,this->clustering,this->timeout); - - mutex.unlock(); - - laserScan2LaserData(scan, data); - return data; - } -} - diff --git a/src/drivers/laser_server/hokuyo/hokuyomanager.h b/src/drivers/laser_server/hokuyo/hokuyomanager.h deleted file mode 100644 index 10e5f9c14..000000000 --- a/src/drivers/laser_server/hokuyo/hokuyomanager.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef HOKUYOMANAGER_H -#define HOKUYOMANAGER_H - -#include -#include -#include "hokuyo.h" -#include "../lasermanager.h" -#include - -//boost -#include - -namespace hokuyo{ - - const std::string DEVICEPORTPRE="/dev/ttyACM"; - - - class HokuyoManager : public LaserManager - { - public: - HokuyoManager(std::string deviceId, double min_ang, double max_ang, int clustering, int timeout, int faceup); - ~HokuyoManager(); - jderobot::LaserData *getLaserData(); - //void update(); - private: - - void laserScan2LaserData(hokuyo::LaserScan scan, jderobot::LaserData *data); - hokuyo::Laser laser_; - boost::signals2::mutex mutex; ///< Mutex for thread-safe access to internal data. - double min_ang; - double max_ang; - int clustering; - int timeout; - int faceup; - }; -} - -#endif // HOKUYOMANAGER_H diff --git a/src/drivers/laser_server/laser_server.cfg b/src/drivers/laser_server/laser_server.cfg deleted file mode 100644 index 1ff4cf98a..000000000 --- a/src/drivers/laser_server/laser_server.cfg +++ /dev/null @@ -1,19 +0,0 @@ -Laser.Endpoints=default -h 0.0.0.0 -p 9998:ws -h 0.0.0.0 -p 11004 - -#Specifies laser type, this currently only works with hokuyo -Laser.Model=hokuyo - -#0 corresponds to /dev/ttyACM0, 1 to /dev/ttyACM1, and so on... -Laser.DeviceId=0 - -#Indicates the beginning and end of the capture of the laser in degrees (0 for the front) -Laser.MinAng=-90 -Laser.MaxAng=90 - -Laser.FaceUp=1 - -#Number of adjascent ranges to be clustered into a single measurement. -#0 -> 513 ranges in measurement -#3 -> 171 ranges in measurement -#... -Laser.Clustering=0 diff --git a/src/drivers/laser_server/laseri.cpp b/src/drivers/laser_server/laseri.cpp deleted file mode 100644 index 65dd2eac8..000000000 --- a/src/drivers/laser_server/laseri.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "laseri.h" - -namespace laser -{ - - LaserI::LaserI(Ice::PropertiesPtr prop) - { - std::string model = prop->getProperty("Laser.Model"); - - if ("Hokuyo"==model || "hokuyo"==model){ - std::string deviceId = prop->getProperty("Laser.DeviceId"); - double min = (double)prop->getPropertyAsInt("Laser.MinAng"); - double max = (double)prop->getPropertyAsInt("Laser.MaxAng"); - int clustering = prop->getPropertyAsInt("Laser.Clustering"); - int faceup = prop->getPropertyAsInt("Laser.FaceUp"); - double min_ang = min*M_PI/180; - double max_ang = max*M_PI/180; - this->manager = new hokuyo::HokuyoManager(deviceId, min_ang, max_ang, clustering, -1, faceup); - }else{ - throw std::invalid_argument( model + " laser is not allowed" ); - } - } - - LaserI::~LaserI() - { - } - - jderobot::LaserDataPtr LaserI::getLaserData(const Ice::Current&) - { - jderobot::LaserDataPtr laserData (manager->getLaserData()); - return laserData; - } -} diff --git a/src/drivers/laser_server/laseri.h b/src/drivers/laser_server/laseri.h deleted file mode 100644 index c9090420d..000000000 --- a/src/drivers/laser_server/laseri.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef LASERI_H -#define LASERI_H -#include -#include -#include -#include -#include -#include - -namespace laser -{ - class LaserI: virtual public jderobot::Laser - { - public: - LaserI(Ice::PropertiesPtr prop); - virtual ~LaserI(); - virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&); - private: - LaserManager *manager; - - }; -} - -#endif // LASERI_H - - diff --git a/src/drivers/laser_server/lasermanager.h b/src/drivers/laser_server/lasermanager.h deleted file mode 100644 index ad3968e83..000000000 --- a/src/drivers/laser_server/lasermanager.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef LASERMANAGER_H -#define LASERMANAGER_H - -#include - -class LaserManager -{ -public: - LaserManager(){} - virtual ~LaserManager(){} - virtual jderobot::LaserData* getLaserData()=0; // "=0" part makes this method pure virtual, and - // also makes this class abstract. - //virtual void update()=0; -}; - - -#endif // LASERMANAGER_H diff --git a/src/drivers/laser_server/main.cpp b/src/drivers/laser_server/main.cpp deleted file mode 100644 index 0fcd2ddf0..000000000 --- a/src/drivers/laser_server/main.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include -//#include -#include "easyiceconfig/EasyIce.h" -#include -//#include - -using namespace std; - -int main(int argc, char *argv[]) -{ - Ice::CommunicatorPtr ic; - try { - //-----------------ICE----------------// - ic = EasyIce::initialize(argc, argv); - - Ice::PropertiesPtr prop = ic->getProperties(); - - std::string Endpoints = prop->getProperty("Laser.Endpoints"); - std::string laserControl_string = "Laser"; - - Ice::ObjectAdapterPtr adapter = - ic->createObjectAdapterWithEndpoints(laserControl_string, Endpoints); - Ice::ObjectPtr object = new laser::LaserI(prop); - adapter->add(object, ic->stringToIdentity(laserControl_string)); - adapter->activate(); - std::cout << "laser_server start on " <waitForShutdown(); - } catch (const Ice::Exception& ex) { - std::cerr << ex << std::endl; - exit(-1); - } catch (const char* msg) { - std::cerr << msg << std::endl; - exit(-1); - } - - if (ic) { - try { - ic->destroy(); - } catch (const Ice::Exception& e) { - std::cerr << e << std::endl; - } - } - - - exit(0); -} -/* -#include -#include -#include -//#include -#include "easyiceconfig/EasyIce.h" -#include -#include -#include - -using namespace std; - -//hokuyo::LaserScan scan_; -//hokuyo::Laser laser_; -//hokuyo::LaserConfig laser_config_; - -int main(int argc, char *argv[]) -{ - //cout << "Hello World!" << endl; - //laser_.open("/dev/ttyACM0"); - //std::string product_name=laser_.getProductName(); - //printf("Conectado con %s", product_name.c_str()); - //laser_.close(); - Ice::CommunicatorPtr ic; - std::string devicePortPre="/dev/ttyACM"; - -//y = x*pi/180 - - try { - //-----------------ICE----------------// - ic = Ice::initialize(argc, argv); - - Ice::PropertiesPtr prop = ic->getProperties(); - - std::string Endpoints = prop->getProperty("Laser.Endpoints"); - std::string laserControl_string = "Laser"; - - std::string deviceId = prop->getProperty("Laser.DeviceId"); - double min = (double)prop->getPropertyAsInt("Laser.MinAng"); - double max = (double)prop->getPropertyAsInt("Laser.MaxAng"); - double min_ang = min*M_PI/180; - double max_ang = max*M_PI/180; - - cout << Endpoints << endl; - -cout << "min: " << min_ang << endl; - -cout << "max: " << max_ang << endl; - - hokuyo::HokuyoManager* manager = new hokuyo::HokuyoManager(deviceId, min_ang, max_ang, 0, -1); - - jderobot::LaserData *data = manager->getLaserData(); - - cout << data->numLaser << endl; - - } catch (const Ice::Exception& ex) { - std::cerr << ex << std::endl; - exit(-1); - } catch (const char* msg) { - std::cerr << msg << std::endl; - exit(-1); - } - exit(0); -}*/