├── doc
    └── JY901使用说明书V4.3.pdf
├── src
    ├── ros
    │   ├── CMakeLists.txt
    │   └── wit_ros.cpp
    ├── CMakeLists.txt
    ├── test
    │   ├── CMakeLists.txt
    │   └── initialization.cpp
    ├── nodelet
    │   ├── CMakeLists.txt
    │   └── wit_nodelet.cpp
    └── driver
    │   ├── CMakeLists.txt
    │   ├── wit_driver.cpp
    │   ├── command.cpp
    │   ├── packet_finder.cpp
    │   └── data.cpp
├── plugins
    └── nodelet_plugins.xml
├── include
    └── wit_node
    │   ├── parameter.hpp
    │   ├── packet_handler
    │       ├── payload_headers.hpp
    │       ├── packet_finder.hpp
    │       └── payload_base.hpp
    │   ├── wit_ros.hpp
    │   ├── data.hpp
    │   ├── wit_driver.hpp
    │   └── command.hpp
├── launch
    └── wit.launch
├── msg
    └── ImuGpsRaw.msg
├── CMakeLists.txt
├── README.md
└── package.xml


/doc/JY901使用说明书V4.3.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/yowlings/wit_node/HEAD/doc/JY901使用说明书V4.3.pdf


--------------------------------------------------------------------------------
/src/ros/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library(wit_ros wit_ros.cpp)
2 | target_link_libraries(wit_ros wit_driver ${catkin_LIBRARIES} )
3 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_subdirectory(driver)
2 | add_subdirectory(ros)
3 | add_subdirectory(nodelet)
4 | add_subdirectory(test)
5 | 


--------------------------------------------------------------------------------
/src/test/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_executable(initialization initialization.cpp)
2 | target_link_libraries(initialization wit_driver)
3 | 


--------------------------------------------------------------------------------
/src/nodelet/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library(wit_nodelet wit_nodelet.cpp)
2 | add_dependencies(wit_nodelet wit_ros)
3 | target_link_libraries(wit_nodelet wit_ros)
4 | 
5 | 


--------------------------------------------------------------------------------
/src/driver/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | file(GLOB SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp)
2 | add_library(wit_driver ${SOURCES})
3 | target_link_libraries(wit_driver ${catkin_LIBRARIES})
4 | 


--------------------------------------------------------------------------------
/plugins/nodelet_plugins.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/libwit_nodelet">
2 |   <class name="wit_node/WitNodelet" type="wit::WitNodelet" base_class_type="nodelet::Nodelet">
3 |     <description>
4 |       Nodelet version of the wit node
5 |     </description>
6 |   </class>
7 | </library>
8 | 
9 | 


--------------------------------------------------------------------------------
/include/wit_node/parameter.hpp:
--------------------------------------------------------------------------------
 1 | #include <string>
 2 | #ifndef PARAMETER_HPP
 3 | #define PARAMETER_HPP
 4 | using namespace std;
 5 | namespace wit {
 6 | class Parameter {
 7 |  public:
 8 |   Parameter() : port_("/dev/ttyUSB0"), baut_rate_(115200), ns("wit") {}
 9 | 
10 |   string port_;
11 |   int baut_rate_;
12 |   string ns;
13 | };
14 | }
15 | #endif  // PARAMETER_HPP
16 | 


--------------------------------------------------------------------------------
/launch/wit.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <arg name="port"                  default = "/dev/ttyUSB0" />
3 |   <node pkg="nodelet" type="nodelet" output="screen" name="wit_nodelet_manager" args="manager"/>
4 |   <node pkg="nodelet" type="nodelet" output="screen" name="wit" args="load wit_node/WitNodelet wit_nodelet_manager" >
5 |     <param name="port" value="$(arg port)" />
6 |   </node>
7 | </launch>
8 | 


--------------------------------------------------------------------------------
/msg/ImuGpsRaw.msg:
--------------------------------------------------------------------------------
 1 | Header header
 2 | float64 time
 3 | float64[] acc
 4 | float64[] gyro
 5 | float64[] rpy
 6 | float64[] mag
 7 | uint16[]  ps #port state
 8 | float64   temperature
 9 | float64   altitude
10 | float64   ap #atmosphere pressure
11 | float64   latitude
12 | float64   longtitude
13 | float64   gpsh #gps height
14 | float64   gpsy #gps yaw
15 | float64   gpsv #gps velocity
16 | float64[] quarternion
17 | uint8     sn #satelites number
18 | float64[] dop
19 | 
20 | 
21 | 


--------------------------------------------------------------------------------
/src/test/initialization.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include <unistd.h>
 3 | #include <ecl/time.hpp>
 4 | #include <iostream>
 5 | #include "../../include/wit_node/wit_driver.hpp"
 6 | using namespace wit;
 7 | class WitManager {
 8 |  public:
 9 |   WitManager() {
10 |     wit::Parameter param;
11 |     param.port_ = "/dev/ttyUSB0";
12 |     param.ns = "wit";
13 |     param.baut_rate_ = 115200;
14 |     try {
15 |       wd_.init(param);
16 |     } catch (ecl::StandardException &e) {
17 |       std::cout << e.what();
18 |     }
19 |   }
20 | 
21 |  private:
22 |   WitDriver wd_;
23 | };
24 | 
25 | int main() {
26 |   WitManager wm;
27 |   ecl::Sleep()(500);
28 |   return 0;
29 | }
30 | 


--------------------------------------------------------------------------------
/src/nodelet/wit_nodelet.cpp:
--------------------------------------------------------------------------------
 1 | #include <nodelet/nodelet.h>
 2 | #include <pluginlib/class_list_macros.h>
 3 | #include <ecl/threads/thread.hpp>
 4 | #include "../../include/wit_node/wit_ros.hpp"
 5 | 
 6 | namespace wit {
 7 | 
 8 | class WitNodelet : public nodelet::Nodelet {
 9 |  public:
10 |   WitNodelet() : shutdown_requested_(false){};
11 |   ~WitNodelet() {
12 |     NODELET_DEBUG_STREAM("Wit : waiting for update thread to finish.");
13 |     shutdown_requested_ = true;
14 |     update_thread_.join();
15 |   }
16 | 
17 |   virtual void onInit() {
18 |     NODELET_DEBUG_STREAM("Wit: initialising nodelet...");
19 |     string nodelet_name = this->getName();
20 |     wit_.reset(new WitRos(nodelet_name));
21 |     if (wit_->init(this->getPrivateNodeHandle())) {
22 |       update_thread_.start(&WitNodelet::update, *this);
23 |       NODELET_INFO_STREAM("Wit : initialised.");
24 | 
25 |     } else {
26 |       NODELET_ERROR_STREAM("Wit : could not initialise! Please restart.");
27 |     }
28 |   }
29 | 
30 |  private:
31 |   void update() {
32 |     Rate spin_rate(10);
33 |     while (!shutdown_requested_ && ok()) {
34 |       wit_->update();
35 |       spin_rate.sleep();
36 |     }
37 |   }
38 | 
39 |   boost::shared_ptr<WitRos> wit_;
40 |   ecl::Thread update_thread_;
41 |   bool shutdown_requested_;
42 | };
43 | }
44 | PLUGINLIB_EXPORT_CLASS(wit::WitNodelet, nodelet::Nodelet);
45 | 


--------------------------------------------------------------------------------
/include/wit_node/packet_handler/payload_headers.hpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * @file include/xbot_arm_driver/packet_handler/payload_headers.hpp
 3 |  *
 4 |  * @brief Byte id's for the individual payload headers.
 5 |  *
 6 |  * Each part of a xbot packet carries one or more payload chunks. Each chunk
 7 |  * is id'd by one of the values here.
 8 |  *
 9 |  * License: BSD
10 |  *   https://raw.github.com/yujinrobot/xbot_core/hydro-devel/xbot_arm_driver/LICENSE
11 |  **/
12 | /*****************************************************************************
13 | ** Ifdefs
14 | *****************************************************************************/
15 | 
16 | #ifndef XBOT_PAYLOAD_HEADERS_HPP_
17 | #define XBOT_PAYLOAD_HEADERS_HPP_
18 | 
19 | /*****************************************************************************
20 | ** Includes
21 | *****************************************************************************/
22 | 
23 | /*****************************************************************************
24 | ** Namespaces
25 | *****************************************************************************/
26 | 
27 | namespace wit {
28 | 
29 | class Header {
30 |  public:
31 |   enum PayloadType {
32 |     TIME = 0x50,
33 |     ACCE = 0x51,
34 |     GYRO = 0x52,
35 |     YPR = 0x53,
36 |     MAG = 0x54,
37 |     STATE = 0x55,
38 |     PALT = 0x56,
39 |     LOLA = 0x57,
40 |     GPSV = 0x58,
41 |     QUAT = 0x59,
42 |     SATE = 0x5a
43 |   };
44 | };
45 | 
46 | }  // namespace xbot
47 | 
48 | #endif /* XBOT_PAYLOAD_HEADERS_HPP_ */
49 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(wit_node)
 3 | 
 4 | ## Compile as C++11, supported in ROS Kinetic and newer
 5 | add_compile_options(-std=c++11)
 6 | 
 7 | set(CMAKE_BUILD_TYPE "Release")
 8 | set(CMAKE_CXX_STANDARD 11)
 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
10 | add_definitions(-D_GLIBCXX_USE_C99=1)
11 | 
12 | find_package(catkin REQUIRED COMPONENTS
13 |   ecl_build
14 |   ecl_devices
15 |   ecl_sigslots
16 |   ecl_time
17 |   ecl_geometry
18 |   roscpp
19 |   rospy
20 |   sensor_msgs
21 |   std_msgs
22 |   nodelet
23 |   pluginlib
24 |   message_generation
25 | )
26 | 
27 | 
28 | ## Generate messages in the 'msg' folder
29 | add_message_files(
30 |  FILES
31 |  ImuGpsRaw.msg
32 | )
33 | 
34 | ## Generate added messages and services with any dependencies listed here
35 | generate_messages(
36 |  DEPENDENCIES
37 |  std_msgs
38 | )
39 | 
40 | catkin_package(
41 |   INCLUDE_DIRS include
42 |   LIBRARIES wit_nodelet
43 |   CATKIN_DEPENDS
44 |   ecl_build
45 |   ecl_devices
46 |   ecl_geometry
47 |   ecl_sigslots
48 |   ecl_time
49 |   nodelet
50 |   pluginlib
51 |   roscpp
52 |   rospy
53 |   sensor_msgs
54 |   std_msgs
55 | )
56 | 
57 | ###########
58 | ## Build ##
59 | ###########
60 | 
61 | ## Specify additional locations of header files
62 | ## Your package locations should be listed before other locations
63 | include_directories(
64 |  include
65 |   ${catkin_INCLUDE_DIRS}
66 | )
67 | 
68 | add_subdirectory(src)
69 | 
70 | 
71 | 
72 | 
73 | 
74 | 
75 | 
76 | 
77 | 
78 | 
79 | 
80 | 
81 | 
82 | 
83 | 
84 | 
85 | 
86 | 
87 | 
88 | 
89 | 
90 | 


