Update time from int to dg::sigtime_t

Author: olivier-stasse

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;

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_;

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;
 

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;

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;

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_;

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,15 +286,14 @@ 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;
   anglesOut.resize(state_.size());
   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);
 }

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_;