test_rtmros_common_pr_1103.l

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l")

(defmethod hironxjsk-interface
  (:init (&rest args &key (type nil) &allow-other-keys)
    (setq robot (instance hironxjsk-robot :init))
    ;;; Define {limb}-controller, usually we can define manually as jsk_robots
    ;(dolist (limb '(:rarm :larm :head :torso))
    ;  (send self :def-limb-controller-method limb))
    ;; If gazebo with ros_control, overwrite :default-controller
    (setq on-gazebo-ros-control
          (and (ros::get-param "/gazebo/time_step" nil)
               ;; rtm-ros-bridge does not have type parametrs
               (ros::get-param "/torso_controller/type" nil)))
    (when on-gazebo-ros-control
      (ros::ros-warn "Found Gazebo/ros_control environment"))
    (prog1
      ;; Hironx has two types of joint_states on one topic: whole body and hand,
      ;; so queue size of joint_states should be two.
      ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L120
      (send-super* :init :joint-states-queue-size 2 :robot robot :type
                   (cond (type type)
                         (on-gazebo-ros-control :gazebo-ros-controller)
                         (t :default-controller))
                   args)
      (dolist (limb '(:rarm :larm :head :torso))
        (let ((ctype (read-from-string (format nil "~A-controller" limb))))
          ;; If :gazebo-ros-controller was passed to *ri* init, {limb}-controller's action was already created, so we just register it as {limb}-controller
          (unless (send self :add-controller ctype)
            ;; If :default-controller was passed to *ri* init, {limb}-controller's action was not created, so we create it.
            ;; If we create it when :gazebo-ros-controller was passed, "old client's goal" warning is showed after :angle-vector
            (send self :add-controller ctype
                  :joint-enable-check t :create-actions t))))

      ;; add hand controller for gazebo with ros_control
      (when on-gazebo-ros-control
        (setq hand-actions (make-hash-table))
        (dolist (hand (list :rhand :lhand))
          ;; initialize hand action
          (sethash hand hand-actions
                   (instance ros::simple-action-client :init
                             (format nil "/~A_controller/follow_joint_trajectory_action"
                                       (string-downcase hand))
                             control_msgs::FollowJointTrajectoryAction
                             :groupname groupname))
          ;; check if hand action is respond (based on baxter-interface)
          (unless
              (and joint-action-enable (send (gethash hand hand-actions) :wait-for-server 3))
            (ros::ros-warn "~A is not respond" (gethash hand hand-actions))
            (ros::ros-info "*** if you do not have hand, you can ignore this message ***"))))
      ;; number of servo motors in one hand
      (setq hand-servo-num 4)))
  (:rarm-controller ()
    (list
      (list
        (cons :group-name "rarm")
        (cons :controller-action "fullbody_controller/follow_joint_trajectory_action")
        (cons :controller-state "fullbody_controller/state")
        (cons :action-type control_msgs::FollowJointTrajectoryAction)
        (cons :joint-names
              (list "RARM_JOINT0" "RARM_JOINT1" "RARM_JOINT2" "RARM_JOINT3" "RARM_JOINT4" "RARM_JOINT5"))))))

(defun test ()
  (hironxjsk-init :type :rarm-controller)
  (objects (list *hironxjsk*))
  (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
  (setq *prev-larm-av* (send *hironxjsk* :larm :angle-vector))
  (dotimes (i 300)
    (send *ri* :angle-vector (send *hironxjsk* :reset-manip-pose) 1000)
    (send *ri* :wait-interpolation)
    (send *ri* :angle-vector (send *hironxjsk* :reset-pose) 1000)
    (send *ri* :wait-interpolation))
  (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
  (setq *curr-larm-av* (send *hironxjsk* :larm :angle-vector))
  (ros::ros-info "larm av diff: ~a" (v- *curr-larm-av* *prev-larm-av*)))