Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[MrsDroneSpawner]: fix and rework mavlink GCS connection #11

Merged
merged 1 commit into from
Jul 25, 2024
Merged

[MrsDroneSpawner]: fix and rework mavlink GCS connection #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 12 additions & 12 deletions ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
2 changes: 1 addition & 1 deletion ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 4 additions & 2 deletions ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@
<arg name="ID"/>
<arg name="PX4_SIM_MODEL"/>
<arg name="PX4_ESTIMATOR" default="ekf2"/>
<arg name="MAVLINK_GCS_UDP_PORT" default=""/> <!-- mavlink port for QGC -->
<arg name="MAVLINK_GCS_UDP_PORT_LOCAL" default=""/> <!-- mavlink port for QGC -->
<arg name="MAVLINK_GCS_UDP_PORT_REMOTE" default=""/> <!-- mavlink port for QGC -->
<arg name="ROMFS_PATH" default="$(find mrs_uav_gazebo_simulation)/ROMFS"/>

<!-- PX4 configs -->
Expand All @@ -19,7 +20,8 @@
<!-- PX4 params -->
<env name="PX4_SIM_MODEL" value="$(arg PX4_SIM_MODEL)" />
<env name="PX4_ESTIMATOR" value="$(arg PX4_ESTIMATOR)" />
<env if="$(eval MAVLINK_GCS_UDP_PORT != '')" name="MAVLINK_GCS_UDP_PORT" value="$(arg MAVLINK_GCS_UDP_PORT)" />
<env if="$(eval MAVLINK_GCS_UDP_PORT_LOCAL != '')" name="MAVLINK_GCS_UDP_PORT_LOCAL" value="$(arg MAVLINK_GCS_UDP_PORT_LOCAL)" />
<env if="$(eval MAVLINK_GCS_UDP_PORT_REMOTE != '')" name="MAVLINK_GCS_UDP_PORT_REMOTE" value="$(arg MAVLINK_GCS_UDP_PORT_REMOTE)" />

<!-- PX4 SITL -->
<node name="sitl_uav$(arg ID)" pkg="px4" type="px4" output="screen" args="$(arg ROMFS_PATH)/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_interactive_mode) -i $(arg ID) -w sitl_uav$(arg ID)"/>
Expand Down
17 changes: 9 additions & 8 deletions ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand Down Expand Up @@ -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
# #}
Expand Down Expand Up @@ -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)]

Expand Down
Loading