controller-action-client
- :super ros::simple-action-client
- :slots time-to-finish last-feedback-msg-stamp ri angle-vector-sequence timer-sequence current-time current-angle-vector previous-angle-vector scale-angle-vector
:init r &rest args
:last-feedback-msg-stamp
:time-to-finish
:action-feedback-cb msg
:push-angle-vector-simulation av tm &optional prev-av
:pop-angle-vector-simulation
:interpolatingp
robot-interface
- :super propertied-object
- :slots robot objects robot-state joint-action-enable warningp controller-type controller-actions controller-timeout namespace controller-table visualization-topic simulation-default-look-all-p joint-states-topic pub-joint-states-topic viewer groupname
robot-interface is object for interacting real robot thorugh JointTrajectoryAction servers and JointState topics, this function uses old nervous-style interface for histrical reason. If JointTrajectoryAcion serve is not found, this instance runs as simulation mode, see "Kinematics Simulator" view as simulatied robot,
(setq *ri* (instance robot-interface :init))
(send *ri* :angle-vector (send *robot* :joint-angle) 2000)
(send *ri* :wait-interpolation)
(send *ri* :state :potentio-vector)
:init
&rest args &key ((:robot r))
((:objects objs))
(type :default-controller)
(use-tf2)
((:groupname nh) robot_multi_queue)
((:namespace ns))
((:joint-states-topic jst) joint_states)
((:joint-states-queue-size jsq) 1)
((:publish-joint-states-topic pjst) nil)
((:controller-timeout ct) 3)
((:visuzlization-marker-topic vmt) robot_interface_marker_array)
((:simulation-look-all sim-look-all) t)
&allow-other-keys
- Create robot interface
- robot : class name of robot
- objects : objects to be displayed in simulation (only for simulation)
- type : method name for defineing controller type
- use-tf2 : use tf2
- groupname : ros nodehandle group name
- namespace : ros nodehandle name space
- joint-states-topic jst : name for subscribing joint state topic
- joint-states-queue-size : queue size of joint state topic. if you have two different node publishing joint_state, better to use 2. default is 1
- publish-joint-state-topic : name for publishing joint state topic (only for simulation)
- conter-timeout : timeout seconds for controller lookup
- visualizatoin-marker-topic : topic name for visualize
:angle-vector-safe
av tm &key (simulation)
(yes)
(wait t)
(norm-threshold 120)
(angle-threshold 80)
(velocity-threshold 240)
- send joint angle to robot with user-level confirmation. This method requires key input
:angle-vector
av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1)
(min-time 1.0)
(end-coords-interpolation nil)
- Send joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops.
- av : joint angle vector [rad]
- tm : (time to goal in [msec])
if designated tm is faster than fastest speed, use fastest speed
if not specified, it will use 1/scale of the fastest speed .
if :fastest is specefied use fastest speed calcurated from max speed - ctype : controller method name
- start-time : time to start moving
- scale : if tm is not specified, it will use 1/scale of the fastest speed
- min-time : minimum time to for time to goal
- end-coords-interpolation : set t if you want to move robot in cartesian space interpolation
:angle-vector-sequence
avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1)
(min-time 0.0)
(end-coords-interpolation nil)
- Send joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops.
:wait-interpolation
&optional (ctype) (timeout 0)
- Wait until last sent motion is finished
- ctype : controller to be wait
- timeout : max time of for waiting
- return values is a list of interpolatingp for all controllers, so (null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped
:interpolatingp
&optional (ctype)
- Check if the last sent motion is executing
return t if interpolating
:wait-interpolation-smooth
time-to-finish &optional (ctype)
- Return time-to-finish [msec] before the sent command is finished. Example code are:
(dolist (av (list av1 av2 av3 av4)) (send *ri* :angle-vector av) (send *ri* :wait-interpolation-smooth 300))
Return value is a list of interpolatingp for all controllers, so (null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped
:interpolating-smoothp
time-to-finish &optional (ctype)
- Check inf the last sent motion will stops for next time-to-finish [msec]
:stop-motion
&key (stop-time 0)
- Stop current executing motion
:cancel-angle-vector
&key ((:controller-actions ca) controller-actions)
((:controller-type ct) controller-type)
(wait)
- Cancerl current joint trajetory action
:worldcoords
- Returns world coords of robot, This method uses caced data
:torque-vector
- Return torque vector of robot, This method uses caced data
:potentio-vector
- Retuns current robot angle vector, This method uses caced data
:reference-vector
- Return reference joint angle vector, This method uread current state.
:actual-vector
- Returns current robot angle vector, This method read curren tstate.
:error-vector
- Returns error vector of the robot, This method read current state.
:state
&rest args
- Read robot state and update internal robot instance
- :wait-until-update nil wait until joint_state is updated
:find-object
name-or-obj
- Returns objects with given name or object of the same name.
:simulation-modep
- Check if simulation mode
:warningp
&optional (w :dummy)
- When warning mode, it wait for user's key input before sending angle-vector to the robot
:go-pos
x y &optional (d 0)
- move robot toward x, y, degree and wait to reach that goal. return t if reached or nil if fail
the robot moves relative to current position.
using [m] and [degree] is historical reason from original hrpsys code
:go-pos-no-wait
x y &optional (d 0)
- no-wait version of :go-pos. this function is always assumed to return t
:go-wait
- wait until :go-pos-no-wait reached to the goal, return t if reached or nil if fail
:go-velocity
x y d &optional (msec 1000) &key (stop t)
(wait)
- move robot at given speed for given msec.
if stop is t, robot stops after msec, use wait t for blocking call
return nil if aborted while waiting by enabling :wait, otherwise return t
:go-stop
- stop go-velocity. return t if robot successfly stops, otherwise return nil
:gripper
&rest args
- get information about gripper
:nod
&key (angle 40)
(time 3000)
- Nod robot head
:eus-mannequin-mode
tmp-robot limb &key ((:time tm) 1000)
((:viewer vwr) viewer)
- Euslisp version mannequin mode.
In every loop, :angle-vector is continuously updated.
If you want to stop this mode, press Enter. - tmp-robot : argument for robot instance and angle-vector of tmp-robot is updated in this method.
- limb : mannequin mode limb such as :rarm, :larm, :rleg, and :lleg.
- tm : angle-vector update time [ms]. 1000 by default.
:add-controller ctype &key (joint-enable-check) (create-actions)
:robot-interface-simulation-callback
:publish-joint-state &optional (joint-list (send robot :joint-list))
:angle-vector-duration start end &optional (scale 1) (min-time 1.0) (ctype controller-type)
:angle-vector-simulation av tm ctype
:state-vector type &key ((:controller-actions ca) controller-actions) ((:controller-type ct) controller-type)
:send-ros-controller action joint-names starttime trajpoints
:set-robot-state1 key msg
:ros-state-callback msg
:wait-until-update-all-joints tgt-tm
:update-robot-state &key (wait-until-update nil)
:default-controller
:sub-angle-vector v0 v1
:robot &rest args
:viewer &rest args
:objects &optional objs
:set-simulation-default-look-at look-at-p
:draw-objects &key look-all
:joint-action-enable &optional (e :dummy)
:spin-once
:send-trajectory joint-trajectory-msg &key ((:controller-actions ca) controller-actions) ((:controller-type ct) controller-type) (starttime 1) &allow-other-keys
:send-trajectory-each action joint-names traj &optional (starttime 0.2)
:ros-wait tm &key (spin) (spin-self) (finish-check) &allow-other-keys
:joint-trajectory-to-angle-vector-list move-arm joint-trajectory &key ((:diff-sum diff-sum) 0) ((:diff-thre diff-thre) 50) (show-trajectory t) (send-trajectory t) ((:speed-scale speed-scale) 1.0) &allow-other-keys
:show-goal-hand-coords coords move-arm
:find-descendants-dae-links l
:show-mesh-traj-with-color link-body-list link-coords-list &key ((:lifetime lf) 20) (ns robot_traj) ((:color col) #f(0.5 0.5 0.5))
robot-move-base-interface
- :super robot-interface
- :slots move-base-action move-base-trajectory-action move-base-goal-msg move-base-goal-coords move-base-goal-map-to-frame base-frame-id odom-topic go-pos-unsafe-goal-msg current-goal-coords
: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 ]
: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 ]
:init &rest args &key (move-base-action-name move_base) ((:base-frame-id base-frame-id-name) /base_footprint) (base-controller-action-name /base_controller/follow_joint_trajectory) ((:odom-topic odom-topic-name) /base_odometry/odom) &allow-other-keys
:odom-callback msg
:go-stop &optional (force-stop t)
:make-plan st-cds goal-cds &key (start-frame-id /world) (goal-frame-id /world)
:move-to coords &rest args &key (no-wait nil) &allow-other-keys
:move-to-send coords &key (frame-id /world) (wait-for-server-timeout 5) (count 0) &allow-other-keys
:move-to-wait &rest args &key (retry 10) (frame-id /world) &allow-other-keys
:go-waitp
:go-pos x y &optional (d 0)
:go-pos-no-wait x y &optional (d 0)
:go-wait
:go-velocity x y d &optional (msec 1000) &key (stop t) (wait)
:go-pos-unsafe &rest args
:go-pos-unsafe-no-wait x y &optional (d 0)
:go-pos-unsafe-wait
:state &rest args
ros-interface
- :super robot-interface
- :slots nil
:init &rest args
make-robot-interface-from-name
name &rest args
- make a robot model from string: (make-robot-model "pr2")
init-robot-from-name
robot-name &rest args
- call ${robot}-init function
clear-costmap
&key (node-name /move_base_node)
- reset local costmap and clear unknown grid around robot
change-inflation-range
&optional (range 0.2) &key (node-name /move_base_node)
(costmap-name local_costmap)
(inflation-name inflation)
- change inflation range of local costmap
robot-init
&optional (robot-name (ros::get-param /robot/type))
- Initialize robot-model and robot-interface instances respectively, such as pr2 and ri.
Behavior:- Without /robot/type and argument, or no interface file is found, robot instance and interface instance are not created and errors are invoked.
- If argument are specified and robot interface file is found, robot instance and interface with given name are created.
- If no argument is specified, /robot/type ROSPARAM is set, and robot interface file is found, robot instance and interface with given name are created.
Typical usage:
(robot-init) ;; With setting /robot/type ROSPARAM anywhere.
(robot-init (or (ros::get-param "/robot/type") "pr2")) ;; If /robot/type ROSPARAM is specified, use it. Otherwise, use "pr2" by default.
Configuring user-defined robot:
This function searches robot interface file from rospack plugins.
If user want to use their own robots from robot-init function,
please write export tag in [user_defined_rospackage]/package.xml as pr2eus/package.xml.
- Without /robot/type and argument, or no interface file is found, robot instance and interface instance are not created and errors are invoked.
joint-list->joint_state jlist &key (position) (effort 0) (velocity 0)
apply-joint_state jointstate robot
apply-trajectory_point names trajpoint robot
apply-joint_trajectory joint-trajectory robot &optional (offset 200.0)