For the MPC, you can publish to /mavros/setpoint_raw/local, basically geometry_msgs/Point position, geometry_msgs/Vector3 velocity, geometry_msgs/Vector3 acceleration_or_force, float32 yaw, float32 yaw_rate format these all can be found in the mavros message mavros_msgs/PositionTarget Documentation
Then after you have your trajectory consisting of Pos, Vel, Acc, and yaw then you can fill the message up and publish
// Publish Setpoints to MAVROS
Position_Setpoint_Pub = _nh.advertise<mavros_msgs::PositionTarget>(
"/mavros/setpoint_raw/local", 10);
...
// with your trajectory message as msg
mavros_msgs::PositionTarget _sp;
_sp.type_mask = 32768; // check first on the type mask in the link that you want to use
_sp.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
_sp.position.x = msg->pos.x;
_sp.position.y = msg->pos.y;
_sp.position.z = msg->pos.z;
_sp.velocity.x = msg->vel.x;
_sp.velocity.y = msg->vel.y;
_sp.velocity.z = msg->vel.z;
_sp.acceleration_or_force.x = msg->acc.x;
_sp.acceleration_or_force.y = msg->acc.y;
_sp.acceleration_or_force.z = msg->acc.z;
Position_Setpoint_Pub.publish(pos_sp);
Really nice answer. Just want to ask what if I don’t have all of this info? For example, my vehicle model does not want to use acceleration/force but only velocity and position. Is it okay to not fill in the acceleration/force part? Will the message just assume them (i.e. acceleration)?
I notice it might do with the type_mask term, but could you explain how the type mask work?
For example, say I want to only use position (x, y, z), how should I get the type mask? Thank you very much! looking for your reply!!