--------------------------------------------------------------------------------
/include/wit_node/wit_ros.hpp:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | #ifndef WIT_ROS_HPP
 4 | #define WIT_ROS_HPP
 5 | 
 6 | #include <geometry_msgs/PointStamped.h>
 7 | #include <ros/ros.h>
 8 | #include <sensor_msgs/Imu.h>
 9 | #include <sensor_msgs/NavSatFix.h>
10 | #include <std_msgs/Empty.h>
11 | #include <std_msgs/Float64.h>
12 | #include <wit_node/ImuGpsRaw.h>
13 | #include <chrono>
14 | #include <cstdlib>
15 | #include <ecl/sigslots.hpp>
16 | #include <ecl/threads.hpp>
17 | #include <fstream>
18 | #include <iostream>
19 | #include "wit_driver.hpp"
20 | 
21 | using namespace std;
22 | using namespace ros;
23 | 
24 | namespace wit {
25 | class WitRos {
26 |  public:
27 |   WitRos(string &node_name);
28 |   ~WitRos();
29 |   bool init(NodeHandle &nh);
30 |   bool update();
31 | 
32 |  private:
33 |   /*********************
34 |    ** Variables
35 |    **********************/
36 |   string name_;
37 |   WitDriver wd_;
38 | 
39 |   /*********************
40 |    ** Ros Publishers
41 |    **********************/
42 |   void advertiseTopics(NodeHandle &nh);
43 |   Publisher imu_pub_;
44 |   Publisher gps_pub_;
45 |   Publisher raw_data_pub_;
46 |   Publisher related_yaw_pub_;
47 | 
48 |   /*********************
49 |    ** Ros Subscribers
50 |    **********************/
51 |   Subscriber reset_offset_sub_;
52 |   void subscribeTopics(NodeHandle &nh);
53 | 
54 |   void subscribeResetOffset(const std_msgs::Empty msg);
55 | 
56 |   /*********************
57 |    ** SigSlots
58 |    **********************/
59 | 
60 |   ecl::Slot<> slot_stream_data_;
61 | 
62 |   /*********************
63 |    ** Slot Callbacks
64 |    **********************/
65 |   void processStreamData();
66 | };
67 | }
68 | 
69 | #endif
70 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # wit_node
  2 | 
  3 | This is the ROS nodelet package for wit motion company imu and gps sensor. Providing driver, ros driver and nodelet intergrating program.
  4 | 
  5 | 
  6 | 
  7 | ## Dependencies and Install
  8 | 
  9 | 1. ROS
 10 | 2. ros-<distro>-ecl
 11 | 
 12 | Install ecl by
 13 | 
 14 | ```bash
 15 | sudo apt install ros-<distro>-ecl
 16 | ```
 17 | 
 18 | 
 19 | 
 20 | ## Usage
 21 | 
 22 | Launch the only ROS launch file:
 23 | 
 24 | ```bash
 25 | roslaunch wit_node wit.launch
 26 | ```
 27 | 
 28 | About parameter:
 29 | 
 30 | -port
 31 | 
 32 | This is the port that device name in Linux system, for example the default port name is "/dev/ttyUSB0"
 33 | 
 34 | 
 35 | 
 36 | ## Msg
 37 | 
 38 | ### ImuGpsRaw
 39 | 
 40 | > Header header
 41 | >
 42 | > float64 time
 43 | >
 44 | > float64[] acc
 45 | >
 46 | > float64[] gyro
 47 | >
 48 | > float64[] rpy
 49 | >
 50 | > float64[] mag
 51 | >
 52 | > uint16[]  ps #port state
 53 | >
 54 | > float64   temperature
 55 | >
 56 | > float64   altitude
 57 | >
 58 | > float64   ap #atmosphere pressure
 59 | >
 60 | > float64   latitude
 61 | >
 62 | > float64   longtitude
 63 | >
 64 | > float64   gpsh #gps height
 65 | >
 66 | > float64   gpsy #gps yaw
 67 | >
 68 | > float64   gpsv #gps velocity
 69 | >
 70 | > float64[] quarternion
 71 | >
 72 | > uint8     sn #satelites number
 73 | >
 74 | > float64[] dop
 75 | 
 76 | ## Published Topics
 77 | 
 78 | ### /imu (sensor_msgs/Imu)
 79 | 
 80 | The standard ROS imu sensor msg which include orientation by filtered RPY.
 81 | 
 82 | ### /gps (sensor_msgs/NavSatFix)
 83 | 
 84 | The standard ROS gps or navigation satellites msg.
 85 | 
 86 | ### /wit/raw_data (wit_node/ImuGpsRaw)
 87 | 
 88 | All raw data provided by the wit device, including nine axises data, atmosphere pressure, temperature, latitude,longitude, altitude, satellites number .etc
 89 | 
 90 | ### /wit/related_yaw (std_msgs/Float64)
 91 | 
 92 | The offseted imu yaw data, which means the zero direction is start direction.
 93 | 
 94 | 
 95 | 
 96 | ## Subscribed Topics
 97 | 
 98 | ### /wit/reset_offset (std_msgs/Empty)
 99 | 
100 | Reset the offset yaw angle to current yaw,  so the zero direction is turned to current direction.
101 | 
102 | 
103 | 
104 | 


--------------------------------------------------------------------------------
/include/wit_node/packet_handler/packet_finder.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PACKET_FINDER_HPP_
 2 | #define PACKET_FINDER_HPP_
 3 | #include <ecl/containers.hpp>
 4 | #include <ecl/sigslots.hpp>
 5 | #include <iomanip>
 6 | 
 7 | namespace wit {
 8 | 
 9 | class PacketFinderBase {
10 |  public:
11 |   typedef ecl::PushAndPop<unsigned char> BufferType;
12 | 
13 |   enum packetFinderState {
14 |     clearBuffer = 0,
15 |     waitingForStx,
16 |     waitingForFlag,
17 |     waitingForPayloadSize,
18 |     waitingForPayloadToEtx,
19 |     waitingForEtx,
20 |   };
21 |   enum packetFinderState state;
22 | 
23 |  protected:
24 |   unsigned int size_stx;
25 |   unsigned int size_etx;
26 |   unsigned int size_length_field;
27 |   bool variable_size_payload;
28 |   unsigned int size_max_payload;
29 |   unsigned int size_payload;
30 |   unsigned int size_checksum_field;
31 | 
32 |   BufferType STX;
33 |   BufferType ETX;
34 |   BufferType buffer;
35 | 
36 |   bool verbose;
37 | 
38 |   ecl::Signal<const std::string &> sig_warn, sig_error;
39 | 
40 |  public:
41 |   PacketFinderBase(); /**< Default constructor. Use with configure(). **/
42 | 
43 |   virtual ~PacketFinderBase(){};
44 | 
45 |   void configure(const std::string &sigslots_namespace,
46 |                  const BufferType &putStx, const BufferType &putEtx,
47 |                  unsigned int sizeLengthField, unsigned int sizeMaxPayload,
48 |                  unsigned int sizeChecksumField, bool variableSizePayload);
49 |   void clear();
50 |   void enableVerbose();
51 |   virtual bool update(const unsigned char *incoming,
52 |                       unsigned int numberOfIncoming);
53 |   virtual bool checkSum();
54 |   unsigned int numberOfDataToRead();
55 |   void getBuffer(BufferType &bufferRef);
56 |   void getPayload(BufferType &bufferRef);
57 | 
58 |  protected:
59 |   bool WaitForStx(const unsigned char datum);
60 |   bool WaitForFlag(const unsigned char datum);
61 |   bool waitForPayloadSize(const unsigned char *incoming,
62 |                           unsigned int numberOfIncoming);
63 |   bool waitForEtx(const unsigned char incoming, bool &foundPacket);
64 |   bool waitForPayloadAndEtx(const unsigned char *incoming,
65 |                             unsigned int numberOfIncoming, bool &foundPacket);
66 | };
67 | };
68 | 
69 | #endif /* PACKET_FINDER_HPP_ */
70 | 


--------------------------------------------------------------------------------
/include/wit_node/data.hpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #ifndef DATA_HPP__
 3 | #define DATA_HPP__
 4 | 
 5 | #include "packet_handler/payload_base.hpp"
 6 | 
 7 | #include <stdint.h>
 8 | 
 9 | namespace wit {
10 | 
11 | /*****************************************************************************
12 | ** Interface
13 | *****************************************************************************/
14 | 
15 | class Data : public packet_handler::payloadBase {
16 |  public:
17 |   Data() : packet_handler::payloadBase(false, 30){};
18 | 
19 |   struct IMUGPS {
20 |     double timestamp;
21 |     double a[3];
22 |     double temperature;
23 |     double w[3];
24 |     double rpy[3];
25 |     double mag[3];
26 |     uint16_t d[4];
27 |     float pressure;
28 |     float altitude;
29 |     float longtitude;
30 |     float latitude;
31 |     float gpsv;
32 |     double gpsh;
33 |     double gpsy;
34 |     double q[4];
35 |     uint16_t satelites;
36 |     double gpsa[3];
37 | 
38 |   } imugps_;
39 | 
40 |   struct Flags {
41 |     static const uint8_t TIME = 0x50;
42 |     static const uint8_t ACCE = 0x51;
43 |     static const uint8_t GYRO = 0x52;
44 |     static const uint8_t RPY = 0x53;
45 |     static const uint8_t MAG = 0x54;
46 |     static const uint8_t STATE = 0x55;
47 |     static const uint8_t PALT = 0x56;
48 |     static const uint8_t LOLA = 0x57;
49 |     static const uint8_t GPSV = 0x58;
50 |     static const uint8_t QUAT = 0x59;
51 |     static const uint8_t SATE = 0x5a;
52 |   };
53 | 
54 |   bool serialise(ecl::PushAndPop<unsigned char> &byteStream);
55 |   bool deserialise(ecl::PushAndPop<unsigned char> &byteStream);
56 |   bool desTime(ecl::PushAndPop<unsigned char> &byteStream);
57 |   bool desAcce(ecl::PushAndPop<unsigned char> &byteStream);
58 |   bool desGyro(ecl::PushAndPop<unsigned char> &byteStream);
59 |   bool desRpy(ecl::PushAndPop<unsigned char> &byteStream);
60 |   bool desMag(ecl::PushAndPop<unsigned char> &byteStream);
61 |   bool desState(ecl::PushAndPop<unsigned char> &byteStream);
62 |   bool desPalt(ecl::PushAndPop<unsigned char> &byteStream);
63 |   bool desLola(ecl::PushAndPop<unsigned char> &byteStream);
64 |   bool desGpsv(ecl::PushAndPop<unsigned char> &byteStream);
65 |   bool desQuat(ecl::PushAndPop<unsigned char> &byteStream);
66 |   bool desSate(ecl::PushAndPop<unsigned char> &byteStream);
67 | 
68 |   void build_special_variable(float &variable,
69 |                               ecl::PushAndPop<unsigned char> &byteStream);
70 | };
71 | }
72 | 
73 | #endif /* DATA_HPP__ */
74 | 


