Ask Your Question
1

How to subscribe to a topic, while publishing to another

asked 2016-01-06 18:42:26 -0500

Borob gravatar image

updated 2016-01-06 19:47:13 -0500

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?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-01-06 18:55:14 -0500

fsteinhardt gravatar image

updated 2016-01-07 04:59:58 -0500

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.

edit flag offensive delete link more

Comments

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.

Borob gravatar image Borob  ( 2016-01-06 19:50:29 -0500 )edit
0

answered 2017-03-13 12:26:11 -0500

AA gravatar image

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="">

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-01-06 18:42:26 -0500

Seen: 658 times

Last updated: Mar 13 '17