[Fetch] jsk_fetch_robot/fetcheus/fetch-interface.l :angle-vector, :angle-vector-sequence supports old API

[k-okada]

this is rewrited version of #791
related to jsk-ros-pkg/jsk_pr2eus#302, some people like old :angle-vector API, like

:angle-vector av tm :use-torso t

currently we have to use

:angle-vector av tm nil 0 :use-torso t

[knorth55]

@k-okada This is just a question, but start-offset-time in :angle-vector-motion-plan is equal to start-time in angle-vector?

[k-okada]

@knorth55 , I think so, originally we have trouble if we send trajectory startring from current time, and receiver (robot controller) receved that trajectry as a past time (assuming it takes time during data transfer), the controller did not execute the time, that's why we added start-time, may we no longer need that one, for example start-jsk/rtmros_common#1013

:angle-vector

   (let ((cacts (gethash ctype controller-table)))
     (mapcar
      #'(lambda (action param)
          (send self :send-ros-controller
                action (cdr (assoc :joint-names param)) ;; action server and joint-names
                start-time  ;; start time
                (list
                 (list av                                     ;; positions
                       (instantiate float-vector (length av)) ;; velocities
                       (/ tm 1000.0)))))                      ;; duration
      cacts (send self ctype)))

:angle-vector-motion-plan

                 (when time-from-start
                   (setq points
                         (let (tmp-points)
                           (dolist (pt points)
                             (send pt :time_from_start
                                   (ros::time
                                     (+ time-from-start
                                        start-offset-time
                                        (send (send pt :time_from_start)
                                              :to-sec))))
                             (setq tmp-points
                                   (append tmp-points (list pt))))
                           tmp-points)))

[knorth55]

@k-okada I made a PR k-okada#2

[knorth55]

@k-okada can you merge this?