Skip to content
Snippets Groups Projects
Commit 10de45ee authored by cs-99's avatar cs-99
Browse files

No commit message

No commit message
parent 1fb4ac0f
No related branches found
No related tags found
No related merge requests found
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
void callback(std_msgs::Int32ConstPtr& ptr) {
}
int main(int argc, char**argv){
ros::init(argc, argv, "garage");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Int32>("position", 1);
if (!pub) throw "publisher failed";
std_msgs::Int32 x;
x.data = 6;
while(pub.getNumSubscribers() == 0) {};
ros::Duration(3).sleep();
pub.publish(x);
ros::Duration(5).sleep();
}
\ No newline at end of file
...@@ -38,19 +38,19 @@ std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){ ...@@ -38,19 +38,19 @@ std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){
return arr; // no explicit move needed since return value is rvalue return arr; // no explicit move needed since return value is rvalue
}; };
inline int32_t bytes_to_int_32(byte *buf){ inline int32_t network_bytes_to_int_32(byte *buf){
uint32_t h = ntohl(*reinterpret_cast<uint32_t*>(buf)); uint32_t h = ntohl(*reinterpret_cast<uint32_t*>(buf));
return *reinterpret_cast<int32_t*>(&h); return *reinterpret_cast<int32_t*>(&h);
} }
inline uint16_t bytes_to_uint_16(byte *buf) { inline uint16_t network_bytes_to_uint_16(byte *buf) {
return ntohs(*reinterpret_cast<uint16_t*>(buf)); return ntohs(*reinterpret_cast<uint16_t*>(buf));
} }
// wrapping get_bytes_socket for converting to 32 bit integer // wrapping get_bytes_socket for converting to 32 bit integer
int32_t get_int_socket(int fd) { int32_t get_int_socket(int fd) {
auto ack = get_bytes_socket(fd, 4); auto ack = get_bytes_socket(fd, 4);
return bytes_to_int_32(ack.get()); return network_bytes_to_int_32(ack.get());
} }
// converts num to network order and adds it to byte array starting from index // converts num to network order and adds it to byte array starting from index
...@@ -81,6 +81,30 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf) { ...@@ -81,6 +81,30 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf) {
return true; return true;
} }
// returns file descriptor
int create_connected_socket(std::string IP, uint16_t port){
int socketfd;
if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
throw "failed to create socket";
}
ROS_INFO("socket created successfully");
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(port);
{
int x = inet_pton(AF_INET, IP.c_str(), &addr.sin_addr);
if (x != 1) {
throw "IP could not be converted";
}
}
if (connect(socketfd, (struct sockaddr*)&addr, sizeof(sockaddr_in)) < 0) {
throw "failed to connect";
}
ROS_INFO("successfully connected");
return socketfd;
}
// consts for TRON ---------------------------------------------------------- // consts for TRON ----------------------------------------------------------
const byte GET_ERROR_MSG = 127; const byte GET_ERROR_MSG = 127;
...@@ -96,24 +120,70 @@ const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set ...@@ -96,24 +120,70 @@ const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set
// global vars ------------------------------------------------------------- // global vars -------------------------------------------------------------
// ROS uses little endian for its messages
const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
struct Channel { struct Channel {
std::string name;
int32_t identifier; int32_t identifier;
bool is_input; bool is_input;
std::vector<std::string> vars; // associated variables in Uppaal std::vector<std::string> vars; // associated variables in Uppaal
Channel(int32_t id, bool is_input) : identifier(id), is_input(is_input){}; Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){};
Channel() = default; // default constructor needed for std::map
}; };
std::map<std::string, Channel> channels; std::vector<Channel> channels;
struct Mapping {
std::string topic; // ROS topic name
// offset in byte for next value (from last value)
std::vector<int> byte_offset;
// custom conversion function used if data is not int32 and therefore
// need to be converted to an int32 first
// nullptr if not used
// needs to make sure count of bytes used gets added to *int (parameter)
// to indicate beginning of next field
std::vector<int32_t(*)(byte*, int*)> converters;
Channel *chan; // TRON channel
/* note: all vectors need to have the same size
example:
topic = "example_topic"
byte_offset = [4, 2]
converters = *some pointer converting 8 bytes to int32, nullptr
channel = pointer to struct Channel "example channel"
This example maps any message from topic "example_topic" to "example_channel".
8 Bytes starting from index 4 are converted via the given pointer
and mapped to the first variable of the corresponding channel.
The conversion function also adds 8 to the current position,
thus the next field starts 8 bytes + 2 bytes for offset after the last
known position, which was 4.
Because the second converter points to nullptr, it is considered an int32,
which is mapped to the second variable of the channel.
The next position is incremented by 4 if there is no conversion function given.
Note that the first 4 Bytes of the topics message are ignored in this example.
*/
};
std::vector<Mapping> mappings;
int socket_fd; int socket_fd;
// keep track of acknowledgements that are missing // keep track of acknowledgements that are missing
/* note: since communication is asynchronous this value can only be /* note: since communication is asynchronous this value can only be
compared reliably with 0 after testing is terminated */ compared reliably with 0 after testing is terminated */
/* note: TRON only acknowledges if virtual clock is used */
int acks_missing = 0; int acks_missing = 0;
// tron specific functions ----------------------------------------------------- // tron specific functions -----------------------------------------------------
Channel* get_chan(std::string chan_name) {
Channel *chan = nullptr;
for (Channel& cur_chan : channels) if (cur_chan.name == chan_name) chan = &cur_chan;
if (chan == nullptr) throw "channel not declared";
return chan;
}
void get_error_msg(int32_t errorcode) { void get_error_msg(int32_t errorcode) {
ROS_WARN("got error, trying to get corresponding message"); ROS_WARN("got error, trying to get corresponding message");
byte get_err_msg_msg[5]; byte get_err_msg_msg[5];
...@@ -129,10 +199,9 @@ void get_error_msg(int32_t errorcode) { ...@@ -129,10 +199,9 @@ void get_error_msg(int32_t errorcode) {
void add_var_to_channel(std::string channel, bool is_input, std::string var) { void add_var_to_channel(std::string channel, bool is_input, std::string var) {
byte msg[6 + var.length()]; byte msg[6 + var.length()];
if (channels.find(channel) == channels.end()) throw "channel not declared"; Channel *chan = get_chan(channel);
Channel& chan = channels.at(channel);
msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT; msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
add_int32_in_network_order(chan.identifier, msg, 1); add_int32_in_network_order(chan->identifier, msg, 1);
msg[5] = (byte) var.length(); msg[5] = (byte) var.length();
for (int i = 0; i < var.length(); i++) msg[6+i] = var.at(i); for (int i = 0; i < var.length(); i++) msg[6+i] = var.at(i);
ROS_INFO("attaching variable %s to channel %s", var.c_str(), channel.c_str()); ROS_INFO("attaching variable %s to channel %s", var.c_str(), channel.c_str());
...@@ -140,7 +209,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) { ...@@ -140,7 +209,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) {
int32_t ack = get_int_socket(socket_fd); int32_t ack = get_int_socket(socket_fd);
if (ack < 0) get_error_msg(ack); if (ack < 0) get_error_msg(ack);
ROS_INFO("success: attached variable"); ROS_INFO("success: attached variable");
chan.vars.push_back(var); chan->vars.push_back(var);
} }
void send_channel_decl_msg(bool is_input, std::string name) { void send_channel_decl_msg(bool is_input, std::string name) {
...@@ -164,7 +233,7 @@ void send_channel_decl_msg(bool is_input, std::string name) { ...@@ -164,7 +233,7 @@ void send_channel_decl_msg(bool is_input, std::string name) {
if (channel_identifier == 0) throw "did not get channel identifier"; if (channel_identifier == 0) throw "did not get channel identifier";
// assigned channel ID successfully // assigned channel ID successfully
ROS_INFO("success: identifier for channel %s is %i", name.c_str(), channel_identifier); ROS_INFO("success: identifier for channel %s is %i", name.c_str(), channel_identifier);
channels[name] = Channel(channel_identifier, is_input); channels.push_back(Channel(name, channel_identifier, is_input));
} }
void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
...@@ -173,8 +242,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ ...@@ -173,8 +242,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds); byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
// htonl does not exist for long int // htonl does not exist for long int
const bool IS_BIG_ENDIAN = htonl(47) == 47; if (SYS_IS_BIG_ENDIAN) {
if (IS_BIG_ENDIAN) {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[i]; for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[i];
} else { } else {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[7-i]; for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[7-i];
...@@ -208,10 +276,11 @@ void request_start() { ...@@ -208,10 +276,11 @@ void request_start() {
void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count); std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count);
if (channels.find(chan_name) == channels.end()) throw "channel not declared"; Channel *chan = nullptr;
Channel chan = channels.at(chan_name); for (Channel& cur_chan : channels) if (cur_chan.name == chan_name) chan = &cur_chan;
if (chan == nullptr) throw "channel not declared";
add_int32_in_network_order(chan.identifier, msg.get(), 0); add_int32_in_network_order(chan->identifier, msg.get(), 0);
unsigned short var_count_network_order = htons(var_count); unsigned short var_count_network_order = htons(var_count);
byte *var_count_bytes = reinterpret_cast<byte*>(&var_count_network_order); byte *var_count_bytes = reinterpret_cast<byte*>(&var_count_network_order);
...@@ -224,33 +293,35 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){ ...@@ -224,33 +293,35 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
ROS_INFO("sending to output channel %s", chan_name.c_str()); ROS_INFO("sending to output channel %s", chan_name.c_str());
if (var_count == 0) ROS_INFO("no variables attached"); if (var_count == 0) ROS_INFO("no variables attached");
for (unsigned short i = 0; i < var_count; i++) for (unsigned short i = 0; i < var_count; i++)
ROS_INFO("attached value %i to variable %s", vars[i], chan.vars[i].c_str()); ROS_INFO("attached value %i to variable %s", vars[i], chan->vars[i].c_str());
send_bytes(socket_fd, msg.get(), 6 + 4 * var_count); send_bytes(socket_fd, msg.get(), 6 + 4 * var_count);
acks_missing++; acks_missing++;
} }
// returns file descriptor void init_topic_channel_mapping(const std::string topic, const std::string channel){
int create_connected_socket(std::string IP, uint16_t port){ Mapping map;
int socketfd; map.topic = topic;
if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { map.chan = get_chan(channel);
throw "failed to create socket"; mappings.push_back(map);
} }
ROS_INFO("socket created successfully");
struct sockaddr_in addr; void add_var_to_mapping(int mapping_index, int byte_offset,
addr.sin_family = AF_INET; int32_t (*conv)(byte*, int*) = nullptr) {
addr.sin_port = htons(port); Mapping& map = mappings[mapping_index];
{ map.byte_offset.push_back(byte_offset);
int x = inet_pton(AF_INET, IP.c_str(), &addr.sin_addr); map.converters.push_back(conv);
if (x != 1) {
throw "IP could not be converted";
} }
// this function is a little cumbersome to use
// recommended to use init_channel_topic_mapping and add_var_to_mapping instead
void create_channel_topic_mapping_with_vars(const std::string topic,
const std::string channel,
std::vector<int> byte_offset,
std::vector<int32_t (*)(byte*, int*)> conv){
init_topic_channel_mapping(topic, channel);
for (int i = 0; i < byte_offset.size(); i++) {
add_var_to_mapping(mappings.size(), byte_offset[i], conv[i]);
} }
if (connect(socketfd, (struct sockaddr*)&addr, sizeof(sockaddr_in)) < 0) {
throw "failed to connect";
}
ROS_INFO("successfully connected");
return socketfd;
} }
void configuration_phase(){ void configuration_phase(){
...@@ -258,8 +329,11 @@ void configuration_phase(){ ...@@ -258,8 +329,11 @@ void configuration_phase(){
therefore heap allocation can be avoided most of the time in called functions */ therefore heap allocation can be avoided most of the time in called functions */
send_channel_decl_msg(true, "ausloesen"); send_channel_decl_msg(true, "ausloesen");
add_var_to_channel("ausloesen", true, "zahl"); add_var_to_channel("ausloesen", true, "zahl");
send_channel_decl_msg(false, "position"); send_channel_decl_msg(false, "position");
init_topic_channel_mapping("/position", "position");
add_var_to_channel("position", false, "zahl"); add_var_to_channel("position", false, "zahl");
add_var_to_mapping(0, 0);
// not obvious in documentation: local variables are not supported // not obvious in documentation: local variables are not supported
//add_var_to_channel(socketfd, "ausloesen", "lokal"); //add_var_to_channel(socketfd, "ausloesen", "lokal");
uint64_t microseconds = 1000000; // one second uint64_t microseconds = 1000000; // one second
...@@ -267,7 +341,6 @@ void configuration_phase(){ ...@@ -267,7 +341,6 @@ void configuration_phase(){
set_time_unit_and_timeout(microseconds, 100); set_time_unit_and_timeout(microseconds, 100);
} }
// TODO implement callbacks/topic messages
void process_TRONs_msgs(){ void process_TRONs_msgs(){
/* note: TRONs communication after start is not guaranteed to be synchronous, /* note: TRONs communication after start is not guaranteed to be synchronous,
thus incoming messages must be checked for their content */ thus incoming messages must be checked for their content */
...@@ -276,7 +349,7 @@ void process_TRONs_msgs(){ ...@@ -276,7 +349,7 @@ void process_TRONs_msgs(){
byte bytes[4]; byte bytes[4];
if (!read_4_bytes_nonblock(socket_fd, bytes)) if (!read_4_bytes_nonblock(socket_fd, bytes))
break; // no bytes left to process break; // no bytes left to process
int32_t next = bytes_to_int_32(bytes); int32_t next = network_bytes_to_int_32(bytes);
// bytes are acknowledgement // bytes are acknowledgement
if (next == ACK_SINGLE) { if (next == ACK_SINGLE) {
...@@ -287,33 +360,29 @@ void process_TRONs_msgs(){ ...@@ -287,33 +360,29 @@ void process_TRONs_msgs(){
// bytes are channel identifier // bytes are channel identifier
// find corresponding channel // find corresponding channel
std::string chan_name; Channel *chan = nullptr;
for (std::pair<const std::string, Channel>& pair : channels) { for (Channel& cur_chan : channels) if (cur_chan.identifier == next) chan = &cur_chan;
if (pair.second.identifier == next) chan_name = pair.first;
}
if (chan_name.empty())
/* note: this only happens if message number 12 in TRON User Manual is received, /* note: this only happens if message number 12 in TRON User Manual is received,
which should not be the case according to the documentation */ which should not be the case according to the documentation */
if (chan == nullptr)
throw "channel could not be identified"; throw "channel could not be identified";
ROS_INFO("got channel identifier (%s) for input", chan_name.c_str());
ROS_INFO("got channel identifier (%s) for input", chan->name.c_str());
// channel identified, assuming all following bytes are correct // channel identified, assuming all following bytes are correct
// get number of variables // get number of variables
recv(socket_fd, bytes, 2, MSG_DONTWAIT); recv(socket_fd, bytes, 2, MSG_DONTWAIT);
byte_info(bytes, 2, false); byte_info(bytes, 2, false);
uint16_t var_count = bytes_to_uint_16(bytes); uint16_t var_count = network_bytes_to_uint_16(bytes);
ROS_INFO("got variable count %i", var_count); ROS_INFO("got variable count %i", var_count);
// process variables // process variables
for (uint16_t i = 0; i < var_count; i++) { for (uint16_t i = 0; i < var_count; i++) {
recv(socket_fd, bytes, 4, MSG_DONTWAIT); recv(socket_fd, bytes, 4, MSG_DONTWAIT);
next = bytes_to_int_32(bytes); next = network_bytes_to_int_32(bytes);
Channel& c = channels.at(chan_name); std::string var = chan->vars[i];
std::string var = channels.at(chan_name).vars[i]; ROS_INFO("got variable number %i: value of %s is %i", i+1, var.c_str(), next);
ROS_INFO("got variable number %i: value of %s is %i", i+1, channels.at(chan_name).vars[i].c_str(), next);
// TODO transfer message to topic possibly via custom callback
} }
// send acknowledgement // send acknowledgement
...@@ -323,16 +392,40 @@ void process_TRONs_msgs(){ ...@@ -323,16 +392,40 @@ void process_TRONs_msgs(){
} }
} }
void testCallback(const std_msgs::Int32::ConstPtr& msg){ // callback reports to TRON like defined in mappings -----------------------------
int32_t x = msg->data; template<class T>
report_now("position", 1, &x); void mapCallback(const ros::MessageEvent<T>& event){
std::string topic = event.getConnectionHeader().at("topic");
byte *bytes = reinterpret_cast<byte*>(event.getMessage().get());
for (Mapping& mapping : mappings) {
if (mapping.topic.c_str() == topic) {
int var_count = mapping.byte_offset.size();
int32_t vars[var_count * 4];
int last_pos = 0;
for (int i = 0; i < var_count; i++) {
last_pos += mapping.byte_offset[i];
if (mapping.converters[i] == nullptr) {
if (SYS_IS_BIG_ENDIAN)
vars[i] = network_bytes_to_int_32(&bytes[last_pos]);
else vars[i] = *reinterpret_cast<int32_t*>(&bytes[last_pos]);
last_pos += 4;
continue;
} else {
vars[i] = mapping.converters[i](&bytes[last_pos], &last_pos);
}
}
report_now(mapping.chan->name, var_count, vars);
}
} }
}
// place custom callbacks here (using report_now)
// ----------------------------------------------------------------
int main(int argc, char**argv){ int main(int argc, char**argv){
ros::init(argc, argv, "TRON dapter"); ros::init(argc, argv, "TRON dapter");
ros::NodeHandle nh; ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Int32>("test_topic", 1);
if (!pub) throw "publisher nicht erstellt";
try { try {
const std::string IP = "127.0.0.1"; const std::string IP = "127.0.0.1";
...@@ -341,20 +434,15 @@ int main(int argc, char**argv){ ...@@ -341,20 +434,15 @@ int main(int argc, char**argv){
configuration_phase(); configuration_phase();
// subscribe to topics and add callbacks which use report_now------------------------------------------- // subscribe to topics and add callbacks which use report_now
ros::Subscriber sub = nh.subscribe("test_topic", 1, testCallback); ros::Subscriber sub = nh.subscribe("position", 1, mapCallback<std_msgs::Int32>);
// ----------------------------------------------------------- // -----------------------------------------------------------
request_start(); request_start();
// testing phase loop // testing phase loop
ros::Rate test_phase_freq(1); ros::Rate test_phase_freq(1);
int y = 2;
while (ros::ok()) { while (ros::ok()) {
std_msgs::Int32 x;
x.data = 6;
if (y==2) pub.publish(x);
y++; // TRON does not seem to send acknowledgements back
process_TRONs_msgs(); process_TRONs_msgs();
ros::spinOnce(); ros::spinOnce();
test_phase_freq.sleep(); test_phase_freq.sleep();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment