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
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Christoph Schröter
bachelor thesis
Commits
10de45ee
Commit
10de45ee
authored
4 years ago
by
cs-99
Browse files
Options
Downloads
Patches
Plain Diff
No commit message
No commit message
parent
1fb4ac0f
No related branches found
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
garage.cpp
+20
-0
20 additions, 0 deletions
garage.cpp
main.cpp
+153
-65
153 additions, 65 deletions
main.cpp
with
173 additions
and
65 deletions
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
This diff is collapsed.
Click to expand it.
main.cpp
+
153
−
65
View file @
10de45ee
...
@@ -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
,
test
Callback
);
ros
::
Subscriber
sub
=
nh
.
subscribe
(
"
position
"
,
1
,
map
Callback
<
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
();
...
...
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