position control of uav

edit

Hi guys , i would like to create a node for position control. It should get current position values and the target position values . For control i want to use a common PID Control for each coordinate.The node should output cmd_velocity message (geometry_msgs/Twist topic)

Is there maybe ready code for my goal? Can i write a simple publisherfor that goal? So it is possible that a common publisher communicate with a gazebo plugin? How the code might look like?

thanks adrian