How to subscribe to a topic, while publishing to another

edit

Hey guys,

I want to write a basic controller for a quadrotor. For a target_position it should take the current_position from a simulated quadrotor, calculate an error between the two and send the command to the quadrotor to go to the position.

The current position is a topic of type PoseStamped. (/ground_truth_to_tf/pose). The topic to control the velocity is of type Twist (/cmd_vel).

My idea was: I subscribe to the current_position, and if the error is bigger than some threshold, I correct the movement. So, in the callback of the current_position topic, I want to publish a newly corrected velocity.

But here is the problem: I only want to publish one message, but on every callback from my subscription. I can't however only send one message, so I am stuck.

The code would look something like this:

//Callback method of the current_position
void positionFeedback(const geometry_msgs::PoseStamped::ConstPtr &msg, geometry_msgs::Pose *target)
{   
    geometry_msgs::Pose current_pose = msg->pose;
    geometry_msgs::Point direction = calculatePositionError(current_pose, *target);

    float distance = std::sqrt(std::pow(direction.x, 2) + std::pow(direction.y, 2)  + std::pow(direction.z, 2));
    if (distance > POSITION_ACCURACY){
        flyToTarget(direction); //method that publishes the velocity to the topic /cmd_vel
    }
}

void flyToTarget(geometry_msgs::Point target_position){
    ros::NodeHandle steer;
    ros::Publisher helmsman = steer.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
    ros::Rate loop_rate(10);
    int count = 0;
    while (ros::ok())
    {
        geometry_msgs::Twist new_vel;
        float multiplier = 3;
        new_vel.linear.x = - target_position.x * multiplier;
        new_vel.linear.y = - target_position.y * multiplier;
        new_vel.linear.z = - target_position.z * multiplier;
        helmsman.publish(new_vel);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
}

So my question is, how can I do this loop? I only use basic ros stuff, this can't be that hard?

//Edit:

To clarify:

I want to make the velocity of the quadrotor dependant on it's current position. (I want to scale the velocity with a function that smoothly lowers the velocity as the quadrotor approaches the target). So I thought about invoking the velocity-update on the callback of the position:

positionCallback1( calculateAndPublishNewVelocity(...) )
positionCallback2( calculateAndPublishNewVelocity(...) )
...
positionCallbackN( calculateAndPublishNewVelocity(...) ) // distance to target smaller than my POSITION_ACCURACY

But, when I do this for the first time, my publish call never terminates, so I am stuck with:

positionCallback1(calculateAndPublishNewVelocity(...) ...

My question is: How would this be done in general? Is my approach the right one? I don't know how to escape the publish part. As I understand it, it sends messages to the topic until it is terminated by the user or via shutdown. I essentially only want to send one message for everytime the callback of the current_position is received. Right now I get segmentation fault.

There is a 5 year old question on ros.answers on how to send only one message to a topic, but it seems that there is no straight forward approach to do this.

So, how would this be done?