Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

wait-interpolation does not come back #2106

Closed
knorth55 opened this issue May 18, 2017 · 20 comments
Closed

wait-interpolation does not come back #2106

knorth55 opened this issue May 18, 2017 · 20 comments
Labels

Comments

@knorth55
Copy link
Member

knorth55 commented May 18, 2017

:wait-interpolation does not come back.

@k-okada

Test Script

https://gist.github.com/knorth55/1c7bdfa9004c5de0615c5e27c18ce000

Debug Output

https://gist.github.com/knorth55/0c2bad449fb51425e172fbf9b8855abb

[http://baxter:11311][133.11.216.160] uchimi@sheeta:~/ros/jsk_apc/src/start-jsk/jsk_apc/jsk_arc2017_baxter/euslisp$ roseus test-wait-interpolation.l 
configuring by "/opt/ros/indigo/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x5ca0ad0[16374] --> 0x60ff680[32748] top=3d1c
irtgl irtglc irtviewer 
EusLisp 9.23( 1.1.0) for Linux64 created on ip-172-31-23-158(Mon Mar 13 06:38:31 PDT 2017)
roseus ;; loading roseus("") on euslisp((9.23 ip-172-31-23-158 Mon Mar 13 06:38:31 PDT 2017  1.1.0))
eustf roseus_c_util 
;; extending gcstack 0x60ff680[32738] --> 0x6b11050[65476] top=7e22
(test-init)
(test)
1.irteusgl$ (test-init)
[ WARN] [1495103231.482322394]: [robot/end_effector/right_gripper/gripper_action] action server is not found
[ WARN] [1495103231.482578149]:      goal=0, cancel=0, feedback=0, result=0
[ WARN] [1495103231.482655342]: #<ros::simple-action-client #X7c96578 robot/end_effector/right_gripper/gripper_action> is not respond
[ INFO] [1495103231.482701824]: *** if you do not have gripper, you can ignore this message ***
[ WARN] [1495103233.508653625]: topic /robot_interface_marker_array already advertised
[ WARN] [1495103233.519848511]: topic /robot/head/head_action/goal already advertised
[ WARN] [1495103233.519929752]: topic /robot/head/head_action/cancel already advertised
[ WARN] [1495103233.823254702]: topic /robot/end_effector/right_gripper/command already advertised
[ WARN] [1495103233.823334139]: topic /robot/end_effector/left_gripper/command already advertised
[ WARN] [1495103233.823379490]: topic /robot/xdisplay already advertised
[ WARN] [1495103233.823432238]: topic /robot/head/command_head_nod already advertised
[ WARN] [1495103233.836651385]: topic /robot/end_effector/right_gripper/gripper_action/goal already advertised
[ WARN] [1495103233.836838967]: topic /robot/end_effector/right_gripper/gripper_action/cancel already advertised
[ WARN] [1495103233.862620023]: topic /robot/end_effector/left_gripper/gripper_action/goal already advertised
[ WARN] [1495103233.862846283]: topic /robot/end_effector/left_gripper/gripper_action/cancel already advertised
[ WARN] [1495103236.983670053]: [robot/end_effector/right_gripper/gripper_action] action server is not found
[ WARN] [1495103236.983817485]:      goal=0, cancel=0, feedback=0, result=0
[ WARN] [1495103236.983904649]: #<ros::simple-action-client #X12f3b648 robot/end_effector/right_gripper/gripper_action> is not respond
[ INFO] [1495103236.983960368]: *** if you do not have gripper, you can ignore this message ***
[ WARN] [1495103237.005189106]: topic /vacuum_gripper/limb/left already advertised
[ WARN] [1495103237.005359392]: topic /vacuum_gripper/limb/right already advertised
[ WARN] [1495103237.005471428]: topic /gripper_front/limb/left/servo/torque already advertised
[ WARN] [1495103237.005562194]: topic /gripper_front/limb/right/servo/torque already advertised
;; #<rotational-joint #X66f70a8 head_pan> :joint-angle(-85.4077) violate min-angle(-80.0021)
;; #<rotational-joint #X65a0798 left_s0> :joint-angle(97.5146) violate max-angle(97.4991)
;; #<rotational-joint #X1c4bf430 head_pan> :joint-angle(-85.4077) violate min-angle(-80.0021)
#f(-80.0021 97.4707 -2.46094 -94.5923 134.561 91.3623 11.4478 0.0 5.896456e-14 -28.5864 -46.7358 72.9492 49.4604 -9.71191 88.1104 -56.7554 39.3417)
2.irteusgl$ (test)
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X66f70a8 head_pan> :joint-angle(-85.4077) violate min-angle(-80.0021)
;; #<rotational-joint #X65a0798 left_s0> :joint-angle(97.5146) violate max-angle(97.4991)
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X1c4bf430 head_pan> :joint-angle(-85.4297) violate min-angle(-80.0021)
$ rostopic echo /robot/head/head_action/status
---
header: 
  seq: 49937
  stamp: 
    secs: 1495103247
    nsecs: 213005065
  frame_id: ''
status_list: 
  - 
    goal_id: 
      stamp: 
        secs: 1495103242
        nsecs: 283062934
      id: 1495103242282292706_/hoge_1495103193105773638_27480_/robot/head/head_action_0
    status: 8
    text: This goal was canceled because another goal was received by the simple action server
  - 
    goal_id: 
      stamp: 
        secs: 1495103242
        nsecs: 285640954
      id: 1495103242285056196_/hoge_1495103193105773638_27480_/robot/head/head_action_0
    status: 3
    text: ''
---
header: 
  seq: 49938
  stamp: 
    secs: 1495103247
    nsecs: 413225889
  frame_id: ''
status_list: []
---
header: 
  seq: 49939
  stamp: 
    secs: 1495103247
    nsecs: 612998008
  frame_id: ''
status_list: []
$ rostopic echo /robot/head/head_action/result
---
header: 
  seq: 518
  stamp: 
    secs: 1495103242
    nsecs: 285852909
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1495103242
      nsecs: 283062934
    id: 1495103242282292706_/hoge_1495103193105773638_27480_/robot/head/head_action_0
  status: 8
  text: This goal was canceled because another goal was received by the simple action server
result: 

---
header: 
  seq: 519
  stamp: 
    secs: 1495103242
    nsecs: 297673940
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1495103242
      nsecs: 285640954
    id: 1495103242285056196_/hoge_1495103193105773638_27480_/robot/head/head_action_0
  status: 3
  text: ''
result: 

---
@k-okada
Copy link
Member

k-okada commented May 20, 2017

we this is very difficult problem, the root cause if that roseus only support one subscriber for one topic,
jsk-ros-pkg/jsk_roseus#519 for potential solution only for actionlib client,
jsk-ros-pkg/jsk_roseus#407 for discussion.

@YutoUchimi
Copy link
Collaborator

@k-okada
Without roslaunch jsk_arc2017_baxter baxter.launch (i.e. on Kinematic Simulator), the problem reported above does not occur.
But when I ran roslaunch jsk_arc2017_baxter baxter.launch and then roseus test-wait-interpolation.l, it occured.
As soon as I ran (test) function, the messages shown below flew in baxter.launch terminal.

[ INFO] [1495502905.841263615]: /gripper_trajectory_server: Executing requested joint trajectory for left/ gripper
[ INFO] [1495502905.846096493]: /gripper_trajectory_server: Executing requested joint trajectory for right/ gripper
[INFO] [WallTime: 1495502905.853196] /baxter_joint_trajectory: Executing requested joint trajectory
[INFO] [WallTime: 1495502905.856959] /baxter_joint_trajectory: Executing requested joint trajectory
[ INFO] [1495502905.891644130]: /gripper_trajectory_server: Succeeded for left/ gripper
[ INFO] [1495502905.896373289]: /gripper_trajectory_server: Succeeded for right/ gripper
[INFO] [WallTime: 1495502906.006313] /baxter_joint_trajectory: Joint Trajectory Action Succeeded for left arm
[INFO] [WallTime: 1495502906.012384] /baxter_joint_trajectory: Joint Trajectory Action Succeeded for right arm

@k-okada
Copy link
Member

k-okada commented May 23, 2017 via email

@knorth55
Copy link
Member Author

I checked it with gazebo simulator, but it does not solve the problem.
test_wait_interpolation

roseus debug: https://gist.github.com/knorth55/c8e76f8d5ce62877665259e790d25661

@knorth55
Copy link
Member Author

I tested with

$ roslaunch jsk_2016_01_baxter_apc baxter_sim.launch
# other pane
$ roseus test-wait-interpolation.l

@knorth55
Copy link
Member Author

[DEBUG] [1495616999.631652760, 403.579000000]: [/robot/head/head_action] find-status-by-goal-id goal_id:403570000000_/hoge_1495616808583406046_12132_/robot/head/head_action_0 == 403575000000_/hoge_1495616808583406046_12132_/robot/head/head_action_0 nil
[DEBUG] [1495616999.631918665, 403.579000000]: [/robot/head/head_action] find-status-by-goal-id goal_id:403575000000_/hoge_1495616808583406046_12132_/robot/head/head_action_0 == 403575000000_/hoge_1495616808583406046_12132_/robot/head/head_action_0 t

@k-okada
Copy link
Member

k-okada commented May 26, 2017 via email

@pazeshun
Copy link
Collaborator

pazeshun commented May 26, 2017

I wrote _on_sea_joint_state but I don't see that error in real robot.
I want to know the output of rostopic echo /robot/limb/<right | left>/gravity_compensation_torques in your environment.
RethinkRobotics/baxter_interface#77 (comment)

@knorth55
Copy link
Member Author

[ERROR] [WallTime: 1495761897.518435] [87.089000] bad callback: <bound method Limb._on_sea_joint_state of <baxter_interface.limb.Limb object at 0x7f69c71ca590>> Traceback (most recent call last): File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback cb(msg) File "/home/k-okada/catkin_ws/ws_jsk_apc/src/FOR_LATEST_BAXTEREUS/baxter_interface/src/baxter_interface/limb.py", line 195, in _on_sea_joint_state self._joint_commanded_angle[name] = msg.commanded_position[idx] IndexError: tuple index out of range 

This error occurs when you use baxter_interface in jsk_apc branch.
It doesn't occur in master branch

─▪ wstool info -t .
workspace: /home/shingo/ros/indigo/src

 Localname                                S SCM Version (Spec)                                         UID  (Spec)                 URI  (Spec) [http(s)://...]
 ---------                                - --- --------------                                         -----------                 ---------------------------
 FOR_PCL1.8/ros-perception/perception_pcl   git indigo-devel                                           a9cc711516cf                git://github.com/ros-perception/perception_pcl.git
 FOR_PCL1.8/OctoMap/octomap_mapping         git indigo-devel                                           122bc6023092                git://github.com/OctoMap/octomap_mapping.git
 FOR_MOVEIT/moveit_robots                   git kinetic-devel                                          a223e7777631                github.com/ros-planning/moveit_robots.git
 FOR_SIMULATION/baxter_common             V git master  (jsk_apc)                                      6c4b0f375fe4 (a722703d2ab6) github.com/RethinkRobotics/baxter_common.git    (github.com/knorth55/baxter_common.git)
 FOR_SIMULATION/baxter_simulator          V git gripper-sim                                            80d47cc24400                github.com/RethinkRobotics/baxter_simulator.git    (github.com/knorth55/baxter_simulator.git)
 FOR_LATEST_BAXTEREUS/baxter_interface    V git master  (jsk_apc)                                      aea70d96eb0f (1058313dfdc1) github.com/RethinkRobotics/baxter_interface.git    (github.com/wkentaro/baxter_interface.git)
 start-jsk/jsk_apc                          git master                                                 ea1cc4d0ec40                github.com/start-jsk/jsk_apc.git
 ros-drivers/openni2_camera                 git indigo-devel                                           c60a46c91ebc                github.com/ros-drivers/openni2_camera.git
 ros-drivers/axis_camera                    git <detached>  (8e17a795120ce97e59506c4228d023247451119e) 8e17a795120c                github.com/ros-drivers/axis_camera.git
 jsk-ros-pkg/jsk_pr2eus                     git master                                                 53e38ba09267                github.com/jsk-ros-pkg/jsk_pr2eus.git
 jsk-ros-pkg/jsk_robot                      git master                                                 0d386bcfcc97                github.com/jsk-ros-pkg/jsk_robot.git
 jsk-ros-pkg/jsk_recognition                git master                                                 31b4e60612af                github.com/jsk-ros-pkg/jsk_recognition.git
 jsk-ros-pkg/jsk_model_tools                git master                                                 862c0e6b46dd                github.com/jsk-ros-pkg/jsk_model_tools.git
 jsk-ros-pkg/jsk_common                     git master                                                 daa4dad83aab                github.com/jsk-ros-pkg/jsk_common.git
 RethinkRobotics/baxter_tools               git <detached>  (v1.2.0)                                   0da6ea300e1f                github.com/RethinkRobotics/baxter_tools.git
 RethinkRobotics/baxter_examples            git <detached>  (v1.2.0)                                   033756294b59                github.com/RethinkRobotics/baxter_examples.git
 RethinkRobotics/baxter                     git <detached>  (v1.2.0)                                   d2f2ebd1166b                github.com/RethinkRobotics/baxter.git

Also detected these repositories in the workspace, add using 'wstool scrape' or 'wstool set':

 Localname              SCM   URI  (Spec) [http(s)://...]
 ---------              ---   ---------------------------
 FA-I-sensor            --git https://github.com/RoboticMaterials/FA-I-sensor.git
 jsk-ros-pkg/jsk_roseus --git git://github.com/jsk-ros-pkg/jsk_roseus.git
 proximity_gripper_demo --git [email protected]:knorth55/proximity_gripper_demo.git

@pazeshun
Copy link
Collaborator

Oh, does baxter_sim.launch not publish the same /robot/limb/<right | left>/gravity_compensation_torques as real robot?
Then RethinkRobotics/baxter_interface#77 is bad PR....
I'll check.

@knorth55
Copy link
Member Author

knorth55 commented May 26, 2017

It is published, and it seems values in msg are correctly set.

$ rostopic echo -n2 /robot/limb/right/gravity_compensation_torques
header: 
  seq: 9006
  stamp: 
    secs: 116
    nsecs: 760000000
  frame_id: base
name: ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
commanded_position: []
commanded_velocity: []
commanded_acceleration: []
commanded_effort: []
actual_position: [-0.2726580461055823, 1.0470000111083042, 0.020832877809258576, 0.4983703989903141, -0.08187191303224761, 0.0356256514383535, 0.02590261830741003]
actual_velocity: [-1.790584137615158e-07, -4.884687808535537e-06, 7.576766461844993e-06, 2.053590664691191e-05, 9.180419368902437e-06, -5.226140336383654e-06, -1.1565164862003564e-05]
actual_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
gravity_model_effort: [-2.324380934055779e-17, -15.216235448076615, 0.13533974153901387, 0.007158229029370099, 0.008567042292249827, 0.13145252735240476, -6.650358935959678e-05]
gravity_only: []
hysteresis_model_effort: []
crosstalk_model_effort: []
hystState: 0.0
---
header: 
  seq: 9007
  stamp: 
    secs: 116
    nsecs: 770000000
  frame_id: base
name: ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
commanded_position: []
commanded_velocity: []
commanded_acceleration: []
commanded_effort: []
actual_position: [-0.27265804475092636, 1.0469999796519485, 0.0208329587975582, 0.4983700889647462, -0.0818717175338497, 0.0356261292149993, 0.025902374731095357]
actual_velocity: [9.24092321917973e-08, -2.7578802850888414e-05, -5.300224654981131e-06, -3.099341527145441e-05, 1.0874543677646053e-05, 4.8920947617842565e-05, -1.3270427869597849e-05]
actual_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
gravity_model_effort: [-6.548008089765104e-17, -15.216241357477442, 0.1353402353322854, 0.007153465370786764, 0.008567128027017019, 0.1314528683999635, -6.650431123148232e-05]
gravity_only: []
hysteresis_model_effort: []
crosstalk_model_effort: []
hystState: 0.0
---

@pazeshun
Copy link
Collaborator

commanded_position in gravity_compensation_torques is needed in RethinkRobotics/baxter_interface#77, but not set in the msg.
This is problem.
I'll check real robot immediately after rebooted.

@pazeshun
Copy link
Collaborator

In real robot immediately after rebooted:

$ rostopic echo -n1 /robot/limb/right/gravity_compensation_torques
header: 
  seq: 2052
  stamp: 
    secs: 1495770600
    nsecs: 959728316
  frame_id: base
name: ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
commanded_position: [-1.5182574848095882, 0.05944175553055978, 0.20785439675847356, 1.6171992456281974, -1.7203594536134914, 0.26422819071326253, -0.14189322287940076]
commanded_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
commanded_acceleration: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
commanded_effort: [-1.1990744323703706, 0.033632528774387965, 0.07915340865486473, -0.6162767815331759, -0.34476218207718456, -0.39967869428350455, 0.3554616980727336]
actual_position: [-1.517873989612617, 0.05944175553055978, 0.20785439675847356, 1.6171992456281974, -1.7203594536134914, 0.2646116859102339, -0.14266021327334347]
actual_velocity: [0.0027488935713754254, 0.001570796326500243, 0.006283185306000972, -0.005890486224375911, -0.0007853981632501215, 0.010995574285501701, -0.012959069693627005]
actual_effort: [-1.184, -5.252, 4.684, 0.696, 0.5, 0.828, 0.0]
gravity_model_effort: [-0.28791646428292916, 0.05172480858204587, 2.986495355848296, 1.7966103237690754, 0.10314258609082952, 0.22566454263834423, 0.0021081471087682525]
gravity_only: [-0.28791646428292916, -35.103312696255585, 2.986495355848296, 1.7966103237690754, 0.10314258609082952, 0.22566454263834423, 0.0021081471087682525]
hysteresis_model_effort: [0.0, -34.78877857098779, 0.0, 0.0, 0.0, 0.0, 0.0]
crosstalk_model_effort: [0.0, 0.36625893384984387, 0.0, 0.0, 0.0, 0.0, 0.0]
hystState: 0.0
---

commanded_position is set even if servo is off.
I think this is problem in baxter_simulator.

@k-okada
Copy link
Member

k-okada commented May 26, 2017

the message is

  seq: 1905
  stamp: 
    secs: 34
    nsecs: 139000000
  frame_id: base
name: ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
commanded_position: []
commanded_velocity: []
commanded_acceleration: []
commanded_effort: []
actual_position: [-0.08563917051068426, 0.8435443082652787, -0.2980024830513255, 0.8062248545676693, 0.6760952738672232, 0.21053041882895585, -0.5040563191609548]
actual_velocity: [0.02564951995860664, -0.1589155933406523, 0.049193527742846924, 0.23206720419496962, 0.08789991869789135, -0.19260765888123244, -0.9914256473214198]
actual_effort: [2.8167107019799964, -50.0, -50.0, 50.0, -5.461448169673382, 15.0, 1.8385653251253498]
gravity_model_effort: [-1.668360159517738e-15, -20.607587669036054, -3.0595963806902233, 2.256693958865532, -0.16933443907135712, 0.5055258981501114, -0.001007266729485934]
gravity_only: []
hysteresis_model_effort: []
crosstalk_model_effort: []

I have confirmed we can pass test as

5.irteusgl$ test
[ INFO] [1495768438.686762641, 403.698000000]: send-angle-vector to larm
moveit environment is not correctly set, execute :angle-vector-raw instead                                                            
[ INFO] [1495768438.713513715, 403.705000000]: send-angle-vector to rarm
moveit environment is not correctly set, execute :angle-vector-raw instead                                                            
[ WARN] [1495768438.721620686, 403.707000000]: [/robot/head/head_action] action-result-cb may recieved old client's goal
[ WARN] [1495768438.721707128, 403.707000000]:      expected goal id 403705000000_/hoge_1495768183116421724_28778_/robot/head/head_ac\
tion_1
[ WARN] [1495768438.721758314, 403.707000000]:      recieved goal id 403702000000_/hoge_1495768183116421724_28778_/robot/head/head_ac\
tion_1
[ INFO] [1495768438.743890842, 403.709000000]: wait rarm
[ INFO] [1495768439.915950239, 404.010000000]: wait larm
[ INFO] [1495768439.916226578, 404.010000000]: finished
"finished~%"
"finished~%"

with following modification

diff --git a/baxter_sim_hardware/src/baxter_emulator.cpp b/baxter_sim_hardware/src/baxter_emulator.cpp
index 4b51f47..daf5879 100644
--- a/baxter_sim_hardware/src/baxter_emulator.cpp
+++ b/baxter_sim_hardware/src/baxter_emulator.cpp
@@ -502,6 +502,12 @@ void baxter_emulator::reset_head_nod(const ros::TimerEvent &t) {
 void baxter_emulator::update_jnt_st(const sensor_msgs::JointState &msg) {
   jstate_msg = msg;
   float threshold = 0.0009;
+left_gravity.commanded_position.resize(left_gravity.name.size());
+left_gravity.commanded_velocity.resize(left_gravity.name.size());
+left_gravity.commanded_effort.resize(left_gravity.name.size());
+right_gravity.commanded_position.resize(left_gravity.name.size());
+right_gravity.commanded_velocity.resize(left_gravity.name.size());
+right_gravity.commanded_effort.resize(left_gravity.name.size());
 left_gravity.actual_position.resize(left_gravity.name.size());
 left_gravity.actual_velocity.resize(left_gravity.name.size());
 left_gravity.actual_effort.resize(left_gravity.name.size());

#2116,
jsk-ros-pkg/jsk_roseus@e77e5c7

@knorth55
Copy link
Member Author

knorth55 commented May 26, 2017

@k-okada
Copy link
Member

k-okada commented May 26, 2017

@knorth55 in #2106 (comment)

I checked it with gazebo simulator, but it does not solve the problem.

do you mean the roseus stops with error (jsk-ros-pkg/jsk_roseus#519 (comment)) or :wait-interpolation do not returns.

in the former case, it should fixed with jsk-ros-pkg/jsk_roseus@e77e5c7, in the later case, you may not using the jsk-ros-pkg/jsk_roseus#519 branch, you should see someething like

[ WARN] [1495768438.721620686, 403.707000000]: [/robot/head/head_action] action-result-cb may recieved old client's goal
[ WARN] [1495768438.721707128, 403.707000000]:      expected goal id 403705000000_/hoge_1495768183116421724_28778_/robot/head/head_ac\
tion_1
[ WARN] [1495768438.721758314, 403.707000000]:      recieved goal id 403702000000_/hoge_1495768183116421724_28778_/robot/head/head_ac\
tion_1

@knorth55
Copy link
Member Author

@k-okada
I checked with your new commit jsk-ros-pkg/jsk_roseus@e77e5c7, and wait-interpolation comes back with baxter_simulator

4.irteusgl$ (test)
moveit environment is not correctly set, execute :angle-vector-raw instead
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X6b66ae0 left_s1> :joint-angle(59.9887) violate max-angle(59.9887)
;; #<rotational-joint #X6bb50a0 right_s1> :joint-angle(59.9887) violate max-angle(59.9887)
[ WARN] [1495771809.151813638, 185.761000000]: [/robot/head/head_action] action-result-cb may recieved old client's goal
[ WARN] [1495771809.153099535, 185.762000000]:      expected goal id 185758000000_/hoge_1495771648137038982_31518_/robot/head/head_action_2
[ WARN] [1495771809.153422624, 185.762000000]:      recieved goal id 185750000000_/hoge_1495771648137038982_31518_/robot/head/head_action_2
"finished~%"
"finished~%"

@pazeshun can you check test code with real robot if possible? https://gist.github.com/knorth55/1c7bdfa9004c5de0615c5e27c18ce000

@pazeshun
Copy link
Collaborator

wait-interpolation returned with real robot!

2.irteusgl$ test
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X53089c8 head_pan> :joint-angle(-82.2437) violate min-angle(-80.0021)
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X5613e88 head_pan> :joint-angle(-82.2217) violate min-angle(-80.0021)
[ WARN] [1495773639.948403104]: [/robot/head/head_action] action-result-cb may recieved old client's goal
[ WARN] [1495773639.948515949]:      expected goal id 1495773638141984783_/hoge_1495773578755315015_135528_/robot/head/head_action_0
[ WARN] [1495773639.948566200]:      recieved goal id 1495773638138721575_/hoge_1495773578755315015_135528_/robot/head/head_action_0
[ WARN] [1495773641.944685488]: [/robot/head/head_action] :wait-for-result finished with preempted status
"finished~%"
"finished~%"

I checked with the following workspace with jsk-ros-pkg/jsk_roseus#519 branch:

pazeshun@sheeta:~/ros/jsk_apc_ws/src$ wstool info
workspace: /home/pazeshun/ros/jsk_apc_ws/src

 Localname                             S SCM Version (Spec)   UID  (Spec)  URI  (Spec) [http(s)://...]
 ---------                             - --- --------------   -----------  ---------------------------
 FOR_GRIPPER_V5/dynamixel_motor          git gripper-v5-devel 84b6ae79f5d7 github.com/pazeshun/dynamixel_motor.git
 FOR_SIMULATION/baxter_common            git jsk_apc          a722703d2ab6 github.com/knorth55/baxter_common.git
 FOR_SIMULATION/baxter_simulator         git gripper-sim      80d47cc24400 github.com/knorth55/baxter_simulator.git
 FOR_PCL1.8/perception_pcl               git indigo-devel     a9cc711516cf github.com/ros-perception/perception_pcl.git
 FOR_PCL1.8/octomap_mapping              git indigo-devel     122bc6023092 github.com/OctoMap/octomap_mapping.git
 FOR_LATEST_BAXTEREUS/jsk_robot          git master           0d386bcfcc97 github.com/jsk-ros-pkg/jsk_robot.git
 FOR_LATEST_BAXTEREUS/baxter_interface   git jsk_apc          1058313dfdc1 github.com/wkentaro/baxter_interface.git
 start-jsk/jsk_apc                       git master           ea1cc4d0ec40 github.com/start-jsk/jsk_apc.git
 FOR_ASTRA/openni2_camera                git indigo-devel     c0991d9dcfcc github.com/ros-drivers/openni2_camera.git
 jsk-ros-pkg/jsk_recognition             git master           18e78cc2121f github.com/jsk-ros-pkg/jsk_recognition.git
 jsk-ros-pkg/jsk_common                  git master           5e0316b0b7f5 github.com/jsk-ros-pkg/jsk_common.git

Also detected these repositories in the workspace, add using 'wstool scrape' or 'wstool set':

 Localname                       SCM   URI  (Spec) [http(s)://...]
 ---------                       ---   ---------------------------
 FOR_LATEST_BAXTEREUS/jsk_pr2eus --git https://github.com/jsk-ros-pkg/jsk_pr2eus.git
 image_pipeline                  --git https://github.com/ros-perception/image_pipeline.git

I used latest master branch in jsk_pr2eus.

@knorth55
Copy link
Member Author

We need to wait new release of roseus or do not rotate head in the same time.

@wkentaro
Copy link
Member

Fixed via #2097

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

5 participants