--------------------------------------------------------------------------------
/include/wit_node/wit_driver.hpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #ifndef WIT_DRIVER_HPP_
  3 | #define WIT_DRIVER_HPP_
  4 | 
  5 | #include <chrono>
  6 | #include <cstdlib>
  7 | #include <ecl/devices.hpp>
  8 | #include <ecl/exceptions/standard_exception.hpp>
  9 | #include <ecl/threads.hpp>
 10 | #include <fstream>
 11 | #include <iomanip>
 12 | #include <iostream>
 13 | #include <string>
 14 | //#include "command.hpp"
 15 | #include "data.hpp"
 16 | #include "packet_handler/packet_finder.hpp"
 17 | #include "parameter.hpp"
 18 | 
 19 | using namespace std;
 20 | using namespace ecl;
 21 | namespace wit {
 22 | 
 23 | /*****************************************************************************
 24 | ** Parent Interface
 25 | *****************************************************************************/
 26 | // 串口数据解码程序，使用有限状态机原理
 27 | class PacketFinder : public PacketFinderBase {
 28 |  public:
 29 |   virtual ~PacketFinder() {}
 30 |   //重定义校验函数
 31 |   bool checkSum();
 32 | };
 33 | 
 34 | /*****************************************************************************
 35 |  ** Interface [wit]
 36 |  *****************************************************************************/
 37 | /**
 38 |  * @brief  The core wit driver class.
 39 |  *
 40 |  * This connects to the outside world via sigslots and get accessors.
 41 |  **/
 42 | class WitDriver {
 43 |  public:
 44 |   WitDriver();
 45 |   ~WitDriver();
 46 | 
 47 |   //初始化
 48 |   void init(Parameter &param);
 49 |   //  检查是否连接上通信串口
 50 |   bool isConnected() { return connected_; }
 51 |   //  关闭接口
 52 |   void shutdown() { shutdown_requested_ = true; }
 53 |   bool isShutdown() const { return shutdown_requested_; }
 54 |   double getRelatedYaw() const { return relate_yaw_; }
 55 | 
 56 |   bool isAlive() { return alive_; }
 57 |   void resetYawOffset() { yaw_offset_ = data_.imugps_.rpy[2]; }
 58 |   //底盘数据流控制程序，运行于thread线程中
 59 |   void spin();
 60 | 
 61 |   //  传感器数据锁
 62 |   void lockDataAccess();
 63 |   void unlockDataAccess();
 64 | 
 65 |   /******************************************
 66 |   ** Getters - Raw Data Api
 67 |   *******************************************/
 68 |   /* Be sure to lock/unlock the data access (lockDataAccess and
 69 |    * unlockDataAccess)
 70 |    * around any getXXX calls - see the doxygen notes for lockDataAccess. */
 71 |   //  舵机状态信息
 72 |   Data::IMUGPS getData() const { return data_.imugps_; }
 73 | 
 74 |  private:
 75 |   bool connected_;
 76 |   bool alive_;
 77 | 
 78 |   //读取返回数据线程
 79 |   ecl::Thread thread_spin_;
 80 | 
 81 |   bool shutdown_requested_;
 82 |   /*********************
 83 |   ** Driver Paramters
 84 |   **********************/
 85 |   Parameter param_;
 86 | 
 87 |   /*********************
 88 |   ** Packet Handling
 89 |   **********************/
 90 |   Data data_;
 91 |   double relate_yaw_, yaw_offset_;
 92 | 
 93 |   //  下位机串口与串口buffer
 94 |   ecl::Serial serial;
 95 |   PacketFinder packet_finder;
 96 |   PacketFinder::BufferType data_buffer;
 97 |   ecl::Mutex data_mutex_;
 98 | 
 99 |   /*********************
100 |   ** Signals
101 |   **********************/
102 |   ecl::Signal<> sig_stream_data;
103 | };
104 | }
105 | 
106 | #endif
107 | 


--------------------------------------------------------------------------------
/include/wit_node/command.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * @file include/xbot_arm_driver/command.hpp
  3 |  *
  4 |  * @brief Command structure.
  5 |  *
  6 |  * License: BSD
  7 |  *   https://raw.github.com/yujinrobot/xbot_core/hydro-devel/xbot_arm_driver/LICENSE
  8 |  **/
  9 | /*****************************************************************************
 10 | ** Preprocessor
 11 | *****************************************************************************/
 12 | 
 13 | #ifndef WIT_COMMAND_DATA_HPP__
 14 | #define WIT_COMMAND_DATA_HPP__
 15 | 
 16 | /*****************************************************************************
 17 | ** Includes
 18 | *****************************************************************************/
 19 | 
 20 | #include <ecl/containers.hpp>
 21 | #include "packet_handler/payload_base.hpp"
 22 | 
 23 | /*****************************************************************************
 24 | ** Namespace
 25 | *****************************************************************************/
 26 | 
 27 | //#define GRIP_SPEED 20
 28 | //#define MOTOR_SPEED_BIG 1000
 29 | //#define MOTOR_SPEED_TINY 20
 30 | 
 31 | namespace wit {
 32 | 
 33 | class Command : public packet_handler::payloadBase {
 34 |  public:
 35 |   typedef ecl::PushAndPop<unsigned char> Buffer;
 36 |   typedef ecl::Stencil<Buffer> BufferStencil;
 37 | 
 38 |   /**
 39 |    * These values are used to detect the type of sub-payload that is ensuing.
 40 |    */
 41 |   enum Name {
 42 |     Ping = 1,
 43 |     Read = 2,
 44 |     Write = 3,
 45 |     RegWrite = 4,
 46 |     Action = 5,
 47 |     Reset = 6,
 48 |     LeftArm = 7,
 49 |     RightArm = 8,
 50 |     SycnWrite = 0x83
 51 |   };
 52 | 
 53 |   /**
 54 |    * @brief Data structure containing data for commands.
 55 |    *
 56 |    * It is important to keep this
 57 |    * state as it will have to retain knowledge of the last known command in
 58 |    * some instances - e.g. for gp_out commands, quite often the incoming command
 59 |    * is only to set the output for a single led while keeping the rest of the
 60 |    * current
 61 |    * gp_out values as is.
 62 |    *
 63 |    * For generating individual commands we modify the data here, then copy the
 64 |    * command
 65 |    * class (avoid doing mutexes) and spin it off for sending down to the device.
 66 |    */
 67 |   struct Data {
 68 |     Data()
 69 |         : command(Ping),
 70 |           index(0),
 71 |           location(2047),
 72 |           read_start(0x38),
 73 |           read_length(15) {
 74 |       for (int i = 0; i < 12; i++) {
 75 |         sycn_location[i] = 2047;
 76 |       }
 77 |     }
 78 | 
 79 |     Name command;
 80 |     uint8_t index;
 81 |     uint16_t location;
 82 |     uint8_t read_start;
 83 |     uint8_t read_length;
 84 |     uint16_t sycn_location[12];
 85 |     uint16_t left_arm_location[6];
 86 |     uint16_t right_arm_location[6];
 87 |   };
 88 | 
 89 |   virtual ~Command() {}
 90 | 
 91 |   //  读取某个舵机的ERROR信息
 92 |   static Command setSingleMotorErrorRead(const uint8_t &index);
 93 | 
 94 |   //  控制某个舵机运动
 95 |   static Command setSingleMotorControl(const uint8_t &index,
 96 |                                        const uint16_t &location);
 97 | 
 98 |   //  同步控制12个舵机运动
 99 |   static Command setSycnMotorControl(const uint16_t *sycn_location);
100 | 
101 |   //  同步控制6个左手舵机运动
102 |   static Command setLeftArmControl(const uint16_t *left_arm_location);
103 | 
104 |   //  同步控制6个右手舵机运动
105 |   static Command setRightArmControl(const uint16_t *right_arm_location);
106 | 
107 |   //  读取某个舵机的状态信息
108 |   static Command setSingleMotorStatusRead(const uint8_t &index);
109 | 
110 |   Data data;
111 | 
112 |   void resetBuffer(Buffer &buffer);
113 |   bool serialise(ecl::PushAndPop<unsigned char> &byteStream);
114 |   bool deserialise(ecl::PushAndPop<unsigned char> &byteStream) {
115 |     return true;
116 |   } /**< Unused **/
117 | 
118 |  private:
119 |   static const unsigned char header0;
120 |   static const unsigned char header1;
121 | };
122 | 
123 | }  // namespace xbot
124 | 
125 | #endif /* WIT_COMMAND_DATA_HPP__ */
126 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>wit_node</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The wit_node package is a ROS package for wit motion IMU and GPS sensor, including driver and ros nodelet.</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="yowlings@gmail.com">roc</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>BSD</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |    <url type="website">http://wiki.ros.org/wit_node</url>
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |    <author email="yowlings@gmail.com">roc</author>
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>ecl_devices</build_depend>
53 |   <build_depend>ecl_sigslots</build_depend>
54 |   <build_depend>ecl_time</build_depend>
55 |   <build_depend>ecl_build</build_depend>
56 |   <build_depend>ecl_geometry</build_depend>
57 |   <build_depend>roscpp</build_depend>
58 |   <build_depend>rospy</build_depend>
59 |   <build_depend>sensor_msgs</build_depend>
60 |   <build_depend>std_msgs</build_depend>
61 |   <build_depend>nodelet</build_depend>
62 |   <build_depend>pluginlib</build_depend>
63 |   <build_depend>message_generation</build_depend>
64 | 
65 |   <build_export_depend>ecl</build_export_depend>
66 |   <build_export_depend>roscpp</build_export_depend>
67 |   <build_export_depend>rospy</build_export_depend>
68 |   <build_export_depend>sensor_msgs</build_export_depend>
69 |   <build_export_depend>std_msgs</build_export_depend>
70 |   <exec_depend>ecl_devices</exec_depend>
71 |   <exec_depend>ecl_build</exec_depend>
72 |   <exec_depend>ecl_sigslots</exec_depend>
73 |   <exec_depend>ecl_time</exec_depend>
74 |   <exec_depend>ecl_geometry</exec_depend>
75 |   <exec_depend>nodelet</exec_depend>
76 |   <exec_depend>pluginlib</exec_depend>
77 |   <exec_depend>roscpp</exec_depend>
78 |   <exec_depend>rospy</exec_depend>
79 |   <exec_depend>sensor_msgs</exec_depend>
80 |   <exec_depend>std_msgs</exec_depend>
81 | 
82 | 
83 |   <!-- The export tag contains other, unspecified, tags -->
84 |   <export>
85 |     <!-- Other tools can request additional information be placed here -->
86 |     <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />
87 |   </export>
88 | </package>
89 | 


