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.

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

add --mavlink_gcs_udp_port param to the drone spawner #10

Merged
merged 1 commit into from
Jul 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
#!/bin/sh
# 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
Original file line number Diff line number Diff line change
Expand Up @@ -271,6 +271,11 @@ fi
#user defined mavlink streams for instances can be in PATH
. px4-rc.mavlink

if [ -n "$MAVLINK_GCS_UDP_PORT" ]
then
. px4-rc.mavlink_gcs
fi

# execute autostart post script if any
[ -e "$autostart_file".post ] && . "$autostart_file".post

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,9 @@
<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="ROMFS_PATH" default="$(find mrs_uav_gazebo_simulation)/ROMFS"/>

<!-- PX4 configs -->
<arg name="interactive" default="false"/>
<!-- PX4 SITL -->
Expand All @@ -18,9 +19,10 @@
<!-- 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)" />

<!-- 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)"/>

</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -471,7 +471,7 @@ def parse_user_input(self, input_str):
'model': None,
'ids': [],
'names': [],
'spawn_poses': {},
'spawn_poses': {}
}

# parse out the keywords starting with '--'
Expand Down Expand Up @@ -999,6 +999,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]

return robot_params
# #}

Expand Down Expand Up @@ -1051,6 +1055,14 @@ def launch_px4_firmware(self, robot_params):
'PX4_SIM_MODEL:=' + str(robot_params['model']),
'ROMFS_PATH:=' + str(romfs_path)
]

# 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')

roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)]

firmware_process = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence)
Expand Down
Loading