LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

with «angle» & «theta» between -90 and 90

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-27 23:28:43 +02:00
parent 6f4e6eda41
commit 264dca57e2
3 changed files with 81 additions and 105 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
cfg/Commander.cfg
View file

@ -6,11 +6,11 @@ gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane"
gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.)
gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.)
gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 0)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 0)
gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1)
gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 180.)
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.)
gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.)
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.)
gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.)
gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.)
exit(gen.generate(PACKAGE, "hand_control", "Commander"))

24
src/commander.cpp
View file

@ -20,8 +20,6 @@ class Run
{
private:
float xx, yy, zz, theta;
float DEMI_PI;
// xx < 0 : forward
// xx > 0 : backward
@ -61,10 +59,10 @@ class Run
{
mvt->linear.x = - xx * plan_vel;
}
if (fabs(theta) > th_dev_min) {
mvt->angular.z = theta * angle_vel;
}
if (fabs(theta) > th_dev_min) {
mvt->angular.z = theta * angle_vel;
}
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
pub.publish(mvt);
@ -74,16 +72,15 @@ class Run
public:
Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher)
{
DEMI_PI = 2*atan(1.);
}
{
}
void callback(const hand_control::Plan::ConstPtr& msg)
{
ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
{
if(msg->normal.z > 0)
{
yy = msg->normal.x;
@ -97,7 +94,8 @@ class Run
zz = msg->altitude - neutral_z;
theta = msg->angle - DEMI_PI;
theta = msg->angle;
// theta between -90 and 90
if (first_msg)
{
@ -127,7 +125,7 @@ class Run
void run()
{
ros::spin();
ros::spin();
}
};
@ -140,7 +138,7 @@ int main(int argc, char** argv)
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::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);

154
src/estimator.cpp
View file

@ -14,118 +14,96 @@ typedef Eigen::Matrix3f& Matrix;
class Callback {
public:
void
operator()(const PointCloud::ConstPtr& msg)
void operator()(const PointCloud::ConstPtr& msg)
{
if (msg->width >3){
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
ROS_INFO("PointCloud received");
float x, y, z, th, h, c;
x = y = z = th = h = c = 0.;
if (msg->width > 3){
// indices : tout le PointCloud
std::vector<int> indices;
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
float x, y, z, th, h, c;
x = y = z = th = h = c = 0.;
// we consider the whole PointCloud
std::vector<int> indices;
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
// vérifier signature
//estimator.computePointNormal(*msg, indices, x, y, z, c);
//Produit vectoriel des deux première colonnes de Matrix3f
/* x = eg(2,1)*eg(3,2)
- eg(3,1)*eg(2,2);
y = eg(3,1)*eg(1,2)
- eg(1,1)*eg(3,2);
z = eg(1,1)*eg(2,2)
- eg(2,1)*eg(1,2);*/
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
v.normalize();
/*
x = x/sqrt(x*x+y*y+z*z);
y = x/sqrt(x*x+y*y+z*z);
z = x/sqrt(x*x+y*y+z*z);
*/
x = v(0); y=v(1); z=v(2);
h = (analyser.getMean())(2);
//h = altitude(msg);
th =
2 * atan (eg(1,0)
/(1 + eg(0,0)));
// v = eg_1 ^ eg_2 is the plan normal
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
// norm(v) == 1
v.normalize();
x = v(0); y=v(1); z=v(2);
// publication
ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
// h is the altitude
h = (analyser.getMean())(2);
// this formula is good only if :
// -pi/2 <= th <= pi/2
// ie cos(th) == m_x >= 0
float m_x(eg(0,0));
float m_y(eg(1,0));
if(x < 0)
{
m_x *= -1.;
m_y *= -1.;
}
th = 2 * atan(m_y / (1 + m_x));
// -pi/2 <= th <= pi/2
th *= _RAD2DEG;
// -90 <= th <= 90
// publication
ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
}
}
Callback(ros::Publisher& pub) :
publisher(pub) {}
Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){};
private:
ros::Publisher publisher;
pcl::PCA<Point> analyser;
//pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
inline
const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& th,
const float& c, const uint32_t& seq,
const uint64_t& msec64, const uint64_t& number)
{
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x;
ros_msg->normal.y = y;
ros_msg->normal.z = z;
ros_msg->altitude = h;
ros_msg->angle = th;
ros_msg->curvature = c;
ros_msg->number = number;
// uint64_t msec64 is in ms (10-6)
uint64_t sec64 = msec64 / 1000000;
uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0";
return ros_msg;
}
const float _RAD2DEG;
/* inline
float
altitude(const PointCloud::ConstPtr& pcl)
{
int s = pcl->points.size();
float h(0);
for (int i = 0; i < s; ++i)
h += pcl->points[i].z;
return h/( (float) s );
getmean
getVector4fMap
}*/
inline const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& th,
const float& c, const uint32_t& seq,
const uint64_t& msec64, const uint64_t& number)
{
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x;
ros_msg->normal.y = y;
ros_msg->normal.z = z;
ros_msg->altitude = h;
ros_msg->angle = th;
ros_msg->curvature = c;
ros_msg->number = number;
// uint64_t msec64 is in ms (10-6)
uint64_t sec64 = msec64 / 1000000;
uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0";
return ros_msg;
}
};
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "normal_estimator_pca");
ros::NodeHandle node("estimator");//`A vérifier ?
ros::init(argc, argv, "estimator");
ros::NodeHandle node("estimator");
// initialisation
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");