$search
00001 /* 00002 * Copyright (c) 2010, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ros/ros.h> 00031 #include <geometry_msgs/Twist.h> 00032 #include <signal.h> 00033 #include <termios.h> 00034 #include <stdio.h> 00035 #include "boost/thread/mutex.hpp" 00036 #include "boost/thread/thread.hpp" 00037 00038 #define KEYCODE_R 0x43 00039 #define KEYCODE_L 0x44 00040 #define KEYCODE_U 0x41 00041 #define KEYCODE_D 0x42 00042 #define KEYCODE_Q 0x71 00043 00044 class NxtTeleop 00045 { 00046 public: 00047 NxtTeleop(); 00048 void keyLoop(); 00049 void watchdog(); 00050 00051 private: 00052 00053 00054 ros::NodeHandle nh_,ph_; 00055 double linear_, angular_; 00056 ros::Time last_publish_; 00057 double l_scale_, a_scale_; 00058 ros::Publisher vel_pub_; 00059 void publish(double, double); 00060 boost::mutex publish_mutex_; 00061 00062 }; 00063 00064 NxtTeleop::NxtTeleop(): 00065 linear_(0), 00066 angular_(0), 00067 l_scale_(1.0), 00068 a_scale_(1.0) 00069 { 00070 ph_.param("scale_angular", a_scale_, a_scale_); 00071 ph_.param("scale_linear", l_scale_, l_scale_); 00072 00073 vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1); 00074 } 00075 00076 int kfd = 0; 00077 struct termios cooked, raw; 00078 00079 void quit(int sig) 00080 { 00081 tcsetattr(kfd, TCSANOW, &cooked); 00082 ros::shutdown(); 00083 exit(0); 00084 } 00085 00086 00087 int main(int argc, char** argv) 00088 { 00089 ros::init(argc, argv, "nxt_teleop"); 00090 NxtTeleop nxt_teleop; 00091 ros::NodeHandle n; 00092 00093 signal(SIGINT,quit); 00094 00095 boost::thread my_thread(boost::bind(&NxtTeleop::keyLoop, &nxt_teleop)); 00096 00097 00098 ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&NxtTeleop::watchdog, &nxt_teleop)); 00099 00100 ros::spin(); 00101 00102 my_thread.interrupt() ; 00103 my_thread.join() ; 00104 00105 return(0); 00106 } 00107 00108 00109 void NxtTeleop::watchdog() 00110 { 00111 boost::mutex::scoped_lock lock(publish_mutex_); 00112 if (ros::Time::now() > last_publish_ + ros::Duration(0.15)) 00113 publish(0, 0); 00114 } 00115 00116 void NxtTeleop::keyLoop() 00117 { 00118 char c; 00119 00120 00121 // get the console in raw mode 00122 tcgetattr(kfd, &cooked); 00123 memcpy(&raw, &cooked, sizeof(struct termios)); 00124 raw.c_lflag &=~ (ICANON | ECHO); 00125 // Setting a new line, then end of file 00126 raw.c_cc[VEOL] = 1; 00127 raw.c_cc[VEOF] = 2; 00128 tcsetattr(kfd, TCSANOW, &raw); 00129 00130 puts("Reading from keyboard"); 00131 puts("---------------------------"); 00132 puts("Use arrow keys to move the nxt."); 00133 00134 00135 for(;;) 00136 { 00137 // get the next event from the keyboard 00138 if(read(kfd, &c, 1) < 0) 00139 { 00140 perror("read():"); 00141 exit(-1); 00142 } 00143 00144 00145 linear_=angular_=0; 00146 ROS_DEBUG("value: 0x%02X\n", c); 00147 00148 switch(c) 00149 { 00150 case KEYCODE_L: 00151 ROS_DEBUG("LEFT"); 00152 angular_ = 1.0; 00153 break; 00154 case KEYCODE_R: 00155 ROS_DEBUG("RIGHT"); 00156 angular_ = -1.0; 00157 break; 00158 case KEYCODE_U: 00159 ROS_DEBUG("UP"); 00160 linear_ = 1.0; 00161 break; 00162 case KEYCODE_D: 00163 ROS_DEBUG("DOWN"); 00164 linear_ = -1.0; 00165 break; 00166 } 00167 boost::mutex::scoped_lock lock(publish_mutex_); 00168 last_publish_ = ros::Time::now(); 00169 publish(angular_, linear_); 00170 } 00171 00172 return; 00173 } 00174 00175 void NxtTeleop::publish(double angular, double linear) 00176 { 00177 geometry_msgs::Twist vel; 00178 vel.angular.z = a_scale_*angular; 00179 vel.linear.x = l_scale_*linear; 00180 00181 vel_pub_.publish(vel); 00182 00183 00184 return; 00185 } 00186 00187 00188