udpate package to use pepper_robot from ros-naoqi

.rosinstall

@@ -24,6 +24,18 @@
     uri: https://github.com/tork-a/rwt_ros.git
     local-name: rwt_ros
 - git:
-    uri: https://github.com/PR2/pr2_calibration.git
+#    uri: https://github.com/PR2/pr2_calibration.git
+    uri: https://github.com/k-okada/pr2_calibration.git
     local-name: pr2_calibration
+#    version: hydro-devel
+    version: add_cmake_modules
+
+# temporally added until next release because of broken structure in the packages from apt
+- git:
+    uri: https://github.com/PR2/pr2_gripper_sensor.git
+    local-name: pr2_gripper_sensor
+    version: hydro-devel
+- git:
+    uri: https://github.com/PR2/pr2_apps.git
+    local-name: pr2_apps
     version: hydro-devel

.travis.yml

@@ -6,13 +6,16 @@ python:
 compiler:
   - gcc
 env:
-  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false
-  - ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
+  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=true
+  - ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false NOT_TEST_INSTALL=true EXTRA_DEB="ros-hydro-convex-decomposition ros-hydro-ivcon"
+  - ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=true
+  - ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false NOT_TEST_INSTALL=true EXTRA_DEB="ros-indigo-convex-decomposition ros-indigo-ivcon"
 matrix:
   allow_failures:
-    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false
+    - env: ROS_DISTRO=hydro  ROSWS=wstool BUILDER=catkin    USE_DEB=false NOT_TEST_INSTALL=true EXTRA_DEB="ros-hydro-convex-decomposition ros-hydro-ivcon"
+    - env: ROS_DISTRO=indigo  ROSWS=wstool BUILDER=catkin   USE_DEB=false NOT_TEST_INSTALL=true EXTRA_DEB="ros-indigo-convex-decomposition ros-indigo-ivcon"
 before_script:
-  - export ROS_PARALLEL_JOBS="-j2 -l2"
+  - export ROS_PARALLEL_JOBS="-j1 -l1"
 script: source .travis/travis.sh
 notifications:
   email:

jsk_baxter_robot/baxtereus/CHANGELOG.rst

@@ -0,0 +1,65 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package baxtereus
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.6 (2015-04-10)
+------------------
+* [baxter-interface.l] we found that input data must be larget then 3, and add dummy last element works very nice!
+* Contributors: Yuto Inagaki
+
+0.0.5 (2015-04-08)
+------------------
+* [baxter-interface.l] fix typo
+* [baxter-interface.l] overwrite :angle-vector-seuqnce for tm = :fast
+* [baxter-interface.l] notify this warning is ok
+* [baxtereus] add head action client for baxter
+* Contributors: Yuto Inagaki
+
+0.0.4 (2015-01-30)
+------------------
+* currently we do not generate baxter.l from baxter_description on the fly
+* [baxtereus] add wait key for stop-grasp in baxter-interface.l
+* add groupname for baxter-interface.l
+
+0.0.3 (2015-01-09)
+------------------
+
+0.0.2 (2015-01-08)
+------------------
+* add install commands to cmake
+* add baxter-moveit.l
+* Contributors: Kei Okada, Yuto Inagaki
+
+0.0.1 (2014-12-25)
+------------------
+* fix version number
+* add wait time for suction
+* get baxter hand type property
+* fix baxter endcoords and rotate 90
+* add action joint client left_w2 right_w2
+* do not disable joint-action-enable if gripper action is not found, gazebo did not provide gripper joint action for now
+* add tuck-pose and untuck-pose, thanks to wkentaro, iory
+* update baxter.yaml (add wrist yaw, head end-coords) baxter.l
+* add baxter nod function (send *ri* :nod)
+* update baxtereus to use gripper action server
+* add reset-manip-pose
+* add baxter eus sample
+* add :set-baxter-face interface
+* do not generate baxter.l if already exists
+* add start-grasp and stop-grasp for baxter
+* depent to pr2eus speak.l
+* add camera interface
+* add sound tools and eus speak-en
+* fix end-coords
+* add baxter.l since baxter_simple.urdf is not released yet
+* add code to use baxter_simple.urdf
+* add roseus/preus to rundepend
+* fix cmake syntax error
+* fix for baxter_description is installed
+* add missing depends
+* change the reset pose
+* add baxter-interface.l, validated with 73B2 baxter
+* add depends to collada2eus
+* use _simple model for smaller dae/lisp files
+* add jsk_baxter_robot
+* Contributors: Kei Okada, Kentaro Wada, Ryohei Ueda, Tomoya Yoshizawa, Yuto Inagaki, Shintaro Noda

