I’m using an acceleration PD control for a quad simulation on ROS Gazebo using MAVROS PX4 SITL. I have the given the x and y accelerations in the feed-forward command and use the position and velocity for the feedback. The target is to reach a pre-defined setpoint. The quad just hovers at a fixed altitude and doesn’t reach the target position.
Here is my code snippet of subscribed nodes and published nodes:
# Subscribe to drone's local position rospy.Subscriber('mavros/local_position/pose', PoseStamped, cnt.posCb) # Subscribe to drone's velocity rospy.Subscriber("/mavros/local_position/velocity", TwistStamped , cnt.velCb) # Setpoint publisher sp_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)`
The code gives the required acceleration setpoints to the quad as shown by
rostopic echo /mavros/setpoint_raw/local
` --- header: seq: 1620 stamp: secs: 0 nsecs: 0 frame_id: '' coordinate_frame: 1 type_mask: 1080 position: x: 0.0 y: 0.0 z: 4.0 velocity: x: 0.0 y: 0.0 z: 0.0 acceleration_or_force: x: 1.60403175801 y: 1.69204383269 z: 0.0 yaw: 0.0 yaw_rate: 0.0`
I’ve attached the screenshot of the hovering drone.