[Do not merge] Refactor the turtueblteus model as the derived model from robot-move-base-interface

dxl_armed_turtlebot/README.md

@@ -0,0 +1,60 @@
+## Smaples in Gazebo
+
+まずは、下記のコマンドを実行し、gazeboシステム及び仮想ロボットを立ち上げます
+```
+$ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch
+```
+
+その場合、以下のような画面になりと思います。
+
+![gazebo_turtlebot](images/gazebo_turtlebot2.png)
+
+#### 注意:
+gazeboのgui (右図) を立ち上げると、PCの処理負荷が一気に上がるので、通常は`gui:=false`というオプションをつけて、rvizのみ表示させることを推奨します。
+その場合、コマンドは以下のようになります。
+
+```
+$ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch gui:=false
+```
+
+### Grasping test in gazebo via roseus
+
+以下のコマンドを実行し、roseusから目の前にある物体を掴んでみましょう。
+
+```
+$ roscd dxl_armed_turtlebot/euslisp
+$ roseus gazebo-grasp-test.l
+```
+
+このサンプルでは、eusのロボットモデルである`*ri*`を駆使して、台車移動、アーム操作、グリッパー操作という基本動作を実行しています。
+成功すると以下のようになります(gazebo guiの表示画面)。
+
+![gazebo_turtlebot_grasp](images/gazebo_turtlebot_grasp.png)
+
+### Object recognition and approaching via roseus
+前の例では、物体認識を行っておりません。物体認識を踏まえた自律動作は以下のように実行します。
+
+1. 物体認識を実行する. これは演習で習ったもので、詳細は演習資料を参考してください。
+   ```
+   $ roslaunch dxl_armed_turtlebot hsi_color_filter.launch DEFAULT_NAMESPACE:=/camera/depth INPUT:=points h_min:=-120 h_max:=-20 s_min:=120
+   ```
+2. 認識結果を元に、自律行動を実行する
+   ```
+   $ roscd dxl_armed_turtlebot/euslisp
+   $ roseus gazebo-grasp-with-recognition.l
+   ```
+
+### SLAM in Gazebo
+gazebo内でも地図作成は、実機と同様、下記のコマンドを実行するだけ:
+```
+$ rosrun slam_karto slam_karto
+```
+
+rviz上で、2D地図ができていることが確認できるでしょう。
+ロボットの移動はroseusからでもできるが、ここでは、下記のコマンドを紹介します:
+```
+$ roslaunch turtlebot_teleop keyboard_teleop.launch
+```
+機体を一周旋回させると、以下のようになります。
+
+![gazebo_turtlebot_slam](images/gazebo_turtlebot_slam.png)

dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml

@@ -3,50 +3,63 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50  
 # Trajectory Controllers ---------------------------------------
-trajectory_controller:
+fullbody_controller:
   #type: effort_controllers/JointTrajectoryController
   type: velocity_controllers/JointTrajectoryController
   joints:
-    - arm_link1_joint
-    - arm_link2_joint
-    - arm_link3_joint
-    - arm_link4_joint
-    - arm_link5_joint
-    - arm_link6_joint
-    - arm_link7_joint
+    - arm_joint1
+    - arm_joint2
+    - arm_joint3
+    - arm_joint4
+    - arm_joint5
+    - arm_joint6
   constraints:
     goal_time: 0.5                   # Override default
     stopped_velocity_tolerance: 0.02 # Override default
-    arm_link1_joint:
+    arm_joint1:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link2_joint:
+    arm_joint2:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link3_joint:
+    arm_joint3:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link4_joint:
+    arm_joint4:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link5_joint:
+    arm_joint5:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link6_joint:
+    arm_joint6:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
-    arm_link7_joint:
+
+  gains: # Required because we're controlling an effort interface
+    arm_joint1: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint2: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint3: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint4: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint5: {p: 25.0, i: 0.0, d: 0.0}
+    arm_joint6: {p: 25.0, i: 0.0, d: 0.0}
+  state_publish_rate:  100            # Override default
+  action_monitor_rate: 100            # Override default
+  stop_trajectory_duration: 0        # Override default
+
+gripper_controller:
+  #type: effort_controllers/JointTrajectoryController
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+    - gripper_joint
+  constraints:
+    goal_time: 0.5                   # Override default
+    stopped_velocity_tolerance: 0.02 # Override default
+    gripper_joint:
       trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
 
   gains: # Required because we're controlling an effort interface
