[jsk_fetch_startup][fetcheus] moveit bridge for noetic client

[mqcmd196]

Fix https://github.com/jsk-ros-pkg/jsk_robot/issues/1855

[mqcmd196]

In Fetch, create the workspace below and build.

obinata@fetch1075:~/ros/fetch_noetic_ws/src $ cat .rosinstall
- git:
    local-name: jsk_robot
    uri: https://github.com/mqcmd196/jsk_robot
    version: PR/fetch/moveit_noetic
- git:
    local-name: moveit_msgs
    uri: https://github.com/ros-planning/moveit_msgs
    version: 0.11.4

Then execute

rosrun jsk_fetch_startup moveit_noetic_bridge.py

In client, adopt the patch on pr2eus

diff --git a/pr2eus_moveit/euslisp/collision-object-publisher.l b/pr2eus_moveit/euslisp/collision-object-publisher.l
index 21c5e3b..38c637b 100644
--- a/pr2eus_moveit/euslisp/collision-object-publisher.l
+++ b/pr2eus_moveit/euslisp/collision-object-publisher.l
@@ -6,8 +6,8 @@
           servicename scene-srv))

 (defmethod collision-object-publisher
-  (:init (&key (service-name "apply_planning_scene")
-               (scene-service "get_planning_scene")
+  (:init (&key (service-name "apply_planning_scene_noetic")
+               (scene-service "get_planning_scene_noetic")
                (service-wait-time 30))
    (unless (ros::ok) (ros::roseus "publish_collision_eusobj"))
    (setq object-list (make-hash-table))
@@ -271,7 +271,7 @@
      (ros::service-call servicename req)))
   )

-(defun get-planning-scene (&key (scene-service "get_planning_scene")
+(defun get-planning-scene (&key (scene-service "get_planning_scene_noetic")
                                 (components 1023))
   ;;moveit_msgs::PlanningSceneComponents::*SCENE_SETTINGS*
   ;;moveit_msgs::PlanningSceneComponents::*ROBOT_STATE*
diff --git a/pr2eus_moveit/euslisp/robot-moveit.l b/pr2eus_moveit/euslisp/robot-moveit.l
index a378acc..7bc8fa8 100644
--- a/pr2eus_moveit/euslisp/robot-moveit.l
+++ b/pr2eus_moveit/euslisp/robot-moveit.l
@@ -58,12 +58,12 @@
 ;; group-name -> joint-list, target-link
 (defmethod moveit-environment
   (:init
-   (&key ((:scene-service sc-srv) "/get_planning_scene")
-         ((:planning-service pl-srv) "/plan_kinematic_path")
+   (&key ((:scene-service sc-srv) "/get_planning_scene_noetic")
+         ((:planning-service pl-srv) "/plan_kinematic_path_noetic")
          ((:execute-service ex-srv) "/execute_kinematic_path")
          ((:query-planner-interface-service qr-pl-srv) "/query_planner_interface")
-         ((:planning-scene-world pl-sc-world) "/planning_scene_world")
-         ((:state-validity-service sv-srv) "/check_state_validity")
+         ((:planning-scene-world pl-sc-world) "/planning_scene_world_noetic")
+         ((:state-validity-service sv-srv) "/check_state_validity_noetic")
          ((:robot rb)) (frame-id) ;; frame-id needs to be contained in robot_model
          ((:planner-id pl-id) "RRTConnectkConfigDefault")
          (multi-dof-joint-name "virtual_joint")
@@ -110,7 +110,7 @@
    (unless planning-action-client
      (setq planning-action-client
            (instance ros::simple-action-client :init
-                     "/move_group" moveit_msgs::MoveGroupAction
+                     "/move_group_noetic" moveit_msgs::MoveGroupAction
                      :groupname "robot_moveit"))
      (send planning-action-client :wait-for-server)
      ))
@@ -199,11 +199,11 @@
                                      :attempts (if attempts attempts 0)
                                      :timeout (ros::time timeout)
                                      :pose_stamped msg)))
-            (res (ros::service-call "/compute_ik" req)))
+            (res (ros::service-call "/compute_ik_noetic" req)))
        (when (and retry (/= (send res :error_code :val) 1))
          (send req :ik_request :attempts (if attempts (* 2 attempts) 2))
          (send req :ik_request :timeout (ros::time (* 2 timeout)))
-         (setq res (ros::service-call "/compute_ik" req)))
+         (setq res (ros::service-call "/compute_ik_noetic" req)))
        (cond
         ((= (send res :error_code :val) 1) ;; success
          (apply-joint_state (send res :solution :joint_state) robot)) ;; updates joint-list

Then create the hoge.l

(load "package://fetcheus/fetch-interface.l")
(fetch-init)

(objects (list *fetch*))
(send *ri* :angle-vector (send *fetch* :reset-pose) 8000)

failes with

roseus hoge.l

Call Stack (max depth: 20):
  0: at (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n))
  1: at (setq ros::_id (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n)))
  2: at (let (ros::n) (setq ros::n (system:peek ros::buf ros::ptr- :integer)) (incf ros::ptr- 4) (setq ros::_id (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n))) (incf ros::ptr- ros::n))
  3: at (send ros::_object :deserialize ros::buf ros::ptr-)
  4: at (send ros::elem- :deserialize ros::buf ros::ptr-)
  5: at (while #:dolist8594 (setq ros::elem- (pop #:dolist8594)) (send ros::elem- :deserialize ros::buf ros::ptr-) (incf ros::ptr- (send ros::elem- :serialization-length)))
  6: at (let ((ros::elem- nil) (#:dolist8594 ros::_attached_collision_objects)) nil (while #:dolist8594 (setq ros::elem- (pop #:dolist8594)) (send ros::elem- :deserialize ros::buf ros::ptr-) (incf ros::ptr- (send ros::elem- :serialization-length))) nil)
  7: at (dolist (ros::elem- ros::_attached_collision_objects) (send ros::elem- :deserialize ros::buf ros::ptr-) (incf ros::ptr- (send ros::elem- :serialization-length)))
  8: at (let (ros::n) (setq ros::n (system:peek ros::buf ros::ptr- :integer)) (incf ros::ptr- 4) (setq ros::_attached_collision_objects (let (ros::r) (dotimes (ros::i ros::n) (push (instance moveit_msgs::attachedcollisionobject :init) ros::r)) ros::r)) (dolist (ros::elem- ros::_attached_collision_objects) (send ros::elem- :deserialize ros::buf ros::ptr-) (incf ros::ptr- (send ros::elem- :serialization-length))))
  9: at (send ros::_robot_state :deserialize ros::buf ros::ptr-)
  10: at (send ros::_scene :deserialize ros::buf ros::ptr-)
  11: at (ros::service-call scene-service req)
  12: at (setq ret (ros::service-call scene-service req))
  13: at (let ((req (instance moveit_msgs::getplanningscenerequest :init)) ret) (send req :components :components components) (unless scene-service (ros::ros-warn ";; get-planning-scene ~A service could not found, so skipping calling service" scene-service) (return-from get-planning-scene nil)) (setq ret (ros::service-call scene-service req)) (if ret (send ret :scene)))
  14: at (get-planning-scene)
  15: at euserror
  16: at euserror
  17: at (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n))
  18: at (setq ros::_id (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n)))
  19: at (let (ros::n) (setq ros::n (system:peek ros::buf ros::ptr- :integer)) (incf ros::ptr- 4) (setq ros::_id (subseq ros::buf ros::ptr- (+ ros::ptr- ros::n))) (incf ros::ptr- ros::n))
  And more...
/opt/ros/noetic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: illegal start/end index in (subseq ros::buf ros::ptr- (+ ros::ptr- ro

[mqcmd196]

Something wrong in fetcheus

(send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm
               :start-offset-time (if start-offset-time start-offset-time start-time)
               :clear-velocities clear-velocities :use-torso use-torso args)

[mqcmd196]

https://docs.python.org/ja/2.7/library/imp.html#imp.find_module

[mqcmd196]

When use load_source

melodic_attached_collision_old =  imp.load_source("AttachedCollisionObject", "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_AttachedCollisionObject.py")
aco_old = melodic_attached_collision_old.AttachedCollisionObject()
aco_old.object._md5sum # 'dbba710596087da521c07564160dfccb'

when use find_module

f, path, description = imp.find_module("moveit_msgs", ["/opt/ros/melodic/lib/python2.7/dist-packages"])
melodic_attached_collision_new = imp.load_module("_AttachedCollisionObject", f, path, description)
aco_new = melodic_attached_collision_new.AttachedCollisionObject()
aco_new.object._md5sum # 'dbba710596087da521c07564160dfccb'

hmm...