Solving + Param

This commit is contained in:
_Luc_ 2015-05-27 20:10:33 +02:00
parent d7897cf7d0
commit f0545db439
4 changed files with 31 additions and 4 deletions

View file

@ -10,4 +10,7 @@ gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement co
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 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("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", "Commande"))

19
launch/all-abs.launch Normal file
View file

@ -0,0 +1,19 @@
<launch>
<include file="$(find hand_control)/launch/all-param.launch" />
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator_pca">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<node name="commander" pkg="hand_control" type="commande-abs">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

View file

@ -65,7 +65,7 @@ class Run
mvt->angular.z * angle_vel;
}
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
assert(mvt->linear.x != 0. || mvt->linear.y != 0.);
pub.publish(mvt);
ROS_INFO("cmd published");
}//end publish
@ -117,6 +117,9 @@ class Run
neutral_z = c.neutral_alt;
min_number = c.min_points_number;
up_factor = c.up_fact;
plan_vel = c.plan_vel;
z_vel = c.z_vel;
angle_vel = c.angle_vel;
}
void run()

View file

@ -17,6 +17,7 @@ class Callback {
void
operator()(const PointCloud::ConstPtr& msg)
{
if (msg->width >3){
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
ROS_INFO("PointCloud received");
@ -50,17 +51,18 @@ class Callback {
x = v(0); y=v(1); z=v(2);
h = (analyser.getMean())(3);
h = (analyser.getMean())(2);
//h = altitude(msg);
th = -90
+ 2 * atan (eg(2,1)
th =
2 * atan (eg(2,1)
/(1 + eg(1,1)));
// 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) {}