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
Show whitespace changes
Inline Side-by-side
garage.cpp 0 → 100644
+ 20
− 0
View file @ 10de45ee
#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
main.cpp
+ 153
− 65
View file @ 10de45ee
......@@ -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
};
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));
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));
}
// wrapping get_bytes_socket for converting to 32 bit integer
int32_t get_int_socket(int fd) {
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
......@@ -81,6 +81,30 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf) {
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 ----------------------------------------------------------
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
// global vars -------------------------------------------------------------
// ROS uses little endian for its messages
const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
struct Channel {
std::string name;
int32_t identifier;
bool is_input;
std::vector<std::string> vars; // associated variables in Uppaal
Channel(int32_t id, bool is_input) : identifier(id), is_input(is_input){};
Channel() = default; // default constructor needed for std::map
Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){};
};
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;
// keep track of acknowledgements that are missing
/* note: since communication is asynchronous this value can only be
compared reliably with 0 after testing is terminated */
/* note: TRON only acknowledges if virtual clock is used */
int acks_missing = 0;
// 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) {
ROS_WARN("got error, trying to get corresponding message");
byte get_err_msg_msg[5];
......@@ -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) {
byte msg[6 + var.length()];
if (channels.find(channel) == channels.end()) throw "channel not declared";
Channel& chan = channels.at(channel);
Channel *chan = get_chan(channel);
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();
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());
......@@ -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);
if (ack < 0) get_error_msg(ack);
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) {
......@@ -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";
// assigned channel ID successfully
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){
......@@ -173,8 +242,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
// htonl does not exist for long int
const bool IS_BIG_ENDIAN = htonl(47) == 47;
if (IS_BIG_ENDIAN) {
if (SYS_IS_BIG_ENDIAN) {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[i];
} else {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[7-i];
......@@ -208,10 +276,11 @@ void request_start() {
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);
if (channels.find(chan_name) == channels.end()) throw "channel not declared";
Channel chan = channels.at(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";
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);
......@@ -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());
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++;
}
// 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";
void init_topic_channel_mapping(const std::string topic, const std::string channel){
Mapping map;
map.topic = topic;
map.chan = get_chan(channel);
mappings.push_back(map);
}
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";
void add_var_to_mapping(int mapping_index, int byte_offset,
int32_t (*conv)(byte*, int*) = nullptr) {
Mapping& map = mappings[mapping_index];
map.byte_offset.push_back(byte_offset);
map.converters.push_back(conv);
}
// 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(){
......@@ -258,8 +329,11 @@ void configuration_phase(){
therefore heap allocation can be avoided most of the time in called functions */
send_channel_decl_msg(true, "ausloesen");
add_var_to_channel("ausloesen", true, "zahl");
send_channel_decl_msg(false, "position");
init_topic_channel_mapping("/position", "position");
add_var_to_channel("position", false, "zahl");
add_var_to_mapping(0, 0);
// not obvious in documentation: local variables are not supported
//add_var_to_channel(socketfd, "ausloesen", "lokal");
uint64_t microseconds = 1000000; // one second
......@@ -267,7 +341,6 @@ void configuration_phase(){
set_time_unit_and_timeout(microseconds, 100);
}
// TODO implement callbacks/topic messages
void process_TRONs_msgs(){
/* note: TRONs communication after start is not guaranteed to be synchronous,
thus incoming messages must be checked for their content */
......@@ -276,7 +349,7 @@ void process_TRONs_msgs(){
byte bytes[4];
if (!read_4_bytes_nonblock(socket_fd, bytes))
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
if (next == ACK_SINGLE) {
......@@ -287,33 +360,29 @@ void process_TRONs_msgs(){
// bytes are channel identifier
// find corresponding channel
std::string chan_name;
for (std::pair<const std::string, Channel>& pair : channels) {
if (pair.second.identifier == next) chan_name = pair.first;
}
if (chan_name.empty())
Channel *chan = nullptr;
for (Channel& cur_chan : channels) if (cur_chan.identifier == next) chan = &cur_chan;
/* 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)
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
// get number of variables
recv(socket_fd, bytes, 2, MSG_DONTWAIT);
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);
// process variables
for (uint16_t i = 0; i < var_count; i++) {
recv(socket_fd, bytes, 4, MSG_DONTWAIT);
next = bytes_to_int_32(bytes);
Channel& c = channels.at(chan_name);
std::string var = channels.at(chan_name).vars[i];
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
next = network_bytes_to_int_32(bytes);
std::string var = chan->vars[i];
ROS_INFO("got variable number %i: value of %s is %i", i+1, var.c_str(), next);
}
// send acknowledgement
......@@ -323,16 +392,40 @@ void process_TRONs_msgs(){
}
}
void testCallback(const std_msgs::Int32::ConstPtr& msg){
int32_t x = msg->data;
report_now("position", 1, &x);
// callback reports to TRON like defined in mappings -----------------------------
template<class T>
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){
ros::init(argc, argv, "TRON dapter");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Int32>("test_topic", 1);
if (!pub) throw "publisher nicht erstellt";
try {
const std::string IP = "127.0.0.1";
......@@ -341,20 +434,15 @@ int main(int argc, char**argv){
configuration_phase();
// subscribe to topics and add callbacks which use report_now-------------------------------------------
ros::Subscriber sub = nh.subscribe("test_topic", 1, testCallback);
// subscribe to topics and add callbacks which use report_now
ros::Subscriber sub = nh.subscribe("position", 1, mapCallback<std_msgs::Int32>);
// -----------------------------------------------------------
request_start();
// testing phase loop
ros::Rate test_phase_freq(1);
int y = 2;
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();
ros::spinOnce();
test_phase_freq.sleep();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment