diff --git a/src/dg_ros_mapping.hpp b/src/dg_ros_mapping.hpp
index 98e639b4e2d8062ccb8e7d4967f63f34d06c0435..c1715f9a185810909f1a7cf9fd092b7ac965eab3 100644
--- a/src/dg_ros_mapping.hpp
+++ b/src/dg_ros_mapping.hpp
@@ -36,11 +36,13 @@
#include "dynamic_graph_bridge_msgs/msg/matrix.hpp"
#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
+namespace dg = dynamicgraph;
+
namespace dynamic_graph_bridge {
/** @brief Conventient renaming for ease of implementation. */
-typedef dynamicgraph::Vector DgVector;
+typedef dg::Vector DgVector;
/** @brief Conventient renaming for ease of implementation. */
-typedef dynamicgraph::Matrix DgMatrix;
+typedef dg::Matrix DgMatrix;
/** @brief Conventient renaming for ease of implementation. */
typedef dynamic_graph_bridge_msgs::msg::Vector RosVector;
/** @brief Conventient renaming for ease of implementation. */
@@ -103,14 +105,14 @@ class DgRosMapping {
/** @brief Ros type as a shared pointer. */
typedef std::shared_ptr<RosType> ros_shared_ptr_t;
/** @brief Output signal type. */
- typedef dynamicgraph::SignalTimeDependent<dg_t, int> signal_out_t;
+ typedef dg::SignalTimeDependent<dg_t, dg::sigtime_t> signal_out_t;
/** @brief Output signal type. */
- typedef dynamicgraph::SignalTimeDependent<timestamp_t, int>
+ typedef dg::SignalTimeDependent<timestamp_t, dg::sigtime_t>
signal_timestamp_out_t;
/** @brief Input signal type. */
- typedef dynamicgraph::SignalPtr<dg_t, int> signal_in_t;
+ typedef dg::SignalPtr<dg_t, dg::sigtime_t> signal_in_t;
/** @brief Signal callback function types. */
- typedef std::function<dg_t&(dg_t&, int)> signal_callback_t;
+ typedef std::function<dg_t&(dg_t&, dg::sigtime_t)> signal_callback_t;
/** @brief Name of the signal type. Used during the creation of signals. */
static const std::string signal_type_name;
diff --git a/src/ros_publish.hpp b/src/ros_publish.hpp
index 32131cb719e43dcc9b2511f44a33babffa993e01..939deda4c5f37951b0409e510dd47d6ab81a8df4 100644
--- a/src/ros_publish.hpp
+++ b/src/ros_publish.hpp
@@ -20,6 +20,8 @@
#include "dg_ros_mapping.hpp"
+namespace dg = dynamicgraph;
+
namespace dynamic_graph_bridge {
/** @brief Publish dynamic-graph information into ROS. */
class RosPublish : public dynamicgraph::Entity {
@@ -36,13 +38,14 @@ class RosPublish : public dynamicgraph::Entity {
* @brief Tuple composed by the generated input signal and its callback
* function. The callback function publishes the input signal content.
*/
- typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ typedef std::tuple<std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
PublisherCallback>
BindedSignal;
/**
* @brief Construct a new RosPublish object.
*
+
* @param n
*/
RosPublish(const std::string& n);
@@ -79,6 +82,7 @@ class RosPublish : public dynamicgraph::Entity {
void add(const std::string& signal, const std::string& topic);
/**
+
* @brief Remove a signal to publish to ROS.
*
* @param signal name.
@@ -130,7 +134,7 @@ class RosPublish : public dynamicgraph::Entity {
/** @brief Trigger signal publishing the signal values every other
* iterations. */
- dynamicgraph::SignalTimeDependent<int, int> trigger_;
+ dynamicgraph::SignalTimeDependent<int, dg::sigtime_t> trigger_;
/** @brief Publishing rate. Default is 50ms. */
rclcpp::Duration rate_nanosec_;
diff --git a/src/ros_subscribe.hpp b/src/ros_subscribe.hpp
index bf2dc0ee9d3214415b45052b422dd235fdcd8876..7be1d0e045c28526833a6d32e19622afb7e7623d 100644
--- a/src/ros_subscribe.hpp
+++ b/src/ros_subscribe.hpp
@@ -21,6 +21,7 @@
#include "dg_ros_mapping.hpp"
#include "fwd.hpp"
+namespace dg = dynamicgraph;
namespace dynamic_graph_bridge {
/**
* @brief Publish ROS information in the dynamic-graph.
@@ -33,8 +34,8 @@ class RosSubscribe : public dynamicgraph::Entity {
* @brief Tuple composed by the generated input signal and its callback
* function. The callback function publishes the input signal content.
*/
- typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
- std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ typedef std::tuple<std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
+ std::shared_ptr<dg::SignalBase<dg::sigtime_t> >,
rclcpp::SubscriptionBase::SharedPtr>
BindedSignal;
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index 40fd488ad2ba96dfabb820aec8f66a257aa957f3..bd673bce41523e022240bee042e0c06c95b03c7c 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -68,7 +68,7 @@ void RosSubscribe::add(const std::string& signal_name,
std::make_shared<signal_out_t>(full_signal_name);
DgRosMapping<RosType, DgType>::set_default(signal_ptr);
signal_ptr->setDependencyType(
- dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ dynamicgraph::TimeDependency<dg::sigtime_t>::ALWAYS_READY);
std::get<0>(binded_signal) = signal_ptr;
this->signalRegistration(*std::get<0>(binded_signal));
@@ -83,7 +83,7 @@ void RosSubscribe::add(const std::string& signal_name,
signal_timestamp_ptr->setConstant(
DgRosMapping<RosType, DgType>::epoch_time());
signal_timestamp_ptr->setDependencyType(
- dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ dynamicgraph::TimeDependency<dg::sigtime_t>::ALWAYS_READY);
this->signalRegistration(*signal_timestamp_ptr);
}
std::get<1>(binded_signal) = signal_timestamp_ptr;
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index e0ea9d5996eb55fa3bd23d26d74066a11ee65306..e162b677b75cb18a6aa193e76424a849ddaa535b 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -3,6 +3,7 @@
#include "dynamic_graph_bridge/ros.hpp"
using namespace dynamic_graph_bridge;
+namespace dg = dynamicgraph;
ImplTestSotExternalInterface::ImplTestSotExternalInterface()
: device_(new ImplTestSotMockDevice("RobotName")) {
@@ -73,7 +74,8 @@ void ImplTestSotExternalInterface::cleanupSetSensors(
}
void ImplTestSotExternalInterface::getControl(
- std::map<std::string, dynamicgraph::sot::ControlValues> &controlValues) {
+ std::map<std::string, dynamicgraph::sot::ControlValues> &controlValues,
+ const double &) {
// Put the default named_ctrl_vec inside the map controlValues.
controlValues["control"] = named_ctrl_vec_;
controlValues["baseff"] = named_base_ff_vec_;
@@ -87,6 +89,10 @@ void ImplTestSotExternalInterface::setNoIntegration(void) {
std::cout << "setNoIntegration" << std::endl;
}
+void ImplTestSotExternalInterface::setControlSize(const dg::size_type &) {
+ std::cout << "setControlSize" << std::endl;
+}
+
extern "C" {
dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface() {
return new ImplTestSotExternalInterface;
diff --git a/tests/impl_test_sot_external_interface.hh b/tests/impl_test_sot_external_interface.hh
index d84ae7b93264b64d210f543484e6368bf7ed9542..f6e0f0cc0655f6b3c187f5038cb2570708529724 100644
--- a/tests/impl_test_sot_external_interface.hh
+++ b/tests/impl_test_sot_external_interface.hh
@@ -7,6 +7,7 @@
#include "impl_test_sot_mock_device.hh"
+namespace dg = dynamicgraph;
namespace dynamic_graph_bridge {
class ImplTestSotExternalInterface
: public dynamicgraph::sot::AbstractSotExternalInterface {
@@ -25,12 +26,15 @@ class ImplTestSotExternalInterface
virtual void cleanupSetSensors(
std::map<std::string, dynamicgraph::sot::SensorValues> &) final;
virtual void getControl(
- std::map<std::string, dynamicgraph::sot::ControlValues> &) final;
+ std::map<std::string, dynamicgraph::sot::ControlValues> &,
+ const double &) final;
virtual void setSecondOrderIntegration(void);
virtual void setNoIntegration(void);
+ virtual void setControlSize(const dg::size_type &);
+
/// Embedded python interpreter accessible via Corba/ros
boost::shared_ptr<dynamic_graph_bridge::RosPythonInterpreterServer>
py_interpreter_srv_;
diff --git a/tests/impl_test_sot_mock_device.cpp b/tests/impl_test_sot_mock_device.cpp
index c79e5b996fdd5caf2203aa25b6a1206d2c5b97c6..c9e9e9ad7f21dc56d68e001c706f92b5ab65d333 100644
--- a/tests/impl_test_sot_mock_device.cpp
+++ b/tests/impl_test_sot_mock_device.cpp
@@ -99,25 +99,13 @@ ImplTestSotMockDevice::ImplTestSotMockDevice(std::string RobotName)
dataForces.setZero();
for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
- using namespace dynamicgraph::command;
- std::string docstring;
- /* Command increment. */
- docstring =
- "\n"
- " Integrate dynamics for time step provided as input\n"
- "\n"
- " take one floating point number as input\n"
- "\n";
- addCommand("increment",
- makeCommandVoid1((Device&)*this, &Device::increment, docstring));
-
sotDEBUGOUT(25);
}
ImplTestSotMockDevice::~ImplTestSotMockDevice() {}
void ImplTestSotMockDevice::setSensorsForce(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
int map_sot_2_urdf[4] = {2, 0, 3, 1};
sotDEBUGIN(15);
map<string, dgsot::SensorValues>::iterator it;
@@ -140,7 +128,7 @@ void ImplTestSotMockDevice::setSensorsForce(
}
void ImplTestSotMockDevice::setSensorsIMU(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
// TODO: Confirm if this can be made quaternion
it = SensorsIn.find("attitude");
@@ -171,7 +159,7 @@ void ImplTestSotMockDevice::setSensorsIMU(
}
void ImplTestSotMockDevice::setSensorsEncoders(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("motor-angles");
@@ -202,7 +190,7 @@ void ImplTestSotMockDevice::setSensorsEncoders(
}
void ImplTestSotMockDevice::setSensorsVelocities(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("velocities");
@@ -219,7 +207,7 @@ void ImplTestSotMockDevice::setSensorsVelocities(
}
void ImplTestSotMockDevice::setSensorsTorquesCurrents(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("torques");
if (it != SensorsIn.end()) {
@@ -242,7 +230,7 @@ void ImplTestSotMockDevice::setSensorsTorquesCurrents(
}
void ImplTestSotMockDevice::setSensorsGains(
- map<string, dgsot::SensorValues>& SensorsIn, int t) {
+ map<string, dgsot::SensorValues>& SensorsIn, dg::sigtime_t t) {
map<string, dgsot::SensorValues>::iterator it;
it = SensorsIn.find("p_gains");
if (it != SensorsIn.end()) {
@@ -267,7 +255,7 @@ void ImplTestSotMockDevice::setSensors(
map<string, dgsot::SensorValues>& SensorsIn) {
sotDEBUGIN(25);
map<string, dgsot::SensorValues>::iterator it;
- int t = stateSOUT.getTime() + 1;
+ dg::sigtime_t t = stateSOUT.getTime() + 1;
setSensorsForce(SensorsIn, t);
setSensorsIMU(SensorsIn, t);
@@ -298,7 +286,7 @@ void ImplTestSotMockDevice::cleanupSetSensors(
}
void ImplTestSotMockDevice::getControl(
- map<string, dgsot::ControlValues>& controlOut) {
+ map<string, dgsot::ControlValues>& controlOut, const double&) {
ODEBUG5FULL("start");
sotDEBUGIN(25);
vector<double> anglesOut, velocityOut;
@@ -306,7 +294,6 @@ void ImplTestSotMockDevice::getControl(
velocityOut.resize(state_.size());
// Integrate control
- increment(timestep_);
sotDEBUG(25) << "state = " << state_ << std::endl;
sotDEBUG(25) << "diff = "
<< ((previousState_.size() == state_.size())
@@ -327,7 +314,7 @@ void ImplTestSotMockDevice::getControl(
velocityOut.resize(state_.size() - 6);
}
- int time = controlSIN.getTime();
+ dg::sigtime_t time = controlSIN.getTime();
for (unsigned int i = 6; i < state_.size(); ++i) {
anglesOut[i - 6] = state_(i);
velocityOut[i - 6] = controlSIN(time)(i);
@@ -342,31 +329,7 @@ void ImplTestSotMockDevice::getControl(
dg::Vector zmpGlobal(4);
for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
zmpGlobal(3) = 1.;
- dgsot::MatrixHomogeneous inversePose;
-
- inversePose = freeFlyerPose().inverse(Eigen::Affine);
- dg::Vector localZmp(4);
- localZmp = inversePose.matrix() * zmpGlobal;
- vector<double> ZMPRef(3);
- for (unsigned int i = 0; i < 3; ++i) ZMPRef[i] = localZmp(i);
-
- controlOut["zmp"].setName("zmp");
- controlOut["zmp"].setValues(ZMPRef);
-
- // Update position of freeflyer in global frame
- Eigen::Vector3d transq_(freeFlyerPose().translation());
- dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
-
- // translation
- for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);
-
- // rotation: quaternion
- baseff_[3] = qt_.w();
- baseff_[4] = qt_.x();
- baseff_[5] = qt_.y();
- baseff_[6] = qt_.z();
- controlOut["baseff"].setValues(baseff_);
ODEBUG5FULL("end");
sotDEBUGOUT(25);
}
diff --git a/tests/impl_test_sot_mock_device.hh b/tests/impl_test_sot_mock_device.hh
index 6cc3b822655a3a5c2db7541daa8c60c5c0ddb7f1..8a8a328f0e0e20c20b00d456d39de52fa8c5e495 100644
--- a/tests/impl_test_sot_mock_device.hh
+++ b/tests/impl_test_sot_mock_device.hh
@@ -15,6 +15,7 @@
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal.h>
+#include <dynamic-graph/fwd.hh>
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/device.hh>
#include <sot/core/matrix-geometry.hh>
@@ -40,7 +41,8 @@ class ImplTestSotMockDevice : public dgsot::Device {
void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
- void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
+ void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
+ const double &period);
void timeStep(double ts) { timestep_ = ts; }
@@ -52,34 +54,34 @@ class ImplTestSotMockDevice : public dgsot::Device {
std::vector<double> baseff_;
/// Accelerations measured by accelerometers
- dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers
- dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> gyrometerSOUT_;
/// motor currents
- dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> currentsSOUT_;
/// joint angles
- dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> joint_anglesSOUT_;
/// motor angles
- dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> motor_anglesSOUT_;
/// proportional and derivative position-control gains
- dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> p_gainsSOUT_;
- dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
+ dynamicgraph::Signal<dg::Vector, dg::sigtime_t> d_gainsSOUT_;
/// Protected methods for internal variables filling
void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn,
- int t);
+ dg::sigtime_t t);
void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn,
- int t);
+ dg::sigtime_t t);
void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn,
- int t);
+ dg::sigtime_t t);
void setSensorsVelocities(
- std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
+ std::map<std::string, dgsot::SensorValues> &SensorsIn, dg::sigtime_t t);
void setSensorsTorquesCurrents(
- std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
+ std::map<std::string, dgsot::SensorValues> &SensorsIn, dg::sigtime_t t);
void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn,
- int t);
+ dg::sigtime_t t);
/// Intermediate variables to avoid allocation during control
dg::Vector dgforces_;