From 95a6eba36ce804baa6ab09e659128c80727304b6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 4 Jan 2023 16:43:45 -0500 Subject: [PATCH] SITL jmavsim make airframes simulator specific -cleanup and simplify jmavsim SITL launch and debug --- .vscode/tasks.json | 43 ------------ .../init.d-posix/airframes/10017_jmavsim_iris | 32 +++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + .../init.d-posix/px4-rc.simulator | 20 +++++- Tools/simulation/jmavsim/jmavsim_run.sh | 3 +- Tools/simulation/jmavsim/sitl_run.sh | 67 ------------------- boards/px4/sitl/sitl.cmake | 2 - platforms/posix/Debug/launch_sitl.json.in | 10 +-- .../sitl_targets_jmavsim.cmake | 63 +++++++++++------ 9 files changed, 98 insertions(+), 143 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris delete mode 100755 Tools/simulation/jmavsim/sitl_run.sh diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 513822a277..f8756d573b 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -49,49 +49,6 @@ "group": "test" } }, - { - "label": "jmavsim build", - "type": "shell", - "command": "ant create_run_jar copy_res", - "options": { - "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim" - }, - "presentation": { - "echo": true, - "reveal": "never", - "revealProblems": "onProblem", - "focus": false, - "panel": "dedicated", - "showReuseMessage": false, - "clear": false, - "close": true - }, - "problemMatcher": [] - }, - { - "label": "jmavsim", - "type": "shell", - "dependsOn": "jmavsim build", - "command": "java -Djava.ext.dirs= -jar jmavsim_run.jar -r 250 -lockstep -tcp localhost:4560 -qgc", - "options": { - "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim/out/production", - "env": { - "PX4_SIM_SPEED_FACTOR": "1" - } - }, - "isBackground": true, - "presentation": { - "echo": true, - "reveal": "never", - "revealProblems": "onProblem", - "focus": false, - "panel": "dedicated", - "showReuseMessage": false, - "clear": false, - "close": true - }, - "problemMatcher": [] - }, { "label": "jmavsim kill", "type": "shell", diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris new file mode 100644 index 0000000000..fbf5540915 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris @@ -0,0 +1,32 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor SITL jMAVSim +# +# @type Quadrotor Wide +# +# @maintainer Julian Oes +# + +. ${R}etc/init.d/rc.mc_defaults + +param set-default CA_AIRFRAME 0 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index fadd7cfb2c..7bfdcade10 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_romfs_files( 10016_iris + 10017_jmavsim_iris 10018_iris_foggy_lidar 10019_omnicopter 10030_px4vision diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index fff745b135..45aa39ab8c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -124,6 +124,20 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then exit 1 fi +elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then + + echo "INFO [init] jMAVSim simulator" + + if jps | grep -i jmavsim; then + kill $(jps | grep -i jmavsim | awk '{print $1}') || true + sleep 1 + fi + + param set IMU_INTEG_RATE 250 + ./jmavsim_run.sh -l -r 250 & + + simulator_mavlink start -h localhost $((4560+px4_instance)) + else # otherwise start simulator (mavlink) module simulator_tcp_port=$((4560+px4_instance)) @@ -134,15 +148,15 @@ else if [ -z "${PX4_SIM_HOSTNAME}" ]; then if [ -z "${PX4_SIM_HOST_ADDR}" ]; then - echo "PX4 SIM HOST: localhost" + echo "INFO [init] PX4_SIM_HOSTNAME: localhost" simulator_mavlink start -c $simulator_tcp_port else - echo "PX4 SIM HOST: ${PX4_SIM_HOST_ADDR}" + echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}" simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}" fi else - echo "PX4 SIM HOST: ${PX4_SIM_HOSTNAME}" + echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}" simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}" fi diff --git a/Tools/simulation/jmavsim/jmavsim_run.sh b/Tools/simulation/jmavsim/jmavsim_run.sh index f43940b1cf..7b70930d51 100755 --- a/Tools/simulation/jmavsim/jmavsim_run.sh +++ b/Tools/simulation/jmavsim/jmavsim_run.sh @@ -2,7 +2,8 @@ set -e -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +SCRIPT_DIR=$(dirname $(readlink -f "$BASH_SOURCE")) + cd "$SCRIPT_DIR/jMAVSim" port=4560 diff --git a/Tools/simulation/jmavsim/sitl_run.sh b/Tools/simulation/jmavsim/sitl_run.sh deleted file mode 100755 index 67f078d0aa..0000000000 --- a/Tools/simulation/jmavsim/sitl_run.sh +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env bash - -set -e - -if [ "$#" -lt 3 ]; then - echo usage: sitl_run.sh sitl_bin debugger src_path build_path - exit 1 -fi - -sitl_bin="$1" -debugger="$2" -src_path="$3" -build_path="$4" - -echo SITL ARGS - -echo sitl_bin: $sitl_bin -echo debugger: $debugger -echo src_path: $src_path -echo build_path: $build_path - -rootfs="$build_path/rootfs" # this is the working directory -mkdir -p "$rootfs" - -# To disable user input -if [[ -n "$NO_PXH" ]]; then - no_pxh=-d -else - no_pxh="" -fi - -jmavsim_pid=`ps aux | grep java | grep "\-jar jmavsim_run.jar" | awk '{ print $2 }'` -if [ -n "$jmavsim_pid" ]; then - kill $jmavsim_pid -fi - -export PX4_SIM_MODEL="iris" - -# Start Java simulator -"$src_path"/Tools/simulation/jmavsim/jmavsim_run.sh -r 250 -l & -SIM_PID=$! - -pushd "$rootfs" >/dev/null - -# Do not exit on failure now from here on because we want the complete cleanup -set +e - -sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc" - -echo SITL COMMAND: $sitl_command - -if [ "$debugger" == "lldb" ]; then - eval lldb -- $sitl_command -elif [ "$debugger" == "gdb" ]; then - eval gdb --args $sitl_command -elif [ "$debugger" == "valgrind" ]; then - eval valgrind --track-origins=yes --leak-check=full -v $sitl_command -elif [ "$debugger" == "callgrind" ]; then - eval valgrind --tool=callgrind -v $sitl_command -else - eval $sitl_command -fi - -popd >/dev/null - -pkill -9 -P $SIM_PID -kill -9 $SIM_PID diff --git a/boards/px4/sitl/sitl.cmake b/boards/px4/sitl/sitl.cmake index d5225004d5..a6b3823e92 100644 --- a/boards/px4/sitl/sitl.cmake +++ b/boards/px4/sitl/sitl.cmake @@ -1,5 +1,3 @@ -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") # If the environment variable 'replay' is defined, we are building with replay # support. In this case, we enable the orb publisher rules. diff --git a/platforms/posix/Debug/launch_sitl.json.in b/platforms/posix/Debug/launch_sitl.json.in index ee86ffb77b..daff77ea1c 100644 --- a/platforms/posix/Debug/launch_sitl.json.in +++ b/platforms/posix/Debug/launch_sitl.json.in @@ -135,7 +135,7 @@ } }, { - "name": "SITL (jmavsim)", + "name": "SITL (jmavsim_iris SYS_AUTOSTART=10017)", "type": "cppdbg", "request": "launch", "program": "${command:cmake.launchTargetPath}", @@ -146,16 +146,13 @@ "cwd": "${command:cmake.buildDirectory}/rootfs", "environment": [ { - "name": "PX4_SIM_MODEL", - "value": "iris" + "name": "PX4_SYS_AUTOSTART", + "value": "10017" } ], - "externalConsole": false, - "preLaunchTask": "jmavsim", "postDebugTask": "jmavsim kill", "linux": { "MIMode": "gdb", - "externalConsole": false, "setupCommands": [ { "description": "Enable pretty-printing for gdb", @@ -171,7 +168,6 @@ }, "osx": { "MIMode": "lldb", - "externalConsole": true, "setupCommands": [ { "text": "pro hand -p true -s false -n false SIGCONT", diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_jmavsim.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_jmavsim.cmake index 30f4ac738a..5e175aa454 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_jmavsim.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_jmavsim.cmake @@ -1,25 +1,48 @@ -px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim") -# create targets for each viewer/model/debugger combination -set(debuggers - none - gdb - lldb - valgrind - callgrind -) +find_program(JAVA_ANT_PATH "ant") +find_package(Java) + +if(JAVA_ANT_PATH AND Java_JAVAC_EXECUTABLE AND Java_JAVA_EXECUTABLE) + + include(UseJava) + + find_jar(LIBVECMATH_JAR + "vecmath" + ) + + if(LIBVECMATH_JAR) + px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim") + + add_custom_target(jmavsim_run_symlink ALL + COMMAND ${CMAKE_COMMAND} -E create_symlink ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jmavsim_run.sh ${PX4_BINARY_DIR}/rootfs/jmavsim_run.sh + BYPRODUCTS ${PX4_BINARY_DIR}/rootfs/jmavsim_run.sh + ) + + # build_jmavsim + add_custom_command( + OUTPUT ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim/out/production/jmavsim_run.jar + COMMAND ${JAVA_ANT_PATH} create_run_jar copy_res + WORKING_DIRECTORY ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim/ + USES_TERMINAL + DEPENDS git_jmavsim jmavsim_run_symlink + COMMENT "building jMAVSim" + ) + add_custom_target(build_jmavsim DEPENDS ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim/out/production/jmavsim_run.jar) + + # launch helper + add_custom_target(jmavsim_iris + COMMAND ${CMAKE_COMMAND} -E env PX4_SYS_AUTOSTART=10017 $ + WORKING_DIRECTORY ${SITL_WORKING_DIR} + USES_TERMINAL + DEPENDS + px4 git_jmavsim build_jmavsim jmavsim_run_symlink + ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris + COMMENT "launching px4 jmavsim_iris (SYS_AUTOSTART=10017)" + ) + add_custom_target(jmavsim DEPENDS jmavsim_iris) # alias -foreach(debugger ${debuggers}) - if(debugger STREQUAL "none") - set(_targ_name "jmavsim") else() - set(_targ_name "jmavsim_${debugger}") + #message(WARNING "jMAVSim requires vecmath.jar, which is not available in your system. Please install it.") endif() - add_custom_target(${_targ_name} - COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/sitl_run.sh $ ${debugger} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR} - WORKING_DIRECTORY ${SITL_WORKING_DIR} - USES_TERMINAL - DEPENDS px4 git_jmavsim - ) -endforeach() +endif()