[pr2eus] Set joint-action-enable from argument for creating kinematics-simulator

[tkmtnt7000]

In this pull request, joint-action-enable variable can be set from the argument.

This change allows robots that do not support follow_joint_trajectory to set the (send *ri* :simulation-modep) value correctly (such as Spot) , so that kinematics-simulator can be used.

The new keyword argument is named joint-action-enable, which is the same name as :joint-action-enable in the member function.

jsk_pr2eus/pr2eus/robot-interface.l

Line 1016 in 27066da

(:joint-action-enable (&optional (e :dummy)) (if (not (eq e :dummy)) (setq joint-action-enable e)) joint-action-enable)

It seems complicate things. I couldn't think of a better name for the variable, so if someone hits on a better name (or should change the name), please let me know.

cc:@708yamaguchi

[708yamaguchi]

Thank you for your PR @tkmtnt7000

The original problem is we cannot create Kinematics Simulator in spot-interface.

Since spot-interface does not use follow_joint_trajectory_action, joint-action-enable is never set to nil in the following part.
(In other words, joint-action-enable is always t.)

jsk_pr2eus/pr2eus/robot-interface.l

Lines 243 to 250 in 27066da

	(unless (and joint-action-enable (send action :wait-for-server controller-timeout))
	(ros::ros-warn "~A is not respond, ~A-interface is disabled" action (send robot :name))
	(ros::ros-warn "~C[3~CmStarting 'Kinematics Simulator'~C[0m" #x1b 49 #x1b)
	(ros::ros-warn "~C[3~Cm (If you do not intend to start Kinematics Simulator, make sure that you can run 'rostopic info /~A/goal' and 'rostopic info /~A/cancel' and check whether Subscribers exists. If there is no Subscribers, please check joint_trajectory_action node.)~C[0m" #x1b 52 (send action :name) (send action :name) #x1b)
	(ros::ros-warn "~C[3~Cm (Please also check 'rostopic info /~A/feedback' and 'rostopic info /~A/result' and check whether Publishers exists. If there is no Publishers, please check joint_trajectory_action node.)~C[0m" #x1b 52 (send action :name) (send action :name) #x1b)
	(ros::ros-warn "~C[3~Cm (If joint_trajectory_action node already exists, you might have a network problem. Please make sure that you can run 'rosnode ping JOINT_TRAJECTORY_ACTION_SERVER_NODE_NAME' and 'rosnode ping ~A')~C[0m" #x1b 52 (ros::get-name) #x1b)
	(when joint-enable-check
	(setq joint-action-enable nil)

In this case, (send self :simulation-modep) always returns nil.

jsk_pr2eus/pr2eus/robot-interface.l

Line 1017 in 27066da

(:simulation-modep () "Check if in simulation mode" (null joint-action-enable))

So we cannot create Kinematics Simulator in :init method.

jsk_pr2eus/pr2eus/robot-interface.l

Lines 190 to 221 in 27066da

	(when (send self :simulation-modep)
	(let ((old-viewer user::*viewer*))
	(when (and x::*display* (> x::*display* 0))
	(setq viewer (get (geo::find-viewer (send robot :name)) :pickviewer))
	(unless viewer
	(setq viewer (instance x::irtviewer :create :title (format nil "~A Kinematics Simulator" (send robot :name)) :view-name (send robot :name) :draw-floor t)))
	(send viewer :objects (list robot))
	(send self :draw-objects)
	(if old-viewer (setq user::*viewer* old-viewer)))
	(send self :objects objs)
	(ros::advertise (cond
	(pub-joint-states-topic pub-joint-states-topic)
	(namespace
	(format nil "~A/~A" namespace joint-states-topic))
	(t joint-states-topic))
	sensor_msgs::JointState)
	(setq *top-selector-interval* 0.01)
	(setq simulation-default-look-all-p sim-look-all)
	;; remove old timer job
	(setq *timer-job*
	(remove-if #'(lambda (x)
	(if (listp x)
	(let ((f (caddr x)))
	(when (and (>= (length f) 3)
	(eq (class (elt f 1)) (class self))
	(eq (elt f 2) :robot-interface-simulation-callback))
	(warning-message 3 "remove ~A from *timer-job*~%" f)
	t))))
	*timer-job*))
	(pushnew `(lambda () (send ,self :robot-interface-simulation-callback)) *timer-job*)
	(warning-message 3 "current *timer-job* is ~A~%" *timer-job*)
	))

[tkmtnt7000]

@708yamaguchi
Thank you very much for your supplementary information.
That's exactly what I wanted to say.

[Affonso-Gui]

Just for clarification, you want to have the option to start the Kinematics Simulator while connected to the real robot or just be able to use it when working offline?

[tkmtnt7000]

I assume the latter one.
Currently in spot-interface, Kinematics Simulator is not created when working offline, so I would like to be able to use it when not connected to the real robot.

[Affonso-Gui]

Being so, is there no alternative to autonomously detect if connected to the real robot and set joint-action-enable to nil when not connected?
Which kind of action server does the spot work with?

The arm controller is listed as FollowJointTrajectoryAction, is this wrong or something like a dummy code? https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_kinova_robot/kinovaeus/kinova-interface.l#L70

[708yamaguchi]

The arm controller is listed as FollowJointTrajectoryAction, is this wrong or something like a dummy code? jsk-ros-pkg/jsk_robot@master/jsk_kinova_robot/kinovaeus/kinova-interface.l#L70

As you said, kinova arm uses FollowJointTrajectoryAction.

Which kind of action server does the spot work with?

We are referring to spot robot that does not have kinova arm.
In this case, spot does not use FollowJointTrajectoryAction.

[Affonso-Gui]

So, for spots without kinova the default-controller is null https://github.com/sktometometo/jsk_robot/blob/develop/spot/jsk_kinova_robot/kinovaeus/kinova-interface.l , and therefore this loop

jsk_pr2eus/pr2eus/robot-interface.l

Line 240 in 27066da

(dolist (action tmp-actions)

is never reached and joint-action-enable remains as the default value of t.

It seems to me that it would make sense to set joint-action-enable to nil if there are no joint controllers, but this would probably mess up with the real-robot interfacing (such as :move-to) since joint-action-enable is currently the only variable for judging if connected to the real robot or not.

Maybe merge this now and then later on think on how we could better conciliate robots with no joint controller to the robot-interface? (e.g. overwrite :simulation-modep, etc)

@tkmtnt7000

[tkmtnt7000]

Certainly, it may be good to think about how to deal with robots no joint controller for the future. (because it's not that I want this to be merged soon for my thesis....)

[k-okada]

how about override :add-controller?

(load "package://pr2eus/robot-interface.l")
(load "irteus/demo/sample-robot-model.l")

(defclass turtlebot-interface
  :super robot-move-base-interface
  :slots ()
  )
(defmethod turtlebot-interface
  (:init
   (&rest args)
   (prog1
       (send-super* :init :robot (instance sample-robot :init) :base-frame-id "base_footprint" :cmd-vel-topic "/cmd_vel" :odom-topic "/odom_combined" :base-controller-action-name nil args)
     ))
  (:default-controller () ) ;; turtlebot does not provide any JTA controllers                                                                                                                                               
  (:add-controller
   (&rest args)
   (ros::ros-info "Wait 1 sec for ~A..." (if namespace (format nil "~A/~A" namespace joint-states-topic) joint-states-topic))
   (dotimes (i 10)
     (send self :spin-once) ;; for :state :potentio-vector                                                                                                                                                                  
     (unix:usleep 100000))
   (when (cdr (assoc :stamp robot-state))
     (print (send (ros::time- (ros::time-now) (cdr (assoc :stamp robot-state))) :to-sec)))
   (unless (and robot-state (assoc :stamp robot-state)
                (cdr (assoc :stamp robot-state))
                (< (send (ros::time- (ros::time-now) (cdr (assoc :stamp robot-state))) :to-sec) 5.0))
     (ros::ros-warn "~A is not respond, ~A-interface is disabled" (if namespace (format nil "~A/~A" namespace joint-states-topic) joint-states-topic) (send robot :name))
     (ros::ros-warn "~C[3~CmStarting 'Kinematics Simulator'~C[0m" #x1b 49 #x1b)
     (setq joint-action-enable nil)))
  )

(setq *ri* (instance turtlebot-interface :init))