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

No commit message 

No commit message
parent e456033e
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 78 additions and 108 deletions
main.cpp
+ 78
− 108
View file @ d0b5685f
......@@ -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;
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment