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

subscriber in commande works

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-07 20:15:04 +02:00
parent 021119a896
commit 191c614fda
2 changed files with 40 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
launch/all.launch
View file

@ -3,16 +3,22 @@
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/> <include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<param name="/filtre/zmax" value="1.1" type="double"/>
<param name="/filtre/hue" value="0.0" type="double"/>
<param name="/filtre/delta_hue" value="10.0" type="double"/>
<node name="filtre" pkg="hand_control" type="filtre"> <node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/> <remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node> </node>
<param name="/filtre/zmax" value="1.1" type="double"/> <node name="estimator" pkg="hand_control" type="normal_estimator">
<param name="/filtre/hue" value="0.0" type="double"/>
<param name="/filtre/delta_hue" value="15.0" type="double"/>
<node name="normal_estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/> <remap from="/estimator/input" to="/filtre/output"/>
</node> </node>
<!--
<node name="commander" pkg="hand_control" type="commande">
<remap from="/commander/input" to="/estimator/output"/>
</node>
-->
</launch> </launch>

34
src/commande.cpp
View file

@ -53,43 +53,65 @@ class Run
void operator()(const hand_control::Plan::ConstPtr& msg) void operator()(const hand_control::Plan::ConstPtr& msg)
{ {
ROS_INFO("plan received");
if (msg->curvature < max_curv) if (msg->curvature < max_curv)
{ {
t_current = msg->header.stamp; t_current = msg->header.stamp;
z_current = msg->normal.z; z_current = msg->normal.z;
if (!first_msg) if (!first_msg)
{
dz = (z_current - z_previous)/((t_current - t_previous).toSec()); dz = (z_current - z_previous)/((t_current - t_previous).toSec());
ROS_INFO("dz = %f", dz);
}
xx = msg->normal.x; xx = msg->normal.x;
ROS_INFO("xx = %f", xx);
yy = msg->normal.y; yy = msg->normal.y;
ROS_INFO("yy = %f", yy);
t_previous = t_current; t_previous = t_current;
z_previous = z_current; z_previous = z_current;
z_current = std::numeric_limits<double>::signaling_NaN(); z_current = std::numeric_limits<double>::signaling_NaN();
t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN(); t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
if (first_msg) first_msg = false; if (first_msg)
{
first_msg = false;
ROS_INFO("first msg received");
}
ROS_INFO("coords updated");
} }
}; };
void run() void run()
{ {
geometry_msgs::Twist zero; ROS_INFO("node running");
zero.linear.x = zero.linear.y = zero.linear.z =
zero.angular.x = zero.angular.y = zero.angular.z = 0.;
while(ros::ok()) while(ros::ok())
{ {
ROS_INFO("loop entered");
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
*mvt = zero;
ROS_INFO("dz : %f > %f : dz_min ?", dz, dz_min);
if (dz > dz_min) if (dz > dz_min)
{
mvt->linear.z = dz * z_vel; mvt->linear.z = dz * z_vel;
ROS_INFO("z changed");
}
ROS_INFO("xx : %f > %f : x_dev_min ?", xx, x_dev_min);
if (xx > x_dev_min) if (xx > x_dev_min)
{
mvt->linear.x = xx * plan_vel; mvt->linear.x = xx * plan_vel;
ROS_INFO("x changed");
}
ROS_INFO("yy : %f > %f : y_dev_min ?", yy, y_dev_min);
if (yy > y_dev_min) if (yy > y_dev_min)
{
mvt->linear.y = yy * plan_vel; mvt->linear.y = yy * plan_vel;
ROS_INFO("y changed");
}
pub.publish(mvt); pub.publish(mvt);
ros::spinOnce(); ros::spinOnce();
@ -135,7 +157,7 @@ int main(int argc, char** argv)
} }
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, 0.05, 0.05, 0.05); Run run(cmd_pub, max_curv, plan_vel, z_vel, 0.0, 0.0, 0.0);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, run); ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, run);
run.run(); run.run();
return 0; return 0;