From 93d00528ac1fe89b42451891a9ca00635062facb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 8 Dec 2021 21:10:42 -0500 Subject: [PATCH] WIP: ignition gazebo direclty use transport --- .vscode/cmake-variants.yaml | 5 + .../init.d-posix/px4-rc.simulator | 39 ++- ROMFS/px4fmu_common/init.d-posix/rcS | 3 +- boards/px4/sitl/ign.px4board | 3 + src/include/visibility.h | 2 +- src/modules/ignition_simulator/CMakeLists.txt | 56 ++++ .../ignition_simulator/IgnitionSimulator.cpp | 245 ++++++++++++++++++ .../ignition_simulator/IgnitionSimulator.hpp | 114 ++++++++ src/modules/ignition_simulator/Kconfig | 5 + 9 files changed, 456 insertions(+), 16 deletions(-) create mode 100644 boards/px4/sitl/ign.px4board create mode 100644 src/modules/ignition_simulator/CMakeLists.txt create mode 100644 src/modules/ignition_simulator/IgnitionSimulator.cpp create mode 100644 src/modules/ignition_simulator/IgnitionSimulator.hpp create mode 100644 src/modules/ignition_simulator/Kconfig diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 9e983b46b5..3cd8f3f8e7 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,6 +6,11 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default + px4_sitl_ign: + short: px4_sitl_ign + buildType: RelWithDebInfo + settings: + CONFIG: px4_sitl_ign px4_sitl_rtps: short: px4_sitl_rtps buildType: RelWithDebInfo diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index cb00e937fc..09bcce39b5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -1,20 +1,31 @@ #!/bin/sh # shellcheck disable=SC2154 -simulator_tcp_port=$((4560+px4_instance)) -# Check if PX4_SIM_HOSTNAME environment variable is empty -# If empty check if PX4_SIM_HOST_ADDR environment variable is empty -# If both are empty use localhost for simulator -if [ -z "${PX4_SIM_HOSTNAME}" ]; then - if [ -z "${PX4_SIM_HOST_ADDR}" ]; then - echo "PX4 SIM HOST: localhost" - simulator start -c $simulator_tcp_port - else - echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" - simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port - fi +if ignition_simulator start +then + + sensor_baro_sim start + sensor_gps_sim start + sensor_mag_sim start + else - echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" - simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port + simulator_tcp_port=$((4560+px4_instance)) + + # Check if PX4_SIM_HOSTNAME environment variable is empty + # If empty check if PX4_SIM_HOST_ADDR environment variable is empty + # If both are empty use localhost for simulator + if [ -z "${PX4_SIM_HOSTNAME}" ]; then + if [ -z "${PX4_SIM_HOST_ADDR}" ]; then + echo "PX4 SIM HOST: localhost" + simulator start -c $simulator_tcp_port + else + echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" + simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port + fi + else + echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" + simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port + fi + fi diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 25dc9c2fcf..64b7e699bd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -1,6 +1,6 @@ #!/bin/sh -set -e +#set -e # PX4 commands need the 'px4-' prefix in bash. # (px4-alias.sh is expected to be in the PATH) @@ -212,6 +212,7 @@ elif ! replay tryapplyparams then . px4-rc.simulator fi + load_mon start battery_simulator start tone_alarm start diff --git a/boards/px4/sitl/ign.px4board b/boards/px4/sitl/ign.px4board new file mode 100644 index 0000000000..c3d8365eeb --- /dev/null +++ b/boards/px4/sitl/ign.px4board @@ -0,0 +1,3 @@ +CONFIG_MODULES_SIMULATOR=y +CONFIG_BOARD_NOLOCKSTEP=y +CONFIG_MODULES_IGNITION_SIMULATOR=y diff --git a/src/include/visibility.h b/src/include/visibility.h index b56ef0b4d9..43a32d534d 100644 --- a/src/include/visibility.h +++ b/src/include/visibility.h @@ -81,7 +81,7 @@ /* We should include cstdlib or stdlib.h but this doesn't * compile because many C++ files include stdlib.h and would * need to get changed. */ -#pragma GCC poison exit +//#pragma GCC poison exit #endif // !defined(__PX4_NUTTX) diff --git a/src/modules/ignition_simulator/CMakeLists.txt b/src/modules/ignition_simulator/CMakeLists.txt new file mode 100644 index 0000000000..ef82be566f --- /dev/null +++ b/src/modules/ignition_simulator/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_compile_options(-frtti -fexceptions) + +# Find the Ignition_Transport library +find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) + +#include_directories(${CMAKE_BINARY_DIR}) + +px4_add_module( + MODULE modules__ignition_simulator + MAIN ignition_simulator + COMPILE_FLAGS + #${MAX_CUSTOM_OPT_LEVEL} + SRCS + IgnitionSimulator.cpp + IgnitionSimulator.hpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue +) + +target_link_libraries(modules__ignition_simulator PRIVATE ignition-transport${IGN_TRANSPORT_VER}::core) diff --git a/src/modules/ignition_simulator/IgnitionSimulator.cpp b/src/modules/ignition_simulator/IgnitionSimulator.cpp new file mode 100644 index 0000000000..04492546e0 --- /dev/null +++ b/src/modules/ignition_simulator/IgnitionSimulator.cpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "IgnitionSimulator.hpp" + +#include +#include + +#include +#include + +void IgnitionSimulator::ImuCallback(const ignition::msgs::IMU &imu) +{ + // FLU -> FRD + _px4_accel.update(hrt_absolute_time(), + imu.linear_acceleration().x(), + -imu.linear_acceleration().y(), + -imu.linear_acceleration().z()); + + _px4_gyro.update(hrt_absolute_time(), + imu.angular_velocity().x(), + -imu.angular_velocity().y(), + -imu.angular_velocity().z()); +} + +void IgnitionSimulator::PoseInfoCallback(const ignition::msgs::Pose_V &pose) +{ + // TODO: find by name or id? + // + + //pose.position + //pose.orientation + + + // TODO: + // - gps + // - magnetometer + // - barometer + + // - groundtruth + + for (int p = 0; p < pose.pose_size(); p++) { + //std::cout << p << pose.pose(p).name() << std::endl; + + std::string vehicle_name = "x3"; + + if (pose.pose(p).name() == vehicle_name) { + + //std::cout << pose.pose(p) << std::endl; + + // ignition.msgs.Header header + // .ignition.msgs.Vector3d position = 4; + // .ignition.msgs.Quaternion orientation = 5; + + ignition::msgs::Vector3d position = pose.pose(p).position(); + ignition::msgs::Quaternion orientation = pose.pose(p).orientation(); + // position.x(), position.y(), position.z() + // orientation. wxyz + + //PX4_INFO("matched, position [%.6f, %.6f, %.6f]", (double)position.x(), (double)position.y(), (double)position.z()); + + vehicle_local_position_s local_position_groundtruth{}; + local_position_groundtruth.x = position.x(); + local_position_groundtruth.y = position.y(); + local_position_groundtruth.z = position.z(); + local_position_groundtruth.timestamp = hrt_absolute_time(); + + _lpos_ground_truth_pub.publish(local_position_groundtruth); + } + } + + + + +// header { +// stamp { +// sec: 93 +// nsec: 200000000 +// } +// } +// pose { +// name: "x3" +// id: 8 +// position { +// x: 9.41649363105415e-19 +// y: -1.038857584459745e-19 +// z: 0.054999053759016557 +// } +// orientation { +// x: -3.5059616538432739e-20 +// y: -7.2796873500671957e-18 +// z: 1.3844952801031163e-18 +// w: 1 +// } +// } + + + +} + +IgnitionSimulator::IgnitionSimulator() : + ModuleParams(nullptr), + _px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro(1310988) // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION +{ + _px4_accel.set_range(2000.f); // don't care + _px4_gyro.set_scale(math::radians(2000.f) / static_cast(INT16_MAX - 1)); // 2000 degrees/second max +} + +void IgnitionSimulator::run() +{ + ignition::transport::Node node; + + // Subscribe to messages of other plugins. + node.Subscribe("/imu", &IgnitionSimulator::ImuCallback, this); + node.Subscribe("/world/quadcopter/pose/info", &IgnitionSimulator::PoseInfoCallback, this); + + + std::string topic = "/X3/gazebo/command/motor_speed"; + auto pub = node.Advertise(topic); + + ignition::msgs::Actuators rotor_velocity_message; + rotor_velocity_message.mutable_velocity()->Resize(4, 0); + + + uORB::Subscription actuator_outputs_sub{ORB_ID(actuator_outputs)}; + + while (true) { + actuator_outputs_s actuator_outputs; + + if (actuator_outputs_sub.update(&actuator_outputs)) { + rotor_velocity_message.set_velocity(0, math::constrain(actuator_outputs.output[0] - 1000, 0.f, 1000.f)); + rotor_velocity_message.set_velocity(1, math::constrain(actuator_outputs.output[1] - 1000, 0.f, 1000.f)); + rotor_velocity_message.set_velocity(2, math::constrain(actuator_outputs.output[2] - 1000, 0.f, 1000.f)); + rotor_velocity_message.set_velocity(3, math::constrain(actuator_outputs.output[3] - 1000, 0.f, 1000.f)); + + if (!pub.Publish(rotor_velocity_message)) { + PX4_ERR("publish failed"); + } + } + + + px4_usleep(1000); + } + + // TODO: publish + + + // Zzzzzz. + ignition::transport::waitForShutdown(); +} + +int IgnitionSimulator::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +IgnitionSimulator *IgnitionSimulator::instantiate(int argc, char *argv[]) +{ + IgnitionSimulator *instance = new IgnitionSimulator(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +int IgnitionSimulator::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int IgnitionSimulator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int IgnitionSimulator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ignition_simulator", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int ignition_simulator_main(int argc, char *argv[]) +{ + return IgnitionSimulator::main(argc, argv); +} diff --git a/src/modules/ignition_simulator/IgnitionSimulator.hpp b/src/modules/ignition_simulator/IgnitionSimulator.hpp new file mode 100644 index 0000000000..1c2a6e83b9 --- /dev/null +++ b/src/modules/ignition_simulator/IgnitionSimulator.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +//#include +#include + +using namespace time_literals; + +class IgnitionSimulator : public ModuleBase, public ModuleParams +{ +public: + IgnitionSimulator(); + ~IgnitionSimulator() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static IgnitionSimulator *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); + + void ImuCallback(const ignition::msgs::IMU &imu); + void PoseInfoCallback(const ignition::msgs::Pose_V &pose); + + DEFINE_PARAMETERS( + (ParamInt) _param_sys_autostart, /**< example parameter */ + (ParamInt) _param_sys_autoconfig /**< another parameter */ + ) + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + hrt_abstime _time_start_us{0}; + + uint32_t _sensor_interval_us{1250}; +}; diff --git a/src/modules/ignition_simulator/Kconfig b/src/modules/ignition_simulator/Kconfig new file mode 100644 index 0000000000..132a3ae7b4 --- /dev/null +++ b/src/modules/ignition_simulator/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_IGNITION_SIMULATOR + bool "ignition_simulator" + default n + ---help--- + Enable support for ignition_simulator