Problem setting modes/cmd_vel in a Navio2/Rasp3/Boat setup using mavros

Hello, I am working with ROS/mavros/navio2/rasp3/RC, and I am having some trouble to understand how to set modes, submodes, or twist topics using C++ code.

So far I was able to:

  1. Move the robot using standard remote control
  2. Change between 3 modes (RTL, MANUAL, and AUTO) in the remote control
  3. Take over the control using mavros/rc topics, control servos, and then give the control back to RC
  4. Set home successfully
  5. Define and change waypoints

Even though I believe the outcome so far was interesting, I still cannot:

  1. Change modes using code
  2. Set cmd_vel using Twists/TwistStamped

I am stilI in the beginning, but I have implemented all sorts of services and topics for this purpose, including but not limited to Arming, SetMode, Disabling fence, setting CommandInt and CommandLong, Setting SYSID_GCS, parameter set and get, and so on. I even tried the service takeoff (which fails and does not seem to make sense for boats). All that, but without success to use cmd_vel or changing modes.

The problem is not at publishing Twist topics, which I do, the real problem is that they have no effect on the boat. I have read over the web and this forum, and it seems I must set Guided mode (or submode flag) to enable cmd_vel or even OFFBOARD mode. However, SetMode has effect on the actual mode. That is, no matter what I set in mode or custom. the service returns okay even though mavros/state never changes.

First question:

  1. Is there any way to SetMode from code? (this is really important to my project)
  2. What is the proper set of steps to use setpoint cmd_vel plugins? Not only publishing Twists, but also setting stuff such as Guided/OFFBOARD modes, setting specific submodes, disabling fencing, arming, setmavframe, and so on. So far there is no example I could find and the docs on OFFBOARD/Guided modes or Guided submode did not help me.

Here are some of the ideas I had for setting Guided mode:
bool WaypointControl::setGuidedMode(bool val)
client = n.serviceClient<mavros_msgs::CommandInt>("/mavros/cmd/command_int");
mavros_msgs::CommandInt guidedService;

guidedService.request.frame = 1;
guidedService.request.command = 92;
    guidedService.request.param1 = 1.0; // > 0.5 enable
    guidedService.request.param1 = 0.0; // <= 0.5 disable

if (
        ROS_INFO("Commanded USV to toogle GUIDED MODE");
        ROS_ERROR("GUIDED MODE set service was called, but returned false");
    ROS_ERROR("Failed to call GUIDED MODE set service");

client = n.serviceClient<mavros_msgs::CommandInt>("/mavros/cmd/command_int");
guidedService.request.frame = 1;
guidedService.request.command = 4000; // set guided submode to add velocities

if (
        ROS_INFO("Commanded USV to toogle GUIDED SUBMODE");
        ROS_ERROR("GUIDED SUBMODE set service was called, but returned false");
    ROS_ERROR("Failed to call SUBGUIDED MODE set service");
    return false;

return guidedService.response.success;


Here is How I try SetMode

bool WaypointControl::setMode(unsigned char mode, std::string custom)
{ // Base Modes
//unsigned char MAV_MODE_PREFLIGHT = 0
//unsigned char MAV_MODE_STABILIZE_DISARMED = 80
//unsigned char MAV_MODE_STABILIZE_ARMED = 208
//unsigned char MAV_MODE_MANUAL_DISARMED = 64
//unsigned char MAV_MODE_MANUAL_ARMED = 192
//unsigned char MAV_MODE_GUIDED_DISARMED = 88
//unsigned char MAV_MODE_GUIDED_ARMED = 216
//unsigned char MAV_MODE_AUTO_DISARMED = 92
//unsigned char MAV_MODE_AUTO_ARMED = 220
//unsigned char MAV_MODE_TEST_DISARMED = 66
//unsigned char MAV_MODE_TEST_ARMED = 194

// Custom Modes are strings which are specific to each unmanned vehicle
//ex: APM:Rover
//Numeric	String
//4		HOLD
//10	AUTO
//11	RTL

// Value Meaning on mav planner
// 0 Manual
// 1 Acro
// 3 Steering
// 4 Hold
// 5 Loiter
// 6 Follow
// 7 Simple
// 10 Auto
// 11 RTL
// 12 SmartRTL
// 15 Guided

client = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
mavros_msgs::SetMode setModeService;

    setModeService.request.base_mode = 0;
    setModeService.request.custom_mode = custom;
    setModeService.request.base_mode = mode;
if ( {
        ROS_ERROR("SetMode called but failed: %d", setModeService.response.mode_sent);
        ROS_INFO("Mode set to: %d %s",mode, custom.c_str());
} else {
    ROS_ERROR("Failed call SetMode");
    return false;
return setModeService.response.mode_sent;


I am no expert on the subject and maybe I am missing something.
Can you help me?

Hi Vitor,

Sorry for the late reply!

It’d be interesting to see the result of your work. May I ask you to tell more about your application?

After you change the mode with one of these topics, is the mode changes in mavros/state?

These standard topics should work. I think there can be something that doesn’t allow to change mode, however, it’s hard to say certainly where the mistake is.

Dear Tatiana,

Even though the setMode method does not return an error, it is not able to change anything when we analyze the mavros/state.

However, I realized I am indeed able to taleover the remote. And when the control
Is able to set mode alone, I can emulate RC behavior. So far the best thing I could achieve is to make the user in RC change mode to GUIDED, and then I perform ROS cmd_vel control. I have concluded that RC takeover is way too risky (sometimes giving back control fails for unknown reasons).

Therefore, I have abandoned any method that denies radio comms (which is prohibited by law here). Also, I quit trying to use the setmode or any other alternative way to enable
guided mode, or any other mode, using MAVROS.

If anyone REALLY needs to change mode using setmode and is facing the same problem I recommend you config a channel just for changing modes and take over that RC channel only.

Still, I am quite disappointed I am not able to change modes using setmode. If you have any clue, please feel free to give me your opinion or suggestion, thanks.

Hi Vitor,

As far as I know, some modes can be set only if you use RC transmitter.

Have you tried to reach out to Ardupilot or ROS forum? It’s possible there can be somebody who tried to implement something similar and might help you with it.

This topic was automatically closed 100 days after the last reply. New replies are no longer allowed.