--------------------------------------------------------------------------------
/src/driver/wit_driver.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include "../../include/wit_node/wit_driver.hpp"
  3 | #include <unistd.h>
  4 | #include <cmath>
  5 | #include <ecl/converters.hpp>
  6 | #include <ecl/geometry/angle.hpp>
  7 | #include <ecl/math.hpp>
  8 | #include <ecl/sigslots.hpp>
  9 | #include <ecl/time/sleep.hpp>
 10 | #include <ecl/time/timestamp.hpp>
 11 | #include <fstream>
 12 | #include <stdexcept>
 13 | 
 14 | namespace wit {
 15 | 
 16 | bool PacketFinder::checkSum() {
 17 |   unsigned int packet_size(buffer.size());
 18 |   unsigned char cs(0);
 19 |   for (unsigned int i = 0; i < packet_size - 1; i++) {
 20 |     cs += buffer[i];
 21 |   }
 22 | 
 23 |   return (cs == buffer[packet_size - 1]) ? true : false;
 24 | }
 25 | 
 26 | WitDriver::WitDriver()
 27 |     : shutdown_requested_(false),
 28 |       connected_(false),
 29 |       alive_(false),
 30 |       yaw_offset_(0.0 / 0.0) {}
 31 | 
 32 | /**
 33 |  * Shutdown the driver - make sure we wait for the thread to finish.
 34 |  */
 35 | WitDriver::~WitDriver() {
 36 |   shutdown_requested_ = true;  // thread's spin() will catch this and terminate
 37 |   thread_spin_.join();
 38 | }
 39 | 
 40 | void WitDriver::init(Parameter &param) {
 41 |   this->param_ = param;
 42 |   std::string ns = param_.ns;
 43 | 
 44 |   // connect signals
 45 |   sig_stream_data.connect(ns + std::string("/stream_data"));
 46 | 
 47 |   try {
 48 |     serial.open(param_.port_, ecl::BaudRate_115200, ecl::DataBits_8,
 49 |                 ecl::StopBits_1, ecl::NoParity);
 50 |     serial.block(4000);  // blocks by default, but just to be clear!
 51 |     connected_ = true;
 52 | 
 53 |   } catch (const ecl::StandardException &e) {
 54 |     connected_ = false;
 55 |     if (e.flag() == ecl::NotFoundError) {
 56 |     } else {
 57 |       throw ecl::StandardException(LOC, e);
 58 |     }
 59 |   }
 60 | 
 61 |   ecl::PushAndPop<unsigned char> stx(1, 0);
 62 |   ecl::PushAndPop<unsigned char> etx(1);
 63 |   stx.push_back(0x55);
 64 |   packet_finder.configure(ns, stx, etx, 1, 256, 1, true);
 65 |   thread_spin_.start(&WitDriver::spin, *this);
 66 | }
 67 | 
 68 | void WitDriver::lockDataAccess() { data_mutex_.lock(); }
 69 | 
 70 | /**
 71 |  * Unlock a previously locked data access privilege.
 72 |  * @sa lockDataAccess()
 73 |  */
 74 | void WitDriver::unlockDataAccess() { data_mutex_.unlock(); }
 75 | 
 76 | /**
 77 |  * @brief Performs a scan looking for incoming data packets.
 78 |  *
 79 |  * Sits on the device waiting for incoming and then parses it, and signals
 80 |  * that an update has occured.
 81 |  *
 82 |  * Or, if in simulation, just loopsback the motor devices.
 83 |  */
 84 | 
 85 | void WitDriver::spin() {
 86 |   ecl::TimeStamp last_signal_time;
 87 |   ecl::Duration timeout(0.1);
 88 |   unsigned char buf[256];
 89 | 
 90 |   while (!shutdown_requested_) {
 91 |     /*********************
 92 |      ** Checking Connection
 93 |      **********************/
 94 |     if (!serial.open()) {
 95 |       try {
 96 |         serial.open(param_.port_, ecl::BaudRate_115200, ecl::DataBits_8,
 97 |                     ecl::StopBits_1, ecl::NoParity);
 98 |         serial.block(4000);  // blocks by default, but just to be clear!
 99 |         connected_ = true;
100 |       } catch (const ecl::StandardException &e) {
101 |         connected_ = false;
102 |         alive_ = false;
103 |         // windows throws OpenError if not connected
104 |         if (e.flag() == ecl::NotFoundError) {
105 |           std::cout
106 |               << "device does not (yet) available on this port, waiting..."
107 |               << std::endl;
108 |         } else if (e.flag() == ecl::OpenError) {
109 |           std::cout << "device failed to open, waiting... ["
110 |                     << std::string(e.what()) << "]" << std::endl;
111 |           ;
112 |         } else {
113 |           throw ecl::StandardException(LOC, e);
114 |         }
115 |         ecl::Sleep(5)();  // five seconds
116 |         continue;
117 |       }
118 |     }
119 | 
120 |     /*********************
121 |      ** Read Incoming
122 |      **********************/
123 |     int n = serial.read((char *)buf, 1);
124 |     //    std::cout << int(buf[0]) << std::endl;
125 |     if (n == 0) {
126 |       continue;
127 |     }
128 |     bool find_packet = packet_finder.update(buf, n);
129 | 
130 |     if (find_packet) {
131 |       PacketFinder::BufferType local_buffer;
132 |       packet_finder.getBuffer(local_buffer);
133 |       packet_finder.getPayload(data_buffer);
134 |       lockDataAccess();
135 |       unsigned short flag = data_buffer[0];
136 |       while (data_buffer.size() > 0) {
137 |         data_.deserialise(data_buffer);
138 |         if (flag == Data::Flags::RPY) {
139 |           if (isnan(yaw_offset_)) yaw_offset_ = data_.imugps_.rpy[2];
140 |           relate_yaw_ = data_.imugps_.rpy[2] - yaw_offset_;
141 |         }
142 |       }
143 |       unlockDataAccess();
144 |       alive_ = true;
145 |       if (flag == Data::Flags::ACCE) {
146 |         sig_stream_data.emit();
147 |       }
148 |       last_signal_time.stamp();
149 | 
150 |     } else {
151 |       if (alive_ && ((ecl::TimeStamp() - last_signal_time) > timeout)) {
152 |         alive_ = false;
153 |       }
154 |     }
155 |   }
156 | }
157 | 
158 | }  // namespace WitDriver
159 | 


