Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
C
cobot1_TRON_testing
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD 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
cobot1_TRON_testing
Commits
6a0b0c2a
Commit
6a0b0c2a
authored
3 years ago
by
Christoph Schröter
Browse files
Options
Downloads
Patches
Plain Diff
No commit message
No commit message
parent
06150721
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/tron_adapter.h
+137
-29
137 additions, 29 deletions
include/tron_adapter.h
src/tron_adapter.cpp
+1
-1
1 addition, 1 deletion
src/tron_adapter.cpp
with
138 additions
and
30 deletions
include/tron_adapter.h
+
137
−
29
View file @
6a0b0c2a
...
@@ -8,36 +8,80 @@ typedef uint8_t byte;
...
@@ -8,36 +8,80 @@ typedef uint8_t byte;
// some networking helper functions ---------------------------------------------------
// some networking helper functions ---------------------------------------------------
// logs bytes via ROS_INFO when sending or receiving them
/**
* logs bytes
* @param buf start of byte array
* @param buf_length how many bytes to log
* @param send true if message gets send, false if received
*/
inline
void
byte_info
(
const
byte
*
buf
,
int
buf_length
,
bool
send
=
true
);
inline
void
byte_info
(
const
byte
*
buf
,
int
buf_length
,
bool
send
=
true
);
// reads count bytes from filedescripter fd
/**
* reads bytes from file descriptor
* @param fd file descriptor
* @param count how many bytes to read
* @return byte array read
*/
std
::
unique_ptr
<
byte
[]
>
get_bytes_socket
(
int
fd
,
int
count
);
std
::
unique_ptr
<
byte
[]
>
get_bytes_socket
(
int
fd
,
int
count
);
// converts bytes to host order and casts to int32
/**
* converts bytes to host order and casts to int32
* @param buf start of 4 bytes to convert
*/
inline
int32_t
network_bytes_to_int_32
(
byte
*
buf
);
inline
int32_t
network_bytes_to_int_32
(
byte
*
buf
);
// converts bytes to host order and casts to uint16
/**
* converts bytes to host order and casts to uint16
* @param buf start of 2 bytes to convert
*/
inline
uint16_t
network_bytes_to_uint_16
(
byte
*
buf
);
inline
uint16_t
network_bytes_to_uint_16
(
byte
*
buf
);
// reads 4 bytes from socket and returns them as an int (converted to host order before)
/**
* reads 4 bytes from socket and returns them as an int (converted to host order before)
* @param fd file descriptor
*/
int32_t
get_int_socket
(
int
fd
);
int32_t
get_int_socket
(
int
fd
);
// converts num to network order and sets the bytes with byte-offset index from buf
/**
* converts int32 to network order and sets it converted to network order into byte array
* @param num number to convert
* @param buf start adress of array to add to
* @param index wanted starting index of bytes in array
*/
void
add_int32_in_network_order
(
int32_t
num
,
byte
*
buf
,
int
index
);
void
add_int32_in_network_order
(
int32_t
num
,
byte
*
buf
,
int
index
);
// wraps system byte sending for automated logging
/**
* wraps systems byte-sending for automated logging
* @param fd file descriptor
* @param buf start adress of byte array to send
* @param length how many bytes to send
*/
inline
void
send_bytes
(
int
fd
,
const
byte
*
buf
,
int
length
);
inline
void
send_bytes
(
int
fd
,
const
byte
*
buf
,
int
length
);
// reads 4 bytes into buf from filedescriptor
/**
* reads 4 bytes instantly, throws if not successfull
* @param fd file descriptor
* @param buf start adress to read into
*/
inline
bool
read_4_bytes_nonblock
(
int
fd
,
byte
*
buf
);
inline
bool
read_4_bytes_nonblock
(
int
fd
,
byte
*
buf
);
/**
* constructs socket connected to given parameters
* @param IP IPv4 adress to connect to
* @param port port to connect to
*/
int
create_connected_socket
(
std
::
string
IP
,
uint16_t
port
);
int
create_connected_socket
(
std
::
string
IP
,
uint16_t
port
);
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// represents a channel in Uppaal
struct
Channel
{
struct
Channel
{
/**
* represents a channel
* @param name name of channel in Uppaal
* @param identifier identifier for TRON
* @param is_input channel used as input or output
* @param vars names of variables appended to this channel
*/
std
::
string
name
;
std
::
string
name
;
int32_t
identifier
;
int32_t
identifier
;
bool
is_input
;
bool
is_input
;
...
@@ -45,8 +89,16 @@ struct Channel {
...
@@ -45,8 +89,16 @@ struct Channel {
Channel
(
std
::
string
name
,
int32_t
id
,
bool
is_input
)
:
name
(
name
),
identifier
(
id
),
is_input
(
is_input
){};
Channel
(
std
::
string
name
,
int32_t
id
,
bool
is_input
)
:
name
(
name
),
identifier
(
id
),
is_input
(
is_input
){};
};
};
// represents a Mapping of one topic and one channel
struct
Mapping
{
struct
Mapping
{
/**
* represents a Mapping of one topic and one channel
* @param topic name of topic in ROS
* @param channel associated channel
* @param input_callback callback used if channel is input
* @param byte_offset offsets of bytes to map (from end of last field)
* @param converters_to_TRON converters if variable is not int32, nullptr if not used
* @param converters_to_topics converters if variable is not int32, nullptr if not used
*/
std
::
string
topic
;
// ROS topic name
std
::
string
topic
;
// ROS topic name
Channel
channel
=
Channel
(
""
,
0
,
true
);
// TRON channel
Channel
channel
=
Channel
(
""
,
0
,
true
);
// TRON channel
...
@@ -62,7 +114,6 @@ struct Mapping {
...
@@ -62,7 +114,6 @@ struct Mapping {
std
::
vector
<
int
>
byte_offset
;
std
::
vector
<
int
>
byte_offset
;
// custom conversion function used if data is not int32 and therefore
// custom conversion function used if data is not int32 and therefore
// need to be converted to an int32 first
// 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)
// needs to make sure count of bytes used gets added to *int (parameter)
// to indicate beginning of next field
// to indicate beginning of next field
std
::
vector
<
int32_t
(
*
)(
byte
*
,
int
*
)
>
converters_to_TRON
;
std
::
vector
<
int32_t
(
*
)(
byte
*
,
int
*
)
>
converters_to_TRON
;
...
@@ -114,44 +165,92 @@ struct TRON_Adapter {
...
@@ -114,44 +165,92 @@ struct TRON_Adapter {
/* note: TRON only acknowledges if virtual clock is used, which it is not */
/* note: TRON only acknowledges if virtual clock is used, which it is not */
int
acks_missing
=
0
;
int
acks_missing
=
0
;
// creates connected socket and sets socket_fd
/**
* creates connected instance of TRON_Adapter
* @param IP IPv4 address to connect to
* @param PORT port to connect to
*/
TRON_Adapter
(
std
::
string
IP
,
uint16_t
PORT
);
TRON_Adapter
(
std
::
string
IP
,
uint16_t
PORT
);
TRON_Adapter
()
=
default
;
// do not use; needed to be able to globally declare adapter instance
/**
* used only to be able to declare adapters as global variables without initialising
// declares used channel to TRON and returns Mapping to add variables
*/
// when done with adding variables, add the Mapping instance to mappings list
TRON_Adapter
()
=
default
;
/**
* declares used channel to TRON and returns Mapping to add variables;
* when done with adding variables, add the Mapping instance to mappings list
* @param topic name of the topic
* @param channel name of channel in Uppaal model
* @param channelIsInput is channel used as input or output
*/
Mapping
createMapping
(
std
::
string
topic
,
std
::
string
channel
,
bool
channelIsInput
);
Mapping
createMapping
(
std
::
string
topic
,
std
::
string
channel
,
bool
channelIsInput
);
// declares used variable to TRON and adds given values to corresponding lists
/**
* declares variable to mapping calling add_var_to_channel and adds given values to corresponding lists
* @param map mapping to append variable to
* @param name_tron name of variable as in Uppaal model
* @param byte_offset offset from ending of last field
* @param conv_to_TRON converter from byte* to int32_t in order to send to TRON, needs to increase given int* by count of bytes used
* @param conv_to_topic conv_to_TRON reversed for messaging topic
*/
void
add_var_to_mapping
(
Mapping
&
map
,
std
::
string
name_tron
,
int
byte_offset
=
0
,
void
add_var_to_mapping
(
Mapping
&
map
,
std
::
string
name_tron
,
int
byte_offset
=
0
,
int32_t
(
*
conv_to_TRON
)(
byte
*
,
int
*
)
=
nullptr
,
int32_t
(
*
conv_to_TRON
)(
byte
*
,
int
*
)
=
nullptr
,
void
(
*
conv_to_topic
)(
int32_t
,
byte
*
,
int
*
)
=
nullptr
);
void
(
*
conv_to_topic
)(
int32_t
,
byte
*
,
int
*
)
=
nullptr
);
// sends time unit and timeout to TRON
/**
* sends time variables to TRON
* @param microseconds how many microseconds represent one time unit
* @param timeout how many time units pass until timeout
*/
void
set_time_unit_and_timeout
(
uint64_t
microseconds
,
int32_t
timeout
);
void
set_time_unit_and_timeout
(
uint64_t
microseconds
,
int32_t
timeout
);
// sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout)
/**
* sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout)
*/
void
request_start
();
void
request_start
();
// reports to channel
/**
* report to TRON
* @param chan channel to activate
* @param var_count how many variables to attach
* @param vars pointer to int32 array with variables to attach
*/
void
report_now
(
Channel
&
chan
,
uint16_t
var_count
=
0
,
int32_t
*
vars
=
nullptr
);
void
report_now
(
Channel
&
chan
,
uint16_t
var_count
=
0
,
int32_t
*
vars
=
nullptr
);
void
report_now
(
std
::
string
chan
,
uint16_t
var_count
=
0
,
int32_t
*
vars
=
nullptr
);
void
report_now
(
std
::
string
chan
,
uint16_t
var_count
=
0
,
int32_t
*
vars
=
nullptr
);
// starts callback for every message received from TRON
/**
* starts callback for every message received from TRON
*/
void
process_TRONs_msgs
();
void
process_TRONs_msgs
();
// gets and logs error message from TRON, then throws since proceeding would lead to more errors
/**
* gets and logs error message from TRON, then throws since proceeding would lead to more errors
* @param errorcode code received from TRON
*/
void
get_error_msg
(
int32_t
errorcode
);
void
get_error_msg
(
int32_t
errorcode
);
// declares channel to TRON and constructs Channel instance
/**
* declares channel to TRON and constructs corresponding instance
* @param is_input is channel used as input or output
* @param name name of channel in Uppaal model
*/
std
::
unique_ptr
<
Channel
>
send_channel_decl_msg
(
bool
is_input
,
std
::
string
name
);
std
::
unique_ptr
<
Channel
>
send_channel_decl_msg
(
bool
is_input
,
std
::
string
name
);
// declares var to channel
/**
* declares variable being attached to channel for TRON
* @param chan channel to associated with variable
* @param is_input is channel used as input or output
* @param var name of int variable in Uppaal model
*/
void
add_var_to_channel
(
Channel
&
chan
,
bool
is_input
,
std
::
string
var
);
void
add_var_to_channel
(
Channel
&
chan
,
bool
is_input
,
std
::
string
var
);
// publishes to fitting publisher from list
/**
* searches publisher list for given topic and publishes to it
* @param topic name of the topic to publish to
* @param ptr pointer to message
*/
template
<
class
T
>
template
<
class
T
>
void
publish_to_topic
(
std
::
string
topic
,
boost
::
shared_ptr
<
T
>
ptr
)
{
void
publish_to_topic
(
std
::
string
topic
,
boost
::
shared_ptr
<
T
>
ptr
)
{
for
(
ros
::
Publisher
&
pub
:
input_publishers
)
for
(
ros
::
Publisher
&
pub
:
input_publishers
)
...
@@ -162,8 +261,12 @@ struct TRON_Adapter {
...
@@ -162,8 +261,12 @@ struct TRON_Adapter {
throw
"did not find publisher for topic"
;
throw
"did not find publisher for topic"
;
}
}
// template callbacks for using specified byte positions
/**
// callback publishing to topics like defined in mapping, used for input_callback
* callback reports to topic like defined in given byte-mapping
* note: needs to be specified for each mapping
* @param map mapping associated with this callback
* @param vars variables received from TRON
*/
template
<
class
T
>
template
<
class
T
>
void
mapping_callback_to_topic
(
Mapping
&
map
,
int32_t
*
vars
){
void
mapping_callback_to_topic
(
Mapping
&
map
,
int32_t
*
vars
){
boost
::
shared_ptr
<
T
>
shared_ptr
=
boost
::
make_shared
<
T
>
();
boost
::
shared_ptr
<
T
>
shared_ptr
=
boost
::
make_shared
<
T
>
();
...
@@ -190,7 +293,12 @@ struct TRON_Adapter {
...
@@ -190,7 +293,12 @@ struct TRON_Adapter {
publish_to_topic
<
T
>
(
map
.
topic
,
shared_ptr
);
publish_to_topic
<
T
>
(
map
.
topic
,
shared_ptr
);
}
}
// callback reports to TRON like defined in mappings
/**
* callback reports to TRON like defined in byte-mappings
* note: this callback reports to all channels associated with calling topic
* @param event message event received from topic
*/
template
<
class
T
>
template
<
class
T
>
void
mappings_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"
);
std
::
string
topic
=
event
.
getConnectionHeader
().
at
(
"topic"
);
...
...
This diff is collapsed.
Click to expand it.
src/tron_adapter.cpp
+
1
−
1
View file @
6a0b0c2a
...
@@ -13,7 +13,7 @@ typedef uint8_t byte;
...
@@ -13,7 +13,7 @@ typedef uint8_t byte;
// some helper functions -----------------------------------------------
// some helper functions -----------------------------------------------
// log bytes
sent/received
// log
s
bytes
inline
void
byte_info
(
const
byte
*
buf
,
int
buf_length
,
bool
send
){
inline
void
byte_info
(
const
byte
*
buf
,
int
buf_length
,
bool
send
){
std
::
stringstream
strstr
;
std
::
stringstream
strstr
;
strstr
<<
(
send
?
"sending"
:
"received"
)
<<
" bytes:"
;
strstr
<<
(
send
?
"sending"
:
"received"
)
<<
" bytes:"
;
...
...
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