plugin: command: Add COMMAND_INT suport.

Fix #98.

mavros/CMakeLists.txt

@@ -78,6 +78,7 @@ add_service_files(
    WaypointPush.srv
    WaypointGOTO.srv
    CommandLong.srv
+   CommandInt.srv
    CommandBool.srv
    CommandMode.srv
    CommandHome.srv

mavros/scripts/mavcmd

@@ -24,7 +24,7 @@ import argparse
 import threading
 
 import rospy
-from mavros.srv import CommandLong, CommandHome, CommandTOL, CommandBool
+from mavros.srv import CommandLong, CommandInt, CommandHome, CommandTOL, CommandBool
 from sensor_msgs.msg import NavSatFix
 
 
@@ -64,6 +64,30 @@ def do_long(args):
     _check_ret(args, ret)
 
 
+def do_int(args):
+    rospy.init_node("mavcmd", anonymous=True)
+
+    try:
+        int_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/command_int", CommandInt)
+        ret = int_cl(frame=args.frame, command=args.command,
+                     current=int(args.current),
+                     autocontinue=int(args.autocontinue),
+                     param1=args.param1,
+                     param2=args.param2,
+                     param3=args.param3,
+                     param4=args.param4,
+                     x=args.x,
+                     y=args.y,
+                     z=args.z)
+    except rospy.ServiceException as ex:
+        fault(ex)
+
+    if not ret.success:
+        fault("Request failed. Check mavros logs.")
+
+    print_if(args.verbose, "Request done.")
+
+
 def do_set_home(args):
     rospy.init_node("mavcmd", anonymous=True)
 
@@ -225,6 +249,20 @@ def main():
     long_args.add_argument('param6', type=float, help="param6 / y_long")
     long_args.add_argument('param7', type=float, help="param7 / z_alt")
 
+    int_args = subarg.add_parser('int', help="Send any command (COMMAND_INT)")
+    int_args.set_defaults(func=do_int)
+    int_args.add_argument('-c', '--current', action='store_true', help="Is current?")
+    int_args.add_argument('-a', '--autocontinue', action='store_true', help="Is autocontinue?")
+    int_args.add_argument('-f', '--frame', type=int, default=3, help="Frame Code (default: %(default)s)")
+    int_args.add_argument('command', type=int, help="Command Code")
+    int_args.add_argument('param1', type=float, help="param1")
+    int_args.add_argument('param2', type=float, help="param2")
+    int_args.add_argument('param3', type=float, help="param3")
+    int_args.add_argument('param4', type=float, help="param4")
+    int_args.add_argument('x', type=int, help="latitude in deg*1E7 or X*1E4 m")
+    int_args.add_argument('y', type=int, help="longitude in deg*1E7 or Y*1E4 m")
+    int_args.add_argument('z', type=float, help="altitude in m, depending on frame")
+
     # Note: arming service provided by mavsafety
 
     set_home_args = subarg.add_parser('sethome', help="Request change home position")

mavros/src/plugins/command.cpp

@@ -30,6 +30,7 @@
 #include <pluginlib/class_list_macros.h>
 
 #include <mavros/CommandLong.h>
+#include <mavros/CommandInt.h>
 #include <mavros/CommandBool.h>
 #include <mavros/CommandMode.h>
 #include <mavros/CommandHome.h>
@@ -71,6 +72,7 @@ class CommandPlugin : public MavRosPlugin {
 
 		cmd_nh = ros::NodeHandle(nh, "cmd");
 		command_long_srv = cmd_nh.advertiseService("command", &CommandPlugin::command_long_cb, this);
+		command_int_srv = cmd_nh.advertiseService("command_int", &CommandPlugin::command_int_cb, this);
 		arming_srv = cmd_nh.advertiseService("arming", &CommandPlugin::arming_cb, this);
 		set_mode_srv = cmd_nh.advertiseService("set_mode", &CommandPlugin::set_mode_cb, this);
 		set_home_srv = cmd_nh.advertiseService("set_home", &CommandPlugin::set_home_cb, this);
@@ -95,6 +97,7 @@ class CommandPlugin : public MavRosPlugin {
 
 	ros::NodeHandle cmd_nh;
 	ros::ServiceServer command_long_srv;
+	ros::ServiceServer command_int_srv;
 	ros::ServiceServer arming_srv;
 	ros::ServiceServer set_mode_srv;
 	ros::ServiceServer set_home_srv;
@@ -196,6 +199,29 @@ class CommandPlugin : public MavRosPlugin {
 		return true;
 	}
 
+	/**
+	 * Common function for COMMAND_INT service callbacks.
+	 */
+	bool send_command_int(uint8_t frame, uint16_t command,
+			uint8_t current, uint8_t autocontinue,
+			float param1, float param2,
+			float param3, float param4,
+			int32_t x, int32_t y,
+			float z,
+			unsigned char &success) {
+
+		/* Note: seems that COMMAND_INT don't produce COMMAND_ACK
+		 * so wait don't needed.
+		 */
+		command_int(frame, command, current, autocontinue,
+				param1, param2,
+				param3, param4,
+				x, y, z);
+
+		success = true;
+		return true;
+	}
+
 	/* -*- low-level send -*- */
 
 	void command_long(uint16_t command, uint8_t confirmation,
@@ -209,16 +235,33 @@ class CommandPlugin : public MavRosPlugin {
 				UAS_PACK_TGT(uas),
 				command,
 				confirmation,
-				param1,
-				param2,
-				param3,
-				param4,
-				param5,
-				param6,
+				param1, param2,
+				param3, param4,
+				param5, param6,
 				param7);
 		UAS_FCU(uas)->send_message(&msg);
 	}
 
+	void command_int(uint8_t frame, uint16_t command,
+			uint8_t current, uint8_t autocontinue,
+			float param1, float param2,
+			float param3, float param4,
+			int32_t x, int32_t y,
+			float z) {
+		mavlink_message_t msg;
+
+		mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
+				UAS_PACK_TGT(uas),
+				frame,
+				command,
+				current,
+				autocontinue,
+				param1, param2,
+				param3, param4,
+				x, y, z);
+		UAS_FCU(uas)->send_message(&msg);
+	}
+
 	/* -*- callbacks -*- */
 
 	bool command_long_cb(mavros::CommandLong::Request &req,
@@ -232,6 +275,16 @@ class CommandPlugin : public MavRosPlugin {
 				res.success, res.result);
 	}
 
+	bool command_int_cb(mavros::CommandInt::Request &req,
+			mavros::CommandInt::Response &res) {
+		return send_command_int(req.frame, req.command,
+				req.current, req.autocontinue,
+				req.param1, req.param2,
+				req.param3, req.param4,
+				req.x, req.y, req.z,
+				res.success);
+	}
+
 	bool arming_cb(mavros::CommandBool::Request &req,
 			mavros::CommandBool::Response &res) {
 

mavros/srv/CommandInt.srv

@@ -0,0 +1,17 @@
+# Generic COMMAND_INT
+
+uint8 frame
+uint16 command
+uint8 current
+uint8 autocontinue
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float32 x	# latitude in deg * 1E7 or local x * 1E4 m
+float32 y	# longitude in deg * 1E7 or local y * 1E4 m
+float32 z	# altitude
+---
+bool success
+# seems that this message don't produce andy COMMAND_ACK messages
+# so no result field