search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
873 stars 986 forks source link

How to use mavros2 #1718

Open dzywater opened 2 years ago

dzywater commented 2 years ago

I don't know how to use mavros2 and want to get some tutorials like "how to connect/arm/land/takeoff/mission/offboard".

Currently in PX4 gazebo simulation (1) ros2 run mavros mavros_node --ros-args --param fcu_url:=udp://:14540@ Teminal shows: [ERROR] [1646107611.895603244] [mavros.odometry]: ODOM: Ex: Could not find a connection between 'map' and 'base_link_frd' because they are not part of the same tree.Tf has two or more unconnected trees. (2) ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool '{value: True}' Iris drone can arm despite (1)'s error. (3)ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL '{min_pitch: 10, yaw: 10, latitude: 8, longitude: 47, altitude: 550}' Iris drone don't takeoff.

vooon commented 2 years ago

What version do you use? Try to build current ros2 branch, as it might have some important fixes. TF error shouldn't affect takeoff, firmware do not aware about tf (but i'm unsure how gazebo simulates position).

Firmware usually tells something when it rejects to fly, so maybe you have some FCU: ... on (1)?

dzywater commented 2 years ago

I use v2.1.0 1

It seems that takeoff cannot be taken. I'm sure I call takeoff service in time before disarming.

Moveover After run: ros2 run mavros mavros_node --ros-args --param fcu_url:=udp://:14540@ Always shows in the terminal 3

Mark-Beaty commented 2 years ago

