Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
roscontrol_sot
Commits
d5f27702
Commit
d5f27702
authored
Nov 07, 2018
by
Olivier Stasse
Browse files
Fix several bugs in logging with the new logging architecture.
parent
0bd99a03
Pipeline
#1870
failed with stage
in 1 second
Changes
3
Pipelines
3
Hide whitespace changes
Inline
Side-by-side
src/log.cpp
View file @
d5f27702
...
...
@@ -62,8 +62,8 @@ void Log::init(ProfileLog &aProfileLog)
void
Log
::
record
(
DataToLog
&
aDataToLog
)
{
if
(
(
aDataToLog
.
motor_angle
.
size
()
!=
aDataTo
Log
.
nbDofs
()
)
||
(
aDataToLog
.
velocities
.
size
()
!=
aDataTo
Log
.
nbDofs
()
))
if
(
(
aDataToLog
.
motor_angle
.
size
()
!=
profile
Log
_
.
nbDofs
)
||
(
aDataToLog
.
velocities
.
size
()
!=
profile
Log
_
.
nbDofs
))
return
;
for
(
unsigned
int
JointID
=
0
;
JointID
<
aDataToLog
.
nbDofs
();
JointID
++
)
...
...
@@ -105,9 +105,9 @@ void Log::record(DataToLog &aDataToLog)
StoredData_
.
duration
[
lrefts_
]
=
time_stop_it_
-
time_start_it_
;
lref_
+=
aDataTo
Log
.
nbDofs
()
;
lref_
+=
profile
Log
_
.
nbDofs
;
lrefts_
++
;
if
(
lref_
>=
aDataTo
Log
.
nbDofs
()
*
aDataTo
Log
.
length
()
)
if
(
lref_
>=
profile
Log
_
.
nbDofs
*
profile
Log
_
.
length
)
{
lref_
=
0
;
lrefts_
=
0
;
...
...
@@ -153,7 +153,7 @@ void Log::save(std::string &fileName)
ostringstream
oss
;
oss
<<
"-forceSensors.log"
;
suffix
=
oss
.
str
();
saveVector
(
fileName
,
suffix
,
StoredData_
.
force_sensors
,
6
);
saveVector
(
fileName
,
suffix
,
StoredData_
.
force_sensors
,
6
*
profileLog_
.
nbForceSensors
);
suffix
=
"-temperatures.log"
;
saveVector
(
fileName
,
suffix
,
StoredData_
.
temperatures
,
profileLog_
.
nbDofs
);
...
...
src/roscontrol-sot-controller.cpp
View file @
d5f27702
...
...
@@ -124,12 +124,17 @@ namespace sot_controller
void
RCSotController
::
initLogs
()
{
ROS_INFO_STREAM
(
"Initialize log data structure"
);
/// Initialize the size of the data to store.
/// Initialize the size of the data to store.
/// Set temporary profileLog to one
/// because DataOneIter is just for one iteration.
unsigned
tmp_length
=
profileLog_
.
length
;
profileLog_
.
length
=
1
;
DataOneIter_
.
init
(
profileLog_
);
/// Set profile Log to real good value for the stored data.
profileLog_
.
length
=
tmp_length
;
/// Initialize the data logger for 300s.
RcSotLog
.
init
(
profileLog_
);
RcSotLog
_
.
init
(
profileLog_
);
}
...
...
@@ -909,7 +914,7 @@ namespace sot_controller
void
RCSotController
::
one_iteration
()
{
// Chrono start
RcSotLog
.
start_it
();
RcSotLog
_
.
start_it
();
/// Update the sensors.
fillSensors
();
...
...
@@ -926,10 +931,10 @@ namespace sot_controller
readControl
(
controlValues_
);
// Chrono stop.
RcSotLog
.
stop_it
();
RcSotLog
_
.
stop_it
();
/// Store everything in Log.
RcSotLog
.
record
(
DataOneIter_
);
RcSotLog
_
.
record
(
DataOneIter_
);
}
void
RCSotController
::
...
...
@@ -1038,7 +1043,7 @@ namespace sot_controller
stopping
(
const
ros
::
Time
&
)
{
std
::
string
afilename
(
"/tmp/sot.log"
);
RcSotLog
.
save
(
afilename
);
RcSotLog
_
.
save
(
afilename
);
SotLoaderBasic
::
CleanUp
();
...
...
src/roscontrol-sot-controller.hh
View file @
d5f27702
...
...
@@ -140,7 +140,7 @@ namespace sot_controller
/// @}
/// \brief Log
rc_sot_system
::
Log
RcSotLog
;
rc_sot_system
::
Log
RcSotLog
_
;
/// @}
const
std
::
string
type_name_
;
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment