Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
3cd1822f
Commit
3cd1822f
authored
Jul 03, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
working on ros_bridge, code not compilable.
parent
51badb8c
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
267 additions
and
220 deletions
+267
-220
WimaController.cc
src/Wima/WimaController.cc
+21
-0
jsongenerator.h
src/comm/ros_bridge/include/jsongenerator.h
+124
-2
messages.h
src/comm/ros_bridge/include/messages.h
+122
-75
messages.cpp
src/comm/ros_bridge/src/messages.cpp
+0
-143
.snake.cpp.kate-swp
test_rosbridge/rosbridge/.snake.cpp.kate-swp
+0
-0
No files found.
src/Wima/WimaController.cc
View file @
3cd1822f
#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/jsongenerator.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "time.h"
#include "assert.h"
...
...
@@ -957,6 +961,7 @@ void WimaController::_eventTimerHandler()
{
static
EventTicker
batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
CHECK_BATTERY_INTERVAL
);
static
EventTicker
snakeEventLoopTicker
(
EVENT_TIMER_INTERVAL
,
SNAKE_EVENT_LOOP_INTERVAL
);
static
EventTicker
ros_bridgeTicker
(
EVENT_TIMER_INTERVAL
,
1000
);
// Battery level check necessary?
if
(
batteryLevelTicker
.
ready
()
)
...
...
@@ -987,6 +992,22 @@ void WimaController::_eventTimerHandler()
emit
snakeProgressChanged
();
}
}
if
(
ros_bridgeTicker
.
ready
())
{
using
namespace
ros_bridge
;
class
Time
time
(
1
,
2
);
JsonGenerator
<>
gen
;
gen
.
set
(
time
);
rapidjson
::
Document
doc
(
rapidjson
::
kObjectType
);
// Write to stdout
cout
<<
"Return value : "
<<
gen
.
run
(
MessageType
::
Time
,
doc
,
doc
.
GetAllocator
())
<<
std
::
endl
;
rapidjson
::
OStreamWrapper
out
(
std
::
cout
);
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer
(
out
);
doc
.
Accept
(
writer
);
std
::
cout
<<
std
::
endl
;
}
}
void
WimaController
::
_smartRTLCleanUp
(
bool
flying
)
...
...
src/comm/ros_bridge/include/jsongenerator.h
View file @
3cd1822f
...
...
@@ -2,15 +2,137 @@
#define JSONGENERATOR_H
#include "rapidjson/include/rapidjson/document.h"
#include "include/messages.h"
#include "assert.h"
namespace
ros_bridge
{
enum
MessageType
{
TimeMessage
,
HeaderMessage
,
Point32Message
,
PolygonMessage
,
PolygonStampedMessage
,
PolygonArrayMessage
};
template
<
class
JsonDocumentType
=
rapidjson
::
Document
,
class
TimeType
,
>
class
AllocatorType
=
rapidjson
::
Document
::
AllocatorType
,
class
TimeType
=
class
Time
,
class
HeaderType
=
class
Header
,
class
PointType
=
class
Point32
,
class
PolygonType
=
class
Polygon
<>
,
class
PolygonStampedType
=
class
PolygonStamped
<>
,
class
PolygonArrayType
=
class
PolygonArray
<>
>
class
JsonGenerator
{
public:
JsonGenerator
()
:
_time
(
nullptr
)
{}
// Add set() function if new MessageType required!
void
add
(
const
TimeType
&
time
)
{
_time
=
&
time
;}
void
add
(
const
HeaderType
&
header
)
{
_header
=
&
header
;}
void
add
(
const
PointType
&
point
)
{
_point
=
&
point
;}
void
add
(
const
PolygonType
&
polygon
)
{
_polygon
=
&
polygon
;}
// Modify run() function if new MessageType required!
bool
run
(
MessageType
type
,
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
switch
(
type
)
{
case
TimeMessage
:
if
(
_time
!=
nullptr
)
return
_timeToJson
(
doc
,
allocator
);
assert
(
false
);
// _time not set!
break
;
case
HeaderMessage
:
if
(
_header
!=
nullptr
)
return
_headerToJson
(
doc
,
allocator
);
assert
(
false
);
// _header not set!
break
;
case
Point32Message
:
if
(
_point
!=
nullptr
)
return
_point32ToJson
(
doc
,
allocator
);
assert
(
false
);
// _point not set!
break
;
case
PolygonMessage
:
if
(
_polygon
!=
nullptr
)
return
_polygonToJson
(
doc
,
allocator
);
assert
(
false
);
// _polygon not set!
break
;
case
PolygonStampedMessage
:
if
(
_polygonStamped
!=
nullptr
)
return
_polygonStampedToJson
(
doc
,
allocator
);
assert
(
false
);
// _polygonStamped not set!
break
;
case
PolygonArrayMessage
:
if
(
_polygonArray
!=
nullptr
)
return
_polygonArrayToJson
(
doc
,
allocator
);
assert
(
false
);
// _polygonArray not set!
break
;
default:
assert
(
false
);
// Unknown MessageType!
}
return
false
;
}
// Modify clear() function if new MessageType required!
void
clear
()
{
_time
=
nullptr
;
_header
=
nullptr
;
_point
=
nullptr
;
_polygon
=
nullptr
;
}
private:
bool
_timeToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
(
_time
->
getSecs
()),
allocator
);
doc
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
(
_time
->
getNSecs
()),
allocator
);
return
true
;
}
// TODO
bool
_HeaderToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
return
false
;
}
bool
_Point32ToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
return
false
;
}
bool
_PolygonToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
return
false
;
}
bool
_PolygonStampedToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
return
false
;
}
bool
_PolygonArrayToJson
(
JsonDocumentType
&
doc
,
AllocatorType
&
allocator
)
{
return
false
;
}
// Add new MessageType here.
const
TimeType
*
_time
;
const
HeaderType
*
_header
;
const
PointType
*
_point
;
const
PolygonType
*
_polygon
;
const
PolygonStampedType
*
_polygonStamped
;
const
PolygonArrayType
*
_polygonArray
;
};
namespace
detail
{
}
}
#endif // JSONGENERATOR_H
src/comm/ros_bridge/include/messages.h
View file @
3cd1822f
...
...
@@ -5,19 +5,28 @@
#include <vector>
#include <array>
#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief C++ representation of ros::Time
class
Time
{
public:
Time
()
;
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
;
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
uint32_t
getSecs
()
const
;
uint32_t
getNSecs
()
const
;
uint32_t
getSecs
()
const
{
return
_secs
;}
uint32_t
getNSecs
()
const
{
return
_secs
;}
void
setSecs
(
uint32_t
secs
);
void
setNSecs
(
uint32_t
nsecs
);
bool
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
const
{
doc
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
(
this
->
getSecs
()),
allocator
);
doc
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
(
this
->
getNSecs
()),
allocator
);
return
true
;
}
private:
uint32_t
_secs
;
...
...
@@ -27,128 +36,166 @@ namespace ros_bridge {
//! @brief C++ representation of std_msgs/Header
class
Header
{
public:
Header
();
Header
(
uint32_t
seq
,
const
Time
&
stamp
,
const
std
::
string
&
frame_id
);
Header
()
:
_seq
(
0
),
_frameId
(
""
)
{}
Header
(
uint32_t
seq
,
const
Time
*
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
)
,
_stamp
(
stamp
)
,
_frameId
(
frame_id
)
{}
uint32_t
getSeq
()
const
;
const
Time
&
getStamp
()
const
;
const
std
::
string
&
getFrameId
()
const
;
uint32_t
getSeq
()
const
{
return
_seq
;}
;
const
Time
*
getStamp
()
const
{
return
_stamp
;}
;
const
std
::
string
&
getFrameId
()
const
{
return
_frameId
;}
;
void
setSeq
(
uint32_t
seq
);
void
setStamp
(
const
Time
&
stamp
);
void
setFrameId
(
const
std
::
string
&
frame_id
);
void
setStamp
(
const
Time
*
stamp
);
void
setFrameId
(
const
std
::
string
&
frameId
);
bool
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
const
{
doc
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
(
this
->
_seq
),
allocator
);
rapidjson
::
Document
stamp
(
rapidjson
::
kObjectType
);
if
(
!
this
->
_stamp
->
toJson
(
stamp
,
allocator
))
return
false
;
doc
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
doc
.
AddMember
(
"frame_id"
,
rapidjson
::
Value
().
SetString
(
this
->
_frameId
.
data
(),
this
->
_frameId
.
length
(),
allocator
),
allocator
);
return
true
;
}
private:
uint32_t
_seq
;
Time
_stamp
;
std
::
string
_frame
_i
d
;
const
Time
*
_stamp
;
std
::
string
_frame
I
d
;
};
//! @brief C++ representation of geometry_msgs/Point32
template
<
class
PointType
>
class
Point32
{
public:
Point32
()
;
Point32
(
_Float32
x
,
_Float32
y
,
_Float32
z
);
public:
Point32
()
:
Point32
(
0
,
0
,
0
)
{}
Point32
(
const
PointType
*
p
)
:
_point
(
p
)
{}
template
<
unsigned
int
i
>
void
set
(
_Float32
component
)
{
static_assert
(
i
<
3
,
"Index out of bonds."
);
_components
[
i
]
=
component
;
}
const
PointType
*
getPoint
()
const
{
return
_point
;}
void
setPoint
(
const
PointType
*
point
)
{
point
;}
template
<
unsigned
int
i
>
_Float32
get
(
void
)
const
bool
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
const
{
static_assert
(
i
<
3
,
"Index out of bonds."
);
return
_components
[
i
];
doc
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
_point
->
x
()),
allocator
);
doc
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
_point
->
y
()),
allocator
);
doc
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
(
_point
->
z
()),
allocator
);
return
true
;
}
private:
std
::
array
<
_Float32
,
3
>
_components
;
const
PointType
*
_point
;
};
//! @brief C++ representation of geometry_msgs/Polygon
template
<
class
PointType
=
Point32
>
template
<
class
PointType
,
class
PointWrapper
=
class
Point32
<
PointType
>
,
template
<
class
>
class
ContainerType
=
std
::
vector
>
class
Polygon
{
public:
Polygon
(){}
Polygon
(
const
std
::
vector
<
Point32
>
&
points
)
:
_points
(
points
)
{}
Polygon
(
const
Polygon
&
poly
)
:
_points
(
poly
.
get
())
{}
Polygon
(
const
ContainerType
<
PointType
>
*
points
)
:
_points
(
&
points
)
{}
const
std
::
vector
<
Point32
>
&
get
()
const
{
return
_points
;
}
void
set
(
std
::
vector
<
Point32
>
&
points
)
{
_points
=
points
;
}
const
ContainerType
<
PointType
>
*
getPoints
()
const
{
return
_points
;}
void
clear
()
{
_points
.
clear
();
}
void
setPoints
(
const
ContainerType
<
PointType
>
*
points
)
{
_points
=
points
;}
PointType
&
at
(
unsigned
int
i
)
bool
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
_points
.
at
(
i
);
}
rapidjson
::
Value
points
(
rapidjson
::
kArrayType
);
void
push_back
(
const
Point32
&
p
)
{
_points
.
push_back
(
p
);
}
for
(
long
i
=
0
;
i
<
_points
->
size
();
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
_points
[
i
].
toJson
(
point
,
allocator
))
return
false
;
points
.
PushBack
(
point
,
allocator
);
}
void
pop_back
()
{
_points
.
pop_back
()
;
doc
.
AddMember
(
"points"
,
points
,
allocator
);
return
true
;
}
private:
std
::
vector
<
PointType
>
_points
;
const
ContainerType
<
PointType
>
*
_points
;
};
//! @brief C++ representation of geometry_msgs/PolygonStamped
template
<
class
HeaderType
=
Header
,
class
PolygonType
=
Polygon
<>
>
template
<
class
PointType
,
class
HeaderType
=
Header
,
template
<
class
>
class
PolygonType
=
Polygon
>
class
PolygonStamped
:
public
PolygonType
{
public:
PolygonStamped
();
PolygonStamped
(
const
Header
&
header
,
const
PolygonType
&
polygon
)
:
PolygonStamped
(
const
Header
*
header
,
const
PolygonType
*
polygon
)
:
_header
(
header
)
,
PolygonType
(
polygon
)
{}
,
_polygon
(
polygon
)
{}
const
HeaderType
*
getHeader
()
const
{
return
_header
;}
const
PolygonType
*
getPolygon
()
const
{
return
_polygon
;}
void
setHeader
(
const
Header
*
header
)
{
_header
=
header
;}
void
setPolygon
(
const
PolygonType
*
polygon
)
{
_polygon
=
polygon
;}
bool
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
this
->
_header
->
toJson
(
header
,
allocator
))
return
false
;
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
this
->
_polygon
->
toJson
(
polygon
,
allocator
))
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
doc
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
return
true
;
}
private:
Header
_header
;
HeaderType
*
_header
;
PolygonType
*
_polygon
;
};
//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template
<
class
PolygonType
=
Polygon
<>
,
class
HeaderType
=
Header
,
template
<
class
>
class
ContainerType
=
std
::
vector
>
template
<
class
PointType
,
class
HeaderType
=
Header
,
class
PolygonType
=
Polygon
<
PointType
>
,
template
<
class
>
class
ContainerType
=
std
::
vector
>
class
PolygonArray
{
public:
PolygonArray
()
:
PolygonArray
(
HeaderType
()
,
ContainerType
<
PolygonType
>
()
,
ContainerType
<
uint32_t
>
()
,
ContainerType
<
_Float32
>
())
{}
PolygonArray
(
const
HeaderType
&
header
,
const
ContainerType
<
PolygonType
>
&
polygons
,
const
ContainerType
<
uint32_t
>
&
labels
,
const
ContainerType
<
_Float32
>
&
likelihood
)
PolygonArray
()
{}
PolygonArray
(
const
HeaderType
*
header
,
const
ContainerType
<
PolygonType
>
*
polygons
,
const
ContainerType
<
uint32_t
>
*
labels
,
const
ContainerType
<
_Float32
>
*
likelihood
)
:
_header
(
header
)
,
_polygons
(
polygons
)
,
_labels
(
labels
)
,
_likelihood
(
likelihood
)
{}
const
HeaderType
&
getHeader
()
const
{
return
_header
;}
const
std
::
vector
<
PolygonType
>
&
getPolygons
()
const
{
return
_polygons
;}
const
std
::
vector
<
uint32_t
>
&
getLabels
()
const
{
return
_labels
;}
const
std
::
vector
<
_Float32
>
&
getLikelihood
()
const
{
return
_likelihood
;}
private:
HeaderType
_header
;
ContainerType
<
PolygonType
>
_polygons
;
ContainerType
<
uint32_t
>
_labels
;
ContainerType
<
_Float32
>
_likelihood
;
HeaderType
_header
;
std
::
vector
<
PolygonType
>
_polygons
;
std
::
vector
<
uint32_t
>
_labels
;
std
::
vector
<
_Float32
>
_likelihood
;
};
}
...
...
src/comm/ros_bridge/src/messages.cpp
View file @
3cd1822f
#include "include/messages.h"
using
namespace
ros_bridge
;
//===================================================================
// Time
//===================================================================
Time
::
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
::
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
uint32_t
Time
::
getSecs
()
const
{
return
_secs
;
}
uint32_t
Time
::
getNSecs
()
const
{
return
_nsecs
;
}
void
Time
::
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;
}
void
Time
::
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;
}
//===================================================================
// Header
//===================================================================
Header
::
Header
()
:
_seq
(
0
),
_frame_id
(
""
)
{}
Header
::
Header
(
uint32_t
seq
,
const
Time
&
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
),
_stamp
(
stamp
),
_frame_id
(
frame_id
)
{}
uint32_t
Header
::
getSeq
()
const
{
return
_seq
;
}
const
Time
&
Header
::
getStamp
()
const
{
return
_stamp
;
}
const
std
::
string
&
Header
::
getFrameId
()
const
{
return
_frame_id
;
}
void
Header
::
setSeq
(
uint32_t
seq
)
{
_seq
=
seq
;
}
void
Header
::
setStamp
(
const
Time
&
stamp
)
{
_stamp
=
stamp
;
}
void
Header
::
setFrameId
(
const
std
::
string
&
frame_id
)
{
_frame_id
=
frame_id
;
}
//===================================================================
// Point32
//===================================================================
Point32
::
Point32
()
:
Point32
(
0
,
0
,
0
)
{}
Point32
::
Point32
(
_Float32
x
,
_Float32
y
,
_Float32
z
)
:
_components
{
x
,
y
,
z
}
{}
//===================================================================
// PolygonStamped
//===================================================================
PolygonStamped
::
PolygonStamped
()
{}
PolygonStamped
::
PolygonStamped
(
const
Header
&
header
,
const
Polygon
&
polygon
)
:
header
(
header
),
polygon
(
polygon
){}
bool
PolygonStamped
::
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
this
->
header
.
toJson
(
header
,
allocator
))
return
false
;
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
this
->
polygon
.
toJson
(
polygon
,
allocator
))
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
doc
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
return
true
;
}
//===================================================================
// PolygonArray
//===================================================================
PolygonArray
::
PolygonArray
()
{}
PolygonArray
::
PolygonArray
(
const
Header
&
header
,
const
std
::
vector
<
PolygonStamped
>
&
polygons
,
const
std
::
vector
<
uint32_t
>
&
labels
,
const
std
::
vector
<
_Float32
>
&
likelihood
)
:
header
(
header
),
polygons
(
polygons
),
labels
(
labels
),
likelihood
(
likelihood
)
{}
bool
PolygonArray
::
toJson
(
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
this
->
header
.
toJson
(
header
,
allocator
))
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
rapidjson
::
Value
polygons
(
rapidjson
::
kArrayType
);
for
(
auto
it
=
this
->
polygons
.
begin
();
it
!=
this
->
polygons
.
end
();
++
it
){
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
it
->
toJson
(
polygon
,
allocator
))
return
false
;
polygons
.
PushBack
(
polygon
,
allocator
);
}
doc
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
rapidjson
::
Value
labels
(
rapidjson
::
kArrayType
);
for
(
auto
it
=
this
->
labels
.
begin
();
it
!=
this
->
labels
.
end
();
++
it
)
labels
.
PushBack
(
rapidjson
::
Value
().
SetUint
(
*
it
),
allocator
);
doc
.
AddMember
(
"labels"
,
labels
,
allocator
);
rapidjson
::
Value
likelihood
(
rapidjson
::
kArrayType
);
for
(
auto
it
=
this
->
likelihood
.
begin
();
it
!=
this
->
likelihood
.
end
();
++
it
)
likelihood
.
PushBack
(
rapidjson
::
Value
().
SetFloat
(
*
it
),
allocator
);
doc
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
return
true
;
}
test_rosbridge/rosbridge/.snake.cpp.kate-swp
0 → 100644
View file @
3cd1822f
File added
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment