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:
- Move the robot using standard remote control
- Change between 3 modes (RTL, MANUAL, and AUTO) in the remote control
- Take over the control using mavros/rc topics, control servos, and then give the control back to RC
- Set home successfully
- Define and change waypoints
Even though I believe the outcome so far was interesting, I still cannot:
- Change modes using code
- 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:
- Is there any way to SetMode from code? (this is really important to my project)
- 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;
if(val)
guidedService.request.param1 = 1.0; // > 0.5 enable
else
guidedService.request.param1 = 0.0; // <= 0.5 disable
if (client.call(guidedService))
{
if(guidedService.response.success)
ROS_INFO("Commanded USV to toogle GUIDED MODE");
else
ROS_ERROR("GUIDED MODE set service was called, but returned false");
}
else
{
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 (client.call(guidedService))
{
if(guidedService.response.success)
ROS_INFO("Commanded USV to toogle GUIDED SUBMODE");
else
ROS_ERROR("GUIDED SUBMODE set service was called, but returned false");
}
else
{
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
// http://wiki.ros.org/mavros/CustomModes
//ex: APM:Rover
//Numeric String
//0 MANUAL
//2 LEARNING
//3 STEERING
//4 HOLD
//10 AUTO
//11 RTL
//15 GUIDED
//16 INITIALISING
// 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;
if(!custom.empty())
{
setModeService.request.base_mode = 0;
setModeService.request.custom_mode = custom;
}
else
{
setModeService.request.base_mode = mode;
}
if (client.call(setModeService)) {
if(!setModeService.response.mode_sent)
ROS_ERROR("SetMode called but failed: %d", setModeService.response.mode_sent);
else
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?
Thanks,
VJ