Merge pull request #169 from project-march/feature/PM-64-add-imu-node

Feature: add imu node

Author: RutgerVanBeek

march_imu_manager/CMakeLists.txt

@@ -0,0 +1,94 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(march_imu_manager)
+
+add_compile_options(-std=c++11 -Wall -Wextra -g)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    roslint
+    sensor_msgs
+)
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS
+    sensor_msgs
+)
+
+include_directories(
+    include
+    lib/include
+    ${catkin_INCLUDE_DIRS}
+)
+
+file(GLOB_RECURSE lintfiles
+    "src/*.cpp"
+    "src/*.h"
+    "test/*.cpp"
+)
+
+roslint_cpp(${lintfiles})
+
+set(xsensdeviceapi_LOCATION ${PROJECT_SOURCE_DIR}/lib/libxsensdeviceapi.so)
+set(xstypes_LOCATION ${PROJECT_SOURCE_DIR}/lib/libxstypes.so)
+
+find_library(xsensdeviceapi_LIBRARY NAMES xsensdeviceapi PATHS ${PROJECT_SOURCE_DIR}/lib)
+find_library(xstypes_LIBRARY NAMES xstypes PATHS ${PROJECT_SOURCE_DIR}/lib)
+
+add_library(xsensdeviceapi SHARED IMPORTED)
+set_property(TARGET xsensdeviceapi PROPERTY IMPORTED_LOCATION ${xsensdeviceapi_LOCATION})
+
+add_library(xstypes SHARED IMPORTED)
+set_property(TARGET xstypes PROPERTY IMPORTED_LOCATION ${xstypes_LOCATION})
+
+add_library(${PROJECT_NAME}
+    src/Mtw.cpp
+    src/WirelessMaster.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    ${xsensdeviceapi_LIBRARY}
+    ${xstypes_LIBRARY}
+    ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_node src/main.cpp)
+
+target_link_libraries(${PROJECT_NAME}_node
+    ${PROJECT_NAME}
+    ${xsensdeviceapi_LIBRARY}
+    ${xstypes_LIBRARY}
+    ${catkin_LIBRARIES}
+)
+
+install(
+    FILES
+    ${xsensdeviceapi_LOCATION}
+    ${xsensdeviceapi_LOCATION}.4
+    ${xsensdeviceapi_LOCATION}.4.6.0
+    ${xstypes_LOCATION}
+    ${xstypes_LOCATION}.4
+    ${xstypes_LOCATION}.4.6.0
+    DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(TARGETS ${PROJECT_NAME}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(TARGETS ${PROJECT_NAME}_node
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+    catkin_add_gtest(wireless_master_test test/WirelessMasterTest.cpp)
+    target_link_libraries(wireless_master_test
+        ${PROJECT_NAME}
+        ${xsensdeviceapi_LIBRARY}
+        ${xstypes_LIBRARY}
+        ${catkin_LIBRARIES}
+    )
+endif()

march_imu_manager/include/march_imu_manager/Mtw.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <deque>
+#include <mutex>
+
+#include <ros/ros.h>
+
+#include <xsensdeviceapi.h>
+#include <xstypes.h>
+
+class Mtw : public XsCallback
+{
+    public:
+        Mtw(XsDevice* device, size_t maxBufferSize = 100);
+
+        /**
+         * Returns whether any new packets are available.
+         */
+        bool dataAvailable();
+
+        /**
+         * Returns the oldest packet received from the MTw.
+         * Does not delete it.
+         */
+        const XsDataPacket* getOldestPacket();
+
+        /**
+         * Deletes the oldest packet received from the MTw.
+         */
+        void deleteOldestPacket();
+
+        /**
+         * Returns the device id of the MTw. This id can also be found on the
+         * back of the device.
+         */
+        const XsDeviceId getId() const;
+
+    protected:
+        /**
+         * Callback when new packets are available. This runs in a seperate
+         * thread and only stores the packets in the buffer.
+         */
+        virtual void onLiveDataAvailable(XsDevice* device, const XsDataPacket* packet);
+
+    private:
+        /**
+         * Configures the MTw to output orientation, velocity and acceleration.
+         */
+        void configure();
+
+        std::mutex m_mutex;
+
+        XsDevice* m_device;
+
+        size_t m_maxBufferSize;
+        std::deque<XsDataPacket> m_packetBuffer;
+};

march_imu_manager/include/march_imu_manager/WirelessMaster.h

@@ -0,0 +1,95 @@
+#pragma once
+
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <unordered_map>
+
+#include <ros/ros.h>
+
+#include <xsensdeviceapi.h>
+#include <xstypes.h>
+
+#include "march_imu_manager/Mtw.h"
+
+/**
+ * The wireless master class that connects to MTws and publishes the data on
+ * '/march/imu/' ROS topics.
+ */
+class WirelessMaster : public XsCallback
+{
+    public:
+        WirelessMaster(ros::NodeHandle* node);
+        ~WirelessMaster();
+
+        /**
+         * Finds and constructs a wireless master.
+         * This method must be called first before everything else.
+         */
+        int init();
+
+        /**
+         * Configures the wireless master with given settings.
+         * Can only be configured once init() succeeded.
+         *
+         * @param updateRate the desired update rate of the master in Hz.
+         * @param channel the desired radio channel, defaults to 25.
+         * @returns error code, 0 if successfull, -1 otherwise.
+         */
+        int configure(const int updateRate, const int channel = 25);
+
+        /**
+         * Waits for the given amount of MTws to connect.
+         * Blocks the thread until the amount has connected.
+         *
+         * @param connections the amount of connections to wait for, defaults to 1.
+         */
+        void waitForConnections(const size_t connections = 1);
+
+        /**
+         * Starts the measurement of the MTw. This is required in order to
+         * publish anything in the update loop. Once the measurement is started
+         * no MTws will be able to connect.
+         *
+         * @returns true if successfull, false otherwise.
+         */
+        bool startMeasurement();
+
+        /**
+         * Returns whether the MTws are measuring.
+         */
+        bool isMeasuring() const;
+
+        /**
+         * Publishes all data from the MTws. This is supposed to be called in
+         * an update loop.
+         */
+        void update();
+
+        /**
+         * Finds the closest supported update rate to the given desired rate.
+         *
+         * @param supportedUpdateRates rates that are supported by the master.
+         * @param desiredUpdateRate rate that is desired.
+         */
+        static int findClosestUpdateRate(const XsIntArray& supportedUpdateRates, const int desiredUpdateRate);
+
+    protected:
+        /**
+         * Callback for when new MTws connect or disconnect.
+         * Runs in a seperate thread.
+         */
+        virtual void onConnectivityChanged(XsDevice* dev, XsConnectivityState newState);
+
+    private:
+        ros::NodeHandle* m_node;
+
+        std::mutex m_mutex;
+        std::condition_variable m_cv;
+
+        XsControl* m_control = nullptr;
+        XsDevicePtr m_master = nullptr;
+
+        std::unordered_map<uint32_t, std::unique_ptr<Mtw>> m_connectedMtws;
+        std::unordered_map<uint32_t, ros::Publisher> m_publishers;
+};