--------------------------------------------------------------------------------
/src/ros/wit_ros.cpp:
--------------------------------------------------------------------------------
  1 | #include "../../include/wit_node/wit_ros.hpp"
  2 | #include <float.h>
  3 | #include <tf/tf.h>
  4 | #include <ecl/streams/string_stream.hpp>
  5 | #include <string>
  6 | 
  7 | namespace wit {
  8 | 
  9 | WitRos::WitRos(string &node_name)
 10 |     : name_(node_name), slot_stream_data_(&WitRos::processStreamData, *this) {}
 11 | 
 12 | WitRos::~WitRos() {
 13 |   ROS_INFO_STREAM("Wit: waiting for wd_ thread finish[" << name_ << " ].");
 14 | }
 15 | 
 16 | bool WitRos::init(NodeHandle &nh) {
 17 |   advertiseTopics(nh);
 18 |   subscribeTopics(nh);
 19 |   slot_stream_data_.connect(name_ + string("/stream_data"));
 20 | 
 21 |   Parameter parameter;
 22 | 
 23 |   parameter.ns = name_;
 24 | 
 25 |   if (!nh.getParam("port", parameter.port_)) {
 26 |     ROS_ERROR_STREAM(
 27 |         "Wit : no wit device port given on the parameter server (e.g. "
 28 |         "/dev/ttyUSB0)["
 29 |         << name_ << "].");
 30 |     return false;
 31 |   }
 32 | 
 33 |   /*********************
 34 |    ** Driver Init
 35 |    **********************/
 36 |   try {
 37 |     wd_.init(parameter);
 38 |     ros::Duration(0.1).sleep();
 39 |   } catch (const ecl::StandardException &e) {
 40 |     switch (e.flag()) {
 41 |       case (ecl::OpenError): {
 42 |         ROS_ERROR_STREAM("Wit : could not open connection ["
 43 |                          << parameter.port_ << "][" << name_ << "].");
 44 |         break;
 45 |       }
 46 |       default: {
 47 |         ROS_ERROR_STREAM("Wit : initialisation failed [" << name_ << "].");
 48 |         ROS_DEBUG_STREAM(e.what());
 49 |         break;
 50 |       }
 51 |     }
 52 |     return false;
 53 |   }
 54 | 
 55 |   return true;
 56 | }
 57 | 
 58 | bool WitRos::update() {
 59 |   if (wd_.isShutdown()) {
 60 |     ROS_ERROR_STREAM("Wit : Driver has been shutdown. Stopping update loop. ["
 61 |                      << name_ << "].");
 62 |     return false;
 63 |   }
 64 |   if (!wd_.isConnected()) {
 65 |     ROS_ERROR_STREAM(
 66 |         "Wit : arm serial port is not connetced, please connect the arm.");
 67 |     return false;
 68 |   }
 69 |   processStreamData();
 70 |   return true;
 71 | }
 72 | 
 73 | void WitRos::advertiseTopics(NodeHandle &nh) {
 74 |   imu_pub_ = nh.advertise<sensor_msgs::Imu>("/imu", 1000);
 75 |   gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
 76 |   raw_data_pub_ = nh.advertise<wit_node::ImuGpsRaw>("raw_data", 1000);
 77 |   related_yaw_pub_ = nh.advertise<std_msgs::Float64>("related_yaw", 1000);
 78 | }
 79 | 
 80 | void WitRos::subscribeTopics(NodeHandle &nh) {
 81 |   reset_offset_sub_ = nh.subscribe(string("reset_offset"), 10,
 82 |                                    &WitRos::subscribeResetOffset, this);
 83 | }
 84 | void WitRos::subscribeResetOffset(const std_msgs::Empty msg) {
 85 |   wd_.resetYawOffset();
 86 | }
 87 | 
 88 | void WitRos::processStreamData() {
 89 |   if (ok()) {
 90 |     sensor_msgs::Imu imu_msg;
 91 |     sensor_msgs::NavSatFix gps_msg;
 92 |     wit_node::ImuGpsRaw raw_msg;
 93 |     std_msgs::Float64 yaw_msg;
 94 |     imu_msg.header.frame_id = "imu_link";
 95 |     imu_msg.header.stamp = ros::Time::now();
 96 |     gps_msg.header.stamp = ros::Time::now();
 97 |     raw_msg.header.stamp = ros::Time::now();
 98 |     Data::IMUGPS data = wd_.getData();
 99 |     imu_msg.angular_velocity.x = data.w[0];
100 |     imu_msg.angular_velocity.y = data.w[1];
101 |     imu_msg.angular_velocity.z = data.w[2];
102 | 
103 |     imu_msg.linear_acceleration.x = data.a[0];
104 |     imu_msg.linear_acceleration.y = data.a[1];
105 |     imu_msg.linear_acceleration.z = data.a[2];
106 | 
107 |     imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(
108 |         data.rpy[0], data.rpy[1], data.rpy[2]);
109 | 
110 |     imu_msg.orientation_covariance =
111 |     {0.001, 0.0,    0.0,
112 |      0.0,   0.001,  0.0,
113 |      0.0,   0.0,    0.001};
114 |     imu_msg.angular_velocity_covariance =
115 |     {0.00001, 0.0,      0.0,
116 |      0.0,     0.00001,  0.0,
117 |      0.0,     0.0,      0.00001};
118 |     imu_msg.linear_acceleration_covariance =
119 |     {0.01,  0.0,  0.0,
120 |      0.0,   0.01, 0.0,
121 |      0.0,   0.0,  0.01};
122 | 
123 |     gps_msg.altitude = data.altitude;
124 |     gps_msg.latitude = data.latitude;
125 |     gps_msg.longitude = data.longtitude;
126 |     gps_msg.position_covariance_type =
127 |         sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
128 |     for (int i = 0; i < 3; i++) {
129 |       raw_msg.acc.push_back(data.a[i]);
130 |       raw_msg.gyro.push_back(data.w[i]);
131 |       raw_msg.rpy.push_back(data.rpy[i]);
132 |       raw_msg.mag.push_back(data.mag[i]);
133 |       raw_msg.dop.push_back(data.gpsa[i]);
134 |     }
135 |     for (int i = 0; i < 4; i++) {
136 |       raw_msg.ps.push_back(data.d[i]);
137 |       raw_msg.quarternion.push_back(data.q[i]);
138 |     }
139 |     raw_msg.sn = data.satelites;
140 |     raw_msg.gpsh = data.gpsh;
141 |     raw_msg.gpsy = data.gpsy;
142 |     raw_msg.gpsv = data.gpsv;
143 |     raw_msg.ap = data.pressure;
144 |     raw_msg.longtitude = data.longtitude;
145 |     raw_msg.altitude = data.altitude;
146 |     raw_msg.latitude = data.latitude;
147 |     raw_msg.time = data.timestamp;
148 |     raw_msg.temperature = data.temperature;
149 | 
150 |     yaw_msg.data = wd_.getRelatedYaw();
151 | 
152 |     imu_pub_.publish(imu_msg);
153 |     gps_pub_.publish(gps_msg);
154 |     raw_data_pub_.publish(raw_msg);
155 |     related_yaw_pub_.publish(yaw_msg);
156 |   }
157 | }
158 | }
159 | 


--------------------------------------------------------------------------------
/include/wit_node/packet_handler/payload_base.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * @file include/xbot_arm_driver/packet_handler/payload_base.hpp
  3 |  *
  4 |  * @brief Base class for payloads.
  5 |  *
  6 |  * License: BSD
  7 |  *   https://raw.github.com/yujinrobot/xbot_core/hydro-devel/xbot_arm_driver/LICENSE
  8 |  **/
  9 | /*****************************************************************************
 10 |  ** Ifdefs
 11 |  *****************************************************************************/
 12 | 
 13 | #ifndef ROBOT_DATA_HPP_
 14 | #define ROBOT_DATA_HPP_
 15 | 
 16 | /*****************************************************************************
 17 |  ** Includes
 18 |  *****************************************************************************/
 19 | 
 20 | #include <ecl/containers.hpp>
 21 | #include <stdint.h>
 22 | 
 23 | /*****************************************************************************
 24 |  ** Namespaces
 25 |  *****************************************************************************/
 26 | 
 27 | namespace packet_handler
 28 | {
 29 | 
 30 | /*****************************************************************************
 31 |  ** Interface
 32 |  *****************************************************************************/
 33 | /**
 34 |  * @brief
 35 |  * Provides base class for payloads.
 36 |  *
 37 |  */
 38 | class payloadBase
 39 | {
 40 | public:
 41 | 
 42 |   /**
 43 |    * this is simple magic to write the flag, when we get the packet from the host or
 44 |    * when we want to send the data
 45 |    */
 46 |   bool yes;
 47 | 
 48 |   /**
 49 |    * it indicates the type of derived packet. if packet type is dynamic, length of
 50 |    * packet can be changed during communication session. Ohterwise can not.
 51 |    */
 52 |   const bool is_dynamic;
 53 | 
 54 |   /**
 55 |    * it indicates length of data part of packet, except header and length field.
 56 |    * if packet is fixed type, this value should be matched with length field.
 57 |    * if packet is dynamic type, this value indicates minimal value of length field.
 58 |    */
 59 |   const unsigned char length;
 60 | 
 61 |   /*
 62 |    * construct and destruct
 63 |    */
 64 |   payloadBase(const bool is_dynamic_ = false, const unsigned char length_ = 0 )
 65 |     : yes(false)
 66 |     , is_dynamic(is_dynamic_)
 67 |     , length(length_)
 68 |   {};
 69 |   virtual ~payloadBase() {};
 70 | 
 71 |   /*
 72 |    * serialisation
 73 |    */
 74 |   virtual bool serialise(ecl::PushAndPop<unsigned char> & byteStream)=0;
 75 |   virtual bool deserialise(ecl::PushAndPop<unsigned char> & byteStream)=0;
 76 | 
 77 |   // utilities
 78 |   // todo; let's put more useful converters here. Or we may use generic converters
 79 | protected:
 80 |   // below funciton should be replaced wiht converter
 81 |   //###########################################################
 82 |   //#数据在计算机内存中存储是高位在前，低位在后，int型10存储为：00000000000000000000000000001010
 83 |   //#数据在串口传输过程中是低位在前，高位在后,因而10存储为00001010000000000000000000000000
 84 |   //###########################################################
 85 |   template<typename T>
 86 |     void buildVariable(T & V, ecl::PushAndPop<unsigned char> & buffer)
 87 |     {
 88 |       if (buffer.size() < sizeof(T))
 89 |         return;
 90 |       V = static_cast<unsigned char>(buffer.pop_front());
 91 | 
 92 |       unsigned int size_value(sizeof(T));
 93 |       for (unsigned int i = 1; i < size_value; i++)
 94 |       {
 95 |         V |= ((static_cast<unsigned char>(buffer.pop_front())) << (8 * i));
 96 |       }
 97 |     }
 98 | 
 99 |   template<typename T>
100 |     void buildBytes(const T & V, ecl::PushAndPop<unsigned char> & buffer)
101 |     {
102 |       unsigned int size_value(sizeof(T));
103 |       for (unsigned int i = 0; i < size_value; i++)
104 |       {
105 |         buffer.push_back(static_cast<unsigned char>((V >> (i * 8)) & 0xff));
106 |       }
107 |     }
108 | };
109 | 
110 | /**
111 |  * Need to be very careful with this - it will only work across platforms if they
112 |  * happen to be doing reinterpret_cast with the same float standard.
113 |  * @param V
114 |  * @param buffer
115 |  */
116 | template<>
117 | inline   void payloadBase::buildVariable<float>(float & V, ecl::PushAndPop<unsigned char> & buffer)
118 |   {
119 |     if (buffer.size() < 4)
120 |       return;
121 |     unsigned int ui;
122 |     ui = static_cast<unsigned char>(buffer.pop_front());
123 | 
124 |     unsigned int size_value(4);
125 |     for (unsigned int i = 1; i < size_value; i++)
126 |     {
127 |       ui |= ((static_cast<unsigned char>(buffer.pop_front())) << (8 * i));
128 |     }
129 | 
130 |     V = reinterpret_cast<float&>(ui);
131 |   }
132 | 
133 | template<>
134 | inline void payloadBase::buildBytes<float>(const float & V, ecl::PushAndPop<unsigned char> & buffer)
135 |   {
136 |     if (buffer.size() < 4)
137 |       return;
138 |     unsigned int size_value(4);
139 |     unsigned int ui(reinterpret_cast<const unsigned int&>(V));
140 |     for (unsigned int i = 0; i < size_value; i++)
141 |     {
142 |       buffer.push_back(static_cast<unsigned char>((ui >> (i * 8)) & 0xff));
143 |     }
144 |   }
145 | 
146 | //#define FRAC_MAX 9223372036854775807LL /* 2**63 - 1 */
147 | //
148 | //struct dbl_packed
149 | //{
150 | //    int exp;
151 | //    long long frac;
152 | //};
153 | //
154 | //void pack(double x, struct dbl_packed *r)
155 | //{
156 | //    double xf = fabs(frexp(x, &r->exp)) - 0.5;
157 | //
158 | //    if (xf < 0.0)
159 | //    {
160 | //        r->frac = 0;
161 | //        return;
162 | //    }
163 | //
164 | //    r->frac = 1 + (long long)(xf * 2.0 * (FRAC_MAX - 1));
165 | //
166 | //    if (x < 0.0)
167 | //        r->frac = -r->frac;
168 | //}
169 | //
170 | //double unpack(const struct dbl_packed *p)
171 | //{
172 | //    double xf, x;
173 | //
174 | //    if (p->frac == 0)
175 | //        return 0.0;
176 | //
177 | //    xf = ((double)(llabs(p->frac) - 1) / (FRAC_MAX - 1)) / 2.0;
178 | //
179 | //    x = ldexp(xf + 0.5, p->exp);
180 | //
181 | //    if (p->frac < 0)
182 | //        x = -x;
183 | //
184 | //    return x;
185 | //}
186 | 
187 | }
188 | ;
189 | // namespace packet_handler
190 | 
191 | #endif /* ROBOT_DATA_HPP_ */
192 | 


