plugin: command: Add guided_enable shortcut

It enable PX4 OFFBOARD mode.

Issue #126.

mavros/scripts/mavcmd

@@ -24,7 +24,7 @@ import argparse
 import threading
 
 import rospy
-from mavros.srv import CommandLong, CommandHome, CommandTOL
+from mavros.srv import CommandLong, CommandHome, CommandTOL, CommandBool
 from sensor_msgs.msg import NavSatFix
 
 
@@ -195,6 +195,18 @@ def do_land_cur_gps(args):
         fault("Something went wrong. Topic timed out.")
 
 
+def do_guided_en(args):
+    rospy.init_node("mavcmd", anonymous=True)
+
+    try:
+        guided_enable_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/guided_enable", CommandBool)
+        ret = guided_enable_cl(value=args.enable)
+    except rospy.ServiceException as ex:
+        fault(ex)
+
+    _check_ret(args, ret)
+
+
 def main():
     parser = argparse.ArgumentParser(description="Commad line tool for sending commands to MAVLink device.")
     parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros")
@@ -251,6 +263,12 @@ def main():
     land_cur_args.add_argument('yaw', type=float, help="Desired Yaw")
     land_cur_args.add_argument('altitude', type=float, help="Altitude")
 
+    guided_args = subarg.add_parser('guided', help="Request enable OFFBOARD mode (PX4)")
+    guided_args.set_defaults(func=do_guided_en)
+    guided_group = guided_args.add_mutually_exclusive_group()
+    guided_group.add_argument('-e', '--enable', dest='enable', action='store_true', default=True, help="enable OFFBOARD mode (default)")
+    guided_group.add_argument('-d', '--disable', dest='enable', action='store_false', help="disable OFFBOARD mode")
+
     args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
     args.func(args)
 

mavros/src/plugins/command.cpp

@@ -76,6 +76,7 @@ class CommandPlugin : public MavRosPlugin {
 		set_home_srv = cmd_nh.advertiseService("set_home", &CommandPlugin::set_home_cb, this);
 		takeoff_srv = cmd_nh.advertiseService("takeoff", &CommandPlugin::takeoff_cb, this);
 		land_srv = cmd_nh.advertiseService("land", &CommandPlugin::land_cb, this);
+		guided_srv = cmd_nh.advertiseService("guided_enable", &CommandPlugin::guided_cb, this);
 	}
 
 	std::string const get_name() const {
@@ -99,6 +100,7 @@ class CommandPlugin : public MavRosPlugin {
 	ros::ServiceServer set_home_srv;
 	ros::ServiceServer takeoff_srv;
 	ros::ServiceServer land_srv;
+	ros::ServiceServer guided_srv;
 
 	std::list<CommandTransaction *> ack_waiting_list;
 	static constexpr int ACK_TIMEOUT_MS = 5000;
@@ -286,6 +288,15 @@ class CommandPlugin : public MavRosPlugin {
 				req.latitude, req.longitude, req.altitude,
 				res.success, res.result);
 	}
+
+	bool guided_cb(mavros::CommandBool::Request &req,
+			mavros::CommandBool::Response &res) {
+
+		return send_command_long_and_wait(MAV_CMD_NAV_GUIDED_ENABLE, 1,
+				(req.value)? 1.0 : 0.0,
+				0, 0, 0, 0, 0, 0,
+				res.success, res.result);
+	}
 };
 
 }; // namespace mavplugin