teleop_wiimote.h
Go to the documentation of this file.
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


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