WIP: ignition gazebo direclty use transport

.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

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

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

boards/px4/sitl/ign.px4board

@@ -0,0 +1,3 @@
+CONFIG_MODULES_SIMULATOR=y
+CONFIG_BOARD_NOLOCKSTEP=y
+CONFIG_MODULES_IGNITION_SIMULATOR=y

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)
 
 

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)

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 <uORB/topics/actuator_outputs.h>
+#include <uORB/Subscription.hpp>
+
+#include <iostream>
+#include <string>
+
+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<float>(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<ignition::msgs::Actuators>(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);
+}

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 <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+#include <ignition/msgs.hh>
+#include <ignition/transport.hh>
+
+//#include <ignition/gazebo/components/Imu.hh>
+#include <ignition/msgs/imu.pb.h>
+
+using namespace time_literals;
+
+class IgnitionSimulator : public ModuleBase<IgnitionSimulator>, 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<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
+		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
+	)
+
+	// Subscriptions
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
+	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
+	uORB::Publication<vehicle_local_position_s>   _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};
+};

src/modules/ignition_simulator/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_IGNITION_SIMULATOR
+	bool "ignition_simulator"
+	default n
+	---help---
+		Enable support for ignition_simulator