teleop_turtle_key.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <geometry_msgs/Twist.h>
3 #include <signal.h>
4 #include <termios.h>
5 #include <stdio.h>
6 
7 #define KEYCODE_R 0x43
8 #define KEYCODE_L 0x44
9 #define KEYCODE_U 0x41
10 #define KEYCODE_D 0x42
11 #define KEYCODE_Q 0x71
12 
14 {
15 public:
16  TeleopTurtle();
17  void keyLoop();
18 
19 private:
20 
21 
25 
26 };
27 
29  linear_(0),
30  angular_(0),
31  l_scale_(2.0),
32  a_scale_(2.0)
33 {
34  nh_.param("scale_angular", a_scale_, a_scale_);
35  nh_.param("scale_linear", l_scale_, l_scale_);
36 
37  twist_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
38 }
39 
40 int kfd = 0;
41 struct termios cooked, raw;
42 
43 void quit(int sig)
44 {
45  (void)sig;
46  tcsetattr(kfd, TCSANOW, &cooked);
47  ros::shutdown();
48  exit(0);
49 }
50 
51 
52 int main(int argc, char** argv)
53 {
54  ros::init(argc, argv, "teleop_turtle");
55  TeleopTurtle teleop_turtle;
56 
57  signal(SIGINT,quit);
58 
59  teleop_turtle.keyLoop();
60 
61  return(0);
62 }
63 
64 
66 {
67  char c;
68  bool dirty=false;
69 
70 
71  // get the console in raw mode
72  tcgetattr(kfd, &cooked);
73  memcpy(&raw, &cooked, sizeof(struct termios));
74  raw.c_lflag &=~ (ICANON | ECHO);
75  // Setting a new line, then end of file
76  raw.c_cc[VEOL] = 1;
77  raw.c_cc[VEOF] = 2;
78  tcsetattr(kfd, TCSANOW, &raw);
79 
80  puts("Reading from keyboard");
81  puts("---------------------------");
82  puts("Use arrow keys to move the turtle.");
83 
84 
85  for(;;)
86  {
87  // get the next event from the keyboard
88  if(read(kfd, &c, 1) < 0)
89  {
90  perror("read():");
91  exit(-1);
92  }
93 
94  linear_=angular_=0;
95  ROS_DEBUG("value: 0x%02X\n", c);
96 
97  switch(c)
98  {
99  case KEYCODE_L:
100  ROS_DEBUG("LEFT");
101  angular_ = 1.0;
102  dirty = true;
103  break;
104  case KEYCODE_R:
105  ROS_DEBUG("RIGHT");
106  angular_ = -1.0;
107  dirty = true;
108  break;
109  case KEYCODE_U:
110  ROS_DEBUG("UP");
111  linear_ = 1.0;
112  dirty = true;
113  break;
114  case KEYCODE_D:
115  ROS_DEBUG("DOWN");
116  linear_ = -1.0;
117  dirty = true;
118  break;
119  }
120 
121 
122  geometry_msgs::Twist twist;
123  twist.angular.z = a_scale_*angular_;
124  twist.linear.x = l_scale_*linear_;
125  if(dirty ==true)
126  {
127  twist_pub_.publish(twist);
128  dirty=false;
129  }
130  }
131 
132 
133  return;
134 }
135 
136 
137 
void publish(const boost::shared_ptr< M > &message) const
#define KEYCODE_L
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define KEYCODE_D
ros::Publisher twist_pub_
ros::NodeHandle nh_
int main(int argc, char **argv)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define KEYCODE_U
struct termios cooked raw
#define KEYCODE_R
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void quit(int sig)
ROSCPP_DECL void shutdown()
int kfd
#define ROS_DEBUG(...)


turtlesim
Author(s): Josh Faust
autogenerated on Fri Jun 7 2019 22:01:46