Ask Your Question
0

mavros set offboard mode

asked 2017-11-30 05:36:07 -0500

MaximeRector gravatar image

updated 2017-12-01 05:46:30 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2017-11-30 08:30:43 -0500

updated 2017-11-30 13:42:45 -0500

I worked more than a year ago with MAVROS. please, be not so strict, if something wrong.

1) It is possible to control air MAVROS vehicle (PX4 stack) in offboard mode ONLY with position or attitude commands. I used the second one. Control attitude(roll, pitch, yaw) and throttle.

ros::Publisher mav_att_pub = nh.advertise<geometry_msgs::PoseStamped>( "mavros/setpoint_attitude/attitude", 100 );
ros::Publisher mav_thr_pub = nh.advertise<std_msgs::Float64>( "mavros/setpoint_attitude/att_throttle", 100 );

current vehicle orientation you may take here:

ros::Subscriber mav_imu_data_sub = nh.subscribe<sensor_msgs::Imu>( "mavros/imu/data", 100, mav_imu_data_callback );

send angles you may force the vehicle to change its position and orientation in XY plane. send throttle command from PID regulator with a distance or atm pressure sensor in the loop you may control altitude.

2) you MUST periodically send commands to the vehicle because there are PX4 save timer witch change control mode from offboard to stabilize.

3) use this one as starting point

4) use save channel with RC control with kill and change mode switchers for emergency situations

5) set stream rate ros::ServiceClient stream_rate_client = nh.serviceClient<mavros_msgs::streamrate>("mavros/set_stream_rate");

mavros_msgs::StreamRate streamRate;

streamRate.request.stream_id = 0;
streamRate.request.message_rate = 100;
streamRate.request.on_off = 1;

if (true == stream_rate_client.call(streamRate))
{
    ROS_INFO("OK. Stream rate set");
}
else
{
    ROS_INFO("Failed to call service: stream rate");
}

6) use high speed (921600) for serial MAVLINK port and rosrate 50 or higher for control commands

ros::Rate rate( 50.0 );

7) count ROS messages and set ros::Time::now()

full attitude and throttle msgs

// --------------------------------------
// PUB
// --------------------------------------
ros::Publisher mav_att_pub = nh.advertise<geometry_msgs::PoseStamped>( "mavros/setpoint_attitude/attitude", 100 );
ros::Publisher mav_thr_pub = nh.advertise<std_msgs::Float64>( "mavros/setpoint_attitude/att_throttle", 100 );

// attitude
geometry_msgs::PoseStamped cmd_att;
//throttle
std_msgs::Float64 cmd_thr;

// Create attitude command message
cmd_att.header.stamp = ros::Time::now();
cmd_att.header.seq = count;
cmd_att.pose.position.x = 0.0;
cmd_att.pose.position.y = 0.0;
cmd_att.pose.position.z = 0.0;

// here your desired angles
tf::Quaternion mav_orientation = tf::createQuaternionFromRPY( roll, pitch, yaw );

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

mav_att_pub.publish( cmd_att );

// here your throttle value a [0,1]
cmd_thr.data = a;

mav_thr_pub.publish( cmd_thr );

++count;
edit flag offensive delete link more

Comments

Can you be more specific with the altitude commands, so changing the velocity won't work in OFFBOARD mode?

MaximeRector gravatar imageMaximeRector ( 2017-11-30 09:37:09 -0500 )edit

I could not control using velocity commands

0xd1ma gravatar image0xd1ma ( 2017-11-30 10:21:01 -0500 )edit

Mmm.So if i understand your way or idea, You are going to orientate your drone by changing the angles of yaw, pitch and roll and then increase or decrease the throttle to move? So with this way you don't use a gps, or am I wrong? I suppose this is the same way with the APM firmware?

MaximeRector gravatar imageMaximeRector ( 2017-11-30 10:43:58 -0500 )edit

Do you think this would work on APM firmware? Because my pixhawk behaves extremely strange when using px4. I haven't not been able to find a way to search in on google.

MaximeRector gravatar imageMaximeRector ( 2017-11-30 11:24:09 -0500 )edit

you may change angles and move in one plane. it was an indoor project. you may use GPS with standard position mode of PX4. I tried to use APM but prefer PX4.

0xd1ma gravatar image0xd1ma ( 2017-11-30 13:47:11 -0500 )edit

try to use GAZEBO simulation to debug your first steps

0xd1ma gravatar image0xd1ma ( 2017-11-30 13:48:52 -0500 )edit

My first choice is also PX4, but at this moment I can't get it to work :/ Have already tested something in gazebo and will definitely keep using it

MaximeRector gravatar imageMaximeRector ( 2017-11-30 16:29:47 -0500 )edit

Can you look at my new edit (EDIT2)? I'm really appreciating your help!

MaximeRector gravatar imageMaximeRector ( 2017-12-01 05:46:59 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-11-30 05:36:07 -0500

Seen: 1,633 times

Last updated: Dec 01 '17