Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Atos Simulator Control receiverID and messagecounter #587

Merged
merged 2 commits into from
Oct 13, 2023
Merged

Atos Simulator Control receiverID and messagecounter #587

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
779842d
Atos Simulator Control receiverID and messagecounter
Sep 6, 2023
9ea9845
Merge Dev into simulator_control
Oct 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 11 additions & 4 deletions modules/ObjectControl/inc/channel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,16 +19,22 @@ struct MonitorMessage : std::pair<uint32_t,ObjectMonitorType> {};
class Channel : public Loggable
{
public:
Channel(const size_t bufferLength, const int type, rclcpp::Logger log)
Channel(const size_t bufferLength, const int type, rclcpp::Logger log, int id = 0, int transmitter = 0)
: channelType(type),
transmitBuffer(bufferLength, 0),
receiveBuffer(bufferLength, 0),
Loggable(log)
Loggable(log),
objectId(id),
transmitterId(transmitter)
{}
Channel(int type, rclcpp::Logger log) : Channel(1024, type, log) {}
Channel(int type, rclcpp::Logger log, int id = 0) : Channel(1024, type, log, id) {}
struct sockaddr_in addr = {};
int socket = -1;
int channelType = 0; //!< SOCK_STREAM or SOCK_DGRAM
int objectId = 0;
int transmitterId = 0;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
int transmitterId = 0;
int ATOSTransmitterId = 0;

int sentMessageCounter = 0;
int receivedMessageCounter = 0;
std::vector<char> transmitBuffer;
std::vector<char> receiveBuffer;

Expand All @@ -49,5 +55,6 @@ class Channel : public Loggable

friend Channel& operator>>(Channel&,MonitorMessage&);
friend Channel& operator>>(Channel&,ObjectPropertiesType&);

protected:
MessageHeaderType *populateHeaderType(MessageHeaderType *header);
};
6 changes: 3 additions & 3 deletions modules/ObjectControl/inc/objectconnection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@ class ObjectConnection : public Loggable {
Channel cmd;
Channel mntr;

ObjectConnection(rclcpp::Logger log)
: cmd(SOCK_STREAM, log),
mntr(SOCK_DGRAM, log),
ObjectConnection(rclcpp::Logger log, int id)
: cmd(SOCK_STREAM, log, id),
mntr(SOCK_DGRAM, log, id),
Loggable(log) {
pipe(interruptionPipeFds);
}
Expand Down
33 changes: 26 additions & 7 deletions modules/ObjectControl/src/channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,13 @@
#include "iso22133.h"
#include <cstring>
#include "atosTime.h"
#include "header.h"

using namespace ROSChannels;

Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) {
auto nBytes = encodeHEABMessage(&heartbeat.dataTimestamp, heartbeat.controlCenterStatus,
MessageHeaderType header;
auto nBytes = encodeHEABMessage(chnl.populateHeaderType(&header), &heartbeat.dataTimestamp, heartbeat.controlCenterStatus,
chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
if (nBytes < 0) {
throw std::invalid_argument(std::string("Failed to encode HEAB message: ") + strerror(errno));
Expand All @@ -24,7 +26,8 @@ Channel& operator<<(Channel& chnl, const HeabMessageDataType& heartbeat) {
}

Channel& operator<<(Channel& chnl, const ObjectSettingsType& settings) {
auto nBytes = encodeOSEMMessage(&settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
MessageHeaderType header;
auto nBytes = encodeOSEMMessage(chnl.populateHeaderType(&header), &settings, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
if (nBytes < 0) {
throw std::invalid_argument(std::string("Failed to encode OSEM message: ") + strerror(errno));
}
Expand All @@ -39,7 +42,8 @@ Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) {
ssize_t nBytes;

// TRAJ header
nBytes = encodeTRAJMessageHeader(
MessageHeaderType header;
nBytes = encodeTRAJMessageHeader(chnl.populateHeaderType(&header),
traj.id, TRAJECTORY_INFO_RELATIVE_TO_ORIGIN, traj.name.c_str(),traj.name.length(),
static_cast<uint32_t>(traj.points.size()), chnl.transmitBuffer.data(),
chnl.transmitBuffer.size(), false);
Expand Down Expand Up @@ -87,7 +91,8 @@ Channel& operator<<(Channel& chnl, const ATOS::Trajectory& traj) {
}

Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) {
auto nBytes = encodeOSTMMessage(cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
MessageHeaderType header;
auto nBytes = encodeOSTMMessage(chnl.populateHeaderType(&header), cmd, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
if (nBytes < 0) {
throw std::invalid_argument(std::string("Failed to encode OSTM message: ") + strerror(errno));
}
Expand All @@ -99,7 +104,8 @@ Channel& operator<<(Channel& chnl, const ObjectCommandType& cmd) {
}

Channel& operator<<(Channel& chnl, const StartMessageType& strt) {
auto nBytes = encodeSTRTMessage(&strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
MessageHeaderType header;
auto nBytes = encodeSTRTMessage(chnl.populateHeaderType(&header), &strt, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
if (nBytes < 0) {
throw std::invalid_argument(std::string("Failed to encode STRT message: ") + strerror(errno));
}
Expand All @@ -115,8 +121,11 @@ Channel& operator>>(Channel& chnl, MonitorMessage& monitor) {
if (chnl.pendingMessageType() == MESSAGE_ID_MONR) {
struct timeval tv;
TimeSetToCurrentSystemTime(&tv);
HeaderType header;
decodeISOHeader(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), &header, false);
monitor.first = header.transmitterID;
auto nBytes = decodeMONRMessage(chnl.receiveBuffer.data(), chnl.receiveBuffer.size(), tv,
&monitor.first, &monitor.second, false);
&monitor.second, false);
if (nBytes < 0) {
throw std::invalid_argument("Failed to decode MONR message");
}
Expand Down Expand Up @@ -158,7 +167,9 @@ Channel& operator<<(Channel& chnl, const ControlSignal::message_type::SharedPtr
rcmm.throttleManoeuvre.pct = csp->throttle;
rcmm.brakeManoeuvre.pct = csp->brake;
rcmm.steeringManoeuvre.pct = csp->steering_angle;
auto nBytes = encodeRCMMMessage(&rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);

MessageHeaderType header;
auto nBytes = encodeRCMMMessage(chnl.populateHeaderType(&header),&rcmm, chnl.transmitBuffer.data(), chnl.transmitBuffer.size(), false);
if (nBytes < 0) {
throw std::invalid_argument(std::string("Failed to encode RCM message: ") + strerror(errno));
}
Expand Down Expand Up @@ -204,6 +215,14 @@ ISOMessageID Channel::pendingMessageType(bool awaitNext) {
}
}

MessageHeaderType *Channel::populateHeaderType(MessageHeaderType *header) {
memset(header, 0, sizeof (MessageHeaderType));
header->transmitterID = this->transmitterId;
header->receiverID = this->objectId;
header->messageCounter = this->sentMessageCounter++;
return header;
}

void Channel::connect(
std::shared_future<void> stopRequest,
const std::chrono::milliseconds retryPeriod) {
Expand Down
6 changes: 3 additions & 3 deletions modules/ObjectControl/src/testobject.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ using std::placeholders::_1;
TestObject::TestObject(uint32_t id) :
Node("object_" + std::to_string(id)),
osiChannel(SOCK_STREAM, get_logger()),
comms(get_logger()),
comms(get_logger(), id),
conf(get_logger())
{
pathSub = std::make_shared<ROSChannels::Path::Sub>(*this, id, std::bind(&TestObject::onPathMessage, this, _1, id));
Expand All @@ -29,7 +29,7 @@ void TestObject::onPathMessage(const ROSChannels::Path::message_type::SharedPtr

TestObject::TestObject(TestObject&& other) :
osiChannel(SOCK_STREAM, other.get_logger()),
comms(other.get_logger()),
comms(other.get_logger(), other.getTransmitterID()),
state(other.state),
conf(other.conf),
lastMonitor(other.lastMonitor),
Expand Down Expand Up @@ -266,7 +266,7 @@ void TestObject::sendSettings() {

ObjectSettingsType objSettings;
objSettings.desiredID.transmitter = conf.getTransmitterID();
objSettings.desiredID.controlCentre = ::getTransmitterID();
objSettings.desiredID.controlCentre = 0;
objSettings.desiredID.subTransmitter = 0;

objSettings.coordinateSystemOrigin = conf.getOrigin();
Expand Down