Skip to content

Commit

Permalink
fix: move-trajectory wrong document / implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@furushchev
furushchev committed Mar 3, 2018
1 parent d866c3a commit 6706e73
Showing 1 changed file with 30 additions and 54 deletions.
84 changes: 30 additions & 54 deletions pr2eus/robot-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1592,18 +1592,11 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
;;
(:move-trajectory-sequence
(trajectory-points time-list &key (stop t) (start-time) (send-action nil))
"trajectory-points [ list of #f(x y d) ] time-list [list of time span]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time ]
send-action [ send message to action server, it means robot will move ]"
(send self :move-trajectory trajectory-points time-list nil nil
:stop stop :start-time start-time :send-action send-action))
(:move-trajectory
(x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
" x [m/sec] y [m/sec] d [rad/sec] msec [milli second]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time ]
send-action [ send message to action server, it means robot will move ]"
"trajectory-points [ list of #f(x y d) ([m/sec] for x, y; [rad/sec] for d) ]
time-list [list of time span [msec] ]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time [sec or ros::Time] ]
send-action [ send message to action server, it means robot will move ]"
(send self :spin-once)
(let ((odom-cds (send self :state :odom :pose))
(msg (instance trajectory_msgs::JointTrajectory :init))
Expand All @@ -1614,20 +1607,19 @@ send-action [ send message to action server, it means robot will move ]"
(start-time (send msg :header :stamp start-time))
(t (send msg :header :stamp (ros::time-now))))
(send msg :joint_names (list "base_link_x" "base_link_y" "base_link_pan"))
(cond
((and (listp x) (listp y))
(let ((traj-lst
(mapcar #'(lambda (vec)
(send
(make-coords :pos (float-vector (elt vec 0) (elt vec 1) 0)
:rpy (list (elt vec 2) 0 0))
:transform odom-cds :world)) x))
(cur-cds (send odom-cds :copy-worldcoords))
(cur-time 0)
nxt-cds nxt-time ret)
(pprint traj-lst)
(let ((traj-lst
(mapcar #'(lambda (vec)
(send
(make-coords :pos (float-vector
(* 1000.0 (elt vec 0))
(* 1000.0 (elt vec 1)) 0)
:rpy (list (elt vec 2) 0 0))
:transform odom-cds :world)) trajectory-points))
(cur-cds (send odom-cds :copy-worldcoords))
(cur-time 0)
nxt-cds nxt-time ret)
(setq nxt-cds (pop traj-lst)
nxt-time (pop y))
nxt-time (pop time-list))
(while nxt-cds
(let* ((vr (send cur-cds :transformation nxt-cds))
(vp (send cur-cds :rotate-vector (send vr :pos))))
Expand All @@ -1646,7 +1638,7 @@ send-action [ send message to action server, it means robot will move ]"
(incf cur-time (/ nxt-time 1000.0))
(setq cur-cds nxt-cds
nxt-cds (pop traj-lst)
nxt-time (pop y))
nxt-time (pop time-list))
) ;; (while nxt-cds
(push ;; push last point
(instance trajectory_msgs::JointTrajectoryPoint :init
Expand All @@ -1659,42 +1651,26 @@ send-action [ send message to action server, it means robot will move ]"
:time_from_start (ros::time cur-time))
ret)
(send msg :points (nreverse ret))
))
(t
(let ((sec (/ msec 1000.0))
(odom-angle (elt (car (send odom-cds :rpy-angle)) 0))
(odom-pos (scale 0.001 (send odom-cds :pos)))
(pt1 (instance trajectory_msgs::JointTrajectoryPoint :init))
(pt2 (instance trajectory_msgs::JointTrajectoryPoint :init)))
(send pt1 :time_from_start (ros::time))
(send pt2 :time_from_start (ros::time sec))
(send pt1 :positions (v+ odom-pos
(float-vector 0 0 odom-angle)))
(send pt2 :positions (v+ (v+ odom-pos
(float-vector 0 0 (+ odom-angle (* sec d))))
(rotate-vector (scale sec (float-vector x y 0))
odom-angle :z)))
(send pt1 :velocities (rotate-vector (float-vector x y d) odom-angle :z))
(if stop
(send pt2 :velocities (float-vector 0 0 0)) ;; To stop just
(send pt2 :velocities (rotate-vector (float-vector x y d) odom-angle :z)))

(ros::ros-debug "move-trajectory (x y d msec) = (~A ~A ~A ~A)" (* sec x) (* sec y) (* sec d) msec)
(ros::ros-debug " odom-pos ~A, odom-angle ~A" odom-pos odom-angle)
(ros::ros-debug " pt1 ~A" (send pt1 :positions))
(ros::ros-debug " pt2 ~A" (send pt2 :positions))
;;
(send msg :points (list pt1 pt2)))
))
)
(send goal :goal :trajectory msg)
(when (and send-action move-base-trajectory-action)
(send move-base-trajectory-action :send-goal goal)
(let ((acret
(send move-base-trajectory-action :wait-for-result)))
(unless acret (return-from :move-trajectory nil))
(unless acret (return-from :move-trajectory-sequence nil))
(send move-base-trajectory-action :spin-once)))
;;
goal))
(:move-trajectory
(x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
" x [m/sec] y [m/sec] d [rad/sec] msec [milli second]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time ]
send-action [ send message to action server, it means robot will move ]"
(send self :move-trajectory-sequence
(list (float-vector x y d))
(list msec)
:stop stop :start-time start-time :send-action send-action))
;;
(:state
(&rest args)
Expand Down

0 comments on commit 6706e73

Please sign in to comment.