[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. https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/robot-interface.l#L1016 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.)

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/robot-interface.l#L243-L250

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

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/robot-interface.l#L1017

So we cannot create Kinematics Simulator in :init method.

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/robot-interface.l#L190-L221

[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 https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/robot-interface.l#L240 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))