mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-14 05:07:35 +08:00
new Ignition Gazebo simulation interface architecture (#20057)
- much simpler direct interface using Ignition Transport - in tree models and worlds - control allocation output configuration, no more magic actuator mapping to mavlink and back - currently requires no custom Gazebo plugins (keeping things as lightweight and simple as possible) Co-authored-by: Jaeyoung-Lim <jalim@ethz.ch>
This commit is contained in:
Daniel Agar
@@ -0,0 +1,73 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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)
|
||||
#add_compile_options(-fexceptions)
|
||||
|
||||
# Find the Ignition_Transport library
|
||||
find_package(ignition-transport
|
||||
REQUIRED COMPONENTS core
|
||||
NAMES
|
||||
ignition-transport8
|
||||
ignition-transport10
|
||||
ignition-transport11
|
||||
|
||||
#QUIET
|
||||
)
|
||||
|
||||
message(STATUS "PACKAGE_FIND_NAME: ${ignition-transport_NAME}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_MAJOR: ${ignition-transport_VERSION_MAJOR}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_MINOR: ${ignition-transport_VERSION_MINOR}")
|
||||
message(STATUS "PACKAGE_FIND_VERSION_COUNT: ${ignition-transport_VERSION_COUNT}")
|
||||
|
||||
set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__ignition_simulator
|
||||
MAIN ignition_simulator
|
||||
COMPILE_FLAGS
|
||||
#${MAX_CUSTOM_OPT_LEVEL}
|
||||
-O0
|
||||
SRCS
|
||||
IgnitionSimulator.cpp
|
||||
IgnitionSimulator.hpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
mixer_module
|
||||
px4_work_queue
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
||||
target_link_libraries(modules__ignition_simulator PRIVATE ignition-transport${IGN_TRANSPORT_VER}::core)
|
||||
@@ -0,0 +1,382 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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/Subscription.hpp>
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
IgnitionSimulator::IgnitionSimulator(const char *world, const char *model) :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_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
|
||||
_world_name(world),
|
||||
_model_name(model)
|
||||
{
|
||||
_px4_accel.set_range(2000.f); // don't care
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
IgnitionSimulator::~IgnitionSimulator()
|
||||
{
|
||||
// TODO: unsubscribe
|
||||
|
||||
for (auto &sub_topic : _node.SubscribedTopics()) {
|
||||
_node.Unsubscribe(sub_topic);
|
||||
}
|
||||
}
|
||||
|
||||
int IgnitionSimulator::init()
|
||||
{
|
||||
// clock
|
||||
std::string clock_topic = "/world/" + _world_name + "/clock";
|
||||
_node.Subscribe(clock_topic, &IgnitionSimulator::clockCallback, this);
|
||||
|
||||
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
|
||||
_node.Subscribe(world_pose_topic, &IgnitionSimulator::poseInfoCallback, this);
|
||||
|
||||
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
|
||||
_node.EnableStats(imu_topic, true);
|
||||
_node.Subscribe(imu_topic, &IgnitionSimulator::imuCallback, this);
|
||||
|
||||
for (auto &sub_topic : _node.SubscribedTopics()) {
|
||||
PX4_INFO("subscribed: %s", sub_topic.c_str());
|
||||
}
|
||||
|
||||
// output eg /X3/command/motor_speed
|
||||
std::string actuator_topic = _model_name + "/command/motor_speed";
|
||||
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
|
||||
|
||||
if (!_actuators_pub.Valid()) {
|
||||
PX4_ERR("failed to advertise %s", actuator_topic.c_str());
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
ScheduleNow();
|
||||
return OK;
|
||||
}
|
||||
|
||||
int IgnitionSimulator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
const char *world_name = "empty";
|
||||
const char *model_name = nullptr;
|
||||
|
||||
bool error_flag = false;
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "w:m:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'w':
|
||||
// world
|
||||
world_name = myoptarg;
|
||||
break;
|
||||
|
||||
case 'm':
|
||||
// model
|
||||
model_name = myoptarg;
|
||||
break;
|
||||
|
||||
case '?':
|
||||
error_flag = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("unrecognized flag");
|
||||
error_flag = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (error_flag) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
PX4_INFO("world: %s, model: %s", world_name, model_name);
|
||||
|
||||
IgnitionSimulator *instance = new IgnitionSimulator(world_name, model_name);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void IgnitionSimulator::clockCallback(const ignition::msgs::Clock &clock)
|
||||
{
|
||||
// uint64_t time_us = clock.sim().sec() * 1e6 + clock.sim().nsec() / 1000;
|
||||
// PX4_INFO("clock %lu ", time_us);
|
||||
|
||||
struct timespec ts;
|
||||
ts.tv_sec = clock.sim().sec();
|
||||
ts.tv_nsec = clock.sim().nsec();
|
||||
px4_clock_settime(CLOCK_MONOTONIC, &ts);
|
||||
}
|
||||
|
||||
void IgnitionSimulator::imuCallback(const ignition::msgs::IMU &imu)
|
||||
{
|
||||
// FLU -> FRD
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
|
||||
ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
|
||||
imu.linear_acceleration().x(),
|
||||
imu.linear_acceleration().y(),
|
||||
imu.linear_acceleration().z()));
|
||||
|
||||
ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
|
||||
imu.angular_velocity().x(),
|
||||
imu.angular_velocity().y(),
|
||||
imu.angular_velocity().z()));
|
||||
|
||||
//const uint64_t time_us = imu.header().stamp().sec() * 1e6 + imu.header().stamp().nsec() / 1000;
|
||||
const uint64_t time_us = hrt_absolute_time();
|
||||
|
||||
if (time_us != 0) {
|
||||
_px4_accel.update(time_us, accel_b.X(), accel_b.Y(), accel_b.Z());
|
||||
_px4_gyro.update(time_us, gyro_b.X(), gyro_b.Y(), gyro_b.Z());
|
||||
}
|
||||
}
|
||||
|
||||
void IgnitionSimulator::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
{
|
||||
//const uint64_t time_us = pose.header().stamp().sec() * 1e6 + pose.header().stamp().nsec() / 1000;
|
||||
const uint64_t time_us = hrt_absolute_time();
|
||||
|
||||
if (time_us == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int p = 0; p < pose.pose_size(); p++) {
|
||||
if (pose.pose(p).name() == _model_name) {
|
||||
|
||||
const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1);
|
||||
_timestamp_prev = time_us;
|
||||
|
||||
ignition::msgs::Vector3d pose_position = pose.pose(p).position();
|
||||
ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
|
||||
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
|
||||
/**
|
||||
* @brief Quaternion for rotation between ENU and NED frames
|
||||
*
|
||||
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
|
||||
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
|
||||
* This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
|
||||
*/
|
||||
static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
|
||||
|
||||
// ground truth
|
||||
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
|
||||
pose_orientation.w(),
|
||||
pose_orientation.x(),
|
||||
pose_orientation.y(),
|
||||
pose_orientation.z());
|
||||
|
||||
ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
|
||||
ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
|
||||
|
||||
// publish attitude groundtruth
|
||||
vehicle_attitude_s vehicle_attitude_groundtruth{};
|
||||
vehicle_attitude_groundtruth.timestamp_sample = time_us;
|
||||
vehicle_attitude_groundtruth.q[0] = q_nb.W();
|
||||
vehicle_attitude_groundtruth.q[1] = q_nb.X();
|
||||
vehicle_attitude_groundtruth.q[2] = q_nb.Y();
|
||||
vehicle_attitude_groundtruth.q[3] = q_nb.Z();
|
||||
vehicle_attitude_groundtruth.timestamp = hrt_absolute_time();
|
||||
_attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth);
|
||||
|
||||
// publish angular velocity groundtruth
|
||||
const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)};
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{};
|
||||
vehicle_angular_velocity_groundtruth.timestamp_sample = time_us;
|
||||
|
||||
const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt;
|
||||
_euler_prev = euler;
|
||||
angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz);
|
||||
|
||||
vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
|
||||
_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
|
||||
|
||||
if (!_pos_ref.isInitialized()) {
|
||||
_pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time());
|
||||
}
|
||||
|
||||
vehicle_local_position_s local_position_groundtruth{};
|
||||
local_position_groundtruth.timestamp_sample = time_us;
|
||||
|
||||
// position ENU -> NED
|
||||
const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()};
|
||||
const matrix::Vector3d velocity{(position - _position_prev) / dt};
|
||||
const matrix::Vector3d acceleration{(velocity - _velocity_prev) / dt};
|
||||
|
||||
_position_prev = position;
|
||||
_velocity_prev = velocity;
|
||||
|
||||
local_position_groundtruth.ax = acceleration(0);
|
||||
local_position_groundtruth.ay = acceleration(1);
|
||||
local_position_groundtruth.az = acceleration(2);
|
||||
local_position_groundtruth.vx = velocity(0);
|
||||
local_position_groundtruth.vy = velocity(1);
|
||||
local_position_groundtruth.vz = velocity(2);
|
||||
local_position_groundtruth.x = position(0);
|
||||
local_position_groundtruth.y = position(1);
|
||||
local_position_groundtruth.z = position(2);
|
||||
|
||||
local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
|
||||
local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
|
||||
local_position_groundtruth.ref_alt = _param_sim_home_alt.get();
|
||||
local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
|
||||
|
||||
local_position_groundtruth.timestamp = hrt_absolute_time();
|
||||
_lpos_ground_truth_pub.publish(local_position_groundtruth);
|
||||
|
||||
if (_pos_ref.isInitialized()) {
|
||||
// publish position groundtruth
|
||||
vehicle_global_position_s global_position_groundtruth{};
|
||||
global_position_groundtruth.timestamp_sample = time_us;
|
||||
|
||||
_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
|
||||
global_position_groundtruth.lat, global_position_groundtruth.lon);
|
||||
|
||||
global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
|
||||
global_position_groundtruth.timestamp = hrt_absolute_time();
|
||||
_gpos_ground_truth_pub.publish(global_position_groundtruth);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool IgnitionSimulator::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
{
|
||||
ignition::msgs::Actuators rotor_velocity_message;
|
||||
rotor_velocity_message.mutable_velocity()->Resize(num_outputs, 0);
|
||||
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
rotor_velocity_message.set_velocity(i, outputs[i]);
|
||||
}
|
||||
|
||||
if (_actuators_pub.Valid()) {
|
||||
return _actuators_pub.Publish(rotor_velocity_message);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void IgnitionSimulator::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
||||
_mixing_output.update();
|
||||
|
||||
//ScheduleDelayed(1000_us);
|
||||
|
||||
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
|
||||
_mixing_output.updateSubscriptions(true);
|
||||
}
|
||||
|
||||
int IgnitionSimulator::print_status()
|
||||
{
|
||||
//perf_print_counter(_cycle_perf);
|
||||
_mixing_output.printStatus();
|
||||
|
||||
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_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int ignition_simulator_main(int argc, char *argv[])
|
||||
{
|
||||
return IgnitionSimulator::main(argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,125 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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 <lib/geo/geo.h>
|
||||
#include <lib/mixer_module/mixer_module.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/msgs/imu.pb.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class IgnitionSimulator : public ModuleBase<IgnitionSimulator>, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
IgnitionSimulator(const char *world, const char *model);
|
||||
~IgnitionSimulator() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
int init();
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
void mixerChanged() override {}
|
||||
|
||||
void Run() override;
|
||||
|
||||
void clockCallback(const ignition::msgs::Clock &clock);
|
||||
void imuCallback(const ignition::msgs::IMU &imu);
|
||||
void poseInfoCallback(const ignition::msgs::Pose_V &pose);
|
||||
|
||||
// Subscriptions
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Publication<vehicle_angular_velocity_s> _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;
|
||||
|
||||
MapProjection _pos_ref{};
|
||||
|
||||
matrix::Vector3d _position_prev{};
|
||||
matrix::Vector3d _velocity_prev{};
|
||||
matrix::Vector3f _euler_prev{};
|
||||
hrt_abstime _timestamp_prev{};
|
||||
|
||||
const std::string _world_name;
|
||||
const std::string _model_name;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
ignition::transport::Node _node;
|
||||
ignition::transport::Node::Publisher _actuators_pub;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SIM_HOME_LAT>) _param_sim_home_lat,
|
||||
(ParamFloat<px4::params::SIM_HOME_LON>) _param_sim_home_lon,
|
||||
(ParamFloat<px4::params::SIM_HOME_ALT>) _param_sim_home_alt
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_IGNITION_SIMULATOR
|
||||
bool "ignition_simulator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ignition_simulator
|
||||
@@ -0,0 +1,10 @@
|
||||
module_name: SIM_GZ
|
||||
actuator_output:
|
||||
output_groups:
|
||||
- param_prefix: SIM_GZ
|
||||
channel_label: Channel
|
||||
num_channels: 8
|
||||
standard_params:
|
||||
disarmed: { min: 0, max: 1000, default: 0 }
|
||||
min: { min: 0, max: 1000, default: 10 }
|
||||
max: { min: 100, max: 1000, default: 1000 }
|
||||
@@ -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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* simulator origin latitude
|
||||
*
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LAT, 47.397742f);
|
||||
|
||||
/**
|
||||
* simulator origin longitude
|
||||
*
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LON, 8.545594);
|
||||
|
||||
/**
|
||||
* simulator origin altitude
|
||||
*
|
||||
* @unit m
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_ALT, 488.0);
|
||||
@@ -234,6 +234,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_topic("time_offset");
|
||||
add_topic("vehicle_angular_acceleration", 10);
|
||||
add_topic("vehicle_angular_velocity", 10);
|
||||
add_topic("vehicle_angular_velocity_groundtruth", 10);
|
||||
add_topic("vehicle_attitude_groundtruth", 10);
|
||||
add_topic("vehicle_global_position_groundtruth", 100);
|
||||
add_topic("vehicle_local_position_groundtruth", 20);
|
||||
|
||||
@@ -36,7 +36,11 @@ px4_add_library(vehicle_angular_velocity
|
||||
VehicleAngularVelocity.hpp
|
||||
)
|
||||
|
||||
target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_compile_options(vehicle_angular_velocity
|
||||
PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
)
|
||||
|
||||
target_link_libraries(vehicle_angular_velocity
|
||||
PRIVATE
|
||||
|
||||
@@ -112,7 +112,9 @@ bool VehicleAngularVelocity::UpdateSampleRate()
|
||||
}
|
||||
|
||||
// calculate sensor update rate
|
||||
if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) {
|
||||
if (PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 10) && (sample_rate_hz < 10'000)
|
||||
&& PX4_ISFINITE(publish_rate_hz) && (publish_rate_hz > 0)
|
||||
) {
|
||||
// check if sample rate error is greater than 1%
|
||||
const bool sample_rate_changed = (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f;
|
||||
|
||||
|
||||
@@ -35,5 +35,11 @@ px4_add_library(vehicle_imu
|
||||
VehicleIMU.cpp
|
||||
VehicleIMU.hpp
|
||||
)
|
||||
target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
target_compile_options(vehicle_imu
|
||||
PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
)
|
||||
|
||||
target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration)
|
||||
|
||||
@@ -156,7 +156,7 @@ void SensorBaroSim::Run()
|
||||
const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa;
|
||||
|
||||
// convert to hPa
|
||||
float pressure = absolute_pressure_noisy * 0.01f + _sim_baro_off_p.get();
|
||||
float pressure = absolute_pressure_noisy + _sim_baro_off_p.get();
|
||||
|
||||
// calculate temperature in Celsius
|
||||
float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
|
||||
|
||||
@@ -150,7 +150,7 @@ void SensorGpsSim::Run()
|
||||
sensor_gps.vdop = 100.f;
|
||||
}
|
||||
|
||||
// sensor_gps.timestamp_sample = gpos.timestamp;
|
||||
sensor_gps.timestamp_sample = gpos.timestamp_sample;
|
||||
sensor_gps.time_utc_usec = 0;
|
||||
sensor_gps.device_id = device_id.devid;
|
||||
sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees
|
||||
|
||||
Reference in New Issue
Block a user