How to change position speed in offboard mode

[harewei]

Hello

I'm working with gazebo simulator right now (still really new to ros and mavros), and with mavros/setpoint_position/local, I can move the copter to the desired position. However, I can't find how to change the velocity and orientation for the vehicle.

I found http://mavlink.org/messages/common#SET_POSITION_TARGET_LOCAL_NED, but have no idea whether this is related or how to use it.

Could someone give me some help here?

[vooon]

For that you should form message yourself using setpoint_raw and mavros_msgs/PositionTarget. Note that enu-ned transform applied to fields.

[harewei]

Hi vooon, my code before (shortened) had:

ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
geometry_msgs::PoseStamped desired_pose;
desired_pose.pose.position.x = 0;
    desired_pose.pose.position.y = 0;
    desired_pose.pose.position.z = 0;

and offboard was achieved with

// Need to send a few pose before can switch to offboard mode
                for(int i = 100; ros::ok() && i > 0; --i)
                {
                    local_pos_pub.publish(desired_pose);
                    ros::spinOnce();
                    rate.sleep();
                }
                ChangeMode(action, set_mode_client);

Now I attempted to change the above to use setpoint_raw

ros::Publisher setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);
mavros_msgs::PositionTarget targetPosition;
    targetPosition.position.x = 0;
    targetPosition.position.y = 0;
    targetPosition.position.z = 1;

and attempt offboard with

// Need to send a few pose before can switch to offboard mode
                for(int i = 100; ros::ok() && i > 0; --i)
                {
                    setpoint_raw_local_pub.publish(targetPosition);
                    ros::spinOnce();
                    rate.sleep();
                }
                ChangeMode(action, set_mode_client);

I receive an error when I try to go offboard

[ INFO] [1453102165.046590372]: FCU: [cmd] DISARMED by set mode command
[ERROR] [1453102165.096102691]: FCU: Rejecting mode switch cmd
[ERROR] [1453102165.146516310]: FCU: command temporarily rejected: 176

[Jaeyoung-Lim]

You are not sending the time with the messages

You have to send the header information within the topic(which includes time, sequence .etc)

[vooon]

Please use code block formatters (triple `).

You should set header.stamp, coordinate_frame and type_mask.

[harewei]

Sorry about not using code block before, I tried with 4 space indent and ctrl+K as on stackoverflow, but it doesn't work here.

anyway, I modified the code

mavros_msgs::PositionTarget targetPosition;
targetPosition.position.x = 0;
targetPosition.position.y = 0;
targetPosition.position.z = 1;
targetPosition.coordinate_frame = 1;
targetPosition.type_mask = 0b0000000000000000;

but can't quite figure out the header.stamp bit. I read on http://docs.ros.org/api/std_msgs/html/msg/Header.html that it has seq, stamp and frame_id.
Does seq just mean an ever increase counter? (e.g. int counter++ every loop, but what happens when it overflows).
For frame_id, since I'm using FRAME_LOCAL_NED, I assume I can set it to 0.

As for time stamp, I have no idea how that works, so it would be helpful if someone can show me how that's done.

Thanks.

[andre-nguyen]

targetPosition.header.stamp = ros::Time::now();

[harewei]

thanks andre-nguyen

My newest code is now

mavros_msgs::PositionTarget targetPosition;
targetPosition.position.x = 0;
targetPosition.position.y = 0;
targetPosition.position.z = 1;
targetPosition.coordinate_frame = 1;
targetPosition.type_mask = 0b0000000000000000;
targetPosition.header.frame_id = "0";

and

int seq = 0;
while(ros::ok())
{   
    seq++;
    if (seq > 50000)
        seq = 0;
    targetPosition.header.stamp = ros::Time::now();

    if (action == "offboard")
    {       
        // Need to send a few pose before can switch to offboard mode
        for(int i = 100; ros::ok() && i > 0; --i)
        {
            //local_pos_pub.publish(desired_pose);
            setpoint_raw_local_pub.publish(targetPosition);
            ros::spinOnce();
            rate.sleep();
        }
        ChangeMode(action, set_mode_client);
    }

    setpoint_raw_local_pub.publish(targetPosition);

    ros::spinOnce();
    rate.sleep(); 
}

It's still giving me the same error as below when I attempt to switch to OFFBOARD mode

[ WARN] [1453164374.565037941]: CMD: Unexpected command 176, result 1
[ERROR] [1453164374.611809389]: FCU: Rejecting mode switch cmd
[ERROR] [1453164374.662224840]: FCU: command temporarily rejected: 176

[andre-nguyen]

What flight stack are you using?

Have you tried this example: http://dev.px4.io/ros-mavros-offboard.html

[harewei]

PX4 flight stack

I tried that example, it works. From my second post here from the top, if I use

ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);

instead of

ros::Publisher setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);

the simulation runs (offboard, arm, setting xyz etc)

I only changed to setpoint_raw/local because I wished to changed the velocity of the drone when moving to desired positions, and that's what @vooon suggested I use.

[andre-nguyen]

Hhhhm, I don't have experience with setpoint_raw however, I'm confident whatever code on the firmware side is processing the messages is the same that processes local position setpoints.

this however

targetPosition.type_mask = 0b0000000000000000;

doesn't seem quite right. Try disabling anything not related to velocity and position.

[harewei]

I tried changing to

targetPosition.type_mask = 0b0001111111111111;

since I'm only giving out pos information for now, but the result is still the same.

[andre-nguyen]

Well, I have no idea then! :sweat_smile:

Maybe try this mask https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_position.cpp#L92. If you use that you should have the "same code" as what sends local position setpoints. If even that doesn't work then maybe the problem is on the vehicle side.

[harewei]

changing to that that mask still doesn't work. but thanks for your help all the same!

it seems this setpoint_raw thing is pretty new, like october 2015, and I couldn't find a single code on how people use it (closest thing I found was along the lines of "I got this working", which isn't exactly useful...)

guess I'll settle with pre-determined speed for now I guess.

[andre-nguyen]

Well if you want to do velocity control (I guess you could write your own position controller which sends velocity setpoints) you can use http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.setpoint_velocity However, I'm a bit puzzled by your problem because in the end, it all uses the same mavlink message and the same set_position_target_local_ned mavros function...

[vooon]

If i understand correctly @harewei wants to use both position and speed controls, that's why setpoint_raw as other plugins use predefined ignore mask.

Using setpoint_raw is pretty same if you will write your own code to send SET_abc_TARGET_xyz. For ignore mask you may use constants from message:

using mavros_msgs::PositionTarget;

void foo() {
    PositionTarget pt;
    pt.header.stamp = ros::Time::now();
    pt.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
    pt.type_mask = PositionTarget::IGNORE_AFX | PositionTarget::IGNORE_AFY | PositionTarget::IGNORE_AFZ |
        PositionTarget::IGNORE_YAW | PositionTarget::IGNORE_YAW_RATE;
}

[harewei]

For some reason, I could not use setpoint_raw to change to offboard mode in gazebo simulator, but is able to do so using a real drone. It's been raining like crazy lately so I haven't got a chance to test this function in real life though, will do so in the future. thanks.

[magiccjae]

@vooon You are probably the best one to answer my question. What is the best way to give X, Y velocity, Yaw rate while the rotor is holding its altitude? I have tried to publish to /setpoint_velocity/cmd_vel topic with (this is my pseudo code)

msg.twist.linear.x = my_vx msg.twist.linear.y = my_vy msg.twist.linear.z = 0 # because I wanted the rotor to hold its altitude msg.twist.angular.z = my_yawrate and this made the rotor to gradually fall to the ground, not holding its altitude.

Also, I have tried to publish to /setpoint_raw/local topic with

msg.coordinate_frame = 1 msg.type_mask = 2019 # xy velocity, z position, yaw rate msg.velocity.x = my_vx msg.velocity.y = my_vy msg.position.z = my_desired_pz msg.yaw_rate = 0

When I switch to OFFBOARD mode, this doesn't follow my velocity and position command while the rotor is just wandering(I can't tell what the rotor is doing, but it slowly flies to somewhere not falling). I haven't set msg.header.stamp, but even without this, position control is working using /setpoint_raw/local topic.

What other things can I try? or anything I am doing wrong to use /setpoint_velocity/cmd_vel or /setpoint_raw/local topics?

Thank you for your help !

[vooon]

1. Always set Header.stamp! Else you may facing unpredictable problems.
2. cmd_vel problem perhaps related to throttle topic
3. Partial field disabling not implemented in firmware, only full vectors.

Also note that i don't have copters, only planes...

[TSC21]

@magiccjae are you sure you are sending a stream of setpoints to the FCU? You should subscribe to local_position/pose to do a position control based on velocity, meaning, if the vehicle is not in the position you want, calculate an offset between the current position and the desired position and normalized it to a threshold, so you don't get an overshoot (even though MPC_XY_CRUISE parameter would deal with this but I'm not sure).

[magiccjae]

@vooon What do you mean by partial field disabling not implemented in firmware? Does that mean that in order to control X,Y velocity control, Z position, I need enable(not ignore in the type_mask) Z velocity, X, Y positions as well in /setpoint_raw/local topic?

@TSC21 My understanding was that if I give velocity command, FCU closes the loop to move the rotor with the commanded velocity(this hasn't been the case for Z-axis). Are you saying that I need to close the loop on my ROS code?

[TSC21]

If you send a velocity setpoint, the controller will put you at that velocity. The stop condition depends on what you desire. The FCU closes the loop for velocity yes, but if you want to go to a position but setup a velocity controller in ROS side, you need the feedback from the FCU.

[vooon]

I mean that firmware only can ignore whole vector (xyz) of position, velocity & accel. Not by vec.[xyz]

[TSC21]

The most important thing is that you send a stream of setpoints so that the OFFBOARD mode stays on and the control loop functions properly. If not, the FCU will refuse to go to OFFBOARD.

[magiccjae]

@vooon So if I want to command X,Y velocity, Z position, Do I need to set my mask so that both position and velocity (xyz) are not ignored and then only give my values to X,Y velocity and Z position? This makes me wonder why /setpoint_raw exist...

@TSC21 Thank you for your comment ! I see your point : )

[TSC21]

@magiccjae np. Check if this helps you: https://dev.px4.io/en/ros/mavros_offboard.html. It is for position but the logic is similar.

[TSC21]

setpoint_raw basically works with custom MAVROS msgs specific for the purpose while the specific plugins for each setpoint support ROS msg types.

[akshaykulkarni07]

I don't see how the original issue was solved. How to set the speed of the drone when you are giving position setpoints through /mavros/setpoint_raw/local in offboard mode? Please advise on this. Thanks.

[TSC21]

@akshaykvnit you have the answer above. You just have to set the proper bitmask and set MPC_XY_CRUISE PX4 parameter to the max horizontal speed (means that the controller will put the vehicle at that max speed between positions).

[hariharvin]

@TSC21 i have set the proper bit mask such that only position and velocity bit are used remaining all are ignored. I set MPC_XY_CRUISE to max horizontal speed. But when i said vehicle to go to particular position with particular velocity it is taking the velocity values but it is not going to given position (moving further from given position).what could be the reason ?

PX4 website it has shown that both velocity and position controller are cascaded (Velocity is depending on position setpoint) .

Is above can be the reason it not is not taking the position setpoint ?

[rng18-boop]

@hariharvin I am sending velocity setpoints to my drone (i.e controlling velocity of drone). Form what I have understand, it is not possible to control both position and velocity at the same time using setpoints. So it makes sense that your drone is moving with the specified velocity using velocity setpoints but not going to the given position.

I think the drone is moving away from given position since the controller is not perfect. So in the process of trying to follow the velocity setpoints, it messes up position as there is no way of keeping track of where it is going. This explanation may be incorrect so take it with a grain of salt.

I am sending velocity setpoints first, then exiting the loop and sending position setpoints to correct the offset between my desired and current position.

[jlunsan]

1. Always set Header.stamp! Else you may facing unpredictable problems.
2. cmd_vel problem perhaps related to throttle topic
3. Partial field disabling not implemented in firmware, only full vectors.

Also note that i don't have copters, only planes...

@vooon Could you help me controlling the velocity in ArduPlane with mavros? It does not follow my velocity neither publishing in /setpoint_velocity/cmd_vel nor /setpoint_raw.

I have just reached to control it using attitude publishing in /setpoint_raw/attitude. If there is any example in C++ for these cases it would be great.