[TwoDofController] Add instnace_name to error message

snozawa · Jul 23, 2015 · 1e2d92e · 1e2d92e
1 parent cefcacd
commit 1e2d92e
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 1 deletion.
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
@@ -283,8 +283,11 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
  double ke = 0, tc = 0;
  for (int i = 0; i < ST_NUM_LEGS; i++) {
  m_tau_x[i].setup(ke, tc, dt);
+ m_tau_x[i].setErrorPrefix(std::string(m_profile.instance_name));
  m_tau_y[i].setup(ke, tc, dt);
+ m_tau_y[i].setErrorPrefix(std::string(m_profile.instance_name));
  m_f_z.setup(ke, tc, dt);
+ m_f_z.setErrorPrefix(std::string(m_profile.instance_name));
  }
  pangx_ref = pangy_ref = pangx = pangy = 0;
  rdx = rdy = rx = ry = 0;

diff --git a/rtc/Stabilizer/TwoDofController.cpp b/rtc/Stabilizer/TwoDofController.cpp
@@ -15,12 +15,14 @@ TwoDofController::TwoDofController() {
  param = TwoDofController::TwoDofControllerParam(); // use default constructor
  integrator = Integrator(0.0, 0.0);
  integrator.reset();
+ error_prefix = "";
 }
 
 TwoDofController::TwoDofController(TwoDofController::TwoDofControllerParam &_param, unsigned int _range) {
  param.ke = _param.ke; param.tc = _param.tc; param.dt = _param.dt;
  integrator = Integrator(_param.dt, _range);
  integrator.reset();
+ error_prefix = "";
 }
 
 TwoDofController::~TwoDofController() {
@@ -63,7 +65,7 @@ double TwoDofController::update (double _x, double _xd) {
 
  // check parameters
  if (!param.ke || !param.tc || !param.dt){
- std::cerr << "ERROR: parameters are not set." << std::endl;
+ std::cerr << "[" << error_prefix << "]" << "TwoDofController parameters are not set." << std::endl;
  return 0;
  }
 
@@ -78,12 +80,16 @@ double TwoDofController::update (double _x, double _xd) {
 
 }
 
+void TwoDofController::setErrorPrefix(const std::string& _error_prefix) {
+ error_prefix = _error_prefix;
+}
 
 // for compatiblity of Stabilizer 
 TwoDofController::TwoDofController(double _ke, double _tc, double _dt, unsigned int _range) {
  param.ke = _ke; param.tc = _tc; param.dt = _dt;
  integrator = Integrator(_dt, _range);
  integrator.reset();
+ error_prefix = "";
 }
 
 void TwoDofController::setup(double _ke, double _tc, double _dt, unsigned int _range) {

diff --git a/rtc/Stabilizer/TwoDofController.h b/rtc/Stabilizer/TwoDofController.h
@@ -13,6 +13,7 @@
 // </rtc-template>
 
 #include "Integrator.h"
+#include <string>
 
 // interface class for TwoDofController
 class TwoDofControllerInterface {
@@ -50,6 +51,7 @@ class TwoDofController : public TwoDofControllerInterface {
  double update(double _x, double _xd);
  bool getParameter();
  bool getParameter(TwoDofControllerParam &_p);
+ void setErrorPrefix(const std::string& _error_prefix);
 
  // for compatibility of Stabilizer. TODO: replace to new parameter argument
  TwoDofController(double _ke, double _tc, double _dt, unsigned int _range = 0);
@@ -58,6 +60,7 @@ class TwoDofController : public TwoDofControllerInterface {
 private:
  TwoDofControllerParam param;
  Integrator integrator; // integrated (xd - x)
+ std::string error_prefix;
 };
 
 #endif // TWO_DOF_CONTROLLER_H