Revision 08be08ec
Ran astyle, though astyle options still need to be refined
| buggysim/tutorials/teleop_turtle_key.cpp | ||
|---|---|---|
| 1 | 1 |
/** |
| 2 |
* Simple C++ Publisher;
|
|
| 3 |
* Publish keystrokes to the
|
|
| 2 |
* Simple C++ Publisher; |
|
| 3 |
* Publish keystrokes to the |
|
| 4 | 4 |
* |
| 5 | 5 |
*/ |
| 6 | 6 |
#include <ros/ros.h> |
| ... | ... | |
| 9 | 9 |
#include <termios.h> |
| 10 | 10 |
#include <stdio.h> |
| 11 | 11 |
|
| 12 |
#define KEYCODE_R 0x43
|
|
| 12 |
#define KEYCODE_R 0x43 |
|
| 13 | 13 |
#define KEYCODE_L 0x44 |
| 14 | 14 |
#define KEYCODE_U 0x41 |
| 15 | 15 |
#define KEYCODE_D 0x42 |
| ... | ... | |
| 18 | 18 |
class TeleopTurtle |
| 19 | 19 |
{
|
| 20 | 20 |
public: |
| 21 |
TeleopTurtle(); |
|
| 22 |
void keyLoop(); |
|
| 21 |
TeleopTurtle();
|
|
| 22 |
void keyLoop();
|
|
| 23 | 23 |
|
| 24 | 24 |
private: |
| 25 | 25 |
|
| 26 |
|
|
| 27 |
ros::NodeHandle nh_; |
|
| 28 |
double linear_, angular_, l_scale_, a_scale_; |
|
| 29 |
ros::Publisher vel_pub_; |
|
| 30 |
|
|
| 26 |
|
|
| 27 |
ros::NodeHandle nh_;
|
|
| 28 |
double linear_, angular_, l_scale_, a_scale_;
|
|
| 29 |
ros::Publisher vel_pub_;
|
|
| 30 |
|
|
| 31 | 31 |
}; |
| 32 | 32 |
|
| 33 | 33 |
TeleopTurtle::TeleopTurtle(): |
| 34 |
linear_(0), |
|
| 35 |
angular_(0), |
|
| 36 |
l_scale_(2.0), |
|
| 37 |
a_scale_(2.0) |
|
| 34 |
linear_(0),
|
|
| 35 |
angular_(0),
|
|
| 36 |
l_scale_(2.0),
|
|
| 37 |
a_scale_(2.0)
|
|
| 38 | 38 |
{
|
| 39 |
nh_.param("scale_angular", a_scale_, a_scale_);
|
|
| 40 |
nh_.param("scale_linear", l_scale_, l_scale_);
|
|
| 39 |
nh_.param("scale_angular", a_scale_, a_scale_);
|
|
| 40 |
nh_.param("scale_linear", l_scale_, l_scale_);
|
|
| 41 | 41 |
|
| 42 |
vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
|
|
| 42 |
vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
|
|
| 43 | 43 |
} |
| 44 | 44 |
|
| 45 | 45 |
int kfd = 0; |
| ... | ... | |
| 47 | 47 |
|
| 48 | 48 |
void quit(int sig) |
| 49 | 49 |
{
|
| 50 |
tcsetattr(kfd, TCSANOW, &cooked); |
|
| 51 |
ros::shutdown(); |
|
| 52 |
exit(0); |
|
| 50 |
tcsetattr(kfd, TCSANOW, &cooked);
|
|
| 51 |
ros::shutdown();
|
|
| 52 |
exit(0);
|
|
| 53 | 53 |
} |
| 54 | 54 |
|
| 55 | 55 |
|
| 56 | 56 |
int main(int argc, char** argv) |
| 57 | 57 |
{
|
| 58 |
ros::init(argc, argv, "teleop_turtle"); |
|
| 59 |
TeleopTurtle teleop_turtle; |
|
| 58 |
ros::init(argc, argv, "teleop_turtle");
|
|
| 59 |
TeleopTurtle teleop_turtle;
|
|
| 60 | 60 |
|
| 61 |
signal(SIGINT,quit); |
|
| 61 |
signal(SIGINT,quit);
|
|
| 62 | 62 |
|
| 63 |
teleop_turtle.keyLoop(); |
|
| 64 |
|
|
| 65 |
return(0); |
|
| 63 |
teleop_turtle.keyLoop();
|
|
| 64 |
|
|
| 65 |
return(0);
|
|
| 66 | 66 |
} |
| 67 | 67 |
|
| 68 | 68 |
|
| 69 | 69 |
void TeleopTurtle::keyLoop() |
| 70 | 70 |
{
|
| 71 |
char c; |
|
| 72 |
bool dirty=false; |
|
| 73 |
|
|
| 74 |
|
|
| 75 |
// get the console in raw mode |
|
| 76 |
tcgetattr(kfd, &cooked); |
|
| 77 |
memcpy(&raw, &cooked, sizeof(struct termios)); |
|
| 78 |
raw.c_lflag &=~ (ICANON | ECHO); |
|
| 79 |
// Setting a new line, then end of file |
|
| 80 |
raw.c_cc[VEOL] = 1; |
|
| 81 |
raw.c_cc[VEOF] = 2; |
|
| 82 |
tcsetattr(kfd, TCSANOW, &raw); |
|
| 83 |
|
|
| 84 |
puts("Reading from keyboard");
|
|
| 85 |
puts("---------------------------");
|
|
| 86 |
puts("Use arrow keys to move the turtle.");
|
|
| 87 |
|
|
| 88 |
|
|
| 89 |
for(;;) |
|
| 90 |
{
|
|
| 91 |
// get the next event from the keyboard |
|
| 92 |
if(read(kfd, &c, 1) < 0) |
|
| 93 |
{
|
|
| 94 |
perror("read():");
|
|
| 95 |
exit(-1); |
|
| 96 |
} |
|
| 97 |
|
|
| 98 |
linear_=angular_=0; |
|
| 99 |
ROS_DEBUG("value: 0x%02X\n", c);
|
|
| 100 |
|
|
| 101 |
switch(c) |
|
| 102 |
{
|
|
| 71 |
char c; |
|
| 72 |
bool dirty=false; |
|
| 73 |
|
|
| 74 |
|
|
| 75 |
// get the console in raw mode |
|
| 76 |
tcgetattr(kfd, &cooked); |
|
| 77 |
memcpy(&raw, &cooked, sizeof(struct termios)); |
|
| 78 |
raw.c_lflag &=~ (ICANON | ECHO); |
|
| 79 |
// Setting a new line, then end of file |
|
| 80 |
raw.c_cc[VEOL] = 1; |
|
| 81 |
raw.c_cc[VEOF] = 2; |
|
| 82 |
tcsetattr(kfd, TCSANOW, &raw); |
|
| 83 |
|
|
| 84 |
puts("Reading from keyboard");
|
|
| 85 |
puts("---------------------------");
|
|
| 86 |
puts("Use arrow keys to move the turtle.");
|
|
| 87 |
|
|
| 88 |
|
|
| 89 |
for(;;) {
|
|
| 90 |
// get the next event from the keyboard |
|
| 91 |
if(read(kfd, &c, 1) < 0) {
|
|
| 92 |
perror("read():");
|
|
| 93 |
exit(-1); |
|
| 94 |
} |
|
| 95 |
|
|
| 96 |
linear_=angular_=0; |
|
| 97 |
ROS_DEBUG("value: 0x%02X\n", c);
|
|
| 98 |
|
|
| 99 |
switch(c) {
|
|
| 103 | 100 |
case KEYCODE_L: |
| 104 |
ROS_DEBUG("LEFT");
|
|
| 105 |
angular_ = 1.0; |
|
| 106 |
dirty = true; |
|
| 107 |
break; |
|
| 101 |
ROS_DEBUG("LEFT");
|
|
| 102 |
angular_ = 1.0;
|
|
| 103 |
dirty = true;
|
|
| 104 |
break;
|
|
| 108 | 105 |
case KEYCODE_R: |
| 109 |
ROS_DEBUG("RIGHT");
|
|
| 110 |
angular_ = -1.0; |
|
| 111 |
dirty = true; |
|
| 112 |
break; |
|
| 106 |
ROS_DEBUG("RIGHT");
|
|
| 107 |
angular_ = -1.0;
|
|
| 108 |
dirty = true;
|
|
| 109 |
break;
|
|
| 113 | 110 |
case KEYCODE_U: |
| 114 |
ROS_DEBUG("UP");
|
|
| 115 |
linear_ = 1.0; |
|
| 116 |
dirty = true; |
|
| 117 |
break; |
|
| 111 |
ROS_DEBUG("UP");
|
|
| 112 |
linear_ = 1.0;
|
|
| 113 |
dirty = true;
|
|
| 114 |
break;
|
|
| 118 | 115 |
case KEYCODE_D: |
| 119 |
ROS_DEBUG("DOWN");
|
|
| 120 |
linear_ = -1.0; |
|
| 121 |
dirty = true; |
|
| 122 |
break; |
|
| 123 |
} |
|
| 124 |
|
|
| 125 |
|
|
| 126 |
turtlesim::Velocity vel; |
|
| 127 |
vel.angular = a_scale_*angular_; |
|
| 128 |
vel.linear = l_scale_*linear_; |
|
| 129 |
if(dirty ==true) |
|
| 130 |
{
|
|
| 131 |
vel_pub_.publish(vel); |
|
| 132 |
dirty=false; |
|
| 133 |
} |
|
| 134 |
} |
|
| 135 |
|
|
| 136 |
|
|
| 137 |
return; |
|
| 116 |
ROS_DEBUG("DOWN");
|
|
| 117 |
linear_ = -1.0; |
|
| 118 |
dirty = true; |
|
| 119 |
break; |
|
| 120 |
} |
|
| 121 |
|
|
| 122 |
|
|
| 123 |
turtlesim::Velocity vel; |
|
| 124 |
vel.angular = a_scale_*angular_; |
|
| 125 |
vel.linear = l_scale_*linear_; |
|
| 126 |
if(dirty ==true) {
|
|
| 127 |
vel_pub_.publish(vel); |
|
| 128 |
dirty=false; |
|
| 129 |
} |
|
| 130 |
} |
|
| 131 |
|
|
| 132 |
|
|
| 133 |
return; |
|
| 138 | 134 |
} |
| 139 | 135 |
|
| 140 | 136 |
|
Also available in: Unified diff