296 lines
8.1 KiB
C++
296 lines
8.1 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 "display.h"
|
||
|
||
#include <std_msgs/Empty.h>
|
||
#include <geometry_msgs/Twist.h>
|
||
#include <ardrone_autonomy/Navdata.h>
|
||
|
||
class NavdataCallback
|
||
{
|
||
private:
|
||
boost::shared_ptr<Curses> term;
|
||
|
||
public:
|
||
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 NavdataCallback
|
||
|
||
class CmdVelCallback
|
||
{
|
||
private:
|
||
boost::shared_ptr<Curses> term;
|
||
|
||
public:
|
||
CmdVelCallback(const boost::shared_ptr<Curses>& terminal) :
|
||
term(terminal) {}
|
||
|
||
void operator()(const geometry_msgs::Twist::ConstPtr& msg) {
|
||
term->update_topic(msg);
|
||
}
|
||
}; // class CmdVelCallback
|
||
|
||
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:
|
||
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;
|
||
} // main
|
||
|