Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
B
bachelor thesis
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Christoph Schröter
bachelor thesis
Commits
d0b5685f
Commit
d0b5685f
authored
3 years ago
by
cs-99
Browse files
Options
Downloads
Patches
Plain Diff
No commit message
No commit message
parent
e456033e
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
main.cpp
+78
-108
78 additions, 108 deletions
main.cpp
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
(),
chan
nel
.
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
mapping
s
_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
>
));
mapping
s
_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
:
channel
s
)
if
(
cur_
chan
.
identifier
==
next
)
chan
=
&
cur_
chan
;
for
(
Mapping
&
map
:
mapping
s
)
if
(
map
.
chan
nel
.
identifier
==
next
)
chan
=
&
map
.
chan
nel
;
/* 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
;
}
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment