Hey ROS community, I want a simulated drone in Gazebo to perform a task in the xz plan, without touching at the y axis. I want the drone to face the y axis perpendicular to the xz plan, and not to change orientation by giving a waypoint. Here is my python script to publish a ros message with a moveto method, and also an extract of the moveto_action src file.

import rospy
from std_msgs.msg import String
import actionlib
from uav_mavros_takeoff.msg import TakeoffAction, TakeoffGoal
from mavros_moveto.msg import MovetoAction, MovetoGoal
from tf.transformations import quaternion_from_euler
import sys
import random

def planner_client():
    client_takeoff = actionlib.SimpleActionClient('command/takeoff', TakeoffAction)
    client_moveto = actionlib.SimpleActionClient('command/moveto', MovetoAction)

    client_takeoff.wait_for_server()
    goal_takeoff = TakeoffGoal(altitude=1.0)
    client_takeoff.send_goal(goal_takeoff)
    client_takeoff.wait_for_result()

    client_moveto.wait_for_server()
    goal_moveto = MovetoGoal()


    quaternion = quaternion_from_euler(0, 0, -1.57) # roll, pitch, yaw

    waypoints_xz = [[(x,z) for x in range(0,15,5)] for z in reversed(range(0,10,5))]
    waypoints_y = 0
    for i in range(len(waypoints_xz)):
        for j in range(len(waypoints_xz[0])):
            goal_moveto.goal.pose.position.x = waypoints_xz[i][j][0]
            goal_moveto.goal.pose.position.z = waypoints_xz[i][j][1]
            goal_moveto.goal.pose.position.y = waypoints_y

            goal_moveto.goal.pose.orientation.x = quaternion[0]
            goal_moveto.goal.pose.orientation.y = quaternion[1]
            goal_moveto.goal.pose.orientation.z = quaternion[2]
            goal_moveto.goal.pose.orientation.w = quaternion[3]

            print(quaternion)
            print(waypoints_xz[i][j][0], waypoints_xz[i][j][1], waypoints_y)
            client_moveto.send_goal(goal_moveto)
            client_moveto.wait_for_result()

    return 0

if __name__ == '__main__':
    rospy.init_node('planner_client')
    result = planner_client()
    #except rospy.ROSInterruptException:
        #print("program interrupted before completion", file=sys.stderr)

C++:

#include "moveto_action.h"

/**
 * @brief Whether the goal has been reached.
 * @param goal The goal pose to reach.
 * @return Whether the current position is closer that pos_tolerance to the goal.
 */
bool reached (geometry_msgs::PoseStamped goal) {
    double dx = position.pose.position.x - goal.pose.position.x;
    double dy = position.pose.position.y - goal.pose.position.y;

    return dx*dx + dy*dy < pos_tolerance*pos_tolerance;
}

/**
 * @brief Callback function to receive the position of the CPS.
 * @param msg The local position in cartesian coordinates.
 */
void position_cb (const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    position = *msg;
    position_received = true;
    ROS_DEBUG_ONCE("MOVETO - Got local position: (%.2f,%.2f,%.2f)", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);

    static tf2_ros::TransformBroadcaster br;
    geometry_msgs::TransformStamped transformStamped;

    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "map";
    transformStamped.child_frame_id = "base_link";
    transformStamped.transform.translation.x = msg->pose.position.x;
    transformStamped.transform.translation.y = msg->pose.position.y;
    transformStamped.transform.translation.z = msg->pose.position.z;
    transformStamped.transform.rotation = msg->pose.orientation;

    br.sendTransform(transformStamped);
}

/**
 * @brief Callback to execute the move to action server.
 */
bool execute_cb (const mavros_moveto::MovetoGoal::ConstPtr& goal, Server* as)
{
    ROS_DEBUG("MOVETO - Executing move to action...");

    Rate rate(loop_rate);

    // goal position
    geometry_msgs::PoseStamped goal_position;
    goal_position.pose.position.x = goal->goal.pose.position.x;
    goal_position.pose.position.y = goal->goal.pose.position.y;
    goal_position.pose.position.z = goal->goal.pose.position.z;

    // communication to client
    mavros_moveto::MovetoFeedback feedback;
    mavros_moveto::MovetoResult result;

    // move
    while (ok() && as->isPreemptRequested() == false && reached(goal_position) == false) {
        // publish goal to fcu
        goal_pub.publish(goal_position);

        // publish feedback to client
        feedback.pose = position;
        as->publishFeedback(feedback);

        // wait
        rate.sleep();
        spinOnce();
    }

    ROS_DEBUG("MOVETO - Reached position (%.2f, %.2f, %.2f)", position.pose.position.x, position.pose.position.y, position.pose.position.z);

    // communicate result
    if (reached(goal_position)) {
        result.reached = true;
        as->setSucceeded(result);
    }
    else if (as->isPreemptRequested()) {
        result.reached = false;
        as->setPreempted(result);
    }
    else {
        result.reached = false;
        as->setAborted(result);
    }

    return true;
}

/**
 * @brief A ROS node that provides the move to action server.
 * @param argc Number of command line arguments.
 * @param argv Array of command line arguments.
 * @return Success.
 */
int main(int argc, char **argv) {
    init(argc, argv, "moveto_action");
    NodeHandle nh;

    // read parameters
    nh.getParam(this_node::getName() + "/loop_rate", loop_rate);
    int queue_size;
    nh.getParam(this_node::getName() + "/queue_size", queue_size);
    nh.getParam(this_node::getName() + "/pos_tolerance", pos_tolerance);

    // ros communication
    Subscriber pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", queue_size, position_cb);
    goal_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", queue_size);

    Rate rate(loop_rate);

    // wait for position information
    while (ok() && !position_received) {
        ROS_DEBUG_ONCE("MOVETO - Waiting for local position...");
        spinOnce();
        rate.sleep();
    }
    ROS_DEBUG("MOVETO - Local position received");


    // provide action server
    Server server(nh, "command/moveto", boost::bind(&execute_cb, _1, &server), false);
    server.start();
    ROS_DEBUG("MOVETO - Action server available");
    spin();

    return 0;
}