mavros set offboard mode

edit

Hi,

I'm trying to control a pixhawk via mavros. I have written a test node which set the mode to offboard, arm the drone and send some mavros/setpoint_velocity/cmd_vel messages to change the z velocity. When I run the node the arming works (motors start spinning) but that's it, nothing more. I can't find the problem, This is my first time using mavros, so it's all new for me.

EDIT While running my code I did rostopic echo /mavros/state which results in:

---
header: 
  seq: 6144
  stamp: 
    secs: 1512042153
    nsecs: 557924943
  frame_id: ''
connected: True
armed: True
guided: False
mode: "STABILIZE"
system_status: 3
---

So I believe that setting offboard mode isn't working.

My code:

#include "classheader_dronecontrol.hpp"

DroneControl::DroneControl(ros::NodeHandle n)
{
    initRos(n);
}

void DroneControl::initRos(ros::NodeHandle n)
{
    cout << "Start Initialising" << endl;
    this->state_sub = n.subscribe<mavros_msgs::State>("mavros/state", 10, &DroneControl::stateCallback, this);
    this->arming_client = n.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    this->set_mode_client = n.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    this->KFarucomarkerpos_sub = n.subscribe("droneposition", 1, &DroneControl::droneControlCallback, this);
    this->move_pub = n.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel",10);

    cout << "Initialising done!" << endl;

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    this->move_msg.twist.linear.z = 1.0;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        this->move_pub.publish(this->move_msg);
        ros::spinOnce();
        rate.sleep();
    }

    this->offb_set_mode.request.custom_mode = "OFFBOARD";
    this->arm_cmd.request.value = true;
    last_request = ros::Time::now();

    this->set_mode_client.call(this->offb_set_mode);
    this->offb_set_mode.response.mode_sent;
    ROS_INFO("Offboard enabled");
    this->move_pub.publish(this->move_msg);
    ros::spinOnce();
    this->arming_client.call(this->arm_cmd);
    ROS_INFO("Vehicle armed");
    this->move_pub.publish(this->move_msg);

    for(int i = 100; ros::ok() && i > 0; --i){
        this->move_pub.publish(this->move_msg);
        ros::spinOnce();
        rate.sleep();
    }

    ros::spinOnce();
}

void DroneControl::stateCallback(const mavros_msgs::State::ConstPtr& msg)
{
    this->current_state = *msg;
}

EDIT2

I used the provided starting point by 0xd1ma and used his answer for following code. It's a small program where I used roll = 30°, pitch = 30°, yaw = 0° and throttle 0.9. The messages will be published 500 times. The drone (in gazebo) armed perfectly and moved for 5 seconds with a velocity of 0.25 in z-axes (just some test code to get him of the ground), after this the testDroneControl() function gets called, but the drone starts moving randomly and for sure not in the chosen direction.

void DroneControl::testDroneControl()
{
    ros::Rate rate(50.0);

    this->cmd_att.pose.position.x = 0.0;
    this->cmd_att.pose.position.y = 0.0;
    this->cmd_att.pose.position.z = 0.0;

    tf::Quaternion mav_orientation = tf::createQuaternionFromRPY(0.523598775, 0.523598775, 0 );

    this->cmd_att.pose.orientation.x = mav_orientation.x();
    this->cmd_att.pose.orientation.y = mav_orientation.y();
    this->cmd_att.pose.orientation.z = mav_orientation.z();
    this->cmd_att.pose.orientation.w = mav_orientation.w();

    this->cmd_thr.data = 0.9;

    for(int i=0; i<500; i++){
        cout << this->cmd_att.pose ...