// UwbPosition.hpp
#pragma once
#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>
#include <gz/sim/Model.hh>
#include <gz/transport/Node.hh>
#include <gz/plugin/Register.hh>
#include <gz/common/Console.hh>
#include <gz/sim/components/Pose.hh>
#include <gz/sim/components/World.hh>
#include <gz/msgs/pose_v.pb.h>
#include <gz/msgs/pose.pb.h>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <chrono>
#include <iostream>
#include <string>
#include <random>
namespace custom
{
class UwbPositionPlugin:
public gz::sim::System,
public gz::sim::ISystemPreUpdate,
public gz::sim::ISystemPostUpdate
{
public:
// called when plugin is loaded and configured from SDF
void Configure(const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventMgr);
void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;
void PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm) final;
private:
// parent model
gz::sim::Model _model{gz::sim::kNullEntity};
// UART
int _uart_fd = -1;
std::string _uartDevice = "/dev/pts/15"; // use socat virtual port: $ socat -d -d pty,raw,echo=0 pty,raw,echo=0
unsigned int _baudRate = 115200;
float _noiseStdDev = 0.0f;
// Position Noise
std::default_random_engine _randGen;
std::normal_distribution<float> _noiseDist{0.0f,_noiseStdDev}; // mean and stddev
// Publish Timing
double _updateHz = 10.0;
std::chrono::steady_clock::duration _updatePeriod;
std::chrono::steady_clock::time_point _lastUpdateTime;
// subscribe
void OnPoseInfo(const gz::msgs::Pose_V& msg);
// send UWB data
void SendUwbData(const gz::math::Vector3f& pos);
// Add your private member variables and methods here
gz::transport::Node _node;
};
} // end namespace custom
// UwbPosition.cpp
#include "UwbPosition.hpp"
#include <gz/plugin/Register.hh>
#include <gz/sim/components/Pose.hh>
#include <cstring>
#include <chrono>
using namespace custom;
// Register the plugin
GZ_ADD_PLUGIN(
custom::UwbPositionPlugin,
gz::sim::System,
custom::UwbPositionPlugin::ISystemPreUpdate,
custom::UwbPositionPlugin::ISystemPostUpdate
)
GZ_ADD_PLUGIN_ALIAS(custom::UwbPositionPlugin, "custom::UwbPositionPlugin")
///////////////////////
void UwbPositionPlugin::Configure(
const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element>& _sdf,
gz::sim::EntityComponentManager& _ecm,
gz::sim::EventManager& _eventMgr)
{
this->_model = gz::sim::Model(_entity);
if(!this->_model.Valid(_ecm)){
gzerr <<"[UwbPositionPlugin] Model is invalid."<<std::endl;
return;
}
// Load SDF Parameters (optional)
/*
if (_sdf->HasElement("uart_device")) {
this->_uartDevice = _sdf->Get<std::string>("uart_device");
}
if (_sdf->HasElement("baud_rate")) {
this->_baudRate = _sdf->Get<unsigned int>("baud_rate");
}
if (_sdf->HasElement("update_rate")) {
this->_updateHz = _sdf->Get<double>("update_rate");
}
if (_sdf->HasElement("noise_stddev")) {
this->_noiseStdDev = _sdf->Get<float>("noise_stddev");
this->_noiseDist = std::normal_distribution<float>(0.0f, this->_noiseStdDev);
}
*/ // end SDF Parameters
std::cout<<"configuration called"<<std::endl;
gzmsg << "Configure Called!"<<std::endl;
// subscribe data
this->_node.Subscribe("/world/default/pose/info",&UwbPositionPlugin::OnPoseInfo,this);
// Publish timing
this->_updatePeriod = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(1.0/this->_updateHz));
this->_lastUpdateTime = std::chrono::steady_clock::now();
// open UART port
this->_uart_fd = ::open(this->_uartDevice.c_str(), O_RDWR|O_NOCTTY|O_NONBLOCK);
if(this->_uart_fd<0){
gzerr << "[UwbPositionPlugin] Failed to open UART port"<<this->_uartDevice<<" : "<<strerror(errno)<<std::endl;
return;
}
struct termios tty{};
if(tcgetattr(this->_uart_fd, &tty)!=0){
gzerr <<"[UwbPositionPlugin] Err from tcgetattr: "<<strerror(errno)<<std::endl;
return;
}
cfsetospeed(&tty,B115200);
cfsetispeed(&tty,B115200);
tty.c_cflag |= (CLOCAL | CREAD); // Enable receiver
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8; // 8-bit
tty.c_cflag &= ~PARENB; // No parity
tty.c_cflag &= ~CSTOPB; // 1 stop bit
tty.c_cflag &= ~CRTSCTS; // No flow control
tty.c_lflag = 0; // No canonical, echo
tty.c_oflag = 0;
tty.c_cc[VMIN] = 0;
tty.c_cc[VTIME] = 1;
if(tcsetattr(this->_uart_fd, TCSANOW, &tty)!=0){
gzerr<<"[UwbPositionPlugin] Error from tcsetattr: "<<strerror(errno)<<std::endl;
return;
}
gzdbg <<"[UwbPositionPlugin] Plugin configured. Sending to "<<this->_uartDevice<<std::endl;
}
void UwbPositionPlugin::PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm)
{
// Implement pre-update logic here
if(_info.paused||this->_uart_fd <0){ return; }
auto now = std::chrono::steady_clock::now();
if(now-this->_lastUpdateTime< this->_updatePeriod){ return; }
this->_lastUpdateTime = now;
// Get pose
// auto poseOpt = _ecm.Component<gz::sim::components::Pose>(this->_model.Entity());
// if (!poseOpt) {
// gzerr << "[UwbPositionPlugin] No pose component found." << std::endl;
// return;
// }
// gz::math::Vector3d pos = poseOpt->Data().Pos();
// gz::math::Vector3f pos_f(
// static_cast<float>(pos.X()),
// static_cast<float>(pos.Y()),
// static_cast<float>(pos.Z())
// );
// // Add Gaussian noise
// pos_f.X() += this->_noiseDist(this->_randGen);
// pos_f.Y() += this->_noiseDist(this->_randGen);
// pos_f.Z() += this->_noiseDist(this->_randGen);
// // Send over UART
// this->SendUwbData(pos_f);
}
void UwbPositionPlugin::OnPoseInfo(const gz::msgs::Pose_V &msg)
{
gzmsg << "OnPoseInfo called!"<<std::endl;
for(int i=0;i<msg.pose_size();++i)
{
const auto& poseMsg = msg.pose(i);
if(poseMsg.name()=="x500_0")
{
auto posMsg = poseMsg.position();
gz::math::Vector3f pos_f(
static_cast<float>(posMsg.x()),
static_cast<float>(posMsg.y()),
static_cast<float>(posMsg.z())
);
std::cout<<"onpose called: "<<posMsg.x()<<" "<<posMsg.y()<<" "<<posMsg.z()<<std::endl;
this->SendUwbData(pos_f);
}
}
}
void UwbPositionPlugin::SendUwbData(const gz::math::Vector3f &pos)
{
gzmsg << "SendUwbData called!"<< std::endl;
if (this->_uart_fd < 0) return;
const uint8_t LOCATION_DATA = 0x10;
const uint8_t CTRL_IDLE = 0x20;
const uint16_t END_CODE = 0xFFFF;
uint8_t buffer[20] = {};
size_t offset = 0;
float x = pos.X();
float y = pos.Y();
float z = pos.Z();
// Pack data
buffer[offset++] = LOCATION_DATA;
std::memcpy(&buffer[offset], &x, sizeof(float)); offset += sizeof(float);
std::memcpy(&buffer[offset], &y, sizeof(float)); offset += sizeof(float);
std::memcpy(&buffer[offset], &z, sizeof(float)); offset += sizeof(float);
buffer[offset++] = CTRL_IDLE;
std::memcpy(&buffer[offset], &END_CODE, sizeof(uint16_t)); offset += sizeof(uint16_t);
ssize_t bytes_written = write(this->_uart_fd, buffer, offset);
std::cout<<"written bytes "<< bytes_written<<std::endl;
gzmsg << "written bytes: " << bytes_written << std::endl;
}
void UwbPositionPlugin::PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm)
{
// Implement post-update logic here
}
I made this plugin for subscribe gz-local position of x500, and send xyz coordinate to UART port… I configured file settings following Gazebo Plugins | PX4 Guide (main) .
but I run $ make px4_sitl gz_x500….
this plugin is not loaded… plz give many advice….