From 8a867f9189de61fdcb853f1e2818da26ec085f2f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 25 Aug 2014 23:29:27 +0400 Subject: [PATCH] plugin: command: Add COMMAND_INT suport. Fix #98. --- mavros/CMakeLists.txt | 1 + mavros/scripts/mavcmd | 40 ++++++++++++++++++++- mavros/src/plugins/command.cpp | 65 ++++++++++++++++++++++++++++++---- mavros/srv/CommandInt.srv | 17 +++++++++ 4 files changed, 116 insertions(+), 7 deletions(-) create mode 100644 mavros/srv/CommandInt.srv diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt index 2bcfa6e8a..ca455f6dd 100644 --- a/mavros/CMakeLists.txt +++ b/mavros/CMakeLists.txt @@ -78,6 +78,7 @@ add_service_files( WaypointPush.srv WaypointGOTO.srv CommandLong.srv + CommandInt.srv CommandBool.srv CommandMode.srv CommandHome.srv diff --git a/mavros/scripts/mavcmd b/mavros/scripts/mavcmd index a216655d1..7d1406fd4 100755 --- a/mavros/scripts/mavcmd +++ b/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") diff --git a/mavros/src/plugins/command.cpp b/mavros/src/plugins/command.cpp index 4e3fdc20b..59e62b9d8 100644 --- a/mavros/src/plugins/command.cpp +++ b/mavros/src/plugins/command.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -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) { diff --git a/mavros/srv/CommandInt.srv b/mavros/srv/CommandInt.srv new file mode 100644 index 000000000..05efb2f6b --- /dev/null +++ b/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