search

tork-a / rtmros_nextage

ROS-OpenRTM-based opensource robot controller software for dual-armed robot Nextage from Kawada Industries
http://wiki.ros.org/rtmros_nextage
27 stars 39 forks source link

[Major] checkEncoders gets stuck #308

Open 130s opened 7 years ago

130s commented 7 years ago 
In [1]: robot.checkEncoders()
[hrpsys.py]  calib-joint all 
[hrpsys.py]  done
Turn on Hand Servo

Stuck here and not the command doesn't return.

Full log.

$ rospack find hironx_ros_bridge
/opt/ros/indigo/share/hironx_ros_bridge
$ rosversion hironx_ros_bridge
1.1.22

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage --modelfile /opt/jsk/etc/HIRONX/model/main.wrl
Python 2.7.6 (default, Oct 26 2016, 20:30:19) 
Type "copyright", "credits" or "license" for more information.

IPython 1.2.1 -- An enhanced Interactive Python.
?         -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help      -> Python's own help system.
object?   -> Details about 'object', use 'object??' for extra details.
ROS Master not found running. If you intended to use it (either for rosbridge or for Gazebo purposes, make sure you run necessary nodes.
configuration ORB with nextage:15005
[hrpsys.py] waiting ModelLoader
[hrpsys.py] start hrpsys
[hrpsys.py] finding RTCManager and RobotHardware
[hrpsys.py]  waitForRTCManagerAndRoboHardware has renamed to waitForRTCManagerAndRoboHardware: Please update your code
[hrpsys.py] wait for RTCmanager : None
[hrpsys.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x7fd9420c7cf8> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7fd9420c7cf8> isActive? = False 
[hrpsys.py] simulation_mode : False
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x7fd9420c7ea8>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x7fd9420c7e60>
[hrpsys.py]    version  =  315.1.8
;;
;; Loading ImpedanceController < 315.1.9
;;
[hrpsys.py]   bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl
[hrpsys.py] no hrpsys components found running. creating now
[hrpsys.py]   eval : [self.seq, self.seq_svc, self.seq_version] = self.createComp("SequencePlayer","seq")
[hrpsys.py] create Comp -> SequencePlayer : <hrpsys.rtm.RTcomponent instance at 0x7fd9420c0680> (315.1.8)
[hrpsys.py] create CompSvc -> SequencePlayer Service : <OpenHRP._objref_SequencePlayerService instance at 0x7fd9420d1050>
[hrpsys.py]   eval : [self.sh, self.sh_svc, self.sh_version] = self.createComp("StateHolder","sh")
[hrpsys.py] create Comp -> StateHolder : <hrpsys.rtm.RTcomponent instance at 0x7fd9420b2878> (315.1.8)
[hrpsys.py] create CompSvc -> StateHolder Service : <OpenHRP._objref_StateHolderService instance at 0x7fd9420c87a0>
[hrpsys.py]   eval : [self.fk, self.fk_svc, self.fk_version] = self.createComp("ForwardKinematics","fk")
[hrpsys.py] create Comp -> ForwardKinematics : <hrpsys.rtm.RTcomponent instance at 0x7fd9420d1dd0> (315.1.8)
[hrpsys.py] create CompSvc -> ForwardKinematics Service : <OpenHRP._objref_ForwardKinematicsService instance at 0x7fd9420c0d88>
[hrpsys.py]   eval : [self.ic, self.ic_svc, self.ic_version] = self.createComp("ImpedanceController","ic")
[hrpsys.py] create Comp -> ImpedanceController : <hrpsys.rtm.RTcomponent instance at 0x7fd9420d4830> (315.1.8)
[hrpsys.py] create CompSvc -> ImpedanceController Service : <OpenHRP._objref_ImpedanceControllerService instance at 0x7fd9420c5368>
[hrpsys.py]   eval : [self.el, self.el_svc, self.el_version] = self.createComp("SoftErrorLimiter","el")
[hrpsys.py] create Comp -> SoftErrorLimiter : <hrpsys.rtm.RTcomponent instance at 0x7fd9420d1bd8> (315.1.8)
[hrpsys.py] create CompSvc -> SoftErrorLimiter Service : <OpenHRP._objref_SoftErrorLimiterService instance at 0x7fd9420bf320>
[hrpsys.py]   eval : [self.sc, self.sc_svc, self.sc_version] = self.createComp("ServoController","sc")
[hrpsys.py] create Comp -> ServoController : <hrpsys.rtm.RTcomponent instance at 0x7fd9420d4dd0> (315.1.8)
[hrpsys.py] create CompSvc -> ServoController Service : <OpenHRP._objref_ServoControllerService instance at 0x7fd9420d1680>
[hrpsys.py]   eval : [self.log, self.log_svc, self.log_version] = self.createComp("DataLogger","log")
[hrpsys.py] create Comp -> DataLogger : <hrpsys.rtm.RTcomponent instance at 0x7fd9420c0fc8> (315.1.8)
[hrpsys.py] create CompSvc -> DataLogger Service : <OpenHRP._objref_DataLoggerService instance at 0x7fd9420b2cb0>
[hrpsys.py]   eval : [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.createComp("AbsoluteForceSensor","rmfo")
[hrpsys.py] create Comp -> AbsoluteForceSensor : <hrpsys.rtm.RTcomponent instance at 0x7fd9420d41b8> (315.1.8)
[hrpsys.py] create CompSvc -> AbsoluteForceSensor Service : <OpenHRP._objref_AbsoluteForceSensorService instance at 0x7fd9420c5098>
[hrpsys.py] connecting hrpsys components
[rtm.py]    Connect sh.qOut - ic.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect ic.q - el.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect el.q - RobotHardware0.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.servoState - el.servoStateIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - sh.currentQIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - fk.q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.qOut - fk.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.qRef - sh.qIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.tqRef - sh.tqIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.basePos - sh.basePosIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.baseRpy - sh.baseRpyIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.zmpRef - sh.zmpIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.basePosOut - seq.basePosInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.basePosOut - fk.basePosRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.baseRpyOut - seq.baseRpyInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.baseRpyOut - fk.baseRpyRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.qOut - seq.qInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Failed to connect None to [None]([None])
[rtm.py]    Failed to connect None to [None]([None])
[rtm.py]    Failed to connect None to [None]([None])
[rtm.py]    Failed to connect None to [None]([None])
[rtm.py]    Connect RobotHardware0.q - rmfo.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.rhsensor - rmfo.rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect rmfo.off_rhsensor - ic.rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.lhsensor - rmfo.lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect rmfo.off_lhsensor - ic.lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - ic.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - el.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] activating hrpsys components
[hrpsys.py] setup hrpsys logger
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to RobotHardware0_q
[rtm.py]    Connect RobotHardware0.q - log.RobotHardware0_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = dq to RobotHardware0_dq
[rtm.py]    Connect RobotHardware0.dq - log.RobotHardware0_dq (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = tau to RobotHardware0_tau
[rtm.py]    Connect RobotHardware0.tau - log.RobotHardware0_tau (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] sensor names for DataLogger
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = rhsensor to RobotHardware0_rhsensor
[rtm.py]    Connect RobotHardware0.rhsensor - log.RobotHardware0_rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = lhsensor to RobotHardware0_lhsensor
[rtm.py]    Connect RobotHardware0.lhsensor - log.RobotHardware0_lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = qOut to sh_qOut
[rtm.py]    Connect sh.qOut - log.sh_qOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = tqOut to sh_tqOut
[rtm.py]    Connect sh.tqOut - log.sh_tqOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedPoint3D, name = basePosOut to sh_basePosOut
[rtm.py]    Connect sh.basePosOut - log.sh_basePosOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedOrientation3D, name = baseRpyOut to sh_baseRpyOut
[rtm.py]    Connect sh.baseRpyOut - log.sh_baseRpyOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedPoint3D, name = zmpOut to sh_zmpOut
[rtm.py]    Connect sh.zmpOut - log.sh_zmpOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to ic_q
[rtm.py]    Connect ic.q - log.ic_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to el_q
[rtm.py]    Connect el.q - log.el_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : emergencySignal arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.emergencySignal - log.emergencySignal (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedLongSeqSeq, name = servoState to RobotHardware0_servoState
[rtm.py]    Failed to connect RobotHardware0.servoState to None([None])
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = off_rhsensor to rmfo_off_rhsensor
[rtm.py]    Connect rmfo.off_rhsensor - log.rmfo_off_rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = off_lhsensor to rmfo_off_lhsensor
[rtm.py]    Connect rmfo.off_lhsensor - log.rmfo_off_lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] setup joint groups for hrpsys controller
[hrpsys.py] initialized successfully
---------------------------------------------------------------------------
ROSInitException                          Traceback (most recent call last)
/usr/lib/python2.7/dist-packages/IPython/utils/py3compat.pyc in execfile(fname, *where)
    202             else:
    203                 filename = fname
--> 204             __builtin__.execfile(filename, *where)

/opt/ros/indigo/share/nextage_ros_bridge/script/nextage.py in <module>()
     81 
     82     # ROS Client.
---> 83     ros = ROS_Client()
     84 
     85 # for simulated robot

/opt/ros/indigo/lib/python2.7/dist-packages/hironx_ros_bridge/ros_client.pyc in __init__(self, jointgroups)
     85             errormsg = 'No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. ' + \
     86                        'To use ROS, do not forget to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN'
---> 87             raise ROSInitException(errormsg)
     88         except Exception as e:
     89             errormsg = '[ros_client] Unknown exception occurred, so do not create ros client...'

ROSInitException: No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. To use ROS, do not forget to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN

In [1]: robot.checkEncoders()
[hrpsys.py]  calib-joint all 
[hrpsys.py]  done
Turn on Hand Servo

Stuck here.

Log from server side:

$ more /opt/jsk/var/log/rtcd.log
:
2:stageCalibration[12] = 255
2:stageCalibration[13] = 255
2:stageCalibration[14] = 255
readServoState . done.
Timeover: processing time = 11021.3[ms]
11021.3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,StateHolder::goActual()
  0.0, 
14:33:22.485103: servo error limit over: joint = RARM_JOINT2, qRef = -71.0155[deg], q = -129.10
7[deg]
[ServoSerial] sending : FA AF 02 00 24 01 01 01 27  - 9
130s commented 7 years ago

Temporary fix on the server-side is to not run ServoController RTC https://github.com/start-jsk/rtmros_hironx/issues/480

k-okada commented 7 years ago

may be you'd better to revert this -> https://github.com/tork-a/rtmros_nextage/pull/239

-- ◉ Kei Okada

2017-04-05 15:53 GMT+09:00 Isaac I.Y. Saito notifications@github.com:

Temporary fix on the server-side is to not run ServoController RTC start-jsk/rtmros_hironx#480 https://github.com/start-jsk/rtmros_hironx/issues/480

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/tork-a/rtmros_nextage/issues/308#issuecomment-291770950, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3ByUq0gQOnA81-LaCgLkHBjiJ2n8ks5rszpmgaJpZM4Mzzg_ .

k-okada commented 7 years ago

Or calling

[hrpsys.py] no hrpsys components found running. creating now

Is wrong, We need to fix this code not going into this node, may be checking all node is necessary, except servo controller or if we have first 3 RTC, we assume everything is installed. https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L356

130s commented 7 years ago

I will send a PR next week for this.