Robotics StackExchange | Archived questions

Translate rosrun command into roslaunch file

Hi, I am new to ROS and I am try to create a roslaunch file. I want to translate the command rosrun mavros mavsys message_interval --id 30 --rate 200 into roslaunch file so that I only need to use launch file to set the message interval.

Here is my code:

 <node pkg="mavros" type="mavsys" name="mavsys_message_interval" output="screen"> <param name="id" value="30" />
        <param name="rate" value="200" />
    </node>

When I launch the file, it successfully finds the source file that mavros uses to set the message_interval, however, it keep giving me this error:

Traceback (most recent call last):
 File "/home/junjieg2/catkin_ws/src/mavros/mavros/scripts/mavsys", line 143, in main()
 File "/home/junjieg2/catkin_ws/src/mavros/mavros/scripts/mavsys", line 139, in args.func(args)
 AttributeError: 'Namespace' object has no attribute 'func'

Here is the source code for mavros:

from __future__ import print_function

import argparse
import threading

import rospy
import mavros
from mavros.utils import *
from mavros_msgs.msg import State
from mavros_msgs.srv import SetMode, StreamRate, StreamRateRequest, MessageInterval

def do_mode(args):
base_mode = 0
custom_mode = ''

if args.custom_mode is not None:
    custom_mode = args.custom_mode.upper()
if args.base_mode is not None:
    base_mode = args.base_mode

done_evt = threading.Event()
def state_cb(state):
    print_if(args.verbose, "Current mode:", state.mode)
    if state.mode == custom_mode:
        print("Mode changed.")
        done_evt.set()

if custom_mode != '' and not custom_mode.isdigit():
    # with correct custom mode we can wait until it changes
    sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb)
else:
    done_evt.set()

try:
    set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
    ret = set_mode(base_mode=base_mode, custom_mode=custom_mode)
except rospy.ServiceException as ex:
    fault(ex)

if not ret.mode_sent:
    fault("Request failed. Check mavros logs")

if not done_evt.wait(5):
    fault("Timed out!")

def do_rate(args):
    set_rate = rospy.ServiceProxy(mavros.get_topic('set_stream_rate'), StreamRate)
    def _test_set_rate(rate_arg, id):
        if rate_arg is not None:
        try:
            set_rate(stream_id=id, message_rate=rate_arg, on_off=(rate_arg != 0))
        except rospy.ServiceException as ex:
            fault(ex)

_test_set_rate(args.all, StreamRateRequest.STREAM_ALL)
_test_set_rate(args.raw_sensors, StreamRateRequest.STREAM_RAW_SENSORS)
_test_set_rate(args.ext_status, StreamRateRequest.STREAM_EXTENDED_STATUS)
_test_set_rate(args.rc_channels, StreamRateRequest.STREAM_RC_CHANNELS)
_test_set_rate(args.raw_controller, StreamRateRequest.STREAM_RAW_CONTROLLER)
_test_set_rate(args.position, StreamRateRequest.STREAM_POSITION)
_test_set_rate(args.extra1, StreamRateRequest.STREAM_EXTRA1)
_test_set_rate(args.extra2, StreamRateRequest.STREAM_EXTRA2)
_test_set_rate(args.extra3, StreamRateRequest.STREAM_EXTRA3)

if args.stream_id is not None:
    _test_set_rate(args.stream_id[1], args.stream_id[0])

def do_message_interval(args):

    if args.id is None:
        fault("id not specified")
        return

    if args.rate is None:
        fault("rate not specified")
        return

    try:
        set_message_interval = rospy.ServiceProxy(mavros.get_topic('set_message_interval'), MessageInterval)
        set_message_interval(message_id=args.id, message_rate=args.rate)
    except rospy.ServiceException as ex:
        fault(ex) 

    def main():
        parser = argparse.ArgumentParser(description="Change mode and rate on MAVLink device.")
        parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE)
        parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
        parser.add_argument('--wait', action='store_true', help="Wait for establishing FCU connection")
        subarg = parser.add_subparsers()

        mode_args = subarg.add_parser('mode', help="Set mode", formatter_class=argparse.RawTextHelpFormatter)
        mode_args.set_defaults(func=do_mode)
        mode_group = mode_args.add_mutually_exclusive_group(required=True)
        mode_group.add_argument('-b', '--base-mode', type=int, help="Base mode code")
        mode_group.add_argument('-c', '--custom-mode', type=str, help="Custom mode string (same as in ~/state topic)\
            \n\nNOTE: For PX4 Pro AUTO.LOITER, disable RC failsafe, which can be done by setting 'NAV_RCL_ACT' parameter to 0.")

        rate_args = subarg.add_parser('rate', help="Set stream rate")
        rate_args.set_defaults(func=do_rate)
        rate_args.add_argument('--all', type=int, metavar='rate', help="All streams")
        rate_args.add_argument('--raw-sensors', type=int, metavar='rate', hel

p="raw sensors stream")
    rate_args.add_argument('--ext-status', type=int, metavar='rate', help="extended status stream")
    rate_args.add_argument('--rc-channels', type=int, metavar='rate', help="RC channels stream")
    rate_args.add_argument('--raw-controller', type=int, metavar='rate', help="raw conptoller stream")
    rate_args.add_argument('--position', type=int, metavar='rate', help="position stream")
    rate_args.add_argument('--extra1', type=int, metavar='rate', help="Extra 1 stream")
    rate_args.add_argument('--extra2', type=int, metavar='rate', help="Extra 2 stream")
    rate_args.add_argument('--extra3', type=int, metavar='rate', help="Extra 3 stream")
    rate_args.add_argument('--stream-id', type=int, nargs=2, metavar=('id', 'rate'), help="any stream")

    message_interval_args = subarg.add_parser('message_interval', help="Set message interval")
    message_interval_args.set_defaults(func=do_message_interval)
    message_interval_args.add_argument('--id', type=int, metavar='id', help="message id")
    message_interval_args.add_argument('--rate', type=float, metavar='rate', help="message rate")

    args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])

rospy.init_node("mavsys", anonymous=True)
mavros.set_namespace(args.mavros_ns)

if args.wait:
    wait_fcu_connection()

args.func(args)

Asked by Junjie_Gao on 2023-08-06 16:49:59 UTC

Comments

Answers