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
e6811612
Commit
e6811612
authored
3 years ago
by
cs-99
Browse files
Options
Downloads
Patches
Plain Diff
No commit message
No commit message
parent
10de45ee
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
garage.cpp
+28
-8
28 additions, 8 deletions
garage.cpp
main.cpp
+142
-71
142 additions, 71 deletions
main.cpp
with
170 additions
and
79 deletions
garage.cpp
+
28
−
8
View file @
e6811612
...
@@ -2,19 +2,39 @@
...
@@ -2,19 +2,39 @@
#include
<std_msgs/Int32.h>
#include
<std_msgs/Int32.h>
#include
<std_msgs/String.h>
#include
<std_msgs/String.h>
void
callback
(
std_msgs
::
Int32ConstPtr
&
ptr
)
{
int
zahl
=
5
;
int
status
=
0
;
ros
::
Time
zeit
;
void
callback
(
const
std_msgs
::
Int32
::
ConstPtr
&
ptr
)
{
zahl
=
ptr
->
data
;
ROS_INFO
(
"TOPIC received zahl %i"
,
zahl
);
status
++
;
zeit
=
ros
::
Time
::
now
();
}
}
int
main
(
int
argc
,
char
**
argv
){
int
main
(
int
argc
,
char
**
argv
){
ros
::
init
(
argc
,
argv
,
"garage"
);
ros
::
init
(
argc
,
argv
,
"garage"
);
ros
::
NodeHandle
nh
;
ros
::
NodeHandle
nh
;
ros
::
Publisher
pub
=
nh
.
advertise
<
std_msgs
::
Int32
>
(
"position"
,
1
);
ros
::
Publisher
pub
=
nh
.
advertise
<
std_msgs
::
Int32
>
(
"
/
position"
,
1
);
if
(
!
pub
)
throw
"publisher failed"
;
if
(
!
pub
)
throw
"publisher failed"
;
ros
::
Subscriber
sub
=
nh
.
subscribe
(
"/command"
,
1
,
callback
);
// wait till subscribers initialized
while
(
pub
.
getNumSubscribers
()
==
0
)
{
ros
::
spinOnce
();
};
zeit
=
ros
::
Time
::
now
();
ros
::
Rate
rate
(
20
);
while
(
ros
::
ok
()){
ros
::
spinOnce
();
rate
.
sleep
();
if
(
ros
::
Time
::
now
().
toSec
()
-
zeit
.
toSec
()
>
10
)
{
status
=
++
status
%
4
;
if
(
status
==
1
)
{
std_msgs
::
Int32
x
;
std_msgs
::
Int32
x
;
x
.
data
=
6
;
x
.
data
=
++
zahl
;
while
(
pub
.
getNumSubscribers
()
==
0
)
{};
ros
::
Duration
(
3
).
sleep
();
pub
.
publish
(
x
);
pub
.
publish
(
x
);
ros
::
Duration
(
5
).
sleep
();
}
zeit
=
ros
::
Time
::
now
();
}
}
}
}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
main.cpp
+
142
−
71
View file @
e6811612
...
@@ -4,6 +4,11 @@
...
@@ -4,6 +4,11 @@
#include
<netinet/in.h>
#include
<netinet/in.h>
#include
<arpa/inet.h>
#include
<arpa/inet.h>
#include
<std_msgs/Int32.h>
#include
<std_msgs/Int32.h>
#include
<boost/make_shared.hpp>
#include
<memory>
const
std
::
string
IP
=
"127.0.0.1"
;
const
uint16_t
PORT
=
8080
;
typedef
uint8_t
byte
;
typedef
uint8_t
byte
;
...
@@ -122,7 +127,7 @@ const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set
...
@@ -122,7 +127,7 @@ const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set
// ROS uses little endian for its messages
// ROS uses little endian for its messages
const
bool
SYS_IS_BIG_ENDIAN
=
htonl
(
47
)
==
47
;
const
bool
SYS_IS_BIG_ENDIAN
=
htonl
(
47
)
==
47
;
int
socket_fd
;
struct
Channel
{
struct
Channel
{
std
::
string
name
;
std
::
string
name
;
int32_t
identifier
;
int32_t
identifier
;
...
@@ -143,9 +148,13 @@ struct Mapping {
...
@@ -143,9 +148,13 @@ struct Mapping {
// nullptr if not used
// 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
;
std
::
vector
<
int32_t
(
*
)(
byte
*
,
int
*
)
>
converters_to_TRON
;
// similarly the conversion to a topic field takes the value from TRON,
// a pointer to the buffer at which to add the bytes and
// a pointer to an integer to increment by the amount of bytes added
std
::
vector
<
void
(
*
)(
int32_t
,
byte
*
,
int
*
)
>
converters_to_topics
;
C
hannel
*
chan
;
// TRON channel
int
c
hannel
_index
;
// TRON channel
/* note: all vectors need to have the same size
/* note: all vectors need to have the same size
example:
example:
topic = "example_topic"
topic = "example_topic"
...
@@ -167,21 +176,29 @@ struct Mapping {
...
@@ -167,21 +176,29 @@ struct Mapping {
};
};
std
::
vector
<
Mapping
>
mappings
;
std
::
vector
<
Mapping
>
mappings
;
int
socket_fd
;
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
;
// subscribers already have callbacks
std
::
vector
<
ros
::
Subscriber
>
output_subscribers
;
// 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 */
/* note: TRON only acknowledges if virtual clock is used
, which it is not (yet)
*/
int
acks_missing
=
0
;
int
acks_missing
=
0
;
//
tron
specific functions -----------------------------------------------------
// specific functions -----------------------------------------------------
Channel
*
get_chan
(
std
::
string
chan_name
)
{
int
get_channel_index
(
std
::
string
chan_name
)
{
Channel
*
chan
=
nullptr
;
int
i
=
0
;
for
(
Channel
&
cur_chan
:
channels
)
if
(
cur_chan
.
name
==
chan_name
)
chan
=
&
cur_chan
;
for
(
Channel
&
cur_chan
:
channels
)
{
if
(
chan
==
nullptr
)
throw
"channel not declared"
;
if
(
cur_chan
.
name
==
chan_name
)
return
i
;
return
chan
;
i
++
;
}
throw
"channel not declared"
;
}
}
void
get_error_msg
(
int32_t
errorcode
)
{
void
get_error_msg
(
int32_t
errorcode
)
{
...
@@ -199,9 +216,9 @@ void get_error_msg(int32_t errorcode) {
...
@@ -199,9 +216,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
()];
Channel
*
chan
=
get_
chan
(
channel
);
Channel
&
chan
=
chan
nels
[
get_channel_index
(
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
());
...
@@ -209,7 +226,7 @@ void add_var_to_channel(std::string channel, bool is_input, std::string var) {
...
@@ -209,7 +226,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
)
{
...
@@ -301,44 +318,119 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
...
@@ -301,44 +318,119 @@ void report_now(std::string chan_name, uint16_t var_count, int32_t *vars){
void
init_topic_channel_mapping
(
const
std
::
string
topic
,
const
std
::
string
channel
){
void
init_topic_channel_mapping
(
const
std
::
string
topic
,
const
std
::
string
channel
){
Mapping
map
;
Mapping
map
;
map
.
topic
=
topic
;
map
.
topic
=
topic
;
map
.
chan
=
get_chan
(
channel
);
map
.
chan
nel_index
=
get_chan
nel_index
(
channel
);
mappings
.
push_back
(
map
);
mappings
.
push_back
(
map
);
}
}
void
add_var_to_mapping
(
int
mapping_index
,
int
byte_offset
,
void
add_var_to_mapping
(
std
::
string
topic
,
std
::
string
channel
,
int
byte_offset
,
int32_t
(
*
conv
)(
byte
*
,
int
*
)
=
nullptr
)
{
int32_t
(
*
conv_to_TRON
)(
byte
*
,
int
*
)
=
nullptr
,
Mapping
&
map
=
mappings
[
mapping_index
];
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
.
byte_offset
.
push_back
(
byte_offset
);
map
.
converters
.
push_back
(
conv
);
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
){
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
next_pos
=
0
;
for
(
int
i
=
0
;
i
<
var_count
;
i
++
)
{
next_pos
+=
mapping
.
byte_offset
[
i
];
if
(
mapping
.
converters_to_TRON
[
i
]
==
nullptr
)
{
if
(
SYS_IS_BIG_ENDIAN
)
vars
[
i
]
=
network_bytes_to_int_32
(
&
bytes
[
next_pos
]);
else
vars
[
i
]
=
*
reinterpret_cast
<
int32_t
*>
(
&
bytes
[
next_pos
]);
next_pos
+=
4
;
continue
;
}
else
{
vars
[
i
]
=
mapping
.
converters_to_TRON
[
i
](
&
bytes
[
next_pos
],
&
next_pos
);
}
}
report_now
(
channels
[
mapping
.
channel_index
].
name
,
var_count
,
vars
);
}
}
}
}
// this function is a little cumbersome to use
// callback publishing to topics like defined in mappings
// recommended to use init_channel_topic_mapping and add_var_to_mapping instead
template
<
class
T
>
void
create_channel_topic_mapping_with_vars
(
const
std
::
string
topic
,
void
mapping_callback_to_topics
(
std
::
string
channel
,
int32_t
*
vars
,
ros
::
Publisher
&
pub
){
const
std
::
string
channel
,
for
(
Mapping
&
mapping
:
mappings
)
{
std
::
vector
<
int
>
byte_offset
,
if
(
channels
[
mapping
.
channel_index
].
name
==
channel
)
{
std
::
vector
<
int32_t
(
*
)(
byte
*
,
int
*
)
>
conv
){
boost
::
shared_ptr
<
T
>
shared_ptr
=
boost
::
make_shared
<
T
>
();
init_topic_channel_mapping
(
topic
,
channel
);
byte
*
ptr
=
reinterpret_cast
<
byte
*>
(
shared_ptr
.
get
());
for
(
int
i
=
0
;
i
<
byte_offset
.
size
();
i
++
)
{
int
next_pos
=
0
;
add_var_to_mapping
(
mappings
.
size
(),
byte_offset
[
i
],
conv
[
i
]);
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
);
}
}
}
}
}
void
configuration_phase
(){
void
configuration_phase
(
ros
::
NodeHandle
&
nh
){
/* note: for configuration phase maximum message length is 256 Bytes,
/* note: for configuration phase maximum message length is 256 Bytes,
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 */
// channels: declare, init mapping, add vars to TRON channel, add vars to mapping
// for input: add publisher to input_publishers
// 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"
);
send_channel_decl_msg
(
true
,
"ausloesen"
);
init_topic_channel_mapping
(
"/command"
,
"ausloesen"
);
add_var_to_channel
(
"ausloesen"
,
true
,
"zahl"
);
add_var_to_channel
(
"ausloesen"
,
true
,
"zahl"
);
add_var_to_mapping
(
"/command"
,
"ausloesen"
,
0
);
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"
);
send_channel_decl_msg
(
false
,
"position"
);
init_topic_channel_mapping
(
"/position"
,
"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
);
add_var_to_mapping
(
"/position"
,
"position"
,
0
);
output_subscribers
.
push_back
(
nh
.
subscribe
(
"/position"
,
1
,
mapping_callback_to_TRON
<
std_msgs
::
Int32
>
));
// 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
// documentation states 2 signed integers are used for some reason
// documentation states 2 signed integers are used for some reason
set_time_unit_and_timeout
(
microseconds
,
100
);
set_time_unit_and_timeout
(
microseconds
,
100
);
// wait till subscribers initialized
for
(
ros
::
Publisher
&
pub
:
input_publishers
)
{
while
(
pub
.
getNumSubscribers
()
==
0
)
{
ros
::
Duration
(
1
).
sleep
();
};
}
}
}
void
process_TRONs_msgs
(){
void
process_TRONs_msgs
(){
...
@@ -360,7 +452,7 @@ void process_TRONs_msgs(){
...
@@ -360,7 +452,7 @@ void process_TRONs_msgs(){
// bytes are channel identifier
// bytes are channel identifier
// find corresponding channel
// find corresponding channel
Channel
*
chan
=
nullptr
;
const
Channel
*
chan
=
nullptr
;
for
(
Channel
&
cur_chan
:
channels
)
if
(
cur_chan
.
identifier
==
next
)
chan
=
&
cur_chan
;
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,
/* 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 */
...
@@ -377,12 +469,28 @@ void process_TRONs_msgs(){
...
@@ -377,12 +469,28 @@ void process_TRONs_msgs(){
uint16_t
var_count
=
network_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
);
int32_t
vals
[
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
=
network_bytes_to_int_32
(
bytes
);
next
=
network_bytes_to_int_32
(
bytes
);
std
::
string
var
=
chan
->
vars
[
i
];
std
::
string
var
=
chan
->
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
,
var
.
c_str
(),
next
);
vals
[
i
]
=
next
;
}
for
(
Mapping
&
map
:
mappings
)
if
(
channels
[
map
.
channel_index
].
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"
;
break
;
}
}
}
// send acknowledgement
// send acknowledgement
...
@@ -392,51 +500,14 @@ void process_TRONs_msgs(){
...
@@ -392,51 +500,14 @@ void process_TRONs_msgs(){
}
}
}
}
// 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
){
int
main
(
int
argc
,
char
**
argv
){
ros
::
init
(
argc
,
argv
,
"TRON dapter"
);
ros
::
init
(
argc
,
argv
,
"TRON dapter"
);
ros
::
NodeHandle
nh
;
ros
::
NodeHandle
nh
;
try
{
try
{
const
std
::
string
IP
=
"127.0.0.1"
;
const
uint16_t
PORT
=
8080
;
socket_fd
=
create_connected_socket
(
IP
,
PORT
);
socket_fd
=
create_connected_socket
(
IP
,
PORT
);
configuration_phase
();
configuration_phase
(
nh
);
// subscribe to topics and add callbacks which use report_now
ros
::
Subscriber
sub
=
nh
.
subscribe
(
"position"
,
1
,
mapCallback
<
std_msgs
::
Int32
>
);
// -----------------------------------------------------------
request_start
();
request_start
();
...
...
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