Louis-Guillaume DUBOIS
11 years ago
5 changed files with 2 additions and 323 deletions
@ -1,187 +0,0 @@ |
|||
// this code doesn’t work
|
|||
// :-(
|
|||
#include <ros/ros.h> |
|||
#include <ros/time.h> |
|||
#include <locale.h> |
|||
#include <limits> |
|||
#include <math.h> |
|||
#include <assert.h> |
|||
|
|||
#include <pcl_ros/point_cloud.h> |
|||
#include <pcl/point_types.h> |
|||
|
|||
#include <hand_control/Plan.h> |
|||
#include <geometry_msgs/Twist.h> |
|||
#include <math.h> |
|||
|
|||
#include <dynamic_reconfigure/server.h> |
|||
#include <hand_control/Commander_atanConfig.h> |
|||
#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<boost::shared_ptr<Function> > 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<Lin>(); |
|||
f.y = boost::make_shared<Lin>(); |
|||
f.z = boost::make_shared<Lin>(); |
|||
f.th = boost::make_shared<Lin>(); |
|||
} |
|||
|
|||
void callback(const hand_control::Plan::ConstPtr& msg) |
|||
{ |
|||
ROS_INFO("plan received"); |
|||
Data<float> 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<Atan>(); |
|||
f.y = boost::make_shared<Atan>(); |
|||
f.z = boost::make_shared<Atan>(); |
|||
f.th = boost::make_shared<Atan>(); |
|||
} else if (!c.atan and f.x->get_type() == f_atan) |
|||
{ |
|||
f.x = boost::make_shared<Lin>(); |
|||
f.y = boost::make_shared<Lin>(); |
|||
f.z = boost::make_shared<Lin>(); |
|||
f.th = boost::make_shared<Lin>(); |
|||
} |
|||
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<geometry_msgs::Twist>("/cmd_vel", 1); |
|||
Run run(cmd_pub); |
|||
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run); |
|||
|
|||
dynamic_reconfigure::Server<hand_control::Commander_atanConfig> server; |
|||
dynamic_reconfigure::Server<hand_control::Commander_atanConfig>::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); |
|||
@ -1,9 +0,0 @@ |
|||
#ifndef DATA_H |
|||
#define DATA_H |
|||
template <typename T> |
|||
struct Data |
|||
{ |
|||
public: |
|||
T x, y, z, th; |
|||
}; |
|||
#endif |
|||
@ -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) |
|||
|
|||
Loading…
Reference in new issue