rm or comments useless files
This commit is contained in:
Louis-Guillaume DUBOIS
parent
a70483de20
commit
7103ef6277
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)
|
|
||||||
|
|
||||||
|
|
@ -1,3 +1,4 @@
|
||||||
|
// a not very useful test tool
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <pcl_ros/point_cloud.h>
|
#include <pcl_ros/point_cloud.h>
|
||||||
#include <pcl/point_types.h>
|
#include <pcl/point_types.h>
|
||||||
|
|
|
||||||
|
|
@ -1,3 +1,4 @@
|
||||||
|
// a not very useful test programm
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <pcl_ros/point_cloud.h>
|
#include <pcl_ros/point_cloud.h>
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue