Monitor.cpp
Go to the documentation of this file.
1 #include <rtm/CorbaNaming.h>
2 #include "Monitor.h"
3 #include "hrpsys/util/OpenRTMUtil.h"
4 #include "GLscene.h"
5 
6 Monitor::Monitor(CORBA::ORB_var orb, const std::string &i_hostname,
7  int i_port, int i_interval, LogManager<TimedRobotState> *i_log) :
8  m_orb(orb),
9  m_rhCompName("RobotHardware0"),
10  m_shCompName("StateHolder0"),
11  m_log(i_log),
12  m_interval(i_interval)
13 {
14  char buf[128];
15  try {
16  sprintf(buf, "%s:%d", i_hostname.c_str(), i_port);
17  RTC::CorbaNaming naming(orb, buf);
18  m_naming = CosNaming::NamingContext::_duplicate(naming.getRootContext());
19  }catch (CORBA::SystemException& ex) {
20  std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << ex._rep_id() << " with " << buf << std::endl;
21  }catch (const std::string& error){
22  std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << error << " with " << buf << std::endl;
23  }catch (...){
24  std::cerr << "[monitor] Failed to initialize CORBA with " << buf << std::endl;
25  }
26 }
27 
28 bool Monitor::oneStep()
29 {
30  static long long loop = 0;
31  ThreadedObject::oneStep();
32 
33  // RobotHardwareService
34  if (CORBA::is_nil(m_rhService)){
35  try{
36  CosNaming::Name name;
37  name.length(1);
38  name[0].id = CORBA::string_dup(m_rhCompName.c_str());
39  name[0].kind = CORBA::string_dup("rtc");
40  CORBA::Object_var obj = m_naming->resolve(name);
41  RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
42  RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
43  for(CORBA::ULong i=0; i < eclist->length(); ++i){
44  eclist[i]->activate_component(rtc);
45  }
46  const char *ior = getServiceIOR(rtc, "RobotHardwareService");
47  m_rhService = OpenHRP::RobotHardwareService::_narrow(m_orb->string_to_object(ior));
48  }catch(...){
49  if ( (loop%(5*(1000/m_interval))) == 0 )
50  std::cerr << "[monitor] RobotHardwareService could not connect (" << m_rhCompName << ")" << std::endl;
51  }
52  } else {
53  if ( (loop%(5*(1000/m_interval))) == 0 )
54  std::cerr << "[monitor] RobotHardwareService is not found (" << m_rhCompName << ")" << std::endl;
55  }
56  // StateHolderService
57  if (CORBA::is_nil(m_shService)){
58  try{
59  CosNaming::Name name;
60  name.length(1);
61  name[0].id = CORBA::string_dup(m_shCompName.c_str());
62  name[0].kind = CORBA::string_dup("rtc");
63  CORBA::Object_var obj = m_naming->resolve(name);
64  RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
65  const char *ior = getServiceIOR(rtc, "StateHolderService");
66  m_shService = OpenHRP::StateHolderService::_narrow(m_orb->string_to_object(ior));
67  }catch(...){
68  if ( (loop%(5*(1000/m_interval))) == 0 )
69  std::cerr << "[monitor] StateHolderService could not connect (" << m_shCompName << ")" << std::endl;
70  }
71  }else{
72  if ( (loop%(5*(1000/m_interval))) == 0 )
73  std::cerr << "[monitor] StateHolderService is not found (" << m_shCompName << ")" << std::endl;
74  }
75 
76  bool stateUpdate = false;
77  if (!CORBA::is_nil(m_rhService)){
78  OpenHRP::RobotHardwareService::RobotState_var rs;
79  try{
80  m_rhService->getStatus(rs);
81  m_rstate.state = rs;
82  stateUpdate = true;
83  }catch(...){
84  std::cerr << "[monitor] exception in getStatus()" << std::endl;
85  m_rhService = NULL;
86  }
87  }
88 
89  if (!CORBA::is_nil(m_shService)){
90  OpenHRP::StateHolderService::Command_var com;
91  try{
92  m_shService->getCommand(com);
93  m_rstate.command = com;
94  stateUpdate = true;
95  }catch(...){
96  std::cerr << "[monitor] exception in getCommand()" << std::endl;
97  m_shService = NULL;
98  }
99  }
100 
101 
102  if (stateUpdate) {
103  struct timeval tv;
104  gettimeofday(&tv, NULL);
105  m_rstate.time = tv.tv_sec + tv.tv_usec/1e6;
106  m_log->add(m_rstate);
107  }
108  usleep(1000*m_interval);
109  loop ++;
110 
111  return true;
112 }
113 
114 bool Monitor::isConnected()
115 {
116  return !CORBA::is_nil(m_rhService);
117 }
118 
119 extern bool isCalibrated(int s);
120 extern bool isPowerOn(int s);
121 extern bool isServoOn(int s);
122 extern int servoAlarm(int s);
123 extern int temperature(int s);
124 
125 void Monitor::showStatus(hrp::BodyPtr &body)
126 {
127  if (m_log->index()<0) return;
128 
129  LogManager<TimedRobotState> *lm
130  = (LogManager<TimedRobotState> *)m_log;
131  OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;
132 
133  fprintf(stdout, "\e[1;1H"); // home
134  fprintf(stdout, "\x1b[42m"); // greep backgroupd
135 
136  fprintf(stdout, "\e[2KTimestamp %16.4f, elapsed time %8.4f\n", lm->state().time, lm->currentTime());
137  double curr_time, prev_time;
138  std::vector<double> curr_angle, prev_angle, velocity, curr_vel, prev_vel, curr_acc, acceleration;
139  int n = body->numJoints();
140  curr_angle.resize(n); prev_angle.resize(n); curr_vel.resize(n); prev_vel.resize(n); curr_acc.resize(n);
141  velocity.resize(n); acceleration.resize(n);
142  for(int i = 0; i < n; i++) { prev_angle[i] = lm->state().state.angle[i]; velocity[i] = acceleration[i] = curr_vel[i] = 0; }
143  prev_time = lm->state().time - 10; // dummy data
144  while(m_log->index()>0) {
145  curr_time = lm->state().time;
146  for(int i = 0; i < n; i++) {
147  curr_angle[i] = lm->state().state.angle[i];
148  curr_vel[i] = (curr_angle[i] - prev_angle[i])/(curr_time-prev_time);
149  curr_acc[i] = (curr_vel[i] - prev_vel[i])/(curr_time-prev_time);
150  if (fabs(velocity[i]) < fabs(curr_vel[i])) velocity[i] = curr_vel[i];
151  if (fabs(acceleration[i]) < fabs(curr_acc[i])) acceleration[i] = curr_acc[i];
152  }
153  m_log->prev(1);
154  for(int i = 0; i < n; i++) { prev_angle[i] = curr_angle[i]; prev_vel[i] = curr_vel[i];}
155  prev_time = curr_time;
156  }
157  m_log->tail();
158  fprintf(stdout, "\e[2KID PW NAME ANGLE COMMAND ERROR VELOCITY ACCEL. TORQUE SERVO TEMP\n"); // greep backgroupd
159  char buf[256];
160  for (unsigned int i=0; i<body->numJoints(); i++){
161  hrp::Link *l = body->joint(i);
162  if (l){
163  fprintf(stdout,"\e[2K");
164  int ss = rstate.servoState[i][0];
165  // joint ID
166  if (!isCalibrated(ss)){
167  yellow();
168  }else if(isServoOn(ss)){
169  red();
170  }
171  fprintf(stdout, "%2d ",i);
172  black();
173  // power status
174  if (isPowerOn(ss)) blue();
175  fprintf(stdout, " o ");
176  if (isPowerOn(ss)) black();
177  // joint name, current angle, command angle and torque
178  fprintf(stdout, "%20s ", l->name.c_str());
179  // angle
180  if( i<rstate.angle.length() )
181  fprintf(stdout, "%8.3f ", rstate.angle[i]*180/M_PI);
182  else
183  fprintf(stdout, "-------- ");
184  // command
185  if( i<rstate.command.length() )
186  fprintf(stdout, "%8.3f ", rstate.command[i]*180/M_PI);
187  else
188  fprintf(stdout, "-------- ");
189 
190  // error
191  if( i<rstate.angle.length() && i<rstate.command.length() ){
192  double e = (rstate.angle[i]-rstate.command[i])*180/M_PI;
193  if ( abs(e) > 1 ) yellow();
194  if ( abs(e) > 2 ) magenta();
195  if ( abs(e) > 4 ) red();
196  fprintf(stdout, "%8.3f ", e);
197  black();
198  }else{
199  fprintf(stdout, "-------- ");
200  }
201  // velocity
202  if( i<velocity.size() ) {
203  double e = velocity[i]; //*180/M_PI;
204  if ( abs(e) > 2 ) yellow();
205  if ( abs(e) > 10 ) magenta();
206  if ( abs(e) > 20 ) red();
207  fprintf(stdout, "%8.2f ", e);
208  black();
209  }else{
210  fprintf(stdout, "-------- ");
211  }
212  // accleration
213  if( i<acceleration.size() ) {
214  double e = acceleration[i]; //*180/M_PI;
215  if ( abs(e) > 50 ) yellow();
216  if ( abs(e) > 100 ) magenta();
217  if ( abs(e) > 200 ) red();
218  fprintf(stdout, "%8.1f ", e);
219  black();
220  }else{
221  fprintf(stdout, "-------- ");
222  }
223 
224  // torque
225  if( i<rstate.torque.length() )
226  fprintf(stdout, "%6.1f ", rstate.torque[i]*180/M_PI);
227  else
228  fprintf(stdout, "------ ");
229  // servo alarms
230  fprintf(stdout, "%03x ", servoAlarm(ss));
231  // driver temperature
232  int temp = temperature(ss);
233  if (!temp){
234  fprintf(stdout, "-- ");
235  }else{
236  if (temp >= 60) red();
237  fprintf(stdout, "%2d ", temp);
238  if (temp >= 60) black();
239  }
240  fprintf(stdout, "\n");
241  }
242  }
243  fprintf(stdout, "\e[2K---\n");
244 
245  if (rstate.accel.length()){
246  fprintf(stdout, "\e[2K acc:");
247  for (unsigned int i=0; i<rstate.accel.length(); i++){
248  if(i>0)fprintf(stdout, "\e[2K ");
249  fprintf(stdout, " %8.4f %8.4f %8.4f\n",
250  rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
251  }
252  }
253  if (rstate.rateGyro.length()){
254  fprintf(stdout, "\e[2K rate:");
255  for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
256  if(i>0)fprintf(stdout, "\e[2K ");
257  fprintf(stdout, " %8.4f %8.4f %8.4f\n",
258  rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
259  }
260  }
261  if (rstate.force.length()){
262  fprintf(stdout, "\e[2Kforce/torque:");
263  for (unsigned int i=0; i<rstate.force.length(); i++){
264  if(i>0)fprintf(stdout, "\e[2K ");
265  fprintf(stdout, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f\n",
266  rstate.force[i][0],
267  rstate.force[i][1],
268  rstate.force[i][2],
269  rstate.force[i][3],
270  rstate.force[i][4],
271  rstate.force[i][5]);
272  }
273  }
274  fprintf(stdout, "\e[2K\n");
275 }
276 
277 
278 void Monitor::setRobotHardwareName(const char *i_name)
279 {
280  m_rhCompName = i_name;
281 }
282 
283 void Monitor::setStateHolderName(const char *i_name)
284 {
285  m_shCompName = i_name;
286 }
Monitor::m_orb
CORBA::ORB_var m_orb
Definition: Monitor.h:19
ior
ior
Monitor::Monitor
Monitor(CORBA::ORB_var orb, const std::string &i_hostname, int i_port, int i_interval, LogManager< TimedRobotState > *i_log)
Definition: Monitor.cpp:6
Monitor::m_rhService
OpenHRP::RobotHardwareService_var m_rhService
Definition: Monitor.h:22
name
png_infop png_charpp name
isPowerOn
bool isPowerOn(int s)
Definition: util/monitor/GLscene.cpp:54
error
void error(char *msg) const
isCalibrated
bool isCalibrated(int s)
Definition: util/monitor/GLscene.cpp:51
temperature
int temperature(int s)
Definition: util/monitor/GLscene.cpp:63
i
png_uint_32 i
Monitor::isConnected
bool isConnected()
Definition: Monitor.cpp:114
Monitor::m_shCompName
std::string m_shCompName
Definition: Monitor.h:21
getServiceIOR
const char * getServiceIOR(RTC::RTObject_var rtc, const char *sname)
Definition: OpenRTMUtil.cpp:65
TimedRobotState::state
OpenHRP::RobotHardwareService::RobotState state
Definition: TimedRobotState.h:11
boost::shared_ptr< Body >
Monitor::yellow
void yellow()
Definition: Monitor.h:30
Monitor::setStateHolderName
void setStateHolderName(const char *i_name)
Definition: Monitor.cpp:283
gettimeofday
int gettimeofday(struct timeval *tv, struct timezone *tz)
Monitor::showStatus
void showStatus(hrp::BodyPtr &body)
Definition: Monitor.cpp:125
ThreadedObject::oneStep
virtual bool oneStep()
Definition: ThreadedObject.cpp:40
Monitor::m_rhCompName
std::string m_rhCompName
Definition: Monitor.h:21
servoAlarm
int servoAlarm(int s)
Definition: util/monitor/GLscene.cpp:60
Monitor::oneStep
bool oneStep()
Definition: Monitor.cpp:28
isServoOn
bool isServoOn(int s)
Definition: util/monitor/GLscene.cpp:57
Monitor::m_shService
OpenHRP::StateHolderService_var m_shService
Definition: Monitor.h:23
Monitor::m_naming
CosNaming::NamingContext_var m_naming
Definition: Monitor.h:20
LogManager::add
void add(const T &state)
Definition: LogManager.h:17
Monitor::blue
void blue()
Definition: Monitor.h:32
Monitor::red
void red()
Definition: Monitor.h:29
n
n
Monitor::black
void black()
Definition: Monitor.h:34
buf
png_bytep buf
LogManager::index
int index()
Definition: LogManager.h:63
Monitor::m_log
LogManager< TimedRobotState > * m_log
Definition: Monitor.h:25
LogManager::state
T & state()
Definition: LogManager.h:140
naming
naming
M_PI
#define M_PI
TimedRobotState::command
OpenHRP::StateHolderService::Command command
Definition: TimedRobotState.h:10
sprintf
std::string sprintf(char const *__restrict fmt,...)
Monitor::m_interval
int m_interval
Definition: Monitor.h:26
Monitor::m_rstate
TimedRobotState m_rstate
Definition: Monitor.h:24
LogManager::currentTime
double currentTime()
Definition: LogManager.h:55
LogManager::prev
void prev(int delta=1)
Definition: LogManager.h:34
Monitor::magenta
void magenta()
Definition: Monitor.h:33
obj
obj
Monitor::setRobotHardwareName
void setRobotHardwareName(const char *i_name)
Definition: Monitor.cpp:278
usleep
int usleep(useconds_t usec)
LogManager::tail
void tail()
Definition: LogManager.h:46

hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50