From 147607d2a632329f430d5a558a1dcec243d6ed82 Mon Sep 17 00:00:00 2001 From: stibipet Date: Thu, 25 Jul 2024 15:11:11 +0200 Subject: [PATCH] [MrsDroneSpawner]: fix and rework mavlink GCS connection --- .../init.d-posix/px4-rc.mavlink_gcs | 24 +++++++++---------- .../ROMFS/px4fmu_common/init.d-posix/rcS | 2 +- .../config/spawner_params.yaml | 2 ++ .../launch/run_simulation_firmware.launch | 6 +++-- .../mrs_drone_spawner/mrs_drone_spawner.py | 17 ++++++------- 5 files changed, 28 insertions(+), 23 deletions(-) diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs index 2ca2b25..0bcca43 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs +++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs @@ -2,15 +2,15 @@ # shellcheck disable=SC2154 # GCS link -mavlink start -x -u $MAVLINK_GCS_UDP_PORT -r 4000000 -f -mavlink stream -r 100 -s ATTITUDE -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s ATTITUDE_QUATERNION -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data -mavlink stream -r 100 -s ATTITUDE_TARGET -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s HIGHRES_IMU -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data_raw -mavlink stream -r 100 -s LOCAL_POSITION_NED -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s ODOMETRY -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s GLOBAL_POSITION_INT -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 10 -s RC_CHANNELS -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 10 -s SYS_STATUS -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s HEARTBEAT -u $MAVLINK_GCS_UDP_PORT -mavlink stream -r 100 -s DISTANCE_SENSOR -u $MAVLINK_GCS_UDP_PORT +mavlink start -x -u $MAVLINK_GCS_UDP_PORT_LOCAL -r 4000000 -o $MAVLINK_GCS_UDP_PORT_REMOTE +mavlink stream -r 100 -s ATTITUDE -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s ATTITUDE_QUATERNION -u $MAVLINK_GCS_UDP_PORT_LOCAL # /mavros/imu/data +mavlink stream -r 100 -s ATTITUDE_TARGET -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s HIGHRES_IMU -u $MAVLINK_GCS_UDP_PORT_LOCAL # /mavros/imu/data_raw +mavlink stream -r 100 -s LOCAL_POSITION_NED -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s ODOMETRY -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s GLOBAL_POSITION_INT -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 10 -s RC_CHANNELS -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 10 -s SYS_STATUS -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s HEARTBEAT -u $MAVLINK_GCS_UDP_PORT_LOCAL +mavlink stream -r 100 -s DISTANCE_SENSOR -u $MAVLINK_GCS_UDP_PORT_LOCAL diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS index 2e819d3..1c6bde7 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS @@ -271,7 +271,7 @@ fi #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink -if [ -n "$MAVLINK_GCS_UDP_PORT" ] +if [ -n "$MAVLINK_GCS_UDP_PORT_LOCAL" ] && [ -n "$MAVLINK_GCS_UDP_PORT_REMOTE" ] then . px4-rc.mavlink_gcs fi diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml index d7c9c18..a633378 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml @@ -2,6 +2,8 @@ mavlink_config: vehicle_base_port: 14000 mavlink_tcp_base_port: 4560 mavlink_udp_base_port: 14560 + mavlink_gcs_udp_base_port_local: 18000 + mavlink_gcs_udp_base_port_remote: 18100 qgc_udp_port: 14550 sdk_udp_port: 14540 send_vision_estimation: false diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch index 45b00ff..8e2e60a 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch +++ b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch @@ -4,7 +4,8 @@ - + + @@ -19,7 +20,8 @@ - + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py index 68dae53..9d585af 100755 --- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py +++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py @@ -142,6 +142,8 @@ def __init__(self, verbose=False): self.vehicle_base_port = rospy.get_param('~mavlink_config/vehicle_base_port') self.mavlink_tcp_base_port = rospy.get_param('~mavlink_config/mavlink_tcp_base_port') self.mavlink_udp_base_port = rospy.get_param('~mavlink_config/mavlink_udp_base_port') + self.mavlink_gcs_udp_base_port_local = rospy.get_param('~mavlink_config/mavlink_gcs_udp_base_port_local') + self.mavlink_gcs_udp_base_port_remote = rospy.get_param('~mavlink_config/mavlink_gcs_udp_base_port_remote') self.qgc_udp_port = rospy.get_param('~mavlink_config/qgc_udp_port') self.sdk_udp_port = rospy.get_param('~mavlink_config/sdk_udp_port') self.send_vision_estimation = rospy.get_param('~mavlink_config/send_vision_estimation') @@ -999,9 +1001,10 @@ def get_jinja_params_for_one_robot(self, params_dict, index, ID): robot_params['mavlink_config'] = self.get_mavlink_config_for_robot(ID) - if 'mavlink_gcs_udp_port' in params_dict.keys(): - if len(params_dict['mavlink_gcs_udp_port']) > index: - robot_params['mavlink_gcs_udp_port'] = params_dict['mavlink_gcs_udp_port'][index] + if 'enable_mavlink_gcs' in params_dict.keys(): + robot_params['mavlink_gcs_udp_port_local'] = self.mavlink_gcs_udp_base_port_local + ID + robot_params['mavlink_gcs_udp_port_remote'] = self.mavlink_gcs_udp_base_port_remote + ID + rospy.loginfo(f'[MrsDroneSpawner]: Publishing extra mavlink messages on UDP port {robot_params["mavlink_gcs_udp_port_remote"]}') return robot_params # #} @@ -1057,11 +1060,9 @@ def launch_px4_firmware(self, robot_params): ] # do we want to send raw mavlink data to a specific port? (this will also auto connect to QGroundControl) - if 'mavlink_gcs_udp_port' in robot_params.keys(): - if isinstance(robot_params['mavlink_gcs_udp_port'], int): - roslaunch_args.append('MAVLINK_GCS_UDP_PORT:=' + str(robot_params["mavlink_gcs_udp_port"])) - else: - raise CouldNotLaunch(f'\'{robot_params["mavlink_gcs_udp_port"]}\' is not a valid port number') + if 'mavlink_gcs_udp_port_local' in robot_params.keys() and 'mavlink_gcs_udp_port_remote' in robot_params.keys(): + roslaunch_args.append('MAVLINK_GCS_UDP_PORT_LOCAL:=' + str(robot_params['mavlink_gcs_udp_port_local'])) + roslaunch_args.append('MAVLINK_GCS_UDP_PORT_REMOTE:=' + str(robot_params['mavlink_gcs_udp_port_remote'])) roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)]