-    arm_link1_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link2_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link3_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link4_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link5_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link6_joint: {p: 25.0, i: 0.0, d: 0.0}
-    arm_link7_joint: {p: 5.0, i: 0.0, d: 0.0}
+    gripper_joint: {p: 10.0, i: 0.0, d: 0.0}
   state_publish_rate:  100            # Override default
   action_monitor_rate: 100            # Override default
   stop_trajectory_duration: 0        # Override default

dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l

@@ -13,7 +13,7 @@
 ;; アーム台車ロボットのrobot-interfaceクラス定義
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 (defclass dxl-armed-turtlebot-interface
-  :super robot-interface
+  :super robot-move-base-interface
   :slots (gripper-action))
 
 (eval `(defmethod dxl-armed-turtlebot-interface

dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l

@@ -0,0 +1,22 @@
+;; robotの初期化
+(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
+(dxl-armed-turtlebot-init)
+
+;; arm approach  pose
+(send *ri* :angle-vector #f(0 0 -90 0 90 0) 3000)
+;;(send *ri* :wait-interpolation) ;; does not work?
+(unix:sleep 4)
+
+;; open gripper
+(send *ri* :stop-grasp)
+
+;; go-pos
+(send *ri* :go-pos 0.56 0 1)
+(unix:sleep 6)
+
+;; grasp
+;; still some bug in start-grasp, so we have to directly use :move-gripper
+(send *ri* :move-gripper -30 :tm 1500 :wait t)
+(unix:sleep 2)
+
+(send *ri* :angle-vector #f(90 0 45 0 45 45)) ;;  arm tuck pose

dxl_armed_turtlebot/euslisp/gazebo-grasp-with-recognition.l

@@ -0,0 +1,89 @@
+;; robotの初期化
+(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l")
+(dxl-armed-turtlebot-init)
+
+(ros::load-ros-manifest "jsk_recognition_msgs")
+
+(defvar *topic-name* "/camera/depth/boxes")
+(defvar *bounding-box-list* nil)
+
+(setq found-obj nil)
+(setq obj-pos #f(0 0 0))
+
+;; ros::initする
+(ros::roseus "boundingboxarray_subscriber")
+
+;; コールバック関数
+(defun bounding-box-array-cb (msg)
+  (setq *bounding-box-list* (send msg :boxes)) ;; boxesは、BoundingBoxのArray(Euslispではlist)
+  (when *bounding-box-list*
+    (let* ((b (elt *bounding-box-list* 0))
+           (cam->obj-coords (ros::tf-pose->coords (send b :pose)))
+           (cam-coords (send (send *dxl-armed-turtlebot* :camera_depth_optical_frame_lk) :copy-worldcoords)))
+      (setq obj-pos (scale 0.001 (send (send cam-coords :transform cam->obj-coords) :worldpos)))
+      (setq found-obj t)
+      obj-pos))
+  )
+
+(ros::subscribe *topic-name* jsk_recognition_msgs::BoundingBoxArray #'bounding-box-array-cb 1)
+
+(setq found-obj nil)
+(until found-obj
+       (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう
+       (ros::spin-once)
+       (ros::sleep)
+  )
+
+(ros::ros-warn "found target ojbect ~A, appoaching" obj-pos)
+
+(setq 2d-pos (float-vector (elt obj-pos 0) (elt obj-pos 1)))
+
+(send *ri* :go-pos
+      (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 0)
+      (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 1)
+      (rad2deg (atan  (elt obj-pos 1) (elt obj-pos 0))))
+
+;; open gripper
+(send *ri* :stop-grasp)
+
+
+
+(setq found-obj nil)
+(until found-obj
+       (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう
+       (ros::spin-once)
+       (ros::sleep)
+  )
+
+(ros::ros-warn "re-found target object ~A" obj-pos)
+(setq target-cds (make-coords :pos (scale 1000 obj-pos)))
+(send target-cds :translate #f(-200 0 100)) ;;z should be 0, but the link is not rigid in gazebo, so 100 is the height offset for end effector.
+(objects (list *dxl-armed-turtlebot* target-cds))
+
+(send *dxl-armed-turtlebot* :angle-vector #f(0 0 -90 0 90 0))
+(send *dxl-armed-turtlebot* :inverse-kinematics target-cds :rotation-axis :y)
+
+
+(send *dxl-armed-turtlebot* :angle-vector (map float-vector #'(lambda(ang)
+                                     (cond
+                                      ((> ang 90) (- ang 180))
+                                      ((< ang -90) (+ ang 180))
+                                      (t ang)
+                                      ))
+                                 (send *dxl-armed-turtlebot* :angle-vector)))
+
+(ros::ros-warn "the grap arm pose is ~A" (send *dxl-armed-turtlebot* :angle-vector))
+
+(send *ri* :angle-vector  (send *dxl-armed-turtlebot* :angle-vector) 3000)
+
+#|
+(unix:sleep 5)
+(send *ri* :go-pos 0.1 0 0)
+
+;; grasp
+;; still some bug in start-grasp, so we have to directly use :move-gripper
+(send *ri* :move-gripper -30 :tm 1500 :wait t)
+(unix:sleep 2)
+
+(send *ri* :angle-vector #f(80 0 45 0 45 45)) ;;  arm tuck pose
+(send *ri* :go-pos -0.6 0 0) ;;  arm tuck pose

dxl_armed_turtlebot/launch/dxl_armed_turtlebot_gazebo.launch

@@ -53,14 +53,23 @@
     <remap from="$(arg camera)/scan" to="$(arg scan_topic)"/>
   </node>
 
+  <!-- Velocity muxer -->
+  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
+        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
+    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
+    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
+  </node>
+
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <!-- load the controllers -->
   <rosparam file="$(find dxl_armed_turtlebot)/config/dxl_armed_turtlebot_controllers.yaml" />
   <node name="controller_spawner" pkg="controller_manager"
         type="spawner" respawn="false"
         output="screen"
         args="joint_state_controller
-              trajectory_controller
+              fullbody_controller
+              gripper_controller
               "/>
   <!-- send tuck pose -->
   <node name="tuck_arm" pkg="dxl_armed_turtlebot" type="tuck_arm.py" args="-t" />

dxl_armed_turtlebot/map/gazebo_world.yaml

@@ -0,0 +1,7 @@
+image: gazebo_world.pgm
+resolution: 0.050000
+origin: [-3.882911, -3.996318, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

dxl_armed_turtlebot/map/gazebo_world.yaml~

@@ -0,0 +1,7 @@
+image: `rospack find dxl_armed_turtlebot`/map/gazebo_world.pgm
+resolution: 0.050000
+origin: [-3.882911, -3.996318, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

dxl_armed_turtlebot/scripts/tuck_arm.py

@@ -14,12 +14,12 @@ def __init__(self, tuck_cmd):
         self._tuck_rate = rospy.Rate(5.0)  # Hz
         self._tuck_threshold = 0.3  # radians
         self._joint_moves = {
-            'tuck': [pi/2, 0, pi/4, 0, pi/2, pi/2, 0],
-            'untuck': [0, 0, -pi/2, 0, pi/2, 0, 0],
+            'tuck': [pi/2, 0, pi/4, 0, pi/2, pi/2],
+            'untuck': [0, 0, -pi/2, 0, pi/2, 0],
         }
         self._arm_state = 'none'
-        self._pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=1)
-        self._sub = rospy.Subscriber('/trajectory_controller/state', JointTrajectoryControllerState, self._check_arm_state)
+        self._pub = rospy.Publisher('/fullbody_controller/command', JointTrajectory, queue_size=1)
+        self._sub = rospy.Subscriber('/fullbody_controller/state', JointTrajectoryControllerState, self._check_arm_state)
 
     def _check_arm_state(self, msg):
         diff_check = lambda a, b: abs(a - b) <= self._tuck_threshold
@@ -40,13 +40,12 @@ def _prepare_to_tuck(self):
     def _move_to(self, goal):
         traj = JointTrajectory()
         traj.joint_names  = [
-            'arm_link1_joint', 
-            'arm_link2_joint', 
-            'arm_link3_joint',
-            'arm_link4_joint',
-            'arm_link5_joint',
-            'arm_link6_joint',
-            'arm_link7_joint'
+            'arm_joint1', 
+            'arm_joint2', 
+            'arm_joint3',
+            'arm_joint4',
+            'arm_joint5',
+            'arm_joint6',
         ]
         traj.points.append(JointTrajectoryPoint())
         traj.points[0].positions = self._joint_moves[goal]