--------------------------------------------------------------------------------
/src/driver/command.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include "../../include/wit_node/command.hpp"
  3 | 
  4 | namespace wit {
  5 | 
  6 | const unsigned char Command::header0 = 0xFF;
  7 | const unsigned char Command::header1 = 0xFF;
  8 | const uint16_t ALL_SPEED[14] = {10,   1000, 1000, 1000, 1000, 20, 20,
  9 |                                 1000, 1000, 1000, 1000, 20,   20, 10};
 10 | 
 11 | //读取单个舵机的错误状态信息指令发送
 12 | Command Command::setSingleMotorErrorRead(const uint8_t &index) {
 13 |   Command outgoing;
 14 |   outgoing.data.index = index;
 15 |   outgoing.data.command = Command::Ping;
 16 |   return outgoing;
 17 | }
 18 | 
 19 | //控制单个舵机指令发送
 20 | Command Command::setSingleMotorControl(const uint8_t &index,
 21 |                                        const uint16_t &location) {
 22 |   Command outgoing;
 23 |   outgoing.data.index = index;
 24 |   outgoing.data.location = location;
 25 |   outgoing.data.command = Command::Write;
 26 |   return outgoing;
 27 | }
 28 | 
 29 | //读取单个舵机的所有状态信息指令发送
 30 | Command Command::setSingleMotorStatusRead(const uint8_t &index) {
 31 |   Command outgoing;
 32 |   outgoing.data.index = index;
 33 |   outgoing.data.read_start = 0x38;
 34 |   outgoing.data.read_length = 15;
 35 |   outgoing.data.command = Command::Read;
 36 |   return outgoing;
 37 | }
 38 | 
 39 | //编号为1~12的手臂关节舵机控制
 40 | Command Command::setSycnMotorControl(const uint16_t *sycn_location) {
 41 |   Command outgoing;
 42 |   outgoing.data.index = 0xFE;
 43 |   for (int i = 0; i < 12; i++) {
 44 |     outgoing.data.sycn_location[i] = sycn_location[i];
 45 |   }
 46 |   outgoing.data.command = Command::SycnWrite;
 47 |   return outgoing;
 48 | }
 49 | 
 50 | //编号为7~12的左手关节舵机位置控制
 51 | Command Command::setLeftArmControl(const uint16_t *left_arm_location) {
 52 |   Command outgoing;
 53 |   outgoing.data.index = 0xFE;
 54 |   for (int i = 0; i < 6; i++) {
 55 |     outgoing.data.left_arm_location[i] = left_arm_location[i];
 56 |   }
 57 |   outgoing.data.command = Command::LeftArm;
 58 |   return outgoing;
 59 | }
 60 | 
 61 | //编号为1~6的右手手臂关节舵机控制
 62 | Command Command::setRightArmControl(const uint16_t *right_arm_location) {
 63 |   Command outgoing;
 64 |   outgoing.data.index = 0xFE;
 65 |   for (int i = 0; i < 6; i++) {
 66 |     outgoing.data.right_arm_location[i] = right_arm_location[i];
 67 |   }
 68 |   outgoing.data.command = Command::RightArm;
 69 |   return outgoing;
 70 | }
 71 | 
 72 | /*****************************************************************************
 73 | ** Implementation [Serialisation]
 74 | *****************************************************************************/
 75 | /**
 76 |  * 重置缓存区
 77 |  */
 78 | void Command::resetBuffer(Buffer &buffer) {
 79 |   buffer.clear();
 80 |   buffer.resize(256);
 81 |   buffer.push_back(Command::header0);
 82 |   buffer.push_back(Command::header1);
 83 | }
 84 | 
 85 | bool Command::serialise(ecl::PushAndPop<unsigned char> &byteStream) {
 86 |   // need to be sure we don't pass through an emum to the Trans'd buildBytes
 87 |   // functions.
 88 |   unsigned char cmd = static_cast<unsigned char>(data.command);
 89 |   uint8_t size_byte;
 90 |   uint8_t index;
 91 |   //  uint16_t speed;
 92 |   uint16_t time;
 93 |   uint8_t address;
 94 |   uint8_t label1;
 95 |   uint8_t label2;
 96 |   uint8_t sycn_cmd(0x83);
 97 |   switch (data.command) {
 98 |     case Ping:
 99 |       buildBytes(data.index, byteStream);
100 |       size_byte = 0x02;
101 |       buildBytes(size_byte, byteStream);
102 |       buildBytes(cmd, byteStream);
103 |       break;
104 |     case Read:
105 |       buildBytes(data.index, byteStream);
106 |       size_byte = 0x04;
107 |       buildBytes(size_byte, byteStream);
108 |       buildBytes(cmd, byteStream);
109 |       buildBytes(data.read_start, byteStream);
110 |       buildBytes(data.read_length, byteStream);
111 |       break;
112 |     case Write:
113 |       buildBytes(data.index, byteStream);
114 |       size_byte = 0x09;
115 |       buildBytes(size_byte, byteStream);
116 |       buildBytes(cmd, byteStream);
117 |       address = 0x2A;
118 |       buildBytes(address, byteStream);
119 |       buildBytes(data.location, byteStream);
120 |       time = 0;
121 |       buildBytes(time, byteStream);
122 |       buildBytes(ALL_SPEED[data.index], byteStream);
123 |       break;
124 |     case RegWrite:
125 |       buildBytes(cmd, byteStream);
126 |       break;
127 |     case Action:
128 |       index = 0xFE;
129 |       buildBytes(index, byteStream);
130 |       size_byte = 0x02;
131 |       buildBytes(size_byte, byteStream);
132 |       buildBytes(cmd, byteStream);
133 |       break;
134 |     case SycnWrite:
135 |       buildBytes(data.index, byteStream);
136 |       size_byte = 0x58;
137 |       buildBytes(size_byte, byteStream);
138 |       buildBytes(cmd, byteStream);
139 |       label1 = 0x2A;
140 |       buildBytes(label1, byteStream);
141 |       label2 = 0x06;
142 |       buildBytes(label2, byteStream);
143 |       for (uint8_t i = 1; i < 13; i++) {
144 |         buildBytes(i, byteStream);
145 |         buildBytes(data.sycn_location[i - 1], byteStream);
146 |         time = 0;
147 |         buildBytes(time, byteStream);
148 |         buildBytes(ALL_SPEED[i], byteStream);
149 |       }
150 |       break;
151 |     case LeftArm:
152 |       buildBytes(data.index, byteStream);
153 |       size_byte = 0x2E;
154 |       buildBytes(size_byte, byteStream);
155 |       buildBytes(sycn_cmd, byteStream);
156 |       label1 = 0x2A;
157 |       buildBytes(label1, byteStream);
158 |       label2 = 0x06;
159 |       buildBytes(label2, byteStream);
160 |       for (uint8_t i = 7; i < 13; i++) {
161 |         buildBytes(i, byteStream);
162 |         buildBytes(data.left_arm_location[i - 7], byteStream);
163 |         time = 0;
164 |         buildBytes(time, byteStream);
165 |         buildBytes(ALL_SPEED[i], byteStream);
166 |       }
167 |       break;
168 |     case RightArm:
169 |       buildBytes(data.index, byteStream);
170 |       size_byte = 0x2E;
171 |       buildBytes(size_byte, byteStream);
172 |       buildBytes(sycn_cmd, byteStream);
173 |       label1 = 0x2A;
174 |       buildBytes(label1, byteStream);
175 |       label2 = 0x06;
176 |       buildBytes(label2, byteStream);
177 |       for (uint8_t i = 1; i < 7; i++) {
178 |         buildBytes(i, byteStream);
179 |         buildBytes(data.right_arm_location[i - 1], byteStream);
180 | 
181 |         time = 0;
182 |         buildBytes(time, byteStream);
183 |         buildBytes(ALL_SPEED[i], byteStream);
184 |       }
185 |       //    std::cout<<byteStream.size()<<std::endl;
186 |       break;
187 |     default:
188 |       return false;
189 |       break;
190 |   }
191 |   return true;
192 | }
193 | 
194 | }  // namespace xbot
195 | 


