You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
59 lines
1.0 KiB
59 lines
1.0 KiB
#ifndef ROSKEYBOARDTELEOPNODE_H
|
|
#define ROSKEYBOARDTELEOPNODE_H
|
|
|
|
#include <termios.h>
|
|
#include <signal.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <sys/poll.h>
|
|
|
|
#include <boost/thread/thread.hpp>
|
|
#include <ros/ros.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
|
|
#define KEYCODE_W 0x77
|
|
#define KEYCODE_A 0x61
|
|
#define KEYCODE_S 0x73
|
|
#define KEYCODE_D 0x64
|
|
|
|
#define KEYCODE_A_CAP 0x41
|
|
#define KEYCODE_D_CAP 0x44
|
|
#define KEYCODE_S_CAP 0x53
|
|
#define KEYCODE_W_CAP 0x57
|
|
|
|
|
|
|
|
class RosKeyboardTeleopNode
|
|
{
|
|
private:
|
|
double walk_vel_;
|
|
double run_vel_;
|
|
double yaw_rate_;
|
|
double yaw_rate_run_;
|
|
|
|
geometry_msgs::Twist cmdvel_;
|
|
ros::NodeHandle n_;
|
|
ros::Publisher pub_;
|
|
|
|
public:
|
|
RosKeyboardTeleopNode();
|
|
|
|
~RosKeyboardTeleopNode() { }
|
|
void keyboardLoop();
|
|
|
|
void stopRobot()
|
|
{
|
|
cmdvel_.linear.x = 0.0;
|
|
cmdvel_.angular.z = 0.0;
|
|
pub_.publish(cmdvel_);
|
|
}
|
|
};
|
|
|
|
extern int kfd;
|
|
extern struct termios cooked, raw;
|
|
extern bool done;
|
|
extern RosKeyboardTeleopNode* tbk;
|
|
|
|
#endif // ROSKEYBOARDTELEOPNODE_H
|