diff --git a/main.cpp b/main.cpp
index 15f96998be161279e3d73dead2f437b937f23d91..6f91855e7aa89c42c0ec731b9821b9ca41b47976 100644
--- a/main.cpp
+++ b/main.cpp
@@ -135,8 +135,10 @@ struct Channel {
std::vector<std::string> vars; // associated variables in Uppaal
Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){};
};
-std::vector<Channel> channels;
+// used in struct Mapping for convenience
+std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
+void add_var_to_channel(Channel& chan, bool is_input, std::string var);
struct Mapping {
std::string topic; // ROS topic name
@@ -154,13 +156,13 @@ struct Mapping {
// a pointer to an integer to increment by the amount of bytes added
std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
- int channel_index; // TRON channel
+ Channel channel = Channel("", 0, true); // 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"
+ channel = 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
@@ -173,14 +175,58 @@ struct Mapping {
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;
+ // Callback used if TRON sends some message
+ void(*input_callback)(Mapping& map, int32_t*) = nullptr;
+
+ Mapping(std::string topic, std::string channel, bool channelIsInput){
+ this->topic = topic;
+ this->channel = *send_channel_decl_msg(channelIsInput, channel).get();
+ }
+
+ void add_var_to_mapping(std::string name_tron, int byte_offset,
+ int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
+ void (*conv_to_topic)(int32_t, byte*, int*) = nullptr){
+ add_var_to_channel(this->channel, this->channel.is_input, name_tron);
+ this->byte_offset.push_back(byte_offset);
+ this->converters_to_TRON.push_back(conv_to_TRON);
+ this->converters_to_topics.push_back(conv_to_topic);
+ }
+};
+std::list<Mapping> mappings;
std::vector<ros::Publisher> input_publishers;
-// custom callbacks for TRONs messages taking channel name, pointer to values and publisher to publish to
-// key is pair of topic name and channel name in this order
-// not using custom fallbacks leads to mapping_callback_to_topics being used
-std::map<std::pair<std::string, std::string>, void(*)(std::string, int32_t*, ros::Publisher&)> input_callbacks;
+// callback publishing to topics like defined in mapping, used for input_callback
+template<class T>
+void mapping_callback_to_topic(Mapping& map, int32_t* vars){
+ boost::shared_ptr<T> shared_ptr = boost::make_shared<T>();
+ byte* ptr = reinterpret_cast<byte*>(shared_ptr.get());
+ int next_pos = 0;
+ for (int i = 0; i < map.channel.vars.size(); i++){
+ int32_t val = vars[i];
+ next_pos += map.byte_offset[i];
+ if (map.converters_to_topics[i] == nullptr) {
+ byte* val_bytes;
+ if (SYS_IS_BIG_ENDIAN){
+ uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val));
+ val = *reinterpret_cast<int32_t*>(&switched_byte_order);
+ }
+ val_bytes = reinterpret_cast<byte*>(&val);
+ ptr[next_pos++] = val_bytes[0];
+ ptr[next_pos++] = val_bytes[1];
+ ptr[next_pos++] = val_bytes[2];
+ ptr[next_pos++] = val_bytes[3];
+ } else {
+ map.converters_to_topics[i](val, &ptr[next_pos], &next_pos);
+ }
+ }
+ for (ros::Publisher& pub : input_publishers)
+ if (pub.getTopic() == map.topic) {
+ pub.publish(shared_ptr);
+ return;
+ }
+ throw "did not find publisher for topic";
+}
+
// subscribers already have callbacks
std::vector<ros::Subscriber> output_subscribers;
@@ -190,17 +236,6 @@ compared reliably with 0 after testing is terminated */
/* note: TRON only acknowledges if virtual clock is used, which it is not (yet) */
int acks_missing = 0;
-// specific functions -----------------------------------------------------
-
-int get_channel_index(std::string chan_name) {
- int i = 0;
- for (Channel& cur_chan : channels) {
- if (cur_chan.name == chan_name) return i;
- i++;
- }
- throw "channel not declared";
-}
-
void get_error_msg(int32_t errorcode) {
ROS_WARN("got error, trying to get corresponding message");
byte get_err_msg_msg[5];
@@ -214,14 +249,13 @@ void get_error_msg(int32_t errorcode) {
throw "got error from TRON";
}
-void add_var_to_channel(std::string channel, bool is_input, std::string var) {
+void add_var_to_channel(Channel& chan, bool is_input, std::string var) {
byte msg[6 + var.length()];
- Channel& chan = channels[get_channel_index(channel)];
msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
add_int32_in_network_order(chan.identifier, msg, 1);
msg[5] = (byte) var.length();
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(), chan.name.c_str());
send_bytes(socket_fd, msg, 6 + var.length());
int32_t ack = get_int_socket(socket_fd);
if (ack < 0) get_error_msg(ack);
@@ -229,7 +263,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) {
chan.vars.push_back(var);
}
-void send_channel_decl_msg(bool is_input, std::string name) {
+std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) {
// prepare packet
size_t msg_length = 2 + name.length();
byte msg[msg_length];
@@ -250,7 +284,7 @@ void send_channel_decl_msg(bool is_input, std::string name) {
if (channel_identifier == 0) throw "did not get channel identifier";
// assigned channel ID successfully
ROS_INFO("success: identifier for channel %s is %i", name.c_str(), channel_identifier);
- channels.push_back(Channel(name, channel_identifier, is_input));
+ return std::make_unique<Channel>(name, channel_identifier, is_input);
}
void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
@@ -291,13 +325,10 @@ void request_start() {
ROS_INFO("success: starting test phase");
}
-void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
+void report_now(Channel& chan, uint16_t var_count, int32_t *vars){
std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count);
- Channel *chan = nullptr;
- 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);
byte *var_count_bytes = reinterpret_cast<byte*>(&var_count_network_order);
@@ -307,41 +338,17 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
for (unsigned short i = 0; i < var_count; i++)
add_int32_in_network_order(vars[i], msg.get(), 6 + i * 4);
- 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");
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);
acks_missing++;
}
-void init_topic_channel_mapping(const std::string topic, const std::string channel){
- Mapping map;
- map.topic = topic;
- map.channel_index = get_channel_index(channel);
- mappings.push_back(map);
-}
-
-void add_var_to_mapping(std::string topic, std::string channel, int byte_offset,
- int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
- void (*conv_to_topic)(int32_t, byte*, int*) = nullptr) {
- int index = -1;
- for (int i = 0; i < mappings.size(); i++) {
- Mapping& map = mappings[i];
- if (channels[map.channel_index].name == channel && map.topic == topic) index = i;
- }
- if (index < 0) throw "did not find mapping";
-
- Mapping& map = mappings[index];
- map.byte_offset.push_back(byte_offset);
- map.converters_to_TRON.push_back(conv_to_TRON);
- map.converters_to_topics.push_back(conv_to_topic);
-}
-
-
// callback reports to TRON like defined in mappings
template<class T>
-void mapping_callback_to_TRON(const ros::MessageEvent<T>& event){
+void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
std::string topic = event.getConnectionHeader().at("topic");
byte *bytes = reinterpret_cast<byte*>(event.getMessage().get());
for (Mapping& mapping : mappings) {
@@ -361,38 +368,7 @@ void mapping_callback_to_TRON(const ros::MessageEvent<T>& event){
vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos);
}
}
- report_now(channels[mapping.channel_index].name, var_count, vars);
- }
- }
-}
-
-// callback publishing to topics like defined in mappings
-template<class T>
-void mapping_callback_to_topics(std::string channel, int32_t* vars, ros::Publisher& pub){
- for (Mapping& mapping : mappings) {
- if (channels[mapping.channel_index].name == channel) {
- boost::shared_ptr<T> shared_ptr = boost::make_shared<T>();
- byte* ptr = reinterpret_cast<byte*>(shared_ptr.get());
- int next_pos = 0;
- for (int i = 0; i < channels[mapping.channel_index].vars.size(); i++){
- int32_t val = vars[i];
- next_pos += mapping.byte_offset[i];
- if (mapping.converters_to_topics[i] == nullptr) {
- byte* val_bytes;
- if (SYS_IS_BIG_ENDIAN){
- uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val));
- val = *reinterpret_cast<int32_t*>(&switched_byte_order);
- }
- val_bytes = reinterpret_cast<byte*>(&val);
- ptr[next_pos++] = val_bytes[0];
- ptr[next_pos++] = val_bytes[1];
- ptr[next_pos++] = val_bytes[2];
- ptr[next_pos++] = val_bytes[3];
- } else {
- mapping.converters_to_topics[i](val, &ptr[next_pos], &next_pos);
- }
- }
- pub.publish(shared_ptr);
+ report_now(mapping.channel, var_count, vars);
}
}
}
@@ -406,21 +382,18 @@ void configuration_phase(ros::NodeHandle& nh){
// add callback for mapping (key is pair of topic and channel name)
// for output: add subscribers to output_subscribers
- send_channel_decl_msg(true, "ausloesen");
- init_topic_channel_mapping("/command", "ausloesen");
- add_var_to_channel("ausloesen", true, "zahl");
- add_var_to_mapping("/command", "ausloesen", 0);
+ Mapping map = Mapping("/command", "ausloesen", true);
+ map.add_var_to_mapping("zahl", 0);
+ map.input_callback = mapping_callback_to_topic<std_msgs::Int32>;
+ mappings.push_back(map);
input_publishers.push_back(nh.advertise<std_msgs::Int32>("/command", 1));
- input_callbacks[std::pair<std::string, std::string>("/command", "ausloesen")]
- = mapping_callback_to_topics<std_msgs::Int32>;
// output channels: declare, init mapping, add vars to TRON channel and mapping
// add subscriber to list
- send_channel_decl_msg(false, "position");
- init_topic_channel_mapping("/position", "position");
- add_var_to_channel("position", false, "zahl");
- add_var_to_mapping("/position", "position", 0);
+ map = Mapping("/position", "position", false);
+ map.add_var_to_mapping("zahl", 0);
+ mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/position", 1,
- mapping_callback_to_TRON<std_msgs::Int32>));
+ mappings_callback_to_TRON<std_msgs::Int32>));
// not obvious in documentation: local variables are not supported
// add_var_to_channel(socketfd, "ausloesen", "lokal");
uint64_t microseconds = 1000000; // one second
@@ -453,7 +426,7 @@ void process_TRONs_msgs(){
// bytes are channel identifier
// find corresponding channel
const Channel *chan = nullptr;
- for (Channel& cur_chan : channels) if (cur_chan.identifier == next) chan = &cur_chan;
+ for (Mapping& map : mappings) if (map.channel.identifier == next) chan = &map.channel;
/* note: this only happens if message number 12 in TRON User Manual is received,
which should not be the case according to the documentation */
if (chan == nullptr)
@@ -480,15 +453,12 @@ void process_TRONs_msgs(){
}
for (Mapping& map : mappings)
- if (channels[map.channel_index].name == chan->name)
+ if (map.channel.name == chan->name)
for (ros::Publisher& pub : input_publishers){
if (pub.getTopic() == map.topic) {
- if (input_callbacks.find(
- std::pair<std::string, std::string>(map.topic, channels[map.channel_index].name))
- != input_callbacks.end())
- input_callbacks.at(std::pair<std::string, std::string>(map.topic, channels[map.channel_index].name))
- (chan->name, vals, pub);
- else throw "did not find callback";
+ if (map.input_callback != nullptr)
+ map.input_callback(map, vals);
+ else throw "no callback declared";
break;
}
}