Hexacopter impedance control

Hi everyone,
I am trying to implement impedance control in my underactuated hexacopter. I encountered two questions:
1- How can I publish my control outputs that are 6D wrenches to the available mavros topics?
2- Which topics are more logical to be used in this way? I tried to usemavros/actuatorcontrol. To test it, I started to write a simple attitude+rate controller by myself, normalize the attitude, and thrust to publish it on this topic. But it doesn’t work fine. I receive high attitude errors and the drone goes to instability. Is there any code explanation or procedure for using this topic?
ROS kinetic
ubuntu 16.04

@hanieh Here is an example of the offboard rate controller: https://github.com/Jaeyoung-Lim/mavros_controllers

You can maybre reuse all the infrastructure and implement your own controller by adding an option here: https://github.com/Jaeyoung-Lim/mavros_controllers/blob/78171c2d8506328ec82b12bf10e59f7ce6e9f384/geometric_controller/src/geometric_controller.cpp#L340

Thanks, Jaeyoung. I am trying to find out how to use it for my wrenches.
Besides that, do you have any suggestions for using mavros/actuatorcontrol? I was trying to use it on my attitude controller, but I couldn’t find detailed documentation/examples on this topic (Except for the short mavros documentation).

@hanieh Using actuator controls for offboard control is not a good engineering choice.

You are limited in your communication bandwidth and relying on a serial link on a real system which is unsafe - basically you are flying in openloop. Therefore going with something like bodyrate control results in much more robust results