jsk_baxter_robot/baxtereus/CMakeLists.txt

@@ -7,78 +7,9 @@ project(baxtereus)
 find_package(catkin REQUIRED COMPONENTS
   collada_urdf
   euscollada
-  baxter_description
+#  baxter_description
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependencies might have been
-##     pulled in transitively but can be declared for certainty nonetheless:
-##     * add a build_depend tag for "message_generation"
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES baxtereus
@@ -90,29 +21,6 @@ catkin_package(
 ## Build ##
 ###########
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a cpp library
-# add_library(baxtereus
-#   src/${PROJECT_NAME}/baxtereus.cpp
-# )
-
-## Declare a cpp executable
-# add_executable(baxtereus_node src/baxtereus_node.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(baxtereus_node baxtereus_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(baxtereus_node
-#   ${catkin_LIBRARIES}
-# )
 if(EXISTS ${baxter_description_SOURCE_DIR}/urdf)
   set(_baxter_urdf ${baxter_description_SOURCE_DIR}/urdf)
 else()
@@ -148,50 +56,9 @@ if(NOT EXISTS ${PROJECT_SOURCE_DIR}/baxter.l)
   add_custom_target(compile_baxter ALL DEPENDS ${PROJECT_SOURCE_DIR}/baxter.l)
 endif()
 
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS baxtereus baxtereus_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
 
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_baxtereus.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+install(DIRECTORY euslisp
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
 
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+install(FILES baxter.l baxter-interface.l baxter-moveit.l baxter.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

jsk_baxter_robot/baxtereus/baxter-interface.l

@@ -14,29 +14,32 @@
 			      right-gripper-type left-gripper-type))
 (defmethod baxter-interface
   (:init (&rest args)
-   (prog1 (send-super :init :robot baxter-robot :joint-states-topic "/robot/joint_states")
+   (prog1 (send-super :init :robot baxter-robot :joint-states-topic "/robot/joint_states" :groupname "baxter_interface")
      (send self :add-controller :larm-controller)
      (send self :add-controller :rarm-controller)
+     (send self :add-controller :head-controller)
      (ros::advertise "/robot/end_effector/right_gripper/command" baxter_core_msgs::EndEffectorCommand 5) 
      (ros::advertise "/robot/end_effector/left_gripper/command" baxter_core_msgs::EndEffectorCommand 5) 
      (ros::advertise "/robot/xdisplay" sensor_msgs::Image 1)
      (ros::advertise "/robot/head/command_head_nod" std_msgs::Bool 1)
-     (ros::subscribe "/robot/end_effector/right_gripper/properties" baxter_core_msgs::EndEffectorProperties #'send self :right-property-cb)
-     (ros::subscribe "/robot/end_effector/left_gripper/properties" baxter_core_msgs::EndEffectorProperties #'send self :left-property-cb)
+     (ros::subscribe "/robot/end_effector/right_gripper/properties" baxter_core_msgs::EndEffectorProperties #'send self :right-property-cb :groupname groupname)
+     (ros::subscribe "/robot/end_effector/left_gripper/properties" baxter_core_msgs::EndEffectorProperties #'send self :left-property-cb :groupname groupname)
 
      (setq right-gripper-action (instance ros::simple-action-client :init
 					  "robot/end_effector/right_gripper/gripper_action"
 					  control_msgs::GripperCommandAction
-					  ))
+					  :groupname groupname))
      (setq left-gripper-action (instance ros::simple-action-client :init
 					 "robot/end_effector/left_gripper/gripper_action"
 					 control_msgs::GripperCommandAction
+					 :groupname groupname
 					 ))
      (if (ros::has-param "~wait_for_suction")
 	 (setq *wait-for-suction* (read-from-string (ros::get-param "~wait_for_suction"))))
      (dolist (action (list right-gripper-action left-gripper-action))
        (unless (and joint-action-enable (send action :wait-for-server 3))
 	 (ros::ros-warn "~A is not respond" action)
+	 (ros::ros-info "*** if you do not have gripper, you can ignore this message ***")
 	 (return)))
 
      (setq gripper-sequence-id 0)
@@ -51,7 +54,8 @@
   (:default-controller ()
    (append
     (send self :larm-controller)
-    (send self :rarm-controller)))
+    (send self :rarm-controller)
+    (send self :head-controller)))
   (:larm-controller ()
    (list
     (list
@@ -66,6 +70,13 @@
      (cons :controller-state "/robot/limb/right/state")
      (cons :action-type control_msgs::FollowJointTrajectoryAction)
      (cons :joint-names (list "right_s0" "right_s1" "right_e0" "right_e1" "right_w0" "right_w1" "right_w2")))))
+  (:head-controller ()
+   (list
+    (list
+     (cons :controller-action "/robot/head/head_action")
+     (cons :controller-state "/robot/head/head_state")
+     (cons :action-type control_msgs::SingleJointPositionAction)
+     (cons :joint-names (list "head-neck-y")))))
   (:close-head-camera ()
     (send self :close-camera "head_camera")
     )
@@ -131,8 +142,8 @@
    )
 
   (:stop-grasp
-   (&optional (arm :arms) &key (effort 50))
-   (send self :go-grasp arm :pos 100 :effort effort)
+   (&optional (arm :arms) &key (effort 50) (wait nil))
+   (send self :go-grasp arm :pos 100 :effort effort :wait wait)
    (if (or ( and (equal arm :arms) (or (equal right-gripper-type *suction*) (equal left-gripper-type *suction*)))
 	   ( and (equal arm :rarm) (equal right-gripper-type *suction*))
 	   ( and (equal arm :larm) (equal left-gripper-type *suction*)))
@@ -217,6 +228,23 @@
      (ros::publish "/robot/head/command_head_nod" msg)
      )
    )
+  (:angle-vector (av &optional (tm :fast) (ctype controller-type) (start-time 0) &rest args)
+		 (send* self :angle-vector-sequence (list av) (list tm) ctype start-time args))
+  (:angle-vector-sequence (avs &optional (tms :fast) (ctype controller-type) (start-time 0) &key (scale 2.2) (min-time 0.05))
+		 ;; force add current position to the top of avs
+		 (if (atom tms) (setq tms (list tms)))
+		 (push (send self :state :potentio-vector) avs)
+		 (push 50 tms)
+		 (when (= (length avs) 2) ;; when input avs is 1
+		   (setq avs (list (elt avs 0) (midpoint 0.5 (elt avs 0) (elt avs 1)) (elt avs 1)))
+		   (cond ((numberp (elt tms 1))
+			  (setq tms (list (elt tms 0) (/ (elt tms 1) 2) (/ (elt tms 1) 2))))
+			 (t
+			  (setq tms (list (elt tms 0) (elt tms 1) (elt tms 1))))))
+		 (when (= (length avs) 3) ;; when input avs is 1 or 2
+		   (setq avs (append avs (list (elt avs 2))))
+		   (setq tms (append tms (list 50))))
+		 (send-super :angle-vector-sequence avs tms ctype start-time :scale scale :min-time min-time))
   )