From 17a992fb2d022e1e04d4efa822176c453e53dcd6 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 3 Mar 2023 16:19:13 +0100
Subject: [PATCH] [SotLoader] Update to recent modifications in sot-core.
---
include/dynamic_graph_bridge/sot_loader.hh | 2 +-
src/sot_loader.cpp | 11 ++++++-----
2 files changed, 7 insertions(+), 6 deletions(-)
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 53f88d4..33d7a27 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -88,7 +88,7 @@ class SotLoader : public SotLoaderBasic {
// \brief Compute one iteration of control.
// Basically calls fillSensors, the SoT and the readControl.
- void oneIteration();
+ void oneIteration(const double& period);
// \brief Fill the sensors value for the SoT.
void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 09a68c8..2462786 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -59,8 +59,9 @@ struct DataToLog {
void workThreadLoader(SotLoader *aSotLoader) {
ros::Rate rate(1000); // 1 kHz
+ double periodd (1e-3);
+
if (ros::param::has("/sot_controller/dt")) {
- double periodd;
ros::param::get("/sot_controller/dt", periodd);
rate = ros::Rate(1 / periodd);
}
@@ -80,7 +81,7 @@ void workThreadLoader(SotLoader *aSotLoader) {
if (!paused) {
time = ros::Time::now();
- aSotLoader->oneIteration();
+ aSotLoader->oneIteration(periodd);
ros::Duration d = ros::Time::now() - time;
dataToLog.record((time - timeOrigin).toSec(), d.toSec());
@@ -200,15 +201,15 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
void SotLoader::setup() {
fillSensors(sensorsIn_);
sotController_->setupSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
+ sotController_->getControl(controlValues_, 0);
readControl(controlValues_);
}
-void SotLoader::oneIteration() {
+void SotLoader::oneIteration(const double& period) {
fillSensors(sensorsIn_);
try {
sotController_->nominalSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
+ sotController_->getControl(controlValues_, period);
} catch (std::exception &) {
throw;
}
--
GitLab