Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit 12f7a83

Browse files
olivier-stasseolivier-stasse
committed
Update time from int to dg::sigtime_t
1 parent 5f6b95f commit 12f7a83

8 files changed

+56
-74
lines changed

‎src/dg_ros_mapping.hpp‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -36,11 +36,13 @@
3636
#include "dynamic_graph_bridge_msgs/msg/matrix.hpp"
3737
#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
3838

39+
namespace dg = dynamicgraph;
40+
3941
namespace dynamic_graph_bridge {
4042
/** @brief Conventient renaming for ease of implementation. */
41-
typedef dynamicgraph::Vector DgVector;
43+
typedef dg::Vector DgVector;
4244
/** @brief Conventient renaming for ease of implementation. */
43-
typedef dynamicgraph::Matrix DgMatrix;
45+
typedef dg::Matrix DgMatrix;
4446
/** @brief Conventient renaming for ease of implementation. */
4547
typedef dynamic_graph_bridge_msgs::msg::Vector RosVector;
4648
/** @brief Conventient renaming for ease of implementation. */
@@ -103,14 +105,14 @@ class DgRosMapping {
103105
/** @brief Ros type as a shared pointer. */
104106
typedef std::shared_ptr<RosType> ros_shared_ptr_t;
105107
/** @brief Output signal type. */
106-
typedef dynamicgraph::SignalTimeDependent<dg_t, int> signal_out_t;
108+
typedef dg::SignalTimeDependent<dg_t, dg::sigtime_t> signal_out_t;
107109
/** @brief Output signal type. */
108-
typedef dynamicgraph::SignalTimeDependent<timestamp_t, int>
110+
typedef dg::SignalTimeDependent<timestamp_t, dg::sigtime_t>
109111
signal_timestamp_out_t;
110112
/** @brief Input signal type. */
111-
typedef dynamicgraph::SignalPtr<dg_t, int> signal_in_t;
113+
typedef dg::SignalPtr<dg_t, dg::sigtime_t> signal_in_t;
112114
/** @brief Signal callback function types. */
113-
typedef std::function<dg_t&(dg_t&, int)> signal_callback_t;
115+
typedef std::function<dg_t&(dg_t&, dg::sigtime_t)> signal_callback_t;
114116

115117
/** @brief Name of the signal type. Used during the creation of signals. */
116118
static const std::string signal_type_name;

‎src/ros_publish.hpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,8 @@
2020

2121
#include "dg_ros_mapping.hpp"
2222

23+
namespace dg = dynamicgraph;
24+
2325
namespace dynamic_graph_bridge {
2426
/** @brief Publish dynamic-graph information into ROS. */
2527
class RosPublish : public dynamicgraph::Entity {
@@ -36,13 +38,14 @@ class RosPublish : public dynamicgraph::Entity {
3638
* @brief Tuple composed by the generated input signal and its callback
3739
* function. The callback function publishes the input signal content.
3840
*/
39-
typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
41+
typedef std::tuple<std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
4042
PublisherCallback>
4143
BindedSignal;
4244

4345
/**
4446
* @brief Construct a new RosPublish object.
4547
*
48+
4649
* @param n
4750
*/
4851
RosPublish(const std::string& n);
@@ -79,6 +82,7 @@ class RosPublish : public dynamicgraph::Entity {
7982
void add(const std::string& signal, const std::string& topic);
8083

8184
/**
85+
8286
* @brief Remove a signal to publish to ROS.
8387
*
8488
* @param signal name.
@@ -130,7 +134,7 @@ class RosPublish : public dynamicgraph::Entity {
130134

131135
/** @brief Trigger signal publishing the signal values every other
132136
* iterations. */
133-
dynamicgraph::SignalTimeDependent<int, int> trigger_;
137+
dynamicgraph::SignalTimeDependent<int, dg::sigtime_t> trigger_;
134138

135139
/** @brief Publishing rate. Default is 50ms. */
136140
rclcpp::Duration rate_nanosec_;

‎src/ros_subscribe.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "dg_ros_mapping.hpp"
2222
#include "fwd.hpp"
2323

24+
namespace dg = dynamicgraph;
2425
namespace dynamic_graph_bridge {
2526
/**
2627
* @brief Publish ROS information in the dynamic-graph.
@@ -33,8 +34,8 @@ class RosSubscribe : public dynamicgraph::Entity {
3334
* @brief Tuple composed by the generated input signal and its callback
3435
* function. The callback function publishes the input signal content.
3536
*/
36-
typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
37-
std::shared_ptr<dynamicgraph::SignalBase<int> >,
37+
typedef std::tuple<std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
38+
std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
3839
rclcpp::SubscriptionBase::SharedPtr>
3940
BindedSignal;
4041

‎src/ros_subscribe.hxx‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ void RosSubscribe::add(const std::string& signal_name,
6868
std::make_shared<signal_out_t>(full_signal_name);
6969
DgRosMapping<RosType, DgType>::set_default(signal_ptr);
7070
signal_ptr->setDependencyType(
71-
dynamicgraph::TimeDependency<int>::ALWAYS_READY);
71+
dynamicgraph::TimeDependency<dg::sigtime_t>::ALWAYS_READY);
7272
std::get<0>(binded_signal) = signal_ptr;
7373
this->signalRegistration(*std::get<0>(binded_signal));
7474

@@ -83,7 +83,7 @@ void RosSubscribe::add(const std::string& signal_name,
8383
signal_timestamp_ptr->setConstant(
8484
DgRosMapping<RosType, DgType>::epoch_time());
8585
signal_timestamp_ptr->setDependencyType(
86-
dynamicgraph::TimeDependency<int>::ALWAYS_READY);
86+
dynamicgraph::TimeDependency<dg::sigtime_t>::ALWAYS_READY);
8787
this->signalRegistration(*signal_timestamp_ptr);
8888
}
8989
std::get<1>(binded_signal) = signal_timestamp_ptr;

‎tests/impl_test_sot_external_interface.cpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
#include "dynamic_graph_bridge/ros.hpp"
44

55
using namespace dynamic_graph_bridge;
6+
namespace dg = dynamicgraph;
67

78
ImplTestSotExternalInterface::ImplTestSotExternalInterface()
89
: device_(new ImplTestSotMockDevice("RobotName")) {
@@ -73,7 +74,8 @@ void ImplTestSotExternalInterface::cleanupSetSensors(
7374
}
7475

7576
void ImplTestSotExternalInterface::getControl(
76-
std::map<std::string, dynamicgraph::sot::ControlValues> &controlValues) {
77+
std::map<std::string, dynamicgraph::sot::ControlValues> &controlValues,
78+
const double &) {
7779
// Put the default named_ctrl_vec inside the map controlValues.
7880
controlValues["control"] = named_ctrl_vec_;
7981
controlValues["baseff"] = named_base_ff_vec_;
@@ -87,6 +89,10 @@ void ImplTestSotExternalInterface::setNoIntegration(void) {
8789
std::cout << "setNoIntegration" << std::endl;
8890
}
8991

92+
void ImplTestSotExternalInterface::setControlSize(const dg::size_type &) {
93+
std::cout << "setControlSize" << std::endl;
94+
}
95+
9096
extern "C" {
9197
dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface() {
9298
return new ImplTestSotExternalInterface;

‎tests/impl_test_sot_external_interface.hh‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
#include "impl_test_sot_mock_device.hh"
99

10+
namespace dg = dynamicgraph;
1011
namespace dynamic_graph_bridge {
1112
class ImplTestSotExternalInterface
1213
: public dynamicgraph::sot::AbstractSotExternalInterface {
@@ -25,12 +26,15 @@ class ImplTestSotExternalInterface
2526
virtual void cleanupSetSensors(
2627
std::map<std::string, dynamicgraph::sot::SensorValues> &) final;
2728
virtual void getControl(
28-
std::map<std::string, dynamicgraph::sot::ControlValues> &) final;
29+
std::map<std::string, dynamicgraph::sot::ControlValues> &,
30+
const double &) final;
2931

3032
virtual void setSecondOrderIntegration(void);
3133

3234
virtual void setNoIntegration(void);
3335

36+
virtual void setControlSize(const dg::size_type &);
37+
3438
/// Embedded python interpreter accessible via Corba/ros
3539
boost::shared_ptr<dynamic_graph_bridge::RosPythonInterpreterServer>
3640
py_interpreter_srv_;

‎tests/impl_test_sot_mock_device.cpp‎

Lines changed: 9 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -99,25 +99,13 @@ ImplTestSotMockDevice::ImplTestSotMockDevice(std::string RobotName)
9999
dataForces.setZero();
100100
for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
101101

102-
using namespace dynamicgraph::command;
103-
std::string docstring;
104-
/* Command increment. */
105-
docstring =
106-
"\n"
107-
" Integrate dynamics for time step provided as input\n"
108-
"\n"
109-
" take one floating point number as input\n"
110-
"\n";
111-
addCommand("increment",
112-
makeCommandVoid1((Device&)*this, &Device::increment, docstring));
113-
114102
sotDEBUGOUT(25);
115103
}
116104

117105
ImplTestSotMockDevice::~ImplTestSotMockDevice() {}
118106

119107
void ImplTestSotMockDevice::setSensorsForce(
120-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
108+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
121109
int map_sot_2_urdf[4] = {2, 0, 3, 1};
122110
sotDEBUGIN(15);
123111
map<string, dgsot::SensorValues>::iterator it;
@@ -140,7 +128,7 @@ void ImplTestSotMockDevice::setSensorsForce(
140128
}
141129

142130
void ImplTestSotMockDevice::setSensorsIMU(
143-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
131+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
144132
map<string, dgsot::SensorValues>::iterator it;
145133
// TODO: Confirm if this can be made quaternion
146134
it = SensorsIn.find("attitude");
@@ -171,7 +159,7 @@ void ImplTestSotMockDevice::setSensorsIMU(
171159
}
172160

173161
void ImplTestSotMockDevice::setSensorsEncoders(
174-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
162+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
175163
map<string, dgsot::SensorValues>::iterator it;
176164

177165
it = SensorsIn.find("motor-angles");
@@ -202,7 +190,7 @@ void ImplTestSotMockDevice::setSensorsEncoders(
202190
}
203191

204192
void ImplTestSotMockDevice::setSensorsVelocities(
205-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
193+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
206194
map<string, dgsot::SensorValues>::iterator it;
207195

208196
it = SensorsIn.find("velocities");
@@ -219,7 +207,7 @@ void ImplTestSotMockDevice::setSensorsVelocities(
219207
}
220208

221209
void ImplTestSotMockDevice::setSensorsTorquesCurrents(
222-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
210+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
223211
map<string, dgsot::SensorValues>::iterator it;
224212
it = SensorsIn.find("torques");
225213
if (it != SensorsIn.end()) {
@@ -242,7 +230,7 @@ void ImplTestSotMockDevice::setSensorsTorquesCurrents(
242230
}
243231

244232
void ImplTestSotMockDevice::setSensorsGains(
245-
map<string, dgsot::SensorValues>& SensorsIn, int t) {
233+
map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
246234
map<string, dgsot::SensorValues>::iterator it;
247235
it = SensorsIn.find("p_gains");
248236
if (it != SensorsIn.end()) {
@@ -267,7 +255,7 @@ void ImplTestSotMockDevice::setSensors(
267255
map<string, dgsot::SensorValues>& SensorsIn) {
268256
sotDEBUGIN(25);
269257
map<string, dgsot::SensorValues>::iterator it;
270-
int t = stateSOUT.getTime() + 1;
258+
dg::sigtime_t t = stateSOUT.getTime() + 1;
271259

272260
setSensorsForce(SensorsIn, t);
273261
setSensorsIMU(SensorsIn, t);
@@ -298,15 +286,14 @@ void ImplTestSotMockDevice::cleanupSetSensors(
298286
}
299287

300288
void ImplTestSotMockDevice::getControl(
301-
map<string, dgsot::ControlValues>& controlOut) {
289+
map<string, dgsot::ControlValues>& controlOut, const double&) {
302290
ODEBUG5FULL("start");
303291
sotDEBUGIN(25);
304292
vector<double> anglesOut, velocityOut;
305293
anglesOut.resize(state_.size());
306294
velocityOut.resize(state_.size());
307295

308296
// Integrate control
309-
increment(timestep_);
310297
sotDEBUG(25) << "state = " << state_ << std::endl;
311298
sotDEBUG(25) << "diff = "
312299
<< ((previousState_.size() == state_.size())
@@ -327,7 +314,7 @@ void ImplTestSotMockDevice::getControl(
327314
velocityOut.resize(state_.size() - 6);
328315
}
329316

330-
int time = controlSIN.getTime();
317+
dg::sigtime_t time = controlSIN.getTime();
331318
for (unsigned int i = 6; i < state_.size(); ++i) {
332319
anglesOut[i - 6] = state_(i);
333320
velocityOut[i - 6] = controlSIN(time)(i);
@@ -342,31 +329,7 @@ void ImplTestSotMockDevice::getControl(
342329
dg::Vector zmpGlobal(4);
343330
for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
344331
zmpGlobal(3) = 1.;
345-
dgsot::MatrixHomogeneous inversePose;
346-
347-
inversePose = freeFlyerPose().inverse(Eigen::Affine);
348-
dg::Vector localZmp(4);
349-
localZmp = inversePose.matrix() * zmpGlobal;
350-
vector<double> ZMPRef(3);
351-
for (unsigned int i = 0; i < 3; ++i) ZMPRef[i] = localZmp(i);
352-
353-
controlOut["zmp"].setName("zmp");
354-
controlOut["zmp"].setValues(ZMPRef);
355-
356-
// Update position of freeflyer in global frame
357-
Eigen::Vector3d transq_(freeFlyerPose().translation());
358-
dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
359-
360-
// translation
361-
for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);
362-
363-
// rotation: quaternion
364-
baseff_[3] = qt_.w();
365-
baseff_[4] = qt_.x();
366-
baseff_[5] = qt_.y();
367-
baseff_[6] = qt_.z();
368332

369-
controlOut["baseff"].setValues(baseff_);
370333
ODEBUG5FULL("end");
371334
sotDEBUGOUT(25);
372335
}

‎tests/impl_test_sot_mock_device.hh‎

Lines changed: 16 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include <dynamic-graph/signal-ptr.h>
1616
#include <dynamic-graph/signal.h>
1717

18+
#include <dynamic-graph/fwd.hh>
1819
#include <sot/core/abstract-sot-external-interface.hh>
1920
#include <sot/core/device.hh>
2021
#include <sot/core/matrix-geometry.hh>
@@ -40,7 +41,8 @@ class ImplTestSotMockDevice : public dgsot::Device {
4041

4142
void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
4243

43-
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
44+
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
45+
const double &period);
4446

4547
void timeStep(double ts) { timestep_ = ts; }
4648

@@ -52,34 +54,34 @@ class ImplTestSotMockDevice : public dgsot::Device {
5254
std::vector<double> baseff_;
5355

5456
/// Accelerations measured by accelerometers
55-
dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
57+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> accelerometerSOUT_;
5658
/// Rotation velocity measured by gyrometers
57-
dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
59+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> gyrometerSOUT_;
5860
/// motor currents
59-
dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
61+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> currentsSOUT_;
6062
/// joint angles
61-
dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
63+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> joint_anglesSOUT_;
6264
/// motor angles
63-
dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
65+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> motor_anglesSOUT_;
6466

6567
/// proportional and derivative position-control gains
66-
dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
68+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> p_gainsSOUT_;
6769

68-
dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
70+
dynamicgraph::Signal<dg::Vector, dg::sigtime_t> d_gainsSOUT_;
6971

7072
/// Protected methods for internal variables filling
7173
void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn,
72-
int t);
74+
dg::sigtime_t t);
7375
void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn,
74-
int t);
76+
dg::sigtime_t t);
7577
void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn,
76-
int t);
78+
dg::sigtime_t t);
7779
void setSensorsVelocities(
78-
std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
80+
std::map<std::string, dgsot::SensorValues> &SensorsIn, dg::sigtime_t t);
7981
void setSensorsTorquesCurrents(
80-
std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
82+
std::map<std::string, dgsot::SensorValues> &SensorsIn, dg::sigtime_t t);
8183
void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn,
82-
int t);
84+
dg::sigtime_t t);
8385

8486
/// Intermediate variables to avoid allocation during control
8587
dg::Vector dgforces_;

0 commit comments

Comments
 (0)