--------------------------------------------------------------------------------
/src/driver/packet_finder.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * @file /xbot_arm_driver/src/driver/packet_finder.cpp
  3 |  *
  4 |  * @brief Packet handling implementation.
  5 |  *
  6 |  * License: BSD
  7 |  *   https://raw.github.com/yujinrobot/xbot_core/hydro-devel/xbot_arm_driver/LICENSE
  8 |  **/
  9 | 
 10 | /*****************************************************************************
 11 | ** Includes
 12 | *****************************************************************************/
 13 | 
 14 | #include "../../include/wit_node/packet_handler/packet_finder.hpp"
 15 | #include <fstream>
 16 | #include <sstream>
 17 | using namespace std;
 18 | /*****************************************************************************
 19 | ** Namespaces
 20 | *****************************************************************************/
 21 | 
 22 | namespace wit {
 23 | 
 24 | /*****************************************************************************
 25 | ** Implementation
 26 | *****************************************************************************/
 27 | 
 28 | PacketFinderBase::PacketFinderBase() : state(waitingForStx), verbose(false) {}
 29 | 
 30 | /*****************************************************************************
 31 | ** Public
 32 | *****************************************************************************/
 33 | 
 34 | void PacketFinderBase::configure(const std::string &sigslots_namespace,
 35 |                                  const BufferType &putStx,
 36 |                                  const BufferType &putEtx,
 37 |                                  unsigned int sizeLengthField,
 38 |                                  unsigned int sizeMaxPayload,
 39 |                                  unsigned int sizeChecksumField,
 40 |                                  bool variableSizePayload) {
 41 |   size_stx = putStx.size();
 42 |   size_etx = putEtx.size();
 43 |   size_length_field = sizeLengthField;
 44 |   variable_size_payload = variableSizePayload;
 45 |   size_max_payload = sizeMaxPayload;
 46 |   size_payload = variable_size_payload ? 0 : sizeMaxPayload;
 47 |   size_checksum_field = sizeChecksumField;
 48 |   STX = putStx;
 49 |   ETX = putEtx;
 50 |   buffer = BufferType(size_stx + size_length_field + size_max_payload +
 51 |                       size_checksum_field + size_etx);
 52 |   state = waitingForStx;
 53 | 
 54 |   sig_warn.connect(sigslots_namespace + std::string("/ros_warn"));
 55 |   sig_error.connect(sigslots_namespace + std::string("/ros_error"));
 56 | 
 57 |   // todo; exception
 58 |   // Problem1: size_length_field = 1, vairable_size_payload = false
 59 | 
 60 |   clear();
 61 | }
 62 | 
 63 | void PacketFinderBase::clear() {
 64 |   state = waitingForStx;
 65 |   buffer.clear();
 66 | }
 67 | 
 68 | void PacketFinderBase::enableVerbose() { verbose = true; }
 69 | 
 70 | bool PacketFinderBase::checkSum() { return true; }
 71 | 
 72 | unsigned int PacketFinderBase::numberOfDataToRead() {
 73 |   unsigned int num(0);
 74 | 
 75 |   switch (state) {
 76 |     case waitingForEtx:
 77 |       num = 1;
 78 |       break;
 79 | 
 80 |     case waitingForPayloadToEtx:
 81 |       num = size_payload + size_etx + size_checksum_field;
 82 |       break;
 83 | 
 84 |     case waitingForPayloadSize:
 85 |       num = size_checksum_field;
 86 |       break;
 87 | 
 88 |     case waitingForStx:
 89 |     case clearBuffer:
 90 |     default:
 91 |       num = 1;
 92 |       break;
 93 |   }
 94 | 
 95 |   if (verbose) {
 96 |     printf("[state(%d):%02d]", state, num);
 97 |   }
 98 |   return num;
 99 | }
100 | 
101 | void PacketFinderBase::getBuffer(BufferType &bufferRef) { bufferRef = buffer; }
102 | 
103 | void PacketFinderBase::getPayload(BufferType &bufferRef) {
104 |   bufferRef.clear();
105 | 
106 |   bufferRef.resize(buffer.size() - size_stx - size_checksum_field);
107 |   for (unsigned int i = size_stx; i < buffer.size() - size_checksum_field;
108 |        i++) {
109 |     bufferRef.push_back(buffer[i]);
110 |   }
111 | }
112 | 
113 | /**
114 |  * Checks for incoming packets.
115 |  *
116 |  * @param incoming
117 |  * @param numberOfIncoming
118 |  * @return bool : true if a valid incoming packet has been found.
119 |  */
120 | bool PacketFinderBase::update(const unsigned char *incoming,
121 |                               unsigned int numberOfIncoming) {
122 |   // clearBuffer = 0, waitingForStx, waitingForPayloadSize,
123 |   // waitingForPayloadToEtx, waitingForEtx,
124 |   // std::cout << "update [" << numberOfIncoming << "][" << state << "]" <<
125 |   // std::endl;
126 |   if (!(numberOfIncoming > 0)) return false;
127 | 
128 |   bool found_packet(false);
129 | 
130 |   if (state == clearBuffer) {
131 |     buffer.clear();
132 |     state = waitingForStx;
133 |   }
134 |   switch (state) {
135 |     case waitingForStx:
136 |       if (WaitForStx(incoming[0])) {
137 |         state = waitingForFlag;
138 |       }
139 |       break;
140 |     case waitingForFlag:
141 |       if (WaitForFlag(incoming[0])) {
142 |         size_payload = 8;
143 |         state = waitingForPayloadToEtx;
144 |       }
145 | 
146 |       break;
147 |     case waitingForPayloadToEtx:
148 |       if (waitForPayloadAndEtx(incoming, numberOfIncoming, found_packet)) {
149 |         state = clearBuffer;
150 |       }
151 |       break;
152 | 
153 |     default:
154 |       state = waitingForStx;
155 |       break;
156 |   }
157 |   if (found_packet) {
158 |     return checkSum();
159 |   } else {
160 |     return false;
161 |   }
162 | }
163 | /*****************************************************************************
164 | ** Protected
165 | *****************************************************************************/
166 | 
167 | bool PacketFinderBase::WaitForStx(const unsigned char datum) {
168 |   bool found_stx(true);
169 | 
170 |   // add incoming datum
171 |   buffer.push_back(datum);
172 | 
173 |   // check whether we have STX
174 |   for (unsigned int i = 0; i < buffer.size() && i < STX.size(); i++) {
175 |     if (buffer[i] != STX[i]) {
176 |       found_stx = false;
177 |       buffer.pop_front();
178 |       break;
179 |     }
180 |   }
181 | 
182 |   return (found_stx && buffer.size() == STX.size());
183 | }
184 | 
185 | bool PacketFinderBase::WaitForFlag(const unsigned char datum) {
186 |   bool found_id(true);
187 | 
188 |   buffer.push_back(datum);
189 | 
190 |   return found_id;
191 | }
192 | 
193 | bool PacketFinderBase::waitForPayloadSize(const unsigned char *incoming,
194 |                                           unsigned int numberOfIncoming) {
195 |   // push data
196 |   unsigned char first_byte;
197 |   for (unsigned int i = 0; i < numberOfIncoming; i++) {
198 |     first_byte = incoming[i];
199 |     buffer.push_back(incoming[i]);
200 |   }
201 | 
202 |   if (verbose) {
203 |     for (unsigned int i = 0; i < buffer.size(); i++) printf("%02x ", buffer[i]);
204 |     printf("\n");
205 |   }
206 | 
207 |   // check when we need to wait for etx
208 |   if (buffer.size() < size_stx + 1 + size_length_field) {
209 |     return false;
210 |   } else {
211 |     size_payload = buffer[size_stx + 1];
212 |   }
213 | 
214 |   if (verbose) {
215 |     printf("[payloadSize: %d]\n", size_payload);
216 |   }
217 | 
218 |   return true;
219 | }
220 | 
221 | bool PacketFinderBase::waitForEtx(const unsigned char incoming,
222 |                                   bool &foundPacket) {
223 |   // push data
224 |   buffer.push_back(incoming);
225 | 
226 |   // check when we need to wait for etx
227 |   // if minimum payload size is 1
228 |   if (buffer.size() < size_stx + size_etx + 1) {
229 |     return false;
230 |   } else {
231 |     unsigned int number_of_match(0);
232 |     for (unsigned int i = 0; i < ETX.size(); i++) {
233 |       if (buffer[buffer.size() - ETX.size() + i] == ETX[i]) {
234 |         number_of_match++;
235 |       }
236 |     }
237 | 
238 |     if (number_of_match == ETX.size()) {
239 |       foundPacket = true;
240 |       return true;
241 |     }
242 | 
243 |     if (buffer.size() >= size_stx + size_max_payload + size_etx)
244 |       return true;
245 |     else
246 |       return false;
247 |   }
248 | }
249 | 
250 | bool PacketFinderBase::waitForPayloadAndEtx(const unsigned char *incoming,
251 |                                             unsigned int numberOfIncoming,
252 |                                             bool &foundPacket) {
253 |   // push data
254 |   for (unsigned int i = 0; i < numberOfIncoming; i++) {
255 |     buffer.push_back(incoming[i]);
256 |   }
257 | 
258 |   if (size_payload > size_max_payload) {
259 |     state = clearBuffer;
260 | 
261 |     return false;
262 |   }
263 |   //  std::cout << buffer.size() << endl;
264 |   // check when we need to wait for etx
265 |   if (buffer.size() < size_stx + size_length_field + size_payload +
266 |                           size_checksum_field + size_etx) {
267 |     return false;
268 |   } else {
269 |     foundPacket = true;
270 |     return true;
271 |   }
272 | }
273 | }
274 | 


