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.
135 lines
3.0 KiB
135 lines
3.0 KiB
#include "ros/ros.h"
|
|
#include <papillon/BoundingBox.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
#include <std_msgs/Empty.h>
|
|
#include <ncurses.h>
|
|
|
|
#include <opencv/cv.h>
|
|
|
|
#include <sstream>
|
|
|
|
using namespace std;
|
|
|
|
class Drone_control {
|
|
public:
|
|
ros::NodeHandle n;
|
|
ros::Publisher pub_cmd;
|
|
ros::Publisher pub_takeoff;
|
|
ros::Publisher pub_land;
|
|
ros::Subscriber sub_box;
|
|
std_msgs::Empty nope;
|
|
|
|
float THRES_TURN = 0.1;
|
|
bool emergency = false;
|
|
|
|
// 1 left | 0 center | -1 right
|
|
vector<int> decision_acc;
|
|
const static int NB_ACC = 5;
|
|
|
|
|
|
Drone_control() : n("~") {
|
|
pub_cmd = n.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 1);
|
|
pub_takeoff = n.advertise<std_msgs::Empty>("/bebop/takeoff", 1);
|
|
pub_land = n.advertise<std_msgs::Empty>("/bebop/land", 1);
|
|
sub_box = n.subscribe<papillon::BoundingBox>("/papillon/bbox", 1, &Drone_control::on_msg, this);
|
|
|
|
// Initialise decision accumulator
|
|
for (size_t i = 0; i < NB_ACC; ++i)
|
|
decision_acc.push_back(0);
|
|
|
|
ROS_INFO("Built !");
|
|
}
|
|
|
|
|
|
// This processes an image and publishes the result.
|
|
void on_msg(const papillon::BoundingBox::ConstPtr& bbox) {
|
|
geometry_msgs::Twist twist = geometry_msgs::Twist();
|
|
cv::Point2f r_center = cv::Point2f(bbox->x + bbox->width/2, bbox->y + bbox->height/2);
|
|
if (!emergency) {
|
|
decision_acc.pop_back();
|
|
// Image is 1.0 in width and height
|
|
if (r_center.x < 0.5 - THRES_TURN)
|
|
decision_acc.insert(decision_acc.begin(), 1);
|
|
else if (r_center.x > 0.5 + THRES_TURN)
|
|
decision_acc.insert(decision_acc.begin(), -1);
|
|
else
|
|
decision_acc.insert(decision_acc.begin(), 0);
|
|
|
|
int moy = 0;
|
|
for (const auto& d : decision_acc)
|
|
moy += d;
|
|
twist.angular.z = moy / (float)NB_ACC * 0.3;
|
|
}
|
|
|
|
pub_cmd.publish(twist);
|
|
}
|
|
|
|
void key_spin() {
|
|
// >>>>> nCurses initization
|
|
initscr();
|
|
keypad(stdscr, TRUE);
|
|
cbreak();
|
|
noecho();
|
|
timeout(0); // Non blocking getch()
|
|
// <<<<< nCurses initization
|
|
|
|
int c;
|
|
|
|
printw(" ------------------------\n");
|
|
printw("| Papillon HQ |\n");
|
|
printw(" ------------------------ \n");
|
|
printw("| Press F to take off. |\n");
|
|
printw("| Press SPACEBAR to land.|\n");
|
|
printw("| Press Q to exit. |\n");
|
|
printw(" ------------------------\n");
|
|
|
|
bool stop = false;
|
|
|
|
ros::Rate rate(1.0/30.0); // 30 Hz
|
|
while(!stop)
|
|
{
|
|
c = getch();
|
|
|
|
switch(c)
|
|
{
|
|
case 'f':
|
|
{
|
|
// Takeoff
|
|
ROS_DEBUG_STREAM("Taking off...\r");
|
|
pub_takeoff.publish(nope);
|
|
emergency = false;
|
|
printw("Taking off...\r");
|
|
break;
|
|
}
|
|
case ' ':
|
|
{
|
|
// Takeoff
|
|
ROS_DEBUG_STREAM("Landing...\r");
|
|
pub_land.publish(nope);
|
|
printw("Landing...\r");
|
|
emergency = true;
|
|
break;
|
|
}
|
|
case 'q':
|
|
case 'Q':
|
|
{
|
|
ROS_DEBUG_STREAM("EXIT\r");
|
|
stop = true;
|
|
}
|
|
}
|
|
ros::spinOnce();
|
|
}
|
|
printw("\nExiting\n");
|
|
}
|
|
};
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "control");
|
|
Drone_control con=Drone_control();
|
|
con.key_spin();
|
|
|
|
return 0;
|
|
}
|
|
|
|
|