MoveIt! stop feature not working for NXO

[130s]

Stop button on RViz on MoveIt! plugin does not stop NEXTAGE for me.

[hectorherrero]

I just tried it and it does not work for me either. But I have not tried to send cancel signal to corresponding topic. This means that you can not stop a movement or it is problem of the button of the graphical interface?

[130s]

But I have not tried to send cancel signal to corresponding topic.

I'm not sure what you're trying to say :)

It DOES work for Pepper on simulator as the video referenced from the link I shared.

[hectorherrero]

It does work for UR10 robot in simulation.

But I have not tried to send cancel signal to corresponding topic.

Sorry, I'm trying to say that I had not try publishing in /larm_controller/follow_joint_trajectory_action/cancel to preempt the action. Today I have tried this (Nextage in simulation) with the same result, when the GoalID is published in this topic nothing happens.

Instead, I have tested the same with UR10 robot (in this case publishing in /arm_controller/follow_joint_trayectory/cancel and the action is preempted. When the Stop button is pressed the action is preempted and robot stops.

[130s]

Using simulation, I confirmed that joint_trajectory_server->setPreempted() in this line is likely called as far as I look at the output below, which got printed when I press Stop button on RViz (that publishes /X_controller/follow_joint_trajectory_action/cancel). I took a look at the code of hrpsys_ros_bridge::HrpsysJointTrajectoryBridge for awhile but I can't track further.

[ INFO] [1484784142.051105836, 21.919999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1768 Hz, dropped 201)
[ INFO] [1484784142.239635544, 22.114999999]: [HrpsysJointTrajectoryBridge0] @onFollowJointTrajectoryActionPreempt()

[k-okada]

you have to merge start-jsk/rtmros_common#765 to enable this feature.

[130s]

I confirmed on simulation that with start-jsk/rtmros_common#765 the stop feature works as expected.

Testing on real robots (ie. not just on simulation) is requested before that change to be merged start-jsk/rtmros_common#765 (comment), which doesn't seem to have happened since the date the pull request was opened back in 2015. If anyone, including @hectorherrero, is interested in testing this feature, we would appreciate and are gladly going to assist you.

You can apply the change by like this for example.

mkdir -p cws_rtm/src
cd cws_rtm/src
git clone https://github.com/k-okada/rtmros_common.git
cd rtmros_common/
git checkout add_cancel 
git rebase origin/master 
cd ../..
catkin config --no-install
catkin b
source devel/setup.bash

(SIM) rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch
(REAL) roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch  (Add arguments you usually use)

[hectorherrero]

We are very interested on testing this with the real robot. I will try to test this in the next weeks but I do not know if this feature requires a specific version of hrpsys/bridge on the QNX controller. Is there a version requirement on this side?

[130s]

Thank you for your considerations @hectorherrero !
I doubt any higher hrpsys version is required than most of NEXTAGE users use (I also left a comment at start-jsk/rtmros_common#765 (comment) so hopefully I'll get any correction if anything). Let us know if any more questions and thoughts.

UPDATE: This feature probably requires hrpsys 315.5.0 or higher. See #284 (comment)

[hectorherrero]

I have followed the steps you provide above, but trajectory execution does not work. I receive the following output:

HrpsysJointTrajectoryBridge: /usr/include/boost/thread/pthread/recursive_mutex.hpp:115: void boost::recursive_mutex::unlock(): Assertion `!pthread_mutex_unlock(&m)' failed.
[HrpsysJointTrajectoryBridge-4] process has died [pid 31411, exit code -6, cmd /home/hherrero/ros/indigo/catkin_ws/devel/lib/hrpsys_ros_bridge/HrpsysJointTrajectoryBridge -o corba.nameservers:hiro019:15005 -o naming.formats:%n.rtc -o exec_cxt.periodic.type:PeriodicExecutionContext -o exec_cxt.periodic.rate:2000 -o logger.file_name:/tmp/rtc%p.log -o model:/opt/jsk/etc/NEXTAGE/model/main.wrl __name:=HrpsysJointTrajectoryBridge __log:=/home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/HrpsysJointTrajectoryBridge-4.log].
log file: /home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/HrpsysJointTrajectoryBridge-4*.log
Traceback (most recent call last):
  File "/home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/scripts/rtmlaunch.py", line 13, in <module>
    rtmlaunch.main()
  File "/home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/src/openrtm_tools/rtmlaunch.py", line 253, in main
    tree.add_name_server(nameserver, [])
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/tree.py", line 131, in add_name_server
    self._parse_name_server(server, filter, dynamic=dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/tree.py", line 285, in _parse_name_server
    trim_filter(deepcopy(filter), 2), dynamic=dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/nameserver.py", line 54, in __init__
    self._parse_server(address, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/nameserver.py", line 79, in _parse_server
    self._parse_context(root_context, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/directory.py", line 111, in _parse_context
    self._process_binding(binding, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/directory.py", line 165, in _process_binding
    leaf = Component(name, self, obj, dynamic=self.dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/component.py", line 94, in __init__
    self._parse_profile()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/component.py", line 1088, in _parse_profile
    profile = self._obj.get_component_profile()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/rtmidl/RTC_idl.py", line 1344, in get_component_profile
    return _omnipy.invoke(self, "get_component_profile", _0_RTC.RTObject._d_get_component_profile, args)
omniORB.CORBA.COMM_FAILURE: CORBA.COMM_FAILURE(omniORB.COMM_FAILURE_WaitingForReply, CORBA.COMPLETED_MAYBE)
[rtmlaunch_collision_detector-16] process has died [pid 31593, exit code 1, cmd /home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/scripts/rtmlaunch.py /home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/hrpsys_ros_bridge/launch/collision_detector.launch __name:=rtmlaunch_collision_detector __log:=/home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/rtmlaunch_collision_detector-16.log].
log file: /home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/rtmlaunch_collision_detector-16*.log
Traceback (most recent call last):
  File "/home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/scripts/rtmlaunch.py", line 13, in <module>
    rtmlaunch.main()
  File "/home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/src/openrtm_tools/rtmlaunch.py", line 253, in main
    tree.add_name_server(nameserver, [])
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/tree.py", line 131, in add_name_server
    self._parse_name_server(server, filter, dynamic=dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/tree.py", line 285, in _parse_name_server
    trim_filter(deepcopy(filter), 2), dynamic=dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/nameserver.py", line 54, in __init__
    self._parse_server(address, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/nameserver.py", line 79, in _parse_server
    self._parse_context(root_context, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/directory.py", line 111, in _parse_context
    self._process_binding(binding, orb, filter)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/directory.py", line 165, in _process_binding
    leaf = Component(name, self, obj, dynamic=self.dynamic)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/component.py", line 94, in __init__
    self._parse_profile()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/component.py", line 1088, in _parse_profile
    profile = self._obj.get_component_profile()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rtctree/rtmidl/RTC_idl.py", line 1344, in get_component_profile
    return _omnipy.invoke(self, "get_component_profile", _0_RTC.RTObject._d_get_component_profile, args)
omniORB.CORBA.COMM_FAILURE: CORBA.COMM_FAILURE(omniORB.COMM_FAILURE_WaitingForReply, CORBA.COMPLETED_MAYBE)
[rtmlaunch_hrpsys_ros_bridge-9] process has died [pid 31477, exit code 1, cmd /home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/openrtm_tools/scripts/rtmlaunch.py /home/hherrero/ros/indigo/catkin_ws/src/flexbotics/flexbotics_external_libraries/rtmros_common/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch USE_COMMON=true USE_WALKING=false USE_COLLISIONCHECK=false USE_IMPEDANCECONTROLLER=false USE_SOFTERRORLIMIT=false USE_IMAGESENSOR=false USE_ROBOTHARDWARE=true USE_GRASPCONTROLLER=false USE_SERVOCONTROLLER=false USE_TORQUECONTROLLER=false USE_TORQUEFILTER=false USE_EMERGENCYSTOPPER=false USE_REFERENCEFORCEUPDATER=false USE_VELOCITY_OUTPUT=false __name:=rtmlaunch_hrpsys_ros_bridge __log:=/home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/rtmlaunch_hrpsys_ros_bridge-9.log].
log file: /home/hherrero/.ros/log/772bf8d6-e48e-11e6-ae5d-34e6d75b5b49/rtmlaunch_hrpsys_ros_bridge-9*.log
[ERROR] [1485521291.659423386]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 14.008659 seconds). Stopping trajectory.

The content of the HrpsysJointTrajectoryBridge log file (the others are empty)

�[0m[ INFO] [1485521128.896052126]: [HrpsysJointTrajectoryBridge] @Initilize name : HrpsysJointTrajectoryBridge0 done�[0m
�[0m[ INFO] [1485521130.421437118]: ADD_GROUP: larm (/larm_controller)�[0m
�[0m[ INFO] [1485521130.421485928]:     JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5�[0m
�[0m[ INFO] [1485521130.514770407]: ADD_GROUP: rarm (/rarm_controller)�[0m
�[0m[ INFO] [1485521130.515057344]:     JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5�[0m
�[0m[ INFO] [1485521130.569542114]: ADD_GROUP: head (/head_controller)�[0m
�[0m[ INFO] [1485521130.569569042]:     JOINT: HEAD_JOINT0 HEAD_JOINT1�[0m
�[0m[ INFO] [1485521130.628233400]: ADD_GROUP: torso (/torso_controller)�[0m
�[0m[ INFO] [1485521130.628262976]:     JOINT: CHEST_JOINT0�[0m
�[0m[ INFO] [1485521130.688769703]: ADD_GROUP: larm_torso (/larm_torso_controller)�[0m
�[0m[ INFO] [1485521130.688818727]:     JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5 CHEST_JOINT0�[0m
�[0m[ INFO] [1485521130.750269476]: ADD_GROUP: rarm_torso (/rarm_torso_controller)�[0m
�[0m[ INFO] [1485521130.750309643]:     JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5 CHEST_JOINT0�[0m
�[0m[ INFO] [1485521277.652357630]: [HrpsysJointTrajectoryBridge0] @onFollowJointTrajectoryActionGoal()�[0m
�[0m[ INFO] [1485521277.652442858]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm)�[0m
�[0m[ INFO] [1485521277.652484634]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : trajectory.points.size() 10�[0m
�[0m[ INFO] [1485521277.652537086]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 0�[0m
�[0m[ INFO] [1485521277.652556048]: [HrpsysJointTrajectoryBridge0]  0.00165806 0.00329119 -2.26264 -0.267718 -0.00556324 -2.28313e-05�[0m
�[0m[ INFO] [1485521277.652582947]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 1.36451�[0m
�[0m[ INFO] [1485521277.652598728]: [HrpsysJointTrajectoryBridge0]  -0.00874064 -0.0649344 -2.22566 -0.275794 0.0263566 -0.0026458�[0m
�[0m[ INFO] [1485521277.652622723]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 2.72902�[0m
�[0m[ INFO] [1485521277.652636618]: [HrpsysJointTrajectoryBridge0]  -0.0191393 -0.13316 -2.18867 -0.283871 0.0582764 -0.00526877�[0m
�[0m[ INFO] [1485521277.652661556]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 4.09353�[0m
�[0m[ INFO] [1485521277.652674919]: [HrpsysJointTrajectoryBridge0]  -0.029538 -0.201385 -2.15169 -0.291947 0.0901963 -0.00789173�[0m
�[0m[ INFO] [1485521277.652698547]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 5.45804�[0m
�[0m[ INFO] [1485521277.652713116]: [HrpsysJointTrajectoryBridge0]  -0.0399367 -0.269611 -2.1147 -0.300024 0.122116 -0.0105147�[0m
�[0m[ INFO] [1485521277.652742859]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 6.82255�[0m
�[0m[ INFO] [1485521277.652759873]: [HrpsysJointTrajectoryBridge0]  -0.0503354 -0.337837 -2.07772 -0.308101 0.154036 -0.0131377�[0m
�[0m[ INFO] [1485521277.652784024]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 8.18707�[0m
�[0m[ INFO] [1485521277.652798427]: [HrpsysJointTrajectoryBridge0]  -0.0607341 -0.406062 -2.04073 -0.316177 0.185956 -0.0157606�[0m
�[0m[ INFO] [1485521277.652821269]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 9.55158�[0m
�[0m[ INFO] [1485521277.652835139]: [HrpsysJointTrajectoryBridge0]  -0.0711328 -0.474288 -2.00375 -0.324254 0.217876 -0.0183836�[0m
�[0m[ INFO] [1485521277.652865144]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 10.9161�[0m
�[0m[ INFO] [1485521277.652879787]: [HrpsysJointTrajectoryBridge0]  -0.0815315 -0.542513 -1.96676 -0.33233 0.249795 -0.0210066�[0m
�[0m[ INFO] [1485521277.652903499]: [HrpsysJointTrajectoryBridge0] @onJointTrajectoryAction (larm) : time_from_start 12.2806�[0m
�[0m[ INFO] [1485521277.652916969]: [HrpsysJointTrajectoryBridge0]  -0.0919302 -0.610739 -1.92978 -0.340407 0.281715 -0.0236295�[0m
�[0m[ INFO] [1485521277.652957157]: [HrpsysJointTrajectoryBridge0] setJointAnglesSequenceOfGroup: larm�[0m

[longjie]

Hi, this is just a report.

I have same problem when I follow @130s 's instruction. MoveIt can plan the trajectory, but fail to execute.

My error message on MoveIt! is different from @hectorherrero 's.

[ INFO] [1485742088.360059730]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[New Thread 0x7fff95ffb700 (LWP 5640)]
[ INFO] [1485742088.362793826]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1485742088.362888534]: Planner configuration 'right_arm[RRTstarkConfigDefault]' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1485742088.363134817]: right_arm[RRTstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1485742088.363266362]: right_arm[RRTstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1485742088.363311476]: right_arm[RRTstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1485742088.363351749]: right_arm[RRTstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1485742088.363408172]: right_arm[RRTstarkConfigDefault]: problem definition is not set, deferring setup completion...
[New Thread 0x7fff957fa700 (LWP 5641)]
[New Thread 0x7fff94ff9700 (LWP 5642)]
[New Thread 0x7fff7ffff700 (LWP 5643)]
[New Thread 0x7fff87fff700 (LWP 5644)]
[ INFO] [1485742088.365402232]: right_arm[RRTstarkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1485742088.365454303]: right_arm[RRTstarkConfigDefault]: Initial k-nearest value of 3
[ INFO] [1485742088.365535566]: right_arm[RRTstarkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1485742088.365569799]: right_arm[RRTstarkConfigDefault]: Initial k-nearest value of 3
[ INFO] [1485742088.370392400]: right_arm[RRTstarkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1485742088.370449653]: right_arm[RRTstarkConfigDefault]: Initial k-nearest value of 3
[ INFO] [1485742088.371364324]: right_arm[RRTstarkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1485742088.371422988]: right_arm[RRTstarkConfigDefault]: Initial k-nearest value of 3
[Thread 0x7fff95ffb700 (LWP 5640) exited]
[ INFO] [1485742093.363647276]: right_arm[RRTstarkConfigDefault]: Created 1614 new states. Checked 33545 rewire options. 10 goal states in tree.
[Thread 0x7fff87fff700 (LWP 5644) exited]
[ INFO] [1485742093.365693105]: right_arm[RRTstarkConfigDefault]: Created 1580 new states. Checked 32729 rewire options. 10 goal states in tree.
[Thread 0x7fff94ff9700 (LWP 5642) exited]
[ INFO] [1485742093.366360472]: right_arm[RRTstarkConfigDefault]: Created 1462 new states. Checked 29897 rewire options. 10 goal states in tree.
[Thread 0x7fff7ffff700 (LWP 5643) exited]
[ INFO] [1485742093.368269835]: right_arm[RRTstarkConfigDefault]: Created 1597 new states. Checked 33137 rewire options. 10 goal states in tree.
[Thread 0x7fff957fa700 (LWP 5641) exited]
[ INFO] [1485742093.368935560]: ParallelPlan::solve(): Solution found by one or more threads in 5.005487 seconds
[ INFO] [1485742100.720362921]: Received new trajectory execution service request...
[New Thread 0x7fff95ffb700 (LWP 5646)]
[New Thread 0x7fff87fff700 (LWP 5647)]
[ERROR] [1485742102.722675151]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 2.001344 seconds). Stopping trajectory.
[ INFO] [1485742102.722756087]: MoveitSimpleControllerManager: Cancelling execution for rarm_controller
[Thread 0x7fff95ffb700 (LWP 5646) exited]
[ INFO] [1485742102.723601042]: Execution completed: TIMED_OUT

Using release package of rtmros_common, the robot can execute trajectories.

Update: I confirmed it works fine in the simulator.

[longjie]

As mentioned in start-jsk/rtmros_common#765 by @k-okada , hrpsys-base 315.1.9 and 315.2.8 don't have idl for the function 'setJointAnglesSequenceOfGroup'. I guess it is the reason why the real robot cannot execute trajectories. Is it correct? I don't know exact version of hrpsys in my NXO though.

[130s]

Sorry, I have to take back my comment #284 (comment)

As I left a comment start-jsk/rtmros_common#765 (comment) I think hrpsys needs to be higher than 315.5.0. AFAIK, there's no NEXTAGE Open out there that uses hrpsys higher than that (the highest to my knowledge is 315.2.8. Work is in progress to test higher version but not successful yet start-jsk/rtmros_hironx#481).

[longjie]

OK, I'm clear 😄

[k-okada]

clearGroup clearOfGroup could be used instaed of claerJointAngles in old hrpsys

I have confirmed as follows.

1. run old hrpsys, need to copy package.xml from somewhere and run catkin b, (or you can look https://github.com/fkanehiro/hrpsys-base/blob/master/.travis.sh#L285-L293)

 $ wstool info
workspace: /home/k-okada/catkin_ws/ws_hrpsys_1.10

 Localname       S SCM Version (Spec)  UID  (Spec)  URI  (Spec) [http(s)://...]
 ---------       - --- --------------  -----------  ---------------------------
 src/hrpsys-base M git <detached>  (-) 432cfc6ebb67 git://github.com/fkanehiro/hrpsys-base.git
$ wstool diff
diff --git src/hrpsys-base/CMakeLists.txt src/hrpsys-base/CMakeLists.txt
index d90584a..9c4b4d8 100644
--- src/hrpsys-base/CMakeLists.txt
+++ src/hrpsys-base/CMakeLists.txt
@@ -49,7 +49,7 @@ if(ENABLE_INSTALL_RPATH)
   set(CMAKE_INSTALL_RPATH "$ORIGIN/../lib:$ORIGIN/../../../lib:${OPENHRP_LIBRARY_DIRS}")
 endif()
 
-option(COMPILE_JAVA_STUFF "Compile java stuff" ON)
+option(COMPILE_JAVA_STUFF "Compile java stuff" OFF)
 if(COMPILE_JAVA_STUFF)
   set(JAVAC javac)
   set(JAR jar)
diff --git src/hrpsys-base/lib/util/CMakeLists.txt src/hrpsys-base/lib/util/CMakeLists.txt
index 6b159a1..7c638cb 100644
--- src/hrpsys-base/lib/util/CMakeLists.txt
+++ src/hrpsys-base/lib/util/CMakeLists.txt
@@ -53,8 +53,8 @@ target_link_libraries(hrpsysUtil
   ${GLUT_LIBRARIES} 
   ${QHULL_LIBRARIES}
   GLEW
-  boost_thread-mt
-  boost_system-mt
+  boost_thread
+  boost_system
   )
 
 set(target hrpsysUtil)
diff --git src/hrpsys-base/util/simulator/CMakeLists.txt src/hrpsys-base/util/simulator/CMakeLists.txt
index 5d4fd23..efad6e1 100644
--- src/hrpsys-base/util/simulator/CMakeLists.txt
+++ src/hrpsys-base/util/simulator/CMakeLists.txt
@@ -40,7 +40,7 @@ if (NOT APPLE)
      )
 else()
    target_link_libraries(hrpsysext 
-     boost_python-mt
+     boost_python
      hrpsysUtil
      ${PYTHON_LIBRARIES}
      )
$  (cd src/hrpsys-base;  ln -sf ../../../devel/bin/hrpsys-simulator .)
$ (cd src/hrpsys-base;  ln -sf ../../../devel/lib/ ./lib)
$ catkin b
$ rtmlaunch nextage_startup.launch corbaport:=15005

2. send command as follows

In [5]: nxc.playPatternOfGroup('rarm',  [[-1,-1,-1.0,0,0,0]], [5])
Out[5]: True

In [6]: nxc.clearOfGroup('rarm')

In [7]: nxc.playPatternOfGroup('rarm',  [[0,0,-1.9,0,0,0]], [5])
Out[7]: True

In [8]: nxc.clearOfGroup('rarm')

[k-okada]

but real problem is not that clear... function is not supported on old idl, but trajectory execution does not work,
I should read #284 (comment) more carefully, and we can fix this.

I have followed the steps you provide above, but trajectory execution does not work. I receive the following output:

[k-okada]

upated, start-jsk/rtmros_common#765
TODO: send current after clear.

[k-okada]

ok, created new PR for this -> start-jsk/rtmros_common#996,
for those who have real robot please try with this new PR, for those who does not have real robot, please check with both latest hrpsys and old hrpsys (see #284 (comment) for instruction)

[longjie]

Hi, thank you for your effort.

I tried it with the simulator. I have confirmed your branch code can interrupt the motion successfully, while the old (I mean, apt got) system cannot. I will try it with the real robot on my lab soon.

Below is just a report.
I tried your branch code as follows:

$ pwd
~/Codes/ros/nextage_ws/src/rtmros_common$
$ git branch
* fix_joint_trajectory_action
  master
$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch 
(Running simulator)

(Another terminal)
$ roslaunch nextage_moveit_config moveit_planning_execution.launch 

And push rviz 'Stop' button while the motion is executed, the robot stopped immediately.

Now on a new terminal to try with the old (apt got) system,

$ . /opt/ros/indigo/setup.bash
$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch 
(Running simulator)

(Another terminal)
$ roslaunch nextage_moveit_config moveit_planning_execution.launch 

And we cannot stop the motion by pushing the 'Stop' button.

[longjie]

Hi, I have confirmed the stop feature with the real robot (Nextage Open).
The robot stops when the 'Stop' button pushed.
I have tried it several times and it seems working perfectly. Awesome.

I simply done:

• clone rtmros_common from @k-okada PR and build it on the NXO pc
• launch nextage_ros_bridge on the NXO pc (I used a customized launch file)
• launch moveit_planning_execution.launch (I used a customized launch file)
• set the goal state and 'Plan and Execute', and push 'Stop' while the robot moving
• 'Velocity scaling' helps to make the robot motion slower

[130s]

Thank you @longjie for confirmation on the real robot.
I asked rtmros_common to be released into rosdistro to get binary of it start-jsk/rtmros_common#990. Once it becomes available we can test with it and close this ticket.

[hectorherrero]

Just for info, I want to confirm that now the motion is interrupted when stop button is pressed. I have tested on real HiroNx robot. All is working as expected.

Thanks for your effort!!

[130s]

This isn't possible yet using binary (latest available: v1.3.2). Assuming majority of users use binary of rtmros_common, I prefer keeping this open until 1.3.3 or higher will be released.