114 lines
3.7 KiB
C++
114 lines
3.7 KiB
C++
#include <SimCore/Identifier.hpp>
|
|
#include <SimCore/Messages/Protos/GroundTruthTrack.pb.h>
|
|
#include <SimCore/Messages/Track.hpp>
|
|
#include <SimCore/SimCore.hpp>
|
|
#include <WHISPER/Messages/Message.hpp>
|
|
#include <google/protobuf/message.h>
|
|
#include <SimCore/Messages/GroundThruthTrack.hpp>
|
|
|
|
|
|
|
|
|
|
namespace SimCore
|
|
{
|
|
[[deprecated("use SimTrack instead")]]
|
|
GroundTruthTrack::GroundTruthTrack(WHISPER::SourceType src,SimCore::Identifier id,SimCore::TrackKind kind):Track(src, id, kind)
|
|
{
|
|
|
|
}
|
|
|
|
WHISPER::Message GroundTruthTrack::buildMessage(SimCore::Identifier parentID)
|
|
{
|
|
WHISPER::Message msg(parentID.getParentNumber(), parentID.getNumber(),WHISPER::MsgTopics::TRACK , WHISPER::MsgType::GROUND_TRUTH_TRACK, WHISPER::SourceType::GATEWAY);
|
|
|
|
|
|
messages::track::GroundTruthTrack groundTruthTrack;
|
|
|
|
groundTruthTrack.mutable_geocentricposition()->set_x(position_.getGeocentricPos()(SimCore::X));
|
|
groundTruthTrack.mutable_geocentricposition()->set_y(position_.getGeocentricPos()(SimCore::Y));
|
|
groundTruthTrack.mutable_geocentricposition()->set_z(position_.getGeocentricPos()(SimCore::Z));
|
|
|
|
groundTruthTrack.set_contactspeed(speed_);
|
|
groundTruthTrack.set_contactcourse(course_);
|
|
|
|
|
|
|
|
groundTruthTrack.mutable_entityidentifier()->set_number(this->getIdentifier().getNumber());
|
|
groundTruthTrack.mutable_entityidentifier()->set_external((uint32_t)this->getIdentifier().isExternal());
|
|
groundTruthTrack.mutable_entityidentifier()->set_parent(this->getIdentifier().getParentNumber());
|
|
|
|
|
|
groundTruthTrack.mutable_timestamp()->set_seconds(time(NULL));
|
|
|
|
auto any = std::make_shared<google::protobuf::Any>();
|
|
|
|
any->PackFrom(groundTruthTrack);
|
|
msg.addPayLoad(any);
|
|
|
|
return msg;
|
|
}
|
|
|
|
void GroundTruthTrack::setPosition(Position pos) {
|
|
position_ = pos;
|
|
}
|
|
|
|
void GroundTruthTrack::setPosition(double x, double y, double z) {
|
|
Position pos(x, y, z);
|
|
position_ = pos;
|
|
}
|
|
|
|
Position GroundTruthTrack::getPostion() { return position_; }
|
|
|
|
void GroundTruthTrack::setSpeed(double speed) {
|
|
speed_ = speed;
|
|
}
|
|
void GroundTruthTrack::setCourse(double course) {
|
|
course_ = course;
|
|
}
|
|
|
|
void GroundTruthTrack::setSpeedinKnots(double knots) {
|
|
speed_ = knots / SimCore::MsKt;
|
|
}
|
|
|
|
void GroundTruthTrack::setEnvironment(SimCore::EntityKind env )
|
|
{
|
|
environemnt_ = env;
|
|
}
|
|
|
|
double GroundTruthTrack::getSpeed() { return speed_; }
|
|
double GroundTruthTrack::getSpeedinKnots() { return speed_ * SimCore::MsKt; }
|
|
double GroundTruthTrack::getCourse() { return course_; }
|
|
SimCore::EntityKind GroundTruthTrack::getEnvironment(){ return environemnt_; }
|
|
|
|
|
|
|
|
|
|
GroundTruthTrack GroundTruthTrack::unpack(WHISPER::Message msg)
|
|
{
|
|
auto m = msg.getProtoMessage();
|
|
|
|
auto trackMsg = messages::track::GroundTruthTrack();
|
|
if(m.payload().Is<messages::track::GroundTruthTrack>())
|
|
{
|
|
m.payload().UnpackTo(&trackMsg);
|
|
}
|
|
SimCore::Identifier id(trackMsg.mutable_entityidentifier()->parent(),trackMsg.mutable_entityidentifier()->number(),trackMsg.mutable_entityidentifier()->external());
|
|
GroundTruthTrack track((WHISPER::SourceType)m.sourcetype(), id,(SimCore::TrackKind)trackMsg.trackkind());
|
|
if (trackMsg.has_geocentricposition())
|
|
{
|
|
track.setPosition(trackMsg.mutable_geocentricposition()->x(), trackMsg.mutable_geocentricposition()->y(), trackMsg.mutable_geocentricposition()->z());
|
|
}
|
|
if (trackMsg.has_contactcourse())
|
|
{
|
|
track.setCourse(trackMsg.contactcourse());
|
|
}
|
|
if (trackMsg.has_contactspeed())
|
|
{
|
|
track.setSpeed(trackMsg.contactspeed());
|
|
}
|
|
|
|
|
|
return track;
|
|
}
|
|
|
|
} |