Solving + Param
This commit is contained in:
parent
d7897cf7d0
commit
f0545db439
4 changed files with 31 additions and 4 deletions
|
@ -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
19
launch/all-abs.launch
Normal 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>
|
|
@ -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()
|
||||
|
|
|
@ -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,16 +51,17 @@ 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) :
|
||||
|
|
Loading…
Reference in a new issue