Problem reading the second value of setpoint - package pid (ROS) - c++

Question

I was trying to use the package pid (http://wiki.ros.org/pid) to control the stability of a drone (Parrot AR drone 2.0).
I created a launch that use one controller for each axis (x,y and z):
<node name="controller" pkg="pid" type="controller" ns="pid_x" output="screen" >
<param name="node_name" value="pid_x" />
<param name="Kp" value="-0.0887" />
<param name="Ki" value="0.0" /> <!-- Arranca en cero pero es 0.16597 -->
<param name="Kd" value="0.0" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<remap from="setpoint" to="/setpoint_x" />
<remap from="state" to="/ardrone/odometry/pose/pose/position/x" />
<remap from="/pid_x/control_effort" to="/publicador_intermedio_x" />
</node>
<node name="controller" pkg="pid" type="controller" ns="pid_y" output="screen" >
<param name="node_name" value="pid_y" />
<param name="Kp" value="0.0763" />
<param name="Ki" value="0.0" /> <!-- Arranca en cero pero es 0.11037 -->
<param name="Kd" value="0.0" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<remap from="setpoint" to="/pid_y/setpoint_y" />
<remap from="state" to="/pid_y/state" />
<remap from="/pid_y/control_effort" to="/publicador_intermedio_y" />
</node>
<node name="controller" pkg="pid" type="controller" ns="pid_z" output="screen" >
<param name="node_name" value="pid_z" />
<param name="Kp" value="0.9827" />
<param name="Ki" value="0.0" />
<param name="Kd" value="0.0" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<remap from="setpoint" to="/setpoint_z" />
<remap from="state" to="/ardrone/odometry/pose/pose/position/z" />
<remap from="/pid_z/control_effort" to="/publicador_intermedio_z" />
</node>
</launch>
I was just checking how it works using just the second controller, so please don't mind the other ones. The state takes information from the topic pid_y/state that reads the /ardrone/odometry/pose/pose/position and publishes the value of y (if u want to see the node I can post it). Then, pid_controller is publishing in /publicador_intermedio_x that it's a node that works like a conection betweeen the outcome of the controller and the topic /cdm_vel of the drone (i mean, the topic where you need to publish in order to controlling the drone). Finally the last one is the big problem: setpoint. In the pid package we can find an example in order to understand how it works. So I copied the node that publishes the setpoint values in order to make a test. It looks like this:
#include <ros/ros.h>
#include <std_msgs/Float64.h>
int main(int argc, char** argv)
{
//inicio comunicacion con sistema ROS
ros::init(argc, argv, "publicador");
ROS_INFO("Starting setpoint publisher");
ros::NodeHandle nh;
while (ros::ok() && ros::Time(0) == ros::Time::now())
{
ROS_INFO("Setpoint_node spinning, waiting for time to become non-zero");
sleep(1);
}
std_msgs::Float64 contador;
contador.data = 1.0;
//creamos un objeto publicador
ros::Publisher pub = nh.advertise<std_msgs::Float64>("/pid_y/setpoint_y", 1000);
ros::Rate loop_rate(10); // acomodar a frecuencia que no sature, pero q sea mas rapida que publicacion odometria
while (ros::ok())
{
ros::spinOnce();
pub.publish(contador);
contador.data = contador.data + 1;
pub.publish(contador);
ROS_INFO("Valor publicado: [%f]", contador);
loop_rate.sleep(); //duermo hasta proxima iteracion
}
Now that I already explained the code, this is what isn’t working: I run the pid with the nodes that it needs. The launch reads correctly the setpoint and the state (the odometry) but it doesn't publish because:
prev_time is 0, doing nothing
If i understood the code correctly, It’s because prev time uses the time between two consecutive values to calculate the error. That's ok. But when the launch must to read the second setpoint value it never does it. It stays in
Waiting first setpoint value
and never read it, even when the node is publishing the new value. I can't understand what I'm doing wrong.
Can anyone help me?