--------------------------------------------------------------------------------
/src/driver/data.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * @file /xbot_arm_driver/src/driver/core_sensors.cpp
  3 |  *
  4 |  * @brief Implementation of the core sensor packet data.
  5 |  *
  6 |  * License: BSD
  7 |  *   https://raw.github.com/yujinrobot/xbot_core/hydro-devel/xbot_arm_driver/LICENSE
  8 |  **/
  9 | 
 10 | /*****************************************************************************
 11 | ** Includes
 12 | *****************************************************************************/
 13 | 
 14 | #include "../../include/wit_node/data.hpp"
 15 | #include <time.h>
 16 | #include "../../include/wit_node/packet_handler/payload_headers.hpp"
 17 | /*****************************************************************************
 18 | ** Namespaces
 19 | *****************************************************************************/
 20 | 
 21 | namespace wit {
 22 | 
 23 | /*****************************************************************************
 24 | ** Implementation
 25 | *****************************************************************************/
 26 | 
 27 | bool Data::serialise(ecl::PushAndPop<unsigned char> &byteStream) { return true; }
 28 | 
 29 | bool Data::desTime(ecl::PushAndPop<unsigned char> &byteStream) {
 30 |   uint8_t tmpL, tmpH;
 31 |   uint16_t tmp;
 32 |   buildVariable(tmpL, byteStream); // 20YY
 33 |                                    //  std::cout << int(tmpL) << std::endl;
 34 |   buildVariable(tmpL, byteStream); // MM
 35 |   buildVariable(tmpL, byteStream); // DD
 36 |   imugps_.timestamp = tmpL * 24 * 60 * 60.0;
 37 |   buildVariable(tmpL, byteStream); // HH
 38 |   imugps_.timestamp += tmpL * 60 * 60.0;
 39 |   buildVariable(tmpL, byteStream); // MM
 40 |   imugps_.timestamp += tmpL * 60.0;
 41 |   buildVariable(tmpL, byteStream); // SS
 42 |   imugps_.timestamp += tmpL;
 43 |   buildVariable(tmp, byteStream); // MSL,MSH
 44 |   imugps_.timestamp += tmp / 1000.0;
 45 |   if (byteStream.size() == 0) {
 46 |     //    std::cout << imugps_.timestamp << std::endl;
 47 |     return true;
 48 |   } else {
 49 |     return false;
 50 |   }
 51 | }
 52 | 
 53 | bool Data::desAcce(ecl::PushAndPop<unsigned char> &byteStream) {
 54 |   int16_t tmp;
 55 |   buildVariable(tmp, byteStream); // accx
 56 |   imugps_.a[0] = tmp * 16 * 9.8 / 32768;
 57 |   buildVariable(tmp, byteStream); // accy
 58 |   imugps_.a[1] = tmp * 16 * 9.8 / 32768;
 59 |   buildVariable(tmp, byteStream); // accz
 60 |   imugps_.a[2] = tmp * 16 * 9.8 / 32768;
 61 |   buildVariable(tmp, byteStream);
 62 |   imugps_.temperature = tmp / 100.0;
 63 |   if (byteStream.size() == 0) {
 64 |     //    std::cout << imugps_.a[2] << std::endl;
 65 |     return true;
 66 |   } else {
 67 |     return false;
 68 |   }
 69 | }
 70 | bool Data::desGyro(ecl::PushAndPop<unsigned char> &byteStream) {
 71 |   int16_t tmp;
 72 |   buildVariable(tmp, byteStream); // wx
 73 |   imugps_.w[0] = tmp * 2000.0 * 3.1415926 / (32768 * 180.0);
 74 |   buildVariable(tmp, byteStream); // wy
 75 |   imugps_.w[1] = tmp * 2000.0 * 3.1415926 / (32768 * 180.0);
 76 |   buildVariable(tmp, byteStream); // wz
 77 |   imugps_.w[2] = tmp * 2000.0 * 3.1415926 / (32768 * 180.0);
 78 |   buildVariable(tmp, byteStream);
 79 |   imugps_.temperature = tmp / 100.0;
 80 |   if (byteStream.size() == 0) {
 81 |     //    std::cout << imugps_.w[0] << std::endl;
 82 |     return true;
 83 |   } else {
 84 |     return false;
 85 |   }
 86 | }
 87 | 
 88 | // Given the measurement as a float (incorrectly), convert it to decimal degrees.
 89 | static float fixBits(float measurement, const char *name) {
 90 |   static const float seven_zeros = 10000000;
 91 | 
 92 |   int int_measurement = reinterpret_cast<int &>(measurement);
 93 |   float degrees = int_measurement / seven_zeros;
 94 |   float minutes_and_seconds = (int_measurement % (int)seven_zeros) / seven_zeros;
 95 |   return degrees + minutes_and_seconds * 100 / 60;
 96 | }
 97 | 
 98 | bool Data::desLola(ecl::PushAndPop<unsigned char> &byteStream) {
 99 |   buildVariable(imugps_.longtitude, byteStream);
100 |   buildVariable(imugps_.latitude, byteStream);
101 | 
102 |   imugps_.latitude = fixBits(imugps_.latitude, "latitude");
103 |   imugps_.longtitude = fixBits(imugps_.longtitude, "longitude");
104 |   if (byteStream.size() == 0) {
105 |     //    std::cout << imugps_.pressure << std::endl;
106 |     return true;
107 |   } else {
108 |     return false;
109 |   }
110 | }
111 | bool Data::desMag(ecl::PushAndPop<unsigned char> &byteStream) {
112 |   int16_t tmp;
113 |   buildVariable(tmp, byteStream); // magx
114 |   imugps_.mag[0] = tmp;
115 |   buildVariable(tmp, byteStream); // magy
116 |   imugps_.mag[1] = tmp;
117 |   buildVariable(tmp, byteStream); // magz
118 |   imugps_.mag[2] = tmp;
119 |   buildVariable(tmp, byteStream);
120 |   imugps_.temperature = tmp / 100.0;
121 |   if (byteStream.size() == 0) {
122 |     //    std::cout << imugps_.mag[0] << std::endl;
123 |     return true;
124 |   } else {
125 |     return false;
126 |   }
127 | }
128 | bool Data::desPalt(ecl::PushAndPop<unsigned char> &byteStream) {
129 |   buildVariable(imugps_.pressure, byteStream);
130 |   buildVariable(imugps_.altitude, byteStream);
131 |   if (byteStream.size() == 0) {
132 |     //    std::cout << imugps_.pressure << std::endl;
133 |     return true;
134 |   } else {
135 |     return false;
136 |   }
137 | }
138 | bool Data::desQuat(ecl::PushAndPop<unsigned char> &byteStream) {
139 |   int16_t tmp;
140 |   buildVariable(tmp, byteStream);
141 |   imugps_.q[0] = tmp / 32768.0;
142 |   buildVariable(tmp, byteStream);
143 |   imugps_.q[1] = tmp / 32768.0;
144 |   buildVariable(tmp, byteStream);
145 |   imugps_.q[2] = tmp / 32768.0;
146 |   buildVariable(tmp, byteStream);
147 |   imugps_.q[3] = tmp / 32768.0;
148 |   if (byteStream.size() == 0) {
149 |     //    std::cout << imugps_.q[0] << std::endl;
150 |     return true;
151 |   } else {
152 |     return false;
153 |   }
154 | }
155 | bool Data::desSate(ecl::PushAndPop<unsigned char> &byteStream) {
156 |   buildVariable(imugps_.satelites, byteStream);
157 |   uint16_t tmp;
158 |   buildVariable(tmp, byteStream);
159 |   imugps_.gpsa[0] = tmp / 100.0;
160 |   buildVariable(tmp, byteStream);
161 |   imugps_.gpsa[1] = tmp / 100.0;
162 |   buildVariable(tmp, byteStream);
163 |   imugps_.gpsa[2] = tmp / 100.0;
164 |   if (byteStream.size() == 0) {
165 |     //    std::cout << imugps_.gpsa[0] << std::endl;
166 |     return true;
167 |   } else {
168 |     return false;
169 |   }
170 | }
171 | bool Data::desState(ecl::PushAndPop<unsigned char> &byteStream) {
172 |   int16_t tmp;
173 |   buildVariable(tmp, byteStream); // D0
174 |   imugps_.d[0] = tmp;
175 |   buildVariable(tmp, byteStream); // D1
176 |   imugps_.d[1] = tmp;
177 |   buildVariable(tmp, byteStream); // D2
178 |   imugps_.d[2] = tmp;
179 |   buildVariable(tmp, byteStream); // D3
180 |   imugps_.d[3] = tmp;
181 |   if (byteStream.size() == 0) {
182 |     //    std::cout << imugps_.d[0] << std::endl;
183 |     return true;
184 |   } else {
185 |     return false;
186 |   }
187 | }
188 | bool Data::desRpy(ecl::PushAndPop<unsigned char> &byteStream) {
189 |   int16_t tmp;
190 |   buildVariable(tmp, byteStream); // roll
191 |   imugps_.rpy[0] = tmp * 3.1415926 / 32768;
192 |   buildVariable(tmp, byteStream); // pitch
193 |   imugps_.rpy[1] = tmp * 3.1415926 / 32768;
194 |   buildVariable(tmp, byteStream); // yaw
195 |   imugps_.rpy[2] = tmp * 3.1415926 / 32768;
196 |   buildVariable(tmp, byteStream);
197 |   imugps_.temperature = tmp / 100.0;
198 |   if (byteStream.size() == 0) {
199 |     //    std::cout << imugps_.rpy[0] << std::endl;
200 |     return true;
201 |   } else {
202 |     return false;
203 |   }
204 | }
205 | bool Data::desGpsv(ecl::PushAndPop<unsigned char> &byteStream) {
206 |   int16_t tmp;
207 |   buildVariable(tmp, byteStream); // gpsh
208 |   imugps_.gpsh = tmp / 10.0;
209 |   buildVariable(tmp, byteStream); // gpsy
210 |   imugps_.gpsy = tmp / 10.0;
211 |   buildVariable(imugps_.gpsv, byteStream); // accz
212 |   imugps_.gpsv = imugps_.gpsv / 1000.0;
213 |   if (byteStream.size() == 0) {
214 |     //    std::cout << imugps_.gpsv << std::endl;
215 |     return true;
216 |   } else {
217 |     return false;
218 |   }
219 | }
220 | bool Data::deserialise(ecl::PushAndPop<unsigned char> &byteStream) {
221 |   uint8_t flag;
222 |   buildVariable(flag, byteStream);
223 |   switch (flag) {
224 |     case Flags::TIME:
225 |       desTime(byteStream);
226 |       break;
227 |     case Flags::ACCE:
228 |       desAcce(byteStream);
229 |       break;
230 |     case Flags::GYRO:
231 |       desGyro(byteStream);
232 |       break;
233 |     case Flags::RPY:
234 |       desRpy(byteStream);
235 |       break;
236 |     case Flags::MAG:
237 |       desMag(byteStream);
238 |       break;
239 |     case Flags::STATE:
240 |       desState(byteStream);
241 |       break;
242 |     case Flags::PALT:
243 |       desPalt(byteStream);
244 |       break;
245 |     case Flags::LOLA:
246 |       desLola(byteStream);
247 |       break;
248 |     case Flags::GPSV:
249 |       desGpsv(byteStream);
250 |       break;
251 |     case Flags::QUAT:
252 |       desQuat(byteStream);
253 |       break;
254 |     case Flags::SATE:
255 |       desSate(byteStream);
256 |       break;
257 |     default:
258 |       break;
259 |   }
260 | 
261 |   return true;
262 | }
263 | 
264 | void Data::build_special_variable(float &variable, ecl::PushAndPop<unsigned char> &byteStream) {
265 |   if (byteStream.size() < 2) return;
266 | 
267 |   unsigned char a, b;
268 |   buildVariable(a, byteStream);
269 |   buildVariable(b, byteStream);
270 |   variable = ((unsigned int)(a & 0x0f)) / 100.0;
271 | 
272 |   variable += ((unsigned int)(a >> 4)) / 10.0;
273 | 
274 |   variable += (unsigned int)(b & 0x0f);
275 | 
276 |   variable += ((unsigned int)(b >> 4)) * 10;
277 | }
278 | 
279 | } // namespace wit
280 | 


--------------------------------------------------------------------------------