mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-12 20:00:35 +08:00
WIP: ignition gazebo direclty use transport
This commit is contained in:
Daniel Agar
@@ -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);
|
||||
}
|
||||
Reference in New Issue
Block a user