# How to subscribe to a topic, while publishing to another

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::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?

edit retag close merge delete

Sort by » oldest newest most voted

OK, so I misunderstood your problem.

Your function flyToTarget has a never ending while loop, it will not return to the callback and the callback doesn't finish. Get rid of the loop in the callback, you could move the ros loop into your main function. Then publish only a single /cmd_vel per callback.

more

Thank you for your answer, I edited my question to make clear what I mean. Do I have to run the callback in another thread so I can simultaniously access the position error? Or is the callback non blocking? I thought spin() would block.

( 2016-01-06 19:50:29 -0500 )edit

you can subscribe to the current position also in the loop.

You mentioned that you want the control loop to stop under a certain error - add it to you while loop as calculatePositionError<e as="" a="" stopping="" condition.<="" p="">

more