wiimote_controller.h
Go to the documentation of this file.
00001 /*
00002  * ROS Node for interfacing with a wiimote control unit.
00003  * Copyright (c) 2016, Intel Corporation.
00004  *
00005  * This program is free software; you can redistribute it and/or modify it
00006  * under the terms and conditions of the GNU General Public License,
00007  * version 2, as published by the Free Software Foundation.
00008  *
00009  * This program is distributed in the hope it will be useful, but WITHOUT
00010  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00011  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00012  * more details.
00013  */
00014 
00015 /*
00016  * This code is based on the original Python implementation
00017  * created by Andreas Paepcke and Melonee Wise
00018  *  Andreas Paepcke <paepcke@anw.willowgarage.com>
00019  * with contributions by
00020  *  David Lu <davidlu@wustl.edu>
00021  *  Miguel Angel Julian Aguilar <miguel.angel@thecorpora.com>
00022  * See https://github.com/ros-drivers/joystick_drivers/tree/indigo-devel/wiimote
00023  * for details and history.
00024  *
00025  * This C++ implementation used the functionality of the existing
00026  * Python code as the feature requirements.
00027  */
00028 
00029 /*
00030  * Initial C++ implementation by
00031  *   Mark Horn <mark.d.horn@intel.com>
00032  *
00033  * Revisions:
00034  *
00035  */
00036 
00037 #pragma once
00038 #ifndef WIIMOTE_WIIMOTE_CONTROLLER_H
00039 #define WIIMOTE_WIIMOTE_CONTROLLER_H
00040 
00041 #include "ros/ros.h"
00042 #include "sensor_msgs/JoyFeedbackArray.h"
00043 #include "std_srvs/Empty.h"
00044 #include "sensor_msgs/Imu.h"
00045 
00046 #include "wiimote/stat_vector_3d.h"
00047 
00048 // We need to link against these
00049 #include <bluetooth/bluetooth.h>  // libbluetooth.so
00050 namespace wiimote_c
00051 {
00052 #include <cwiid.h>  // libcwiid.so
00053 }
00054 
00055 #define zeroedByCal(raw, zero, one) \
00056   (((raw - zero) * 1.0) / ((one - zero) * 1.0))
00057 
00058 class WiimoteNode
00059 {
00060 public:
00061   WiimoteNode();
00062   ~WiimoteNode();
00063   char *getBluetoothAddr();
00064   void setBluetoothAddr(const char *bt_str);
00065   bool pairWiimote(int flags, int timeout);
00066   int unpairWiimote();
00067 
00068   void publish();
00069 
00070   void setLedState(uint8_t led_state);
00071   void setRumbleState(uint8_t rumble);
00072 
00073 private:
00074   void setReportMode(uint8_t rpt_mode);
00075   void checkFactoryCalibrationData();
00076   void resetMotionPlusState();
00077   void resetNunchukState();
00078   void resetClassicState();
00079 
00080   ros::NodeHandle nh_;
00081 
00082   static wiimote_c::cwiid_err_t cwiidErrorCallback;
00083 
00100   void joySetFeedbackCallback(const sensor_msgs::JoyFeedbackArray::ConstPtr& feedback);
00101   bool serviceImuCalibrateCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00102 
00103   bool isCollectingWiimote();
00104   bool isCollectingNunchuk();
00105   bool isCollectingClassic();
00106   bool isCollectingMotionplus();
00107 
00108   bool isPresentNunchuk();
00109   bool isPresentClassic();
00110   bool isPresentMotionplus();
00111 
00112   bool calibrateJoystick(uint8_t stick[2], uint8_t (&center)[2], const char *name);
00113   void updateJoystickMinMax(uint8_t stick[2], uint8_t (&stick_min)[2],
00114       uint8_t (&stick_max)[2], const char *name);
00115   void calculateJoystickAxisXY(uint8_t stick_current[2], uint8_t stick_min[2],
00116       uint8_t stick_max[2], uint8_t stick_center[2], double (&stick)[2]);
00117 
00118   void publishJoy();
00119   void publishImuData();
00120   void publishWiimoteState();
00121   bool publishWiimoteNunchukCommon();
00122   void publishWiimoteNunchuk();
00123   void publishWiimoteClassic();
00124 
00125   bool getStateSample();
00126 
00127   void setLEDBit(uint8_t led, bool on);
00128   void setRumbleBit(bool on);
00129 
00130   ros::Publisher joy_pub_;
00131   ros::Publisher imu_data_pub_;
00132   ros::Publisher wiimote_state_pub_;
00133   ros::Publisher wiimote_nunchuk_pub_;
00134   ros::Publisher wiimote_classic_pub_;
00135   ros::Publisher imu_is_calibrated_pub_;
00136 
00137   ros::Subscriber joy_set_feedback_sub_;
00138 
00139   ros::ServiceServer imu_calibrate_srv_;
00140 
00141   // bluetooth device address
00142   bdaddr_t bt_device_addr_;
00143 
00144   // wiimote handle
00145   wiimote_c::cwiid_wiimote_t *wiimote_;
00146 
00147   // Last state of the Wiimote
00148   struct wiimote_c::cwiid_state wiimote_state_;
00149   void initializeWiimoteState();
00150   // Time last state sample was taken
00151   uint32_t state_secs_;
00152   uint32_t state_nsecs_;
00153 
00154   // Which data items should be reported in state
00155   uint8_t report_mode_;
00156 
00157   // Calibration status Wiimote
00158   struct wiimote_c::acc_cal wiimote_cal_;  // wiimote acceleration factory calibration
00159   bool wiimote_calibrated_;
00160   ros::Time calibration_time_;
00161 
00162   // Joystick related constants
00163   const uint8_t JOYSTICK_NUNCHUK_DEFAULT_CENTER_ = 127;       // Theoretical center
00164   const uint8_t JOYSTICK_NUNCHUK_20PERCENT_MAX_ = 205;
00165   const uint8_t JOYSTICK_NUNCHUK_20PERCENT_MIN_ = 50;
00166   const uint8_t JOYSTICK_CLASSIC_LEFT_DEFAULT_CENTER_ = 31;   // Theoretical center
00167   const uint8_t JOYSTICK_CLASSIC_LEFT_20PERCENT_MAX_ = 50;
00168   const uint8_t JOYSTICK_CLASSIC_LEFT_20PERCENT_MIN_ = 13;
00169   const uint8_t JOYSTICK_CLASSIC_RIGHT_DEFAULT_CENTER_ = 15;  // Theoretical center
00170   const uint8_t JOYSTICK_CLASSIC_RIGHT_20PERCENT_MAX_ = 25;
00171   const uint8_t JOYSTICK_CLASSIC_RIGHT_20PERCENT_MIN_ = 6;
00172 
00173   // Calibration status Nunchuk
00174   struct wiimote_c::acc_cal nunchuk_cal_;  // nunchuk acceleration factory calibration
00175   bool nunchuk_calibrated_;
00176   bool nunchuk_failed_calibration_;
00177   uint8_t nunchuk_stick_center_[2];  // nunchuk stick center position
00178   bool nunchuk_stick_calibrated_;
00179   uint8_t nunchuk_stick_max_[2];  // Maximums x,y
00180   uint8_t nunchuk_stick_min_[2];  // Minimums x,y
00181 
00182   // Calibration status Classic Controller
00183   uint8_t classic_stick_left_center_[2];   // nunchuk stick center position
00184   bool classic_stick_left_calibrated_;
00185   uint8_t classic_stick_left_max_[2];
00186   uint8_t classic_stick_left_min_[2];
00187   uint8_t classic_stick_right_center_[2];  // nunchuk stick center position
00188   bool classic_stick_right_calibrated_;
00189   uint8_t classic_stick_right_max_[2];
00190   uint8_t classic_stick_right_min_[2];
00191 
00192   const int IGNORE_DATA_POINTS_ = 100;  // throw away the first few data points
00193   const int COVARIANCE_DATA_POINTS_ = 100;
00194   StatVector3d linear_acceleration_stat_;
00195   StatVector3d angular_velocity_stat_;
00196   boost::array<double, 9> linear_acceleration_covariance_;
00197   boost::array<double, 9> angular_velocity_covariance_;
00198 
00199   uint8_t led_state_ = 0;
00200   uint8_t rumble_ = 0;
00201 
00202   uint64_t wiimote_errors = 0;
00203 
00204   // Convert wiimote accelerator readings from g's to m/sec^2:
00205   const double EARTH_GRAVITY_ = 9.80665;  // m/sec^2 @sea_level
00206 
00207   // Method used in Wiimote Python version
00208   // TODO(mdhorn): Repeat experiment or create a new one
00209   // and collect data from multiple wiimotes and use mean.
00210   //
00211   // Turning wiimote gyro readings to radians/sec.
00212   // This scale factor is highly approximate. Procedure:
00213   //    - Tape Wiimote to center of an office chair seat
00214   //    - Rotate the chair at approximately constant speed
00215   //      for 10 seconds. This resulted in 6 chair revolutions
00216   //    - On average, the Wiimote gyro read 3570 during this
00217   //      experiment.
00218   //    - Speed of chair revolving:
00219   //         * One full circle is: 2#pi radians
00220   //         * Six revolutions = 12pi radians. ==> 12pi rad in 10 sec ==> 1.2pi rad/sec
00221   //         * => 3570 == 1.2pi
00222   //         * => x*3570 = 1.2pi
00223   //         * => x = 1.2pi/3570 (1.2pi = 3.769908)
00224   //         * => scale factor = 0.001055997
00225   // So multiplying the gyro readings by this factor
00226   // calibrates the readings to show angular velocity
00227   // in radians/sec.
00228   const double GYRO_SCALE_FACTOR_ = 0.001055997;
00229 };
00230 
00231 #endif  // WIIMOTE_WIIMOTE_CONTROLLER_H


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sat Jun 8 2019 20:54:42