@dzywater (1) I believe that error is related to the mavros_extras package being installed which is still being worked on to be fully supported (see conversation in #1564), I'm currently working on getting mavros_extras working in my system as I need some of the plugins to hopefully simplify my development on a project, but having it installed launches all the plugins by default which I'm working on adjusting currently (hopefully).

If you uninstall the mavros_extras package that will fix that error printing repeatedly, and I assume you likely don't need the plugins for your implementation (at least not yet): EDIT: See bottom block for an actual fix for the issue I just figured out sudo apt-get uninstall ros-[DISTRO]-mavros-extras I'm using ros2 foxy so for me this would look like sudo apt-get uninstall ros-foxy-mavros-extras.

(3) As for your commands, its possible that you giving the lat and long coordinates for takeoff is causing it to reject the command for being out of range of the drone. You can work on adjusting this by looking at the output of the gps: ros2 topic echo /mavros/global_position/global and adjusting your lat and long given in your takeoff command to match or be very close (as in, probably within 0.0001 of current location for lat and long, though you can play with it once you actually get it off the ground)

Or you can just leave those out, which will use the drone's current location from the gps to set takeoff location, though you need absolute altitude rather than relative to home for this command I believe: ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{min_pitch: 0, yaw: 90, altitude: 550}"

With regard to your request for some tutorials, I can pass along some of the other commands I've worked out for doing some useful stuff. 

ros2 service call /mavros/mission/push mavros_msgs/srv/WaypointPush "{waypoints: [{frame: 2, command: 178, is_current: True, autocontinue: True, param1: 2, param2: 2, param3: NaN, param4: 0, x_lat: NaN, y_long: NaN, z_alt: NaN}, \
{frame: 6, command: 16, is_current: False, autocontinue: True, x_lat: 47.3977419, y_long: 8.5455938, z_alt: 13}, \
{frame: 2, command: 178, is_current: False, autocontinue: True, param1: 2, param2: 0.001, param3: NaN, param4: 0, x_lat: NaN, y_long: NaN, z_alt: NaN}, \
{frame: 6, command: 16, is_current: False, autocontinue: True, x_lat: 47.3977419, y_long: 8.5455938, z_alt: 15}, \
{frame: 6, command: 16, is_current: False, autocontinue: True, x_lat: 47.3977419, y_long: 8.5455938, z_alt: 5}]}"

class WaypointPushNode(Node): def init(self): super().init('waypoint_push_node') waypoint_push_client = self.create_client(WaypointPush, '/mavros/mission/push')

    # See /mavros/mavros_msgs/msg/Waypoint.msg and /mavros/mavros_msgs/msg/WaypointList.msg
    waypoint_list = WaypointList()
    waypoint = Waypoint()

    waypoint.frame = 6 # frame 6 = Global relative altitude (to home position)
    waypoint.command = 16 # command 16 = MAV_CMD_NAV_WAYPOINT -> go to waypoint
    waypoint.is_current = False
    waypoint.autocontinue = True
    waypoint.param1 = float(0) # command 16 -> Hold time
    waypoint.param2 = float(0) # command 16 -> Accept Radius (how close you need to be to mark waypoint as reached)
    waypoint.param3 = float("nan") # command 16 -> Pass Radius (set the radius of a turn around a waypoint, positive=clockwise orbit; negative=counter-clockwise orbit)
    waypoint.param4 = float("nan") # command 16 -> Yaw Heading; NaN = current system yaw heading mode (face next waypoint usually)
    waypoint.x_lat = float(some_latitude)
    waypoint.y_long = float(some_longitude)
    waypoint.z_alt = float(10)

    waypoint_list.waypoints.append(waypoint) # You could then keep building and appending more waypoints

    # Populate your request
    request = WaypointPush.Request() 
    # See /mavros/mavros_msgs/srv/WaypointPush.srv
    request.waypoints = waypoint_list

    # Make service call and store service response to check for success and number of waypoints transferred
    response = waypoint_push_client.call(request)

def main(args=None): rclpy.init(args=args) waypoint_push = WaypointPushNode() rclpy.spin(waypoint_push) waypoint_push.destroy_node() rclpy.shutdown()

if name == 'main': main()

___________________
This ended up being a lot more involved and in-depth than I expected it to be, but I hope it helps put you on the right path to learning how to do this stuff yourself.  Note that the above `Waypoint()` structure can be used (and is in the command line example) as things other than strictly waypoints, depending on what you set your command and frame values to.  They are more akin to mission items.  Also I believe that the default setting when the PX4 completes a mission it will enter recovery mode or something similar and will land itself (I believe back at the home location), so you don't need to account for getting your drone back home in your early mission experiments.  Let me know if I can provide any more clarification or help!

_____________________

Edit:
I just figured out the solution to the mavros-extras issue I was having so you wouldn't have to uninstall that package if you don't want, I'm launching my mavros system with this command:
`ros2 run mavros mavros_node --ros-args --params-file ./mavros_param.yaml`
Where mavros_param.yaml looks like this:

mavros_param.yaml

mavros: ros__parameters: {}

mavros_router: ros__parameters: {}

mavros_node: ros__parameters: fcu_url: udp://:14540@x.x.x.x:14580

/**: ros__parameters: plugin_denylist: [odometry]

plugin_allowlist: []


If you try to use this directly you'll need to set the correct IP address for fcu_url.
Adding the `/**:` entry for blocking the odometry plugin fixed the issue I was having with the error being spammed repeatedly.  You'll also be able to use this to specify which plugins you want to allow and which you want to block from launching based on the needs of your system

Thanks to @Vicidel in #1689 for setting me in the right direction to figure this issue out
vooon commented 2 years ago

@Mark-Beaty please note that new python library may hide some interactions with rclpy:

https://github.com/mavlink/mavros/blob/ros2/mavros/mavros/__init__.py#L17 - mavros.Client (node class) provides interfaces to params, missions and others. It's used by mav script.


import mavros

client = mavros.Client()

client.start_spinner()  # note: instead you may add `client` to node spinner, that method creates thread

client.system.wait_fcu_connection(10.0)

clinet.system.subscribe_state(lambda msg: print(msg))

print(client.waypoint.points)
vooon commented 2 years ago

Also note that plugin allow and deny lists only needed for UAS node (by default = /mavros).

vooon commented 2 years ago

mavros_node - is a container that runs Router and UAS nodes so it acts more like mavros_node from ROS1.

All the plugins now runs inside UAS as a subnode. So each plugin has it's own parameter server, it's own resources.

matthewoots commented 2 years ago

@vooon and @dzywater thank you so much for this thread (only thread that is getting ros2 mavros setup for SITL and potentially any system). Several questions related to porting over to ros2.

  1. There is no need for px4_config.yaml and px4_pluginlists.yaml? I've tried to run a launch file like ros1 px4.launch and got stuck at the YAML parser issue https://github.com/ros2/rcl/issues/463, are the configs hardcoded in ros2?
  2. I haven't tested on a platform yet (will try) but is vision pose plugin up and stable for streaming vision pose data over to the FC?
  3. Is DDS-RTPS (comms to uorb messages) the way to go for ros2 with PX4 and not mavros anymore? Will there be documentations for starting at least SITL with ros2 mavros (becasue i think people will have a hard time if they have not tried mavros before and starting on ros2)?

Sorry to tap on this thread to ask these questions!

vooon commented 2 years ago

1) They are not ported yet #1747 2) Those plugins mostly the same as on v1 3) If someone will write them...

mzahana commented 1 year ago

Hi. I keep going into loops through the issues related to ros2. Is there a working example of a launch file for PX4?

ZhongmouLi commented 9 months ago

Hi, @mzahana @dzywater .

I find this thread really important, as little resource exist talking about how to use mavros2 in ros2.

Could you please help me connect mavros2 and my Ardupilot?

Here is what I did:

  1. run sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console 
    
SIM_VEHICLE: Start
SIM_VEHICLE: Killing tasks
SIM_VEHICLE: Starting up at SITL location
SIM_VEHICLE: WAF build
SIM_VEHICLE: Configure waf
SIM_VEHICLE: "/home/robot/RobotFirmware/ardupilot/modules/waf/waf-light" "configure" "--board" "sitl"
Setting top to                           : /home/robot/RobotFirmware/ardupilot 
Setting out to                           : /home/robot/RobotFirmware/ardupilot/build 
Autoconfiguration                        : enabled 
Setting board to                         : sitl 
Using toolchain                          : native 
Checking for 'g++' (C++ compiler)        : /usr/lib/ccache/g++ 
Checking for 'gcc' (C compiler)          : /usr/lib/ccache/gcc 
Checking for c flags '-MMD'              : yes 
Checking for cxx flags '-MMD'            : yes 
CXX Compiler                             : g++ 9.4.0 
Checking for need to link with librt     : not necessary 
Checking for feenableexcept              : yes 
Enabled OpenDroneID                      : no 
Enabled firmware ID checking             : no 
GPS Debug Logging                        : no 
Enabled custom controller                : yes 
Checking for HAVE_CMATH_ISFINITE         : yes 
Checking for HAVE_CMATH_ISINF            : yes 
Checking for HAVE_CMATH_ISNAN            : yes 
Checking for NEED_CMATH_ISFINITE_STD_NAMESPACE : yes 
Checking for NEED_CMATH_ISINF_STD_NAMESPACE    : yes 
Checking for NEED_CMATH_ISNAN_STD_NAMESPACE    : yes 
Checking for header endian.h                   : yes 
Checking for header byteswap.h                 : yes 
Checking for HAVE_MEMRCHR                      : yes 
Configured VSCode Intellisense:                : no 
Checking for program 'python'                  : /usr/bin/python 
Checking for python version >= 2.7.0           : 3.8.10 
Checking for program 'python'                  : /usr/bin/python 
Checking for python version >= 2.7.0           : 3.8.10 
Source is git repository                       : yes 
Update submodules                              : yes 
Checking for program 'git'                     : /usr/bin/git 
Checking for program 'size'                    : /usr/bin/size 
Benchmarks                                     : disabled 
Unit tests                                     : enabled 
Scripting                                      : enabled 
Scripting runtime checks                       : enabled 
Debug build                                    : disabled 
Coverage build                                 : disabled 
SITL 32-bit build                              : disabled 
Checking for program 'rsync'                   : /usr/bin/rsync 
'configure' finished successfully (0.539s)
{'waf_target': 'bin/arducopter', 'default_params_filename': ['default_params/copter.parm', 'default_params/gazebo-iris.parm'], 'external': True, 'model': 'JSON', 'sitl-port': True}
SIM_VEHICLE: Building
SIM_VEHICLE: "/home/robot/RobotFirmware/ardupilot/modules/waf/waf-light" "build" "--target" "bin/arducopter"
Waf: Entering directory `/home/robot/RobotFirmware/ardupilot/build/sitl'
Embedding file locations.txt:Tools/autotest/locations.txt
Embedding file models/Callisto.json:Tools/autotest/models/Callisto.json
Waf: Leaving directory `/home/robot/RobotFirmware/ardupilot/build/sitl'

BUILD SUMMARY Build directory: /home/robot/RobotFirmware/ardupilot/build/sitl Target Text (B) Data (B) BSS (B) Total Flash Used (B) Free Flash (B)

bin/arducopter 3853743 180909 203976 4034652 Not Applicable

Build commands will be stored in build/sitl/compile_commands.json 'build' finished successfully (1.403s) SIM_VEHICLE: Using defaults from (../Tools/autotest/default_params/copter.parm,../Tools/autotest/default_params/gazebo-iris.parm) SIM_VEHICLE: Run ArduCopter SIM_VEHICLE: "/home/robot/RobotFirmware/ardupilot/Tools/autotest/run_in_terminal_window.sh" "ArduCopter" "/home/robot/RobotFirmware/ardupilot/build/sitl/bin/arducopter" "-S" "--model" "JSON" "--speedup" "1" "--slave" "0" "--defaults" "../Tools/autotest/default_params/copter.parm,../Tools/autotest/default_params/gazebo-iris.parm" "-I0" SIM_VEHICLE: Run MavProxy SIM_VEHICLE: "mavproxy.py" "--out" "127.0.0.1:14550" "--out" "127.0.0.1:14551" "--master" "tcp:127.0.0.1:5760" "--sitl" "127.0.0.1:5501" "--map" "--console" RiTW: Starting ArduCopter : /home/robot/RobotFirmware/ardupilot/build/sitl/bin/arducopter -S --model JSON --speedup 1 --slave 0 --defaults ../Tools/autotest/default_params/copter.parm,../Tools/autotest/default_params/gazebo-iris.parm -I0 Connect tcp:127.0.0.1:5760 source_system=255 Loaded module console Loaded module map Log Directory: Telemetry log: mav.tlog Waiting for heartbeat from tcp:127.0.0.1:5760 MAV> SIM_VEHICLE: Keyboard Interrupt received ... SIM_VEHICLE: Killing tasks Connection reset or closed by peer on TCP socket Attempting reconnect [Errno 111] Connection refused sleeping

2. start mavros2 by `ros2 run mavros mavros_node --ros-args --param fcu_url:=udp://@127.0.0.1:14550` and obtain the error

[INFO] [1699715382.961827044] [mavros_node]: Starting mavros_node container [INFO] [1699715382.961902364] [mavros_node]: FCU URL: udp://@127.0.0.1:14550 [INFO] [1699715382.961911687] [mavros_node]: GCS URL: [INFO] [1699715382.961917851] [mavros_node]: UAS Prefix: /uas1 [INFO] [1699715382.961923227] [mavros_node]: Starting mavros router node [INFO] [1699715382.964466889] [mavros_router]: Built-in SIMD instructions: SSE, SSE2 [INFO] [1699715382.964483246] [mavros_router]: Built-in MAVLink package version: 2022.12.30 [INFO] [1699715382.964490009] [mavros_router]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta [INFO] [1699715382.964497086] [mavros_router]: MAVROS Router started [INFO] [1699715382.964517576] [mavros_router]: Requested to add endpoint: type: 0, url: udp://@127.0.0.1:14550 [INFO] [1699715382.964545746] [mavros_router]: Endpoint link[1000] created [INFO] [1699715382.964817177] [mavros_router]: link[1000] opened successfully [INFO] [1699715382.964845379] [mavros_router]: Requested to add endpoint: type: 2, url: /uas1 [INFO] [1699715382.964860543] [mavros_router]: Endpoint link[1001] created [INFO] [1699715382.965226395] [mavros_router]: link[1001] opened successfully [INFO] [1699715382.965248641] [mavros_node]: Starting mavros uas node [INFO] [1699715382.992599129] [mavros]: UAS Executor started, threads: 12 [INFO] [1699715383.003628487] [mavros]: Plugin actuator_control created [INFO] [1699715383.003678684] [mavros]: Plugin actuator_control initialized [INFO] [1699715383.005853171] [mavros]: Plugin altitude created [INFO] [1699715383.005879805] [mavros]: Plugin altitude initialized [INFO] [1699715383.009815181] [mavros]: Plugin command created [INFO] [1699715383.009840707] [mavros]: Plugin command initialized [INFO] [1699715383.014604077] [mavros]: Plugin ftp created [INFO] [1699715383.014635943] [mavros]: Plugin ftp initialized [INFO] [1699715383.017668356] [mavros]: Plugin geofence created [INFO] [1699715383.017705397] [mavros]: Plugin geofence initialized [INFO] [1699715383.022531452] [mavros]: Plugin global_position created [INFO] [1699715383.022586988] [mavros]: Plugin global_position initialized [INFO] [1699715383.025707346] [mavros]: Plugin home_position created [INFO] [1699715383.025738393] [mavros]: Plugin home_position initialized [INFO] [1699715383.029354099] [mavros]: Plugin imu created [INFO] [1699715383.029395636] [mavros]: Plugin imu initialized [INFO] [1699715383.032998168] [mavros]: Plugin local_position created [INFO] [1699715383.033030683] [mavros]: Plugin local_position initialized [INFO] [1699715383.035741632] [mavros]: Plugin manual_control created [INFO] [1699715383.035770847] [mavros]: Plugin manual_control initialized [INFO] [1699715383.038598937] [mavros]: Plugin nav_controller_output created [INFO] [1699715383.038628440] [mavros]: Plugin nav_controller_output initialized [INFO] [1699715383.041951086] [mavros]: Plugin param created [INFO] [1699715383.041979438] [mavros]: Plugin param initialized [INFO] [1699715383.045597702] [mavros]: Plugin rallypoint created [INFO] [1699715383.045644008] [mavros]: Plugin rallypoint initialized [INFO] [1699715383.049036282] [mavros]: Plugin rc_io created [INFO] [1699715383.049067226] [mavros]: Plugin rc_io initialized [INFO] [1699715383.052519152] [mavros]: Plugin setpoint_accel created [INFO] [1699715383.052563988] [mavros]: Plugin setpoint_accel initialized [INFO] [1699715383.056344965] [mavros]: Plugin setpoint_attitude created [INFO] [1699715383.056377431] [mavros]: Plugin setpoint_attitude initialized [INFO] [1699715383.060527794] [mavros]: Plugin setpoint_position created [INFO] [1699715383.060557492] [mavros]: Plugin setpoint_position initialized [INFO] [1699715383.065028545] [mavros]: Plugin setpoint_raw created [INFO] [1699715383.065074539] [mavros]: Plugin setpoint_raw initialized [INFO] [1699715383.069463414] [mavros]: Plugin setpoint_trajectory created [INFO] [1699715383.069493419] [mavros]: Plugin setpoint_trajectory initialized [INFO] [1699715383.073369760] [mavros]: Plugin setpoint_velocity created [INFO] [1699715383.073424540] [mavros]: Plugin setpoint_velocity initialized [INFO] [1699715383.080053952] [mavros]: Plugin sys_status created [INFO] [1699715383.080114865] [mavros]: Plugin sys_status initialized [INFO] [1699715383.084392168] [mavros.time]: TM: Timesync mode: MAVLINK [INFO] [1699715383.085738873] [mavros]: Plugin sys_time created [INFO] [1699715383.085779268] [mavros]: Plugin sys_time initialized [INFO] [1699715383.092068584] [mavros]: Plugin waypoint created [INFO] [1699715383.092133376] [mavros]: Plugin waypoint initialized [INFO] [1699715383.096168514] [mavros]: Plugin wind_estimation created [INFO] [1699715383.096200355] [mavros]: Plugin wind_estimation initialized [INFO] [1699715383.096749920] [mavros]: Built-in SIMD instructions: SSE, SSE2 [INFO] [1699715383.096765440] [mavros]: Built-in MAVLink package version: 2022.12.30 [INFO] [1699715383.096772137] [mavros]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta [INFO] [1699715383.096778961] [mavros]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1 [INFO] [1699715384.078422027] [mavros_router]: link[1001] detected remote address 1.191 [WARN] [1699715385.084221364] [mavros.sys]: VER: broadcast request timeout, retries left 4 [WARN] [1699715386.084006698] [mavros.sys]: VER: broadcast request timeout, retries left 3 [WARN] [1699715387.084127859] [mavros.sys]: VER: unicast request timeout, retries left 2 [WARN] [1699715388.083773872] [mavros.sys]: VER: unicast request timeout, retries left 1 [WARN] [1699715389.084158020] [mavros.sys]: VER: unicast request timeout, retries left 0 [WARN] [1699715390.081450652] [mavros.sys]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities

Rezenders commented 9 months ago

@ZhongmouLi I recommend running mavros with the mavros launchfile, since it includes all the required params.

You can find an example of a launchfile for adupilot here: https://github.com/kas-lab/suave/blob/835b8a0fa5a24fed66a031ccb7eaf36b92de42c7/suave/launch/simulation.launch.py#L34

    mavros_path = get_package_share_directory('mavros')
    mavros_launch_path = os.path.join(
        mavros_path, 'launch', 'apm.launch')
    mavros_node = IncludeLaunchDescription(
        AnyLaunchDescriptionSource(mavros_launch_path),
        launch_arguments={
            'fcu_url': 'udp://127.0.0.1:14551@14555',
            'gcs_url': 'udp://@localhost:14550',
            'system_id': '255',
            'component_id': '240',
            'target_system_id': '1',
            'target_component_id': '1'
            }.items()
        )
ZhongmouLi commented 9 months ago

@Rezenders Thanks so much for your reply.

But I am using ros2 foxy. Do you mean I run mavros instead of mavros2 in foxy?

Rezenders commented 9 months ago

@Rezenders Thanks so much for your reply.

But I am using ros2 foxy. Do you mean I run mavros instead of mavros2 in foxy?

Hi @ZhongmouLi, I am not really sure which mavros version is out for foxy. But what I am doing for ros2 Humble is that I am getting mavros from this github repo instead of the version in apt. The version in the repo is working better for me (at least the last time I tried).

ZhongmouLi commented 8 months ago

@Rezenders Thanks so much, after reading your reply. I finally found the solution.

In fact, it should be 'fcu_url': 'udp://127.0.0.1:14551@14555'

duguguang commented 1 month ago

Also you can create the config file, which names mavros_param.yaml, then run the command: ros2 run mavros mavros_node --ros-args --params-file mavros_parm.yaml

# mavros_param.yaml
mavros:
  ros__parameters: {}

mavros_router:
  ros__parameters: {}

mavros_node:
  ros__parameters:
    fcu_url: /dev/ttyACM0:57600
    gcs_url: udp://@192.168.3.199:14550
    tgt_system: 3
    tgt_component: 1
    log_output: screen
    fcu_protocol: v2.0
    respawn_mavros: false
    pluginlists_yaml: path/to/mavros/launch/px4_pluginlists.yaml
    config_yaml: path/to/mavros/launch/px4_config.yaml
duguguang commented 1 month ago

I use v2.1.0 1

It seems that takeoff cannot be taken. I'm sure I call takeoff service in time before disarming.

Moveover After run: ros2 run mavros mavros_node --ros-args --param fcu_url:=udp://:14540@ Always shows in the terminal 3

Have you ever solved the ODOM:Ex .. problem ?