270 lines
8.8 KiB
C++
270 lines
8.8 KiB
C++
/* Copyright © 2015 CentraleSupélec
|
|
*
|
|
* This file is part of Hand Control.
|
|
*
|
|
* Hand Control is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Hand Control is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Hand Control. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
|
|
#include <ros/ros.h>
|
|
#include <ros/time.h>
|
|
#include <locale.h>
|
|
|
|
#include <std_msgs/Empty.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
#include <ardrone_autonomy/Navdata.h>
|
|
|
|
#include "./display.h"
|
|
|
|
class NavdataCallback {
|
|
private:
|
|
boost::shared_ptr<Curses> term;
|
|
|
|
public:
|
|
explicit NavdataCallback(const boost::shared_ptr<Curses>& terminal) :
|
|
term(terminal) {}
|
|
|
|
void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) {
|
|
term->update_nav_data(msg->batteryPercent, msg->state, msg->tm);
|
|
}
|
|
};
|
|
|
|
class CmdVelCallback {
|
|
private:
|
|
boost::shared_ptr<Curses> term;
|
|
|
|
public:
|
|
explicit CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
|
|
term(terminal) {}
|
|
|
|
void operator()(const geometry_msgs::Twist::ConstPtr& msg) {
|
|
term->update_topic(msg);
|
|
}
|
|
};
|
|
|
|
class Run {
|
|
private:
|
|
std_msgs::Empty empty;
|
|
ros::NodeHandle n;
|
|
ros::Rate loop_rate;
|
|
ros::Publisher cmd, pub_takeoff, pub_land, pub_reset;
|
|
ros::Subscriber data_subscriber, cmdvel_subscriber;
|
|
void land() { pub_land.publish(empty); }
|
|
void takeoff() { pub_takeoff.publish(empty); }
|
|
void reset() { pub_reset.publish(empty); }
|
|
float x_speed, y_speed, z_speed, turn;
|
|
boost::shared_ptr<Curses> term;
|
|
NavdataCallback data_callback;
|
|
CmdVelCallback cmdvel_callback;
|
|
|
|
public:
|
|
explicit Run(const boost::shared_ptr<Curses>& terminal) :
|
|
data_callback(terminal),
|
|
cmdvel_callback(terminal),
|
|
term(terminal),
|
|
loop_rate(30),
|
|
x_speed(0.1),
|
|
y_speed(0.1),
|
|
z_speed(0.1),
|
|
turn(0.1) {
|
|
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
|
|
pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
|
|
pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
|
|
pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
|
|
|
|
data_subscriber =
|
|
n.subscribe<ardrone_autonomy::Navdata>(
|
|
"/ardrone/navdata", 1, data_callback);
|
|
|
|
cmdvel_subscriber =
|
|
n.subscribe<geometry_msgs::Twist>(
|
|
"/cmd_vel", 1, cmdvel_callback);
|
|
|
|
term->update_cmd_speed('x', x_speed);
|
|
term->update_cmd_speed('y', y_speed);
|
|
term->update_cmd_speed('z', z_speed);
|
|
term->update_cmd_speed('t', turn);
|
|
|
|
float a(0);
|
|
int s(0);
|
|
float time(0);
|
|
term->update_nav_data(a, s, time);
|
|
}
|
|
|
|
void operator()() {
|
|
while (ros::ok()) {
|
|
ros::spinOnce();
|
|
|
|
geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist());
|
|
msg->linear.x = msg->linear.y = msg->linear.z =
|
|
msg->angular.x = msg->angular.y = msg->angular.z = 0.;
|
|
|
|
char c = term->getchar();
|
|
|
|
switch (c) {
|
|
case 'k' : { // hover
|
|
cmd.publish(msg);
|
|
term->log_sent("hover !");
|
|
break;
|
|
}
|
|
case 'i' : { // forward
|
|
msg->linear.x = x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("forward !");
|
|
break;
|
|
}
|
|
case ';' : { // backward
|
|
msg->linear.x = -x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("backward !");
|
|
break;
|
|
}
|
|
case 'h' : { // translate left
|
|
msg->linear.y = -y_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("translate left !");
|
|
break;
|
|
}
|
|
case 'm' : { // translate right
|
|
msg->linear.y = y_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("translate right !");
|
|
break;
|
|
}
|
|
case 'j' : { // rotate left
|
|
msg->angular.z = turn;
|
|
cmd.publish(msg);
|
|
term->log_sent("rotate left !");
|
|
break;
|
|
}
|
|
case 'l' : { // rotate right
|
|
msg->angular.z = -turn;
|
|
cmd.publish(msg);
|
|
term->log_sent("rotate right !");
|
|
break;
|
|
}
|
|
case 'u' : { // turn left
|
|
msg->angular.z = turn;
|
|
msg->linear.x = x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("forward left !");
|
|
break;
|
|
}
|
|
case 'o' : { // turn right
|
|
msg->angular.z = -turn;
|
|
msg->linear.x = x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("forward right !");
|
|
break;
|
|
}
|
|
case ',' : { // turn left backward
|
|
msg->angular.z = turn;
|
|
msg->linear.x = -x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("backward left !");
|
|
break;
|
|
}
|
|
case ':' : { // turn right backward
|
|
msg->angular.z = -turn;
|
|
msg->linear.x = -x_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("backward right !");
|
|
break;
|
|
}
|
|
case 'y' : { // up
|
|
msg->linear.z = z_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("up !");
|
|
break;
|
|
}
|
|
case 'n' : { // down
|
|
msg->linear.z = -z_speed;
|
|
cmd.publish(msg);
|
|
term->log_sent("down !");
|
|
break;
|
|
}
|
|
case 't' : { // takeoff
|
|
takeoff();
|
|
term->log_sent("takeoff !");
|
|
break;
|
|
}
|
|
case 'b' : { // land
|
|
land();
|
|
term->log_sent("land !");
|
|
break;
|
|
}
|
|
case 'g' : { // reset
|
|
reset();
|
|
term->log_sent("reset !");
|
|
break;
|
|
}
|
|
case 'a' : { // + x_speed
|
|
x_speed *= 1.1;
|
|
term->update_cmd_speed('x', x_speed);
|
|
break;
|
|
}
|
|
case 'w' : { // - x_speed
|
|
x_speed *= 0.9;
|
|
term->update_cmd_speed('x', x_speed);
|
|
break;
|
|
}
|
|
case 'z' : { // + y_speed
|
|
y_speed *= 1.1;
|
|
term->update_cmd_speed('y', y_speed);
|
|
break;
|
|
}
|
|
case 'x' : { // - y_speed
|
|
y_speed *= 0.9;
|
|
term->update_cmd_speed('y', y_speed);
|
|
break;
|
|
}
|
|
case 'e' : { // + z_speed
|
|
z_speed *= 1.1;
|
|
term->update_cmd_speed('z', z_speed);
|
|
break;
|
|
}
|
|
case 'c' : { // - z_speed
|
|
z_speed *= 0.9;
|
|
term->update_cmd_speed('z', z_speed);
|
|
break;
|
|
}
|
|
case 'r' : { // + turn speed
|
|
turn *= 1.1;
|
|
term->update_cmd_speed('t', turn);
|
|
break;
|
|
}
|
|
case 'v' : { // - turn speed
|
|
turn *= 0.9;
|
|
term->update_cmd_speed('t', turn);
|
|
break;
|
|
}
|
|
default : {
|
|
cmd.publish(msg);
|
|
term->log_sent("hover !");
|
|
}
|
|
} // switch(c)
|
|
loop_rate.sleep();
|
|
} // while
|
|
} // void run()
|
|
}; // class Run
|
|
|
|
int main(int argc, char** argv) {
|
|
setlocale(LC_ALL, "");
|
|
ros::init(argc, argv, "keyboard_cmd");
|
|
boost::shared_ptr<Curses> term(new Curses());
|
|
Run fun(term);
|
|
fun();
|
|
return 0;
|
|
}
|