Fix :trajectory-filter to ignore start-offset-time

[pazeshun]

Include #373

start-offset-time should only affect time stamp of trajectory, which is set in :send-trajectory.
We don't need to set it in trajectory-filter.

• Current state without this PR:
  Total time of trajectory becomes start-offset-time[s] + total-time[ms].
  The execution of first trajectory point takes original time_from_start + start-offset-time.
  As start-offset-time also affects time stamp of trajectory, trajectory finishes start-offset-time * 2 + total-time after calling :angle-vector-motion-plan.

3.irteusgl$ send *ri* :angle-vector-motion-plan (send *baxter* :angle-vector) :start-offset-time 5 :total-time 1000 :ctype :default-controller :move-arm :arms :clear-velocities t
[ INFO] [1534232819.952439981]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; Planned Trajectory Total Time   0.887 [sec]
[ INFO] [1534232819.952852500]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; Scaled Trajectory Total Time 6.000(1.000) [sec]
[ INFO] [1534232819.952914714]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; generated 10 points for 1.0 sec using [both_arms] group
[ INFO] [1534232819.952971371]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; will send to (left_s0 left_s1 left_e0 left_e1 left_w0 left_w1 left_w2 left_gripper_prismatic_joint left_gripper_vacuum_pad_joint right_s0 right_s1 right_e0 right_e1 right_w0 right_w1 right_w2 right_gripper_prismatic_joint right_gripper_vacuum_pad_joint)
[ INFO] [1534232819.953208033]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; send self :angle-vector :head-controller (#<controller-action-client #Xeefbd78 /robot/head/head_action>) (without planning)
;; #<rotational-joint #Xb3ccd50 right_gripper_finger_roll_joint> :joint-angle(-0.078983) violate min-angle(0.0)
;; #<rotational-joint #Xb3cd008 right_gripper_finger_yaw_joint> :joint-angle(-0.225361) violate min-angle(0.0)
[ INFO] [1534232819.975442103]: [/default_robot_interface_1534232610481270195] [ROSEUS_ROSINFO] ;; send self :send-trajectory :default-controller
#<moveit_msgs::motionplanresponse #Xf180c20>

[pazeshun]

#359 doesn't work because start-offset-time is passed via args, too.

[k-okada]

can we add some of the test code in pr2eus and also jsk_robot so that we can find jsk-ros-pkg/jsk_robot#956, jsk-ros-pkg/jsk_robot#955 ?

[pazeshun]

@k-okada Thank you. I'll do that

[pazeshun]

I'll also fix all calling of :trajectory-filter not to pass start-offset-time

[knorth55]

why you need this?
Doesn't #359 solve your problem?

[knorth55]

otherwise, we do not need to modify API.

[pazeshun]

Doesn't #359 solve your problem?

No, #359 doesn't work.
Because start-offset-time is also included in args which is passed to :trajectory-filter.
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus_moveit/euslisp/robot-moveit.l#L575

otherwise, we do not need to modify API.

If maintaining current :trajectory-filter is very important, I can accomplish this by passing args without start-offset-time to :trajectory-filter.
But I feel it is just a temp fix, because following two start-offset-time still coexist and are confusing.

• start-offset-time of :trajectory-filter: Duration to slide time_from_start of each trajectory point.
• start-offset-time of :angle-vector-motion-plan: Duration to slide time stamp of trajectory by https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus_moveit/euslisp/robot-moveit.l#L608

As the latter is the same as start-time of :angle-vector(-raw), I think we should leave the latter and delete (or remane, at least) the former.
Fortunately, I think :trajectory-filter is only used in robot-moveit.l and we can change :trajectory-filter easily.

[pazeshun]

Fixed all calling of :trajectory-filter not to pass start-offset-time and added test

[pazeshun]

@k-okada
I followed #363 (comment) and opened #373 to check test. This PR was rebased on that.

[k-okada]

lgtm