00001 /* 00002 * ROS Node for using a wiimote control unit to direct a robot. 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 * Initial C++ implementation by 00017 * Mark Horn <mark.d.horn@intel.com> 00018 * 00019 * Revisions: 00020 * 00021 */ 00022 00023 #pragma once 00024 #ifndef WIIMOTE_TELEOP_WIIMOTE_H 00025 #define WIIMOTE_TELEOP_WIIMOTE_H 00026 00027 #include "ros/ros.h" 00028 #include "sensor_msgs/Joy.h" 00029 #include "wiimote/State.h" 00030 00031 #include <math.h> 00032 00033 // Sane defaults based on the TurtleBot 00034 // TurtleBot maximum speed documented at 25.6"/second ~= 0.65024 m/s 00035 #define DEFAULT_MAX_LINEAR_X 0.65024 // m/s 00036 // TurtleBot maximum angular speed is documented at 180 degrees Pi / second 00037 #define DEFAULT_MAX_ANGULAR_Z M_PI // rad/s 00038 00039 #define DEFAULT_PERCENT_LINEAR_THROTTLE 0.75 00040 #define DEFAULT_PERCENT_ANGULAR_THROTTLE 0.75 00041 00042 class TeleopWiimote 00043 { 00044 public: 00045 TeleopWiimote(); 00046 00047 private: 00048 void rumbleFeedback(int useconds); 00049 void setLEDFeedback(double value); 00050 void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); 00051 void wiimoteStateCallback(const wiimote::State::ConstPtr& wiistate); 00052 00053 double linear_x_max_velocity_; // m/s 00054 double linear_x_min_velocity_; // m/s 00055 double angular_z_max_velocity_; // rad/s 00056 double angular_z_min_velocity_; // rad/s 00057 00058 double percent_linear_throttle_; // 0.0 - 1.0 (1.0 = 100%) 00059 double percent_angular_throttle_; // 0.0 - 1.0 (1.0 = 100%) 00060 00061 ros::Publisher vel_pub_; 00062 ros::Publisher joy_pub_; 00063 ros::Subscriber joy_sub_; 00064 ros::Subscriber wiimote_sub_; 00065 00066 bool dpad_in_use_ = false; 00067 bool njoy_in_use_ = false; 00068 }; 00069 00070 #endif // WIIMOTE_TELEOP_WIIMOTE_H