diff --git a/src/commander_atan.cpp b/src/commander_atan.cpp deleted file mode 100644 index 456c1d7..0000000 --- a/src/commander_atan.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// this code doesn’t work -// :-( -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include "data.h" - -class Run -{ - private: - - enum FType { f_lin, f_atan, f_undef }; - - class Function - { - private: - virtual void set_grad(const float& grad) = 0; - protected: - float min, max; - FType type; - public: - Function() : min(0.1f), max(0.8f) {} - virtual float f(const float& arg) = 0; - void set(const float& minimum, const float& maximum, const float& grad) - { - min = minimum; - max = maximum; - set_grad(grad); - } - FType get_type() { return type; } - }; - - class Atan : public Function - { - private: - static const float demi_pi; - float t; - virtual void set_grad(const float& grad) - { - t = grad*demi_pi/max; - } - public: - Atan() :t(demi_pi) { type = f_atan; } - virtual float f(const float& arg) - { - float out = max * atan(t * arg) / demi_pi; - if (fabs(out) > min) - return out; - else - return 0.; - } - }; - - class Lin : public Function - { - private: - float t; - virtual void set_grad(const float& grad) - { - t = grad; - } - public: - Lin() { type = f_lin; } - virtual float f(const float& arg) - { - float out = std::min(t * arg, max); - if (fabs(out) > min) - return out; - else - return 0.; - } - }; - - Data > f; - float neutral_z; - float max_curv; - uint64_t min_number; - ros::Publisher pub; - - public: - Run(const ros::Publisher& cmd_publisher) : - pub(cmd_publisher) { - f.x = boost::make_shared(); - f.y = boost::make_shared(); - f.z = boost::make_shared(); - f.th = boost::make_shared(); - } - - void callback(const hand_control::Plan::ConstPtr& msg) - { - ROS_INFO("plan received"); - Data in, out; - in.x = in.y = in.z = in.th = 0.; - if (msg->curvature < max_curv && msg->number > min_number) - { - if(msg->normal.z > 0) - { - in.y = - msg->normal.x; - in.x = msg->normal.y; - } else - { - in.y = - msg->normal.x; - in.x = msg->normal.y; - } - in.z = msg->altitude - neutral_z; - in.th = msg->angle; - } - - geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); - - if (fabs(in.y) > fabs(in.x)) - { - mvt->linear.y = f.y->f(in.y); - } else - { - mvt->linear.x = f.x->f(in.x); - } - - mvt->linear.z = f.z->f(in.z); - - mvt->angular.z = f.th->f(in.th); - - pub.publish(mvt); - ROS_INFO("cmd published"); - } - - void reconfigure(const hand_control::Commander_atanConfig& c, const uint32_t& level) - { - max_curv = c.max_curvature; - neutral_z = c.neutral_alt; - min_number = c.min_points_number; - - if (c.atan and f.x->get_type() != f_atan) - { - f.x = boost::make_shared(); - f.y = boost::make_shared(); - f.z = boost::make_shared(); - f.th = boost::make_shared(); - } else if (!c.atan and f.x->get_type() == f_atan) - { - f.x = boost::make_shared(); - f.y = boost::make_shared(); - f.z = boost::make_shared(); - f.th = boost::make_shared(); - } - f.x->set(c.x_min, c.x_max, c.x_p); - f.y->set(c.y_min, c.y_max, c.y_p); - f.z->set(c.z_min, c.z_max, c.z_p); - f.th->set(c.th_min, c.th_max, c.th_p); - ROS_INFO("parameters reconfigured"); - } - -}; - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "commander_atan"); - ros::NodeHandle node("commander_atan"); - - ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); - Run run(cmd_pub); - ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&Run::reconfigure, &run, _1, _2); - server.setCallback(f); - - ROS_INFO("start spinning"); - ros::spin(); - return 0; -} - -const float Run::Atan::demi_pi = 2*atan(1); diff --git a/src/data.h b/src/data.h deleted file mode 100644 index 8fe89bc..0000000 --- a/src/data.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef DATA_H -#define DATA_H -template -struct Data -{ - public: - T x, y, z, th; -}; -#endif diff --git a/src/keyboard_azerty.py b/src/keyboard_azerty.py deleted file mode 100755 index 8e62ffb..0000000 --- a/src/keyboard_azerty.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -import roslib;# roslib.load_manifest('teleop_twist_keyboard') -import rospy - -from geometry_msgs.msg import Twist -from std_msgs.msg import Empty - -import sys, select, termios, tty - -msg = """ - --------------------- -takeoff>| t|⇑ y|↖ u|↑ i|↗ o| - |---|---|---|---|---|---- - reset>| g|⇐ h|← j| k|→ l|⇒ m| - |---|---|---|---|---|---- - land>| b|⇓ n|↙ ,|↓ ;|↘ :| - --------------------- - -a/w : increase/decrease max speeds by 10% -z/x : increase/decrease only linear speed by 10% -e/c : increase/decrease only angular speed by 10% -anything else : stop - -CTRL-C to quit -""" - -moveBindings = { - # x th y z - 'i':(1,0,0,0), - 'o':(1,-1,0,0), - 'j':(0,1,0,0), - 'l':(0,-1,0,0), - 'u':(1,1,0,0), - ';':(-1,0,0,0), - ':':(-1,1,0,0), - ',':(-1,-1,0,0), - 'h':(0,0,-1,0), - 'm':(0,0,1,0), - 'y':(0,0,0,1), - 'n':(0,0,0,-1), - } - -speedBindings={ - 'a':(1.1,1.1), - 'w':(.9,.9), - 'z':(1.1,1), - 'x':(.9,1), - 'e':(1,1.1), - 'c':(1,.9), - } - -def getKey(): - tty.setraw(sys.stdin.fileno()) - select.select([sys.stdin], [], [], 0) - key = sys.stdin.read(1) - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key - -speed = 0.1 -turn = 0.5 - -def vels(speed,turn): - return "currently:\tspeed %s\tturn %s " % (speed,turn) - -if __name__=="__main__": - settings = termios.tcgetattr(sys.stdin) - pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) - land = rospy.Publisher('/ardrone/land', Empty, queue_size=1) - takeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1) - reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1) - rospy.init_node('keyboard_azerty') - x = 0 - th = 0 - y = 0 - z = 0 - status = 0 - try: - print msg - print vels(speed,turn) - while(1): - key = getKey() - if (key == 't'): - takeoff.publish(Empty()) - print "takeoff" - elif (key == 'g'): - reset.publish(Empty()) - print "reset" - elif (key == 'b'): - land.publish(Empty()) - print "land" - else: - if key in moveBindings.keys(): - x = moveBindings[key][0] - th = moveBindings[key][1] - y = moveBindings[key][2] - z = moveBindings[key][3] - elif key in speedBindings.keys(): - speed = speed * speedBindings[key][0] - turn = turn * speedBindings[key][1] - print vels(speed,turn) - if (status == 14): - print msg - status = (status + 1) % 15 - else: - x = 0 - th = 0 - y = 0 - z = 0 - if (key == '\x03'): - break - twist = Twist() - twist.linear.x = x*speed - twist.linear.y = y*speed - twist.linear.z = z*speed - twist.angular.x = 0; twist.angular.y = 0; - twist.angular.z = th*turn - pub.publish(twist) - except: - print e - finally: - twist = Twist() - twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 - twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 - pub.publish(twist) - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - diff --git a/src/pcl_displayer.cpp b/src/pcl_displayer.cpp index 1fc78d4..6f96468 100644 --- a/src/pcl_displayer.cpp +++ b/src/pcl_displayer.cpp @@ -1,3 +1,4 @@ +// a not very useful test tool #include #include #include diff --git a/src/random_pcl_publisher.cpp b/src/random_pcl_publisher.cpp index 874a408..ea40b53 100644 --- a/src/random_pcl_publisher.cpp +++ b/src/random_pcl_publisher.cpp @@ -1,3 +1,4 @@ +// a not very useful test programm #include #include #include