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("/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](http://answers.ros.org/question/11655/help-publishing-to-cmd_vel-and-subscribing-to-laserscan/) 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?
↧