diff --git a/.rosinstall b/.rosinstall index 24d97ac2c9..1e88c365d5 100644 --- a/.rosinstall +++ b/.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 diff --git a/.travis b/.travis index d0f36a2569..6f6b4f5dbc 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit d0f36a256913411a9d676eadafee8c4bddd0b2e2 +Subproject commit 6f6b4f5dbc3a2fba4bc8a10d20d801e7ac3595df diff --git a/.travis.yml b/.travis.yml index 8759a3ccd8..8295c29164 100644 --- a/.travis.yml +++ b/.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: diff --git a/jsk_baxter_robot/baxtereus/CHANGELOG.rst b/jsk_baxter_robot/baxtereus/CHANGELOG.rst new file mode 100644 index 0000000000..c0ac3d22a8 --- /dev/null +++ b/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 diff --git a/jsk_baxter_robot/baxtereus/CMakeLists.txt b/jsk_baxter_robot/baxtereus/CMakeLists.txt index 740a9f34ee..5684c8403f 100644 --- a/jsk_baxter_robot/baxtereus/CMakeLists.txt +++ b/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}) diff --git a/jsk_baxter_robot/baxtereus/baxter-interface.l b/jsk_baxter_robot/baxtereus/baxter-interface.l index 49b1b19a0b..32edbb6cbc 100644 --- a/jsk_baxter_robot/baxtereus/baxter-interface.l +++ b/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)) ) diff --git a/jsk_baxter_robot/baxtereus/baxter-moveit.l b/jsk_baxter_robot/baxtereus/baxter-moveit.l new file mode 100644 index 0000000000..d78dae31da --- /dev/null +++ b/jsk_baxter_robot/baxtereus/baxter-moveit.l @@ -0,0 +1,45 @@ +(require :baxter-interface + "package://baxtereus/baxter-interface.l") +(require :pr2eus-moveit "package://pr2eus_moveit/euslisp/pr2eus-moveit.l") +(load "package://pr2eus_moveit/tutorials/collision-object-sample.l") + +(defclass baxter-moveit-environment + :super moveit-environment + :slots ()) + +(defmethod baxter-moveit-environment + (:init + (&key ((:robot rb) (baxter))) + (send-super :init :robot rb + :frame-id "WAIST_LINK0" + :multi-dof-joint-name "virtual_joint" + :multi-dof-frame-id "/odom") + ) + (:default-configuration () + (list (list :rarm + (cons :group-name "right_arm") + (cons :target-link + (send self :search-link-from-name "right_s0")) + (cons :joint-list (send robot :rarm :joint-list)) + ) + (list :larm + (cons :group-name "left_arm") + (cons :target-link + (send self :search-link-from-name "left_s0")) + (cons :joint-list (send robot :larm :joint-list)) + ) + ) + ) + ) + +(defun sync-larm (&optional (tm 500)) + (let ((av (send *ri* :state :reference-vector))) + (send *ri* :robot :angle-vector av) + (send *ri* :angle-vector av tm) + (send *ri* :wait-interpolation) + (send *ri* :remove-joint-group "left_arm") + (unix::usleep (* 100 1000)) + (send *ri* :add-joint-group "left_arm" + (list "left_s0" "left_s1" "left_e0" "left_e1" "left_w0" "left_w1" "left_w2")) + (unix::usleep (* 100 1000)) + )) diff --git a/jsk_baxter_robot/baxtereus/sample-for-real-baxter.l b/jsk_baxter_robot/baxtereus/euslisp/sample-for-real-baxter.l similarity index 96% rename from jsk_baxter_robot/baxtereus/sample-for-real-baxter.l rename to jsk_baxter_robot/baxtereus/euslisp/sample-for-real-baxter.l index 07afdcdf66..0b1251f852 100644 --- a/jsk_baxter_robot/baxtereus/sample-for-real-baxter.l +++ b/jsk_baxter_robot/baxtereus/euslisp/sample-for-real-baxter.l @@ -39,4 +39,4 @@ ;;(send *ri* :set-baxter-face FILE_PATH) ) -(ros::ros-info "(main) : start demo") \ No newline at end of file +(ros::ros-info "(main) : start demo") diff --git a/jsk_baxter_robot/baxtereus/package.xml b/jsk_baxter_robot/baxtereus/package.xml index f2b89ec452..63ddf5fdb6 100644 --- a/jsk_baxter_robot/baxtereus/package.xml +++ b/jsk_baxter_robot/baxtereus/package.xml @@ -1,7 +1,7 @@ baxtereus - 0.0.1 + 0.0.6 The baxtereus package Kei Okada @@ -12,7 +12,7 @@ collada_urdf euscollada - baxter_description + collada_urdf euscollada @@ -24,4 +24,4 @@ - \ No newline at end of file + diff --git a/jsk_baxter_robot/jsk_baxter_desktop/CHANGELOG.rst b/jsk_baxter_robot/jsk_baxter_desktop/CHANGELOG.rst new file mode 100644 index 0000000000..fea7ba262e --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_desktop/CHANGELOG.rst @@ -0,0 +1,27 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_baxter_desktop +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ +* change to pure catkin package + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* repair desktop icon +* add jsk baxter desktop +* Contributors: Yuto Inagaki diff --git a/jsk_baxter_robot/jsk_baxter_desktop/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_desktop/CMakeLists.txt index 55d035493b..8d0f585014 100644 --- a/jsk_baxter_robot/jsk_baxter_desktop/CMakeLists.txt +++ b/jsk_baxter_robot/jsk_baxter_desktop/CMakeLists.txt @@ -1,4 +1,170 @@ -if(NOT USE_ROSBUILD) - include(catkin.cmake) - return() -endif() \ No newline at end of file +cmake_minimum_required(VERSION 2.8.3) +project(jsk_baxter_desktop) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + jsk_baxter_startup + rviz +) + +## 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 jsk_baxter_desktop +# CATKIN_DEPENDS jsk_baxter_startup rviz +# DEPENDS system_lib +) + +########### +## 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(jsk_baxter_desktop +# src/${PROJECT_NAME}/jsk_baxter_desktop.cpp +# ) + +## Declare a cpp executable +# add_executable(jsk_baxter_desktop_node src/jsk_baxter_desktop_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(jsk_baxter_desktop_node jsk_baxter_desktop_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(jsk_baxter_desktop_node +# ${catkin_LIBRARIES} +# ) + +############# +## 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 jsk_baxter_desktop jsk_baxter_desktop_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_jsk_baxter_desktop.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + +install(DIRECTORY desktop_shortcut_template icons scripts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +install(FILES baxter_servo_stop.desktop.in DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + + diff --git a/jsk_baxter_robot/jsk_baxter_desktop/catkin.cmake b/jsk_baxter_robot/jsk_baxter_desktop/catkin.cmake deleted file mode 100644 index 7f0c655f79..0000000000 --- a/jsk_baxter_robot/jsk_baxter_desktop/catkin.cmake +++ /dev/null @@ -1,162 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(jsk_baxter_desktop) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - jsk_baxter_startup - rviz -) - -## 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 jsk_baxter_desktop -# CATKIN_DEPENDS jsk_baxter_startup rviz -# DEPENDS system_lib -) - -########### -## 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(jsk_baxter_desktop -# src/${PROJECT_NAME}/jsk_baxter_desktop.cpp -# ) - -## Declare a cpp executable -# add_executable(jsk_baxter_desktop_node src/jsk_baxter_desktop_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(jsk_baxter_desktop_node jsk_baxter_desktop_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(jsk_baxter_desktop_node -# ${catkin_LIBRARIES} -# ) - -############# -## 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 jsk_baxter_desktop jsk_baxter_desktop_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_jsk_baxter_desktop.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/jsk_baxter_robot/jsk_baxter_desktop/desktop_shortcut_template/baxter_startup.desktop.in b/jsk_baxter_robot/jsk_baxter_desktop/desktop_shortcut_template/baxter_startup.desktop.in index 5a36d3115c..422c9b008a 100644 --- a/jsk_baxter_robot/jsk_baxter_desktop/desktop_shortcut_template/baxter_startup.desktop.in +++ b/jsk_baxter_robot/jsk_baxter_desktop/desktop_shortcut_template/baxter_startup.desktop.in @@ -5,6 +5,6 @@ Type=Application Terminal=false Icon[ja_JP]=ICON_BASE_DIRECTORY/baxter_startup.png Name[ja_JP]=JSK StartUp baxter -Exec=xterm -sb -rightbar -sl 99999 +s -title 'JSK StartUp baxter' -hold -e 'source $HOME/ros/hydro/devel/setup.bash; ROS_MASTER_URI=http://BAXTER_ROBOT_NAME:11311 roslaunch jsk_baxter_startup baxter.launch' +Exec=xterm -sb -rightbar -sl 99999 +s -title 'JSK StartUp baxter' -hold -e 'source $HOME/ros/ROS_DISTRO/devel/setup.bash; ROS_MASTER_URI=http://BAXTER_ROBOT_NAME:11311 roslaunch jsk_baxter_startup baxter.launch' Name=JSK StartUp baxter Icon=ICON_BASE_DIRECTORY/baxter_startup.png diff --git a/jsk_baxter_robot/jsk_baxter_desktop/manifest.xml b/jsk_baxter_robot/jsk_baxter_desktop/manifest.xml deleted file mode 100644 index c141a76127..0000000000 --- a/jsk_baxter_robot/jsk_baxter_desktop/manifest.xml +++ /dev/null @@ -1,12 +0,0 @@ - - The jsk_baxter_startup package - - inagaki - TODO - - - - - - - diff --git a/jsk_baxter_robot/jsk_baxter_desktop/package.xml b/jsk_baxter_robot/jsk_baxter_desktop/package.xml index 0be00acd96..66df93d3db 100644 --- a/jsk_baxter_robot/jsk_baxter_desktop/package.xml +++ b/jsk_baxter_robot/jsk_baxter_desktop/package.xml @@ -1,7 +1,7 @@ jsk_baxter_desktop - 0.0.0 + 0.0.6 The jsk_baxter_desktop package diff --git a/jsk_baxter_robot/jsk_baxter_desktop/scripts/setup_baxter_desktop.sh b/jsk_baxter_robot/jsk_baxter_desktop/scripts/setup_baxter_desktop.sh index 5b1dd3e587..248df9b0c1 100755 --- a/jsk_baxter_robot/jsk_baxter_desktop/scripts/setup_baxter_desktop.sh +++ b/jsk_baxter_robot/jsk_baxter_desktop/scripts/setup_baxter_desktop.sh @@ -19,8 +19,9 @@ for template_desktop_file in $template_desktop_files; do sed -i -e "s|ICON_BASE_DIRECTORY|$icon_base_directory|g" $script_base_directory/tmp/$desktop_file_without_extension; sed -i -e "s|SCRIPT_BASE_DIRECTORY|$script_base_directory|g" $script_base_directory/tmp/$desktop_file_without_extension; sed -i -e "s|BAXTER_ROBOT_NAME|$BAXTER_NAME|g" $script_base_directory/tmp/$desktop_file_without_extension; + sed -i -e "s|ROS_DISTRO|$ROS_DISTRO|g" $script_base_directory/tmp/$desktop_file_without_extension; chmod 755 $script_base_directory/tmp/$desktop_file_without_extension; cp $script_base_directory/tmp/$desktop_file_without_extension $HOME/Desktop/$desktop_file_without_extension; done -rm -rf $script_base_directory/tmp \ No newline at end of file +rm -rf $script_base_directory/tmp diff --git a/jsk_baxter_robot/jsk_baxter_startup/CHANGELOG.rst b/jsk_baxter_robot/jsk_baxter_startup/CHANGELOG.rst new file mode 100644 index 0000000000..31fb0539c5 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/CHANGELOG.rst @@ -0,0 +1,75 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_baxter_startup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ +* [jsk_baxter_startup/baxter.launch] head_trajectory_action is available after v1.1.0 +* [jsk_baxter_sensors] add kinecct2 use_machine parameter +* [jsk_baxter_startup] update to add position diff paramter for tweet +* [jsk_baxter_startup] update rviz config +* [jsk_baxter_startup] add head trajectory server for baxter.launch +* [jsk_baxter_startup] modify to prevent baxter.launch fail +* [jsk_baxter_startup] add more dependencies to jsk_baxter_startup +* [jsk_baxter_startup] shift to use kinect2 from kinect +* [jsk_baxter_startup] remove checkerboard yaml rosparam +* [jsk_baxter_startup] add use_color and keep_organized option to baxter self_filter.launch +* [jsk_baxter_startup] add self_filter launch and config to jsk_baxter +* Contributors: Kei Okada, Yuto Inagaki + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* remove jtalk voice +* Contributors: Kei Okada, Yuto Inagaki + +0.0.1 (2014-12-25) +------------------ +* fix typo in baxter_tweet +* add time singal in baxter startup +* move twitter related program to robot_common from jsk_pr2_startup +* repair mongodb.launch and add param +* add gripper action server +* add camera info fixer launch in baxter.launch +* use face_recognition package(procrob_functional) +* add camera_info_fixer, camera_info_std publishes with original param and roi, and camera_info publishes cropped image with same roi, it seems something wrong... +* remove unnecessary components +* add wrench publisher +* add depends +* add image saver +* add sound tools and eus speak-en +* modify params +* modify package name +* add baxter tweet +* mv catkin.cmake to CMakeLists.txt +* fix jsk_baxter_startup/package.xml +* remove baxter_interface and baxter_tools from find_package, they do not need to load as COMPONENTS +* remove unneeded lines +* delete objectdetection_tf_publisher and use checker_board_detector's +* add baxter_description +* update kinect.launch +* delete files correctly +* delete voice directory and move file +* delete text2wave and modify voice_echo.l +* Update jsk_baxter_startup + We added files in jsk_baxter_sensors + - for kinect.launch + - add voice set + - change joy device name +* add baxter joy dir and launch +* add baxter rviz config file for default baxter nodes +* update manifest +* add tmp groovy manifest file +* one more openni => openni_launch space +* update and add catkin.cmake (just rename CMakeLists.txt to catkin.cmake) +* add baxter startup launch file +* Contributors: Kei Okada, Tomoya Yoshizawa, Yuto Inagaki diff --git a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt index b19432a21e..10b16669ba 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt +++ b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_baxter_startup) -find_package(catkin REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS baxter_core_msgs sound_play roseus) catkin_package( # INCLUDE_DIRS include @@ -13,3 +13,13 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} ) + +catkin_add_env_hooks(99.jsk_baxter_startup SHELLS bash zsh + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) + +install(DIRECTORY config jsk_baxter_joy jsk_baxter_lifelog jsk_baxter_moveit jsk_baxter_sensors + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +install(FILES baxter.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/jsk_baxter_robot/jsk_baxter_startup/baxter.launch b/jsk_baxter_robot/jsk_baxter_startup/baxter.launch index 4652582a1a..4632b9b8bb 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/baxter.launch +++ b/jsk_baxter_robot/jsk_baxter_startup/baxter.launch @@ -2,20 +2,25 @@ - + + - + + + + + @@ -24,6 +29,12 @@ + + + + + @@ -56,4 +67,4 @@ pkg="roseus" type="roseus" name="time_signal" output="screen" args="$(find jsk_robot_startup)/lifelog/time-signal.l" /> - \ No newline at end of file + diff --git a/jsk_baxter_robot/jsk_baxter_startup/config/baxter_default.rviz b/jsk_baxter_robot/jsk_baxter_startup/config/baxter_default.rviz index 604f9179a4..4518002e18 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/config/baxter_default.rviz +++ b/jsk_baxter_robot/jsk_baxter_startup/config/baxter_default.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Kinect2 PointCloud1 Splitter Ratio: 0.5 Tree Height: 681 - Class: rviz/Selection @@ -311,6 +312,7 @@ Visualization Manager: Effort: true Grid: true Head Sonar State: true + Kinect2 PointCloud: true Left Hand Range: true LeftWrench: true MarkerArray: true @@ -333,6 +335,7 @@ Visualization Manager: Effort: true Grid: true Head Sonar State: true + Kinect2 PointCloud: true Left Camera: true Left Hand Range: true LeftWrench: true @@ -367,7 +370,7 @@ Visualization Manager: Max Color: 255; 255; 255 Max Intensity: 11 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 9 Name: Head Sonar State Position Transformer: XYZ Queue Size: 100 @@ -461,6 +464,35 @@ Visualization Manager: Topic: /robot/end_effector/right/wrench Torque Color: 204; 204; 51 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Kinect2 PointCloud + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /kinect2/depth_lowres/points + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -492,15 +524,15 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.54962 + Y: 0.544029 + Z: 0.465737 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.245398 + Pitch: 0.460398 Target Frame: Value: Orbit (rviz) - Yaw: 0.285401 + Yaw: 1.7954 Saved: ~ Window Geometry: Displays: diff --git a/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.bash b/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.bash new file mode 100644 index 0000000000..5567bde809 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.bash @@ -0,0 +1,8 @@ +#!/bin/bash +# -*- mode: shell-script -*- + +function rossetbaxter() { + rossetmaster baxter.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=BAXTER + rossetip +} \ No newline at end of file diff --git a/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.zsh b/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.zsh new file mode 100644 index 0000000000..e2e6822fc1 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/env-hooks/99.jsk_baxter_startup.zsh @@ -0,0 +1,8 @@ +#!/bin/zsh +# -*- mode: shell-script -*- + +function rossetbaxter() { + rossetmaster baxter.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=BAXTER + rossetip +} \ No newline at end of file diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/baxter_tweet.launch b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/baxter_tweet.launch index 1035ae7584..9b9e93724e 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/baxter_tweet.launch +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_lifelog/baxter_tweet.launch @@ -1,19 +1,21 @@ + - + + - + @@ -36,4 +38,4 @@ - \ No newline at end of file + diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect.launch b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect.launch index 072ca84e99..024b51408a 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect.launch +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect.launch @@ -1,6 +1,5 @@ - @@ -8,6 +7,9 @@ + + + diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect2.launch b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect2.launch new file mode 100644 index 0000000000..c242484739 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/kinect2.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/openni_self_filter.yaml b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/openni_self_filter.yaml new file mode 100644 index 0000000000..b26ede2046 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/openni_self_filter.yaml @@ -0,0 +1,42 @@ +min_sensor_dist: .2 +self_see_default_padding: .01 +self_see_default_scale: 1.0 +self_see_links: + - name: right_gripper_base + padding: .04 + - name: right_lower_elbow + padding: .04 + - name: right_lower_forearm + padding: .04 + - name: right_lower_shoulder + padding: .04 + - name: right_upper_elbow + padding: .04 + - name: right_lower_forearm + padding: .07 + - name: right_upper_forearm + padding: .04 + - name: right_upper_shoulder + padding: .04 + - name: right_wrist + padding: .04 + + + - name: left_gripper_base + padding: .04 + - name: left_lower_elbow + padding: .04 + - name: left_lower_forearm + padding: .07 + - name: left_lower_shoulder + padding: .04 + - name: left_upper_elbow + padding: .04 + - name: left_lower_forearm + padding: .04 + - name: left_upper_forearm + padding: .04 + - name: left_upper_shoulder + padding: .04 + - name: left_wrist + padding: .04 diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/self_filter.launch b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/self_filter.launch new file mode 100644 index 0000000000..b122090b90 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/self_filter.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice.launch b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice.launch index f46d0e3a63..c2683ea114 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice.launch +++ b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice.launch @@ -1,4 +1,3 @@ - \ No newline at end of file diff --git a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice_echo.l b/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice_echo.l deleted file mode 100755 index 7743440844..0000000000 --- a/jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/voice_echo.l +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env roseus -(ros::roseus "voice_echo") -(ros::load-ros-manifest "std_msgs") - -(defvar *text2wave-jp-path* - (format nil "~A/jsk_baxter_sensors/voice/text2wave-jp" (read-line (piped-fork "rospack find jsk_baxter_startup")))) -(defvar *text2wave-path* - (format nil "~A/text2wave-jp.sh" (read-line (piped-fork "rospack find jtalk")))) - -(defun mei-voice-message-callback - (msg) - (unix:system - (format nil "~A ~A happy /tmp/tmp.wav && aplay /tmp/tmp.wav" - *text2wave-jp-path* - (send msg :data)) - )) - -(ros::subscribe - "/voice_echo" - std_msgs::string - #'mei-voice-message-callback 1) - -(ros::rate 3) -(do-until-key - (if (not (ros::ok)) (return-from nil nil)) - (ros::spin-once) - (ros::sleep) - ) diff --git a/jsk_baxter_robot/jsk_baxter_startup/package.xml b/jsk_baxter_robot/jsk_baxter_startup/package.xml index ff28e52832..1f6cc16793 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/package.xml +++ b/jsk_baxter_robot/jsk_baxter_startup/package.xml @@ -1,7 +1,7 @@ jsk_baxter_startup - 0.0.0 + 0.0.6 The jsk_baxter_startup package Yuto Inagaki BSD @@ -12,7 +12,6 @@ baxter_tools baxter_examples openni_launch - jtalk joy checkerboard_detector topic_tools @@ -21,13 +20,13 @@ dynamic_tf_publisher rostwitter image_view + roseus baxter_interface baxter_description baxter_tools baxter_examples openni_launch - jtalk joy checkerboard_detector topic_tools @@ -37,7 +36,8 @@ rostwitter image_view face_recognition + roseus - \ No newline at end of file + diff --git a/jsk_baxter_robot/jsk_baxter_web/CHANGELOG.rst b/jsk_baxter_robot/jsk_baxter_web/CHANGELOG.rst new file mode 100644 index 0000000000..df63399041 --- /dev/null +++ b/jsk_baxter_robot/jsk_baxter_web/CHANGELOG.rst @@ -0,0 +1,33 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_baxter_web +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ +* do not generate baxter_description if baxter_description is not found +* [jsk_baxter_web] Do not depend on rwt_ros (meta package) +* Delete rospack find and use the result of find package + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* debug the loop of link problem + update to Copy Correctly +* add urdf_viewer for real robot +* delte rwt_ros from find_package +* update .rosinstall to include rwt_ros +* add jsk baxter web +* Contributors: Yuto Inagaki diff --git a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt index f6f68f3a97..9c7072a085 100644 --- a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt +++ b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt @@ -1,15 +1,22 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_baxter_web) -find_package(catkin REQUIRED COMPONENTS - rosbridge_server - baxter_description - robot_state_publisher - tf2_web_republisher -) +find_package(catkin REQUIRED) +find_package(baxter_description) +if(baxter_description_FOUND) + if (EXISTS ${baxter_description_SOURCE_DIR}/meshes) + set(_baxter_description_location ${baxter_description_SOURCE_DIR}/meshes) + elseif (EXISTS ${baxter_description_SOURCE_PREFIX}/meshes) + set(_baxter_description_location ${baxter_description_SOURCE_PREFIX}/meshes) + elseif (EXISTS ${baxter_description_PREFIX}/share/baxter_description/meshes) + set(_baxter_description_location ${baxter_description_PREFIX}/share/baxter_description/meshes) + else (EXISTS ${baxter_description_SOURCE_DIR}/meshes) + message(FATAL_ERROR "cannot find baxter_description") + endif(EXISTS ${baxter_description_SOURCE_DIR}/meshes) -execute_process(COMMAND rospack find baxter_description OUTPUT_VARIABLE baxter_description_PACKAGE_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) -execute_process(COMMAND cp -Ru ${baxter_description_PACKAGE_PATH} ${PROJECT_SOURCE_DIR}/www/ OUTPUT_VARIABLE OUTPUT OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND mkdir -p ${PROJECT_SOURCE_DIR}/www/baxter_description/meshes/) + execute_process(COMMAND cp -Ru ${_baxter_description_location} ${PROJECT_SOURCE_DIR}/www/baxter_description/ OUTPUT_VARIABLE OUTPUT OUTPUT_STRIP_TRAILING_WHITESPACE) +endif() catkin_package( # INCLUDE_DIRS include @@ -21,3 +28,7 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} ) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) diff --git a/jsk_baxter_robot/jsk_baxter_web/package.xml b/jsk_baxter_robot/jsk_baxter_web/package.xml index d0b4773da0..fdc0412b6b 100644 --- a/jsk_baxter_robot/jsk_baxter_web/package.xml +++ b/jsk_baxter_robot/jsk_baxter_web/package.xml @@ -1,7 +1,7 @@ jsk_baxter_web - 0.0.0 + 0.0.6 The jsk_baxter_web package inagaki @@ -10,15 +10,18 @@ BSD catkin rosbridge_server - rwt_ros + roslibjs + ros2djs + ros3djs baxter_description robot_state_publisher - tf2_web_republisher rosbridge_server - rwt_ros + roslibjs + ros2djs + ros3djs baxter_description robot_state_publisher tf2_web_republisher - \ No newline at end of file + diff --git a/jsk_nao_robot/jsk_nao_startup/CHANGELOG.rst b/jsk_nao_robot/jsk_nao_startup/CHANGELOG.rst new file mode 100644 index 0000000000..6b22765e01 --- /dev/null +++ b/jsk_nao_robot/jsk_nao_startup/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_nao_startup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ + +0.0.1 (2014-12-25) +------------------ +* nao_vision and audio launch added +* update nao_driver -> naoqi_driver +* add jsk_nao_startup +* Contributors: Yuki Furuta, Kanae Kochigami diff --git a/jsk_nao_robot/jsk_nao_startup/package.xml b/jsk_nao_robot/jsk_nao_startup/package.xml index a067865177..bed2ad0d19 100644 --- a/jsk_nao_robot/jsk_nao_startup/package.xml +++ b/jsk_nao_robot/jsk_nao_startup/package.xml @@ -1,7 +1,7 @@ jsk_nao_startup - 0.0.0 + 0.0.6 The jsk_nao_startup package diff --git a/jsk_nao_robot/naoeus/euslisp/nao-interface.l b/jsk_nao_robot/naoeus/euslisp/nao-interface.l index 441199942a..7eb124ec4f 100644 --- a/jsk_nao_robot/naoeus/euslisp/nao-interface.l +++ b/jsk_nao_robot/naoeus/euslisp/nao-interface.l @@ -20,11 +20,11 @@ (setq joint-stiffness-trajectory-action (instance ros::simple-action-client :init (if namespace (format nil "~A/joint_stiffness_trajectory" namespace ) "joint_stiffness_trajectory") - nao_msgs::JointTrajectoryAction)) + naoqi_msgs::JointTrajectoryAction)) (ros::advertise (if namespace (format nil "~A/cmd_pose" namespace) "cmd_pose") geometry_msgs::Pose2D 1) (ros::advertise (if namespace (format nil "~A/cmd_vel" namespace) "cmd_vel") geometry_msgs::Twist 1) (ros::advertise "/speech" std_msgs::String 1) - (ros::advertise "/joint_angles" nao_msgs::JointAnglesWithSpeed 1) + (ros::advertise "/joint_angles" naoqi_msgs::JointAnglesWithSpeed 1) ;; (dolist (param (send self controller-type)) ;; (let* ((controller-state (cdr (assoc :controller-state param))) ;; (key (intern (string-upcase controller-state) *keyword-package*))) @@ -43,7 +43,7 @@ ;;"joint_trajectory") ;;(cons :controller-state "joint_trajectory") (cons :controller-state "dummy_state") ;; this is dummy - (cons :action-type nao_msgs::JointTrajectoryAction) + (cons :action-type naoqi_msgs::JointTrajectoryAction) (cons :joint-names (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) (send-all (send robot :joint-list) :name)))) )) @@ -113,7 +113,7 @@ (:start-grasp (&optional (arm :arms)) - (let ((start_grasp_msg (instance nao_msgs::JointAnglesWithSpeed :init))) + (let ((start_grasp_msg (instance naoqi_msgs::JointAnglesWithSpeed :init))) (send start_grasp_msg :header :stamp (ros::time-now)) (send start_grasp_msg :header :seq 1) (send start_grasp_msg :speed 0.5) @@ -132,7 +132,7 @@ )) (:stop-grasp (&optional (arm :arms)) - (let ((stop_grasp_msg (instance nao_msgs::JointAnglesWithSpeed :init))) + (let ((stop_grasp_msg (instance naoqi_msgs::JointAnglesWithSpeed :init))) (send stop_grasp_msg :header :stamp (ros::time-now)) (send stop_grasp_msg :header :seq 1) (send stop_grasp_msg :speed 0.5) diff --git a/jsk_nao_robot/naoeus/manifest.xml b/jsk_nao_robot/naoeus/manifest.xml index 6fc44d6168..59032fd381 100644 --- a/jsk_nao_robot/naoeus/manifest.xml +++ b/jsk_nao_robot/naoeus/manifest.xml @@ -15,7 +15,7 @@ - + diff --git a/jsk_pepper_robot/README.md b/jsk_pepper_robot/README.md index 4663d7c2c5..cb3e1f8361 100644 --- a/jsk_pepper_robot/README.md +++ b/jsk_pepper_robot/README.md @@ -21,10 +21,15 @@ export PYTHONPATH=$HOME/pynaoqi:$PYTHONPATH export NAO_IP="olive.jsk.imi.i.u-tokyo.ac.jp" ``` +Install pepper mesh files with manual approval of license +``` +sudo apt-get install ros-indigo-pepper-meshes +``` + running demo ------------ ``` -rosrun jsk_pepper_startup jsk_pepper_startup.launch +roslaunch jsk_pepper_startup jsk_pepper_startup.launch ``` ``` rosrun jsk_pepper_startup sample.l diff --git a/jsk_pepper_robot/jsk_201504_miraikan/CMakeLists.txt b/jsk_pepper_robot/jsk_201504_miraikan/CMakeLists.txt new file mode 100644 index 0000000000..6c355ecddd --- /dev/null +++ b/jsk_pepper_robot/jsk_201504_miraikan/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_201504_miraikan) + +find_package(catkin REQUIRED) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES jsk_201504_miraikan +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +install(DIRECTORY euslisp + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + diff --git a/jsk_pepper_robot/jsk_201504_miraikan/euslisp/mirai-demo-20150413.l b/jsk_pepper_robot/jsk_201504_miraikan/euslisp/mirai-demo-20150413.l new file mode 100755 index 0000000000..e2267cfc72 --- /dev/null +++ b/jsk_pepper_robot/jsk_201504_miraikan/euslisp/mirai-demo-20150413.l @@ -0,0 +1,32 @@ +#!/usr/bin/env roseus + +(ros::roseus "greeting") +(ros::load-ros-manifest "peppereus") +(load "package://peppereus/pepper-interface.l") +(load "package://jsk_201504_miraikan/euslisp/pepper-motions.l") +(setq *pepper* (pepper)) +(setq *ri* (instance pepper-interface :init)) + + +;; 音声ファイルを再生し、動く関数 +(defun playAudioFile_move (file angle-list time-list) + (ros::ros-info "speak") + (send *ri* :angle-vector-sequence angle-list time-list) + (send *ri* :play-audio-file file) + ) + +(defun demo() + (send *ri* :set-maste-volume 100) + (send *ri* :stop-grasp) + (playAudioFile_move "/audio_file/M1.mp3" *lift_arm_and_bow* (list 1000 100 100 100 100 100 1000)) + (playAudioFile_move "/audio_file/M2.mp3" *pepper_can_cannot* (list 100 100 100 100 100 100 100 100 100 100 150 100 150 100 100 100 100 1000)) + (playAudioFile_move "/audio_file/M3.mp3" *pepper_run* (list 200 200 200 200 200 200 200 100 100 100 100 100 100)) + (playAudioFile_move "/audio_file/M4.mp3" *show_the_camera* (list 100 100 100 100 100 100 100 100 )) + (playAudioFile_move "/audio_file/M5.mp3" *speaker_touch_sensor* (list 100 100 100 100 100 100 100 100 100 100)) + (playAudioFile_move "/audio_file/M6.mp3" (list *reset_pose* *reset_pose* *reset_pose* *reset_pose* *touch_CPU* *touch_CPU* *touch_CPU* *reset_pose*) (list 100 100 100 100 100 100 100 100)) + (playAudioFile_move "/audio_file/M7.mp3" *look_around_hope* (list 100 100 100 100 100 100 100 100 100)) + (playAudioFile_move "/audio_file/M8.mp3" *lift_arm_and_bow* (list 1000 100 100 100 100 100 1000)) + ) + +(demo) + diff --git a/jsk_pepper_robot/jsk_201504_miraikan/euslisp/pepper-motions.l b/jsk_pepper_robot/jsk_201504_miraikan/euslisp/pepper-motions.l new file mode 100644 index 0000000000..d01b528101 --- /dev/null +++ b/jsk_pepper_robot/jsk_201504_miraikan/euslisp/pepper-motions.l @@ -0,0 +1,110 @@ +#!/usr/bin/env roseus + +;; reset pose +(setq *reset_pose* #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0)) + +;; *touch_ears* +;; used when introducing speaker +(setq *touch_ears* #f(20.0 -2.0 -30.0 -15.0 20.0 -70.0 -88.0 -40.0 -15.0 -20.0 70.0 88.0 40.0 0.0 30.0)) + +;; *lift_right_arm* +;; lift right arm up to head +(setq *left_right_arm* #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 -30.0 -20.0 50.0 50.0 -50.0 0.0 0.0)) + +;; *bow* +(setq *bow* #f(2.0 -2.0 -40.0 85.0 10.0 -70.0 -20.0 -40.0 50.0 -1.0 -1.0 50.0 50.0 0.0 20.0)) + +;; *lift_arm_and_bow* +;; first lift arm, then bow. used at fist +(setq *lift_arm_and_bow* (list *left_right_arm* *left_right_arm* *left_right_arm* *bow* *bow* *bow* *reset_pose*)) + +;; *touch_left_head* +(setq *touch_lift_head* #f(2.0 10.0 -5.0 -60.0 60.0 -30.0 -89.5 -60.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0)) + +;; *touch_CPU* +(setq *touch_CPU* #f(28.0 0.0 -57.0 -80.0 30.0 -30.0 -89.5 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0)) + +;; *look_foot* +;; used when introducing touch sensor on foot +(setq *look_foot* #f(2.0 -20.0 -20.0 85.0 10.0 -70.0 -20.0 -40.0 50.0 -10.0 70.0 20.0 -70.0 -20.0 20.0)) +(setq *open_pepper* #f(2.0 -2.0 -5.0 10.0 80.0 -70.0 -40.0 -40.0 10.0 -80.0 70.0 40.0 40.0 0.0 0.0)) + +;; *speaker_touch_sensor* +;; speaker and touchsensor +(setq *speaker_touch_sensor* (list *touch_ears* *touch_ears* *touch_ears* *touch_ears* *open_pepper* *open_pepper* *look_foot* *look_foot* *look_foot* *reset_pose*)) + +;; *look_right* *look_left* +;; used when look around + +;; *look_right* +(setq *look_right* #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 -20.0 -10.0)) +;; *look_left: +(setq *look_left* #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 20.0 -10.0)) + +;; *show the camera on head* +(setq *show_the_camera_on_head* #f(2.0 -2.0 -5.0 10.0 10.0 -60.0 -89.5 -40.0 10.0 -10.0 60.0 89.5 40.0 0.0 5.0)) + +;; *show the camera in mouth* +(setq *show_the_camera_in_mouth* #f(2.0 -2.0 -5.0 10.0 10.0 -60.0 -89.5 -40.0 10.0 -10.0 60.0 89.5 40.0 0.0 -35.0)) + +;;open two hands widely +(setq *open_two_hands_widely* #f(0.0 0.0 0.0 60.0 0.0 -40.0 -30.0 -104.5 -70.0 0.0 -50.0 40.0 104.5 0.0 0.0)) + +;; *show_the_camera* +(setq *show_the_camera* (list *reset_pose* *reset_pose* *reset_pose* *show_the_camera_on_head* *show_the_camera_on_head* *show_the_camera_on_head* *show_the_camera_in_mouth* *open_two_hands_widely* *reset_pose*)) + +;;*pepper_cannot* +;; used when introducing what pepper can't do +(setq *pepper_cannot1* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 -50.0 40.0 -10.0 70.0 70.0 50.0 0.0 0.0)) +(setq *pepper_cannot2* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 -50.0 40.0 -10.0 70.0 70.0 50.0 40.0 0.0)) +(setq *pepper_cannot3* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 -50.0 40.0 -10.0 70.0 70.0 50.0 -40.0 0.0)) +(setq *pepper_cannot4* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 50.0 40.0 -10.0 70.0 70.0 -50.0 0.0 0.0)) +(setq *pepper_cannot5* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 50.0 40.0 -10.0 70.0 70.0 -50.0 40.0 0.0)) +(setq *pepper_cannot6* #f(2.0 -2.0 -5.0 100.0 10.0 -70.0 -70.0 50.0 40.0 -10.0 70.0 70.0 -50.0 -40.0 0.0)) + +;;*pepper_can* +;; used when introducing what pepper can do +(setq *pepper_open_hand* #f(2.0 -2.0 -5.0 30.0 30.0 -70.0 -20.0 -40.0 30.0 -30.0 70.0 20.0 40.0 0.0 0.0)) +(setq *pepper_run1* #f(2.0 -10.0 -15.0 110.0 10.0 -100.0 -80.0 -40.0 60.0 -10.0 70.0 70.0 40.0 0.0 0.0)) +(setq *pepper_run2* #f(2.0 10.0 -15.0 60.0 10.0 -70.0 -70.0 40.0 110.0 -10.0 100.0 80.0 -40.0 0.0 0.0)) +;;move hip front +(setq *pepper_can1* #f(2.0 -2.0 -5.0 50.0 30.0 -70.0 -80.0 -90.0 50.0 -30.0 70.0 80.0 90.0 0.0 0.0)) +;;move hip right +(setq *pepper_can2* #f(2.0 -20.0 -5.0 50.0 30.0 -70.0 -80.0 -90.0 50.0 -30.0 70.0 80.0 90.0 -20.0 0.0)) +;;move hip left +(setq *pepper_can3* #f(2.0 20.0 -5.0 50.0 30.0 -70.0 -80.0 -90.0 50.0 -30.0 70.0 80.0 90.0 20.0 0.0)) +(setq *pepper_can4* #f(2.0 0.0 -5.0 20.0 30.0 -70.0 -20.0 -90.0 20.0 -30.0 70.0 20.0 90.0 0.0 0.0)) +(setq *pepper_can5* #f(2.0 0.0 -5.0 20.0 30.0 -70.0 -20.0 -90.0 20.0 -30.0 70.0 20.0 90.0 0.0 30.0)) +(setq *pepper_can6* #f(2.0 0.0 -5.0 20.0 30.0 -70.0 -20.0 -90.0 20.0 -30.0 70.0 20.0 90.0 0.0 -30.0)) + +;; *pepper_can_cannot* +(setq *pepper_can_cannot* (list *pepper_cannot4* *pepper_cannot5* *pepper_cannot4* *pepper_cannot6* *pepper_cannot4* *pepper_cannot5* *reset_pose* *reset_pose* *reset_pose* *reset_pose* *pepper_can2* *pepper_can1* *pepper_can3* *pepper_can4* *pepper_can5* *pepper_can4* *pepper_can6* *reset_pose*)) + +;;*pepper_watch* +;;used when pepper see something far +(setq *pepper_watch1* #f(2.0 -2.0 -5.0 100.0 50.0 -20.0 -80.0 -40.0 -60.0 -10.0 -10.0 60.0 40.0 0.0 0.0)) +(setq *pepper_watch2* #f(2.0 10.0 0.0 100.0 50.0 -20.0 -80.0 -40.0 -60.0 -10.0 -10.0 60.0 40.0 0.0 0.0)) +(setq *pepper_watch3* #f(2.0 -10.0 0.0 100.0 50.0 -20.0 -80.0 -40.0 -60.0 -10.0 -10.0 60.0 40.0 0.0 0.0)) + +;; *pepper_run* +;;used when introducing her 3D camera +(setq *pepper_run* (list *pepper_watch2* *pepper_watch1* *pepper_watch3* *pepper_watch1* *pepper_watch2* *reset_pose* *reset_pose* *reset_pose* *reset_pose* *pepper_open_hand* *pepper_run1* *pepper_run2* *pepper_run1*)) + +;; +(setq *hope1* #f(2.0 -2.0 -5.0 30.0 10.0 -70.0 -70.0 -40.0 30.0 -10.0 70.0 70.0 40.0 0.0 0.0)) +(setq *hope2* #f(2.0 -2.0 -5.0 20.0 10.0 -70.0 -70.0 -40.0 20.0 -10.0 70.0 70.0 40.0 0.0 10.0)) +;;lift two hands beside face and bow slightly to right +(setq *lift_two_hands_beside_face_and_bow_slightly_to_right* #f(2.0 -20.0 -20.0 -40.0 40.0 -40.0 -89.5 0.0 -40.0 -40.0 40.0 89.5 0.0 0.0 -25.0)) +;;bow slightly to left +(setq *bow_slightly_to_left* #f(2.0 20.0 -20.0 -40.0 40.0 -40.0 -89.5 0.0 -40.0 -40.0 40.0 89.5 0.0 0.0 -25.0)) + +;;show the hope +;;put two hands in front of the body +(setq *put_two_hands_in_front_of_the_body* #f(2.81249 -0.703127 0.351563 20.0 40.957 -5.0 -89.5 -90.0 57.5684 -20.5664 20.0 80.0 27.5956 0.0 -5.0)) +;;open two arms widely and look at the sky +(setq *open_two_arms_widely_and_look_at_the_sky* #f(2.81249 -0.703127 0.351563 -39.9902 40.957 -100.02 -10.0195 -1.32078 57.5684 -20.5664 80.5957 15.7324 27.5956 0.0 -24.9609)) + +;; *look_around_hope* +(setq *look_around_hope* (list *lift_two_hands_beside_face_and_bow_slightly_to_right* *bow_slightly_to_left* *reset_pose* *reset_pose* *reset_pose* *reset_pose* *reset_pose* *reset_pose* *put_two_hands_in_front_of_the_body* *open_two_arms_widely_and_look_at_the_sky* *open_two_arms_widely_and_look_at_the_sky* *reset_pose*)) + + diff --git a/jsk_pepper_robot/jsk_201504_miraikan/package.xml b/jsk_pepper_robot/jsk_201504_miraikan/package.xml new file mode 100644 index 0000000000..1cd2d55529 --- /dev/null +++ b/jsk_pepper_robot/jsk_201504_miraikan/package.xml @@ -0,0 +1,25 @@ + + + jsk_201504_miraikan + 0.0.0 + The jsk_201504_miraikan package + + Kei Okada + + Kanae Kochigami + Jun Jiang + + BSD + + catkin + + jsk_pepper_startup + peppereus + + jsk_pepper_startup + peppereus + + + + + diff --git a/jsk_pepper_robot/jsk_pepper_startup/CHANGELOG.rst b/jsk_pepper_robot/jsk_pepper_startup/CHANGELOG.rst new file mode 100644 index 0000000000..b97717257d --- /dev/null +++ b/jsk_pepper_robot/jsk_pepper_startup/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_pepper_startup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ +* modify msg name and launch file name +* Contributors: Jiang Jun + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* add depends to pepper_bringup +* fix launch file as of Dec 14 +* use jsk_pepper_bringup and now naoqi repos +* add more depends +* tweet when imu is learge +* deleted displaying installed behaviors (only it was test) +* conversation added to face recognition +* remove nao_driver from depends +* add comment to how to modify voices +* add learn face example +* check timestamp to publish images +* add nao_interaction_msgs +* update sample, without face recognition +* use all cameras (top/bottom/depeth) +* use key for recognize-word +* use naoqi_sensors +* add simple demo code +* add nao_dashboard +* add sample/sample.l +* fix package.xml +* move tweet.l under nodes directory, listen /pepper_tweet +* some voice added +* some bugs fixed +* pepper speaking function added +* add jsk_peper_robot (add CMakeLists.txt launch/jsk_pepper_startup.launch package.xml tweet.l) +* Contributors: Kanae Kochigami, Kei Okada diff --git a/jsk_pepper_robot/jsk_pepper_startup/CMakeLists.txt b/jsk_pepper_robot/jsk_pepper_startup/CMakeLists.txt index 762ddf2805..f1bd990c1e 100644 --- a/jsk_pepper_robot/jsk_pepper_startup/CMakeLists.txt +++ b/jsk_pepper_robot/jsk_pepper_startup/CMakeLists.txt @@ -1,159 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_pepper_startup) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -#find_package(catkin REQUIRED COMPONENTS roseus rostwitter) +find_package(catkin REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +catkin_package() - -## 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 jsk_pepper_startup -# CATKIN_DEPENDS nao_driver roseus rostwitter -# DEPENDS system_lib -) - -########### -## 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(jsk_pepper_startup -# src/${PROJECT_NAME}/jsk_pepper_startup.cpp -# ) - -## Declare a cpp executable -# add_executable(jsk_pepper_startup_node src/jsk_pepper_startup_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(jsk_pepper_startup_node jsk_pepper_startup_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(jsk_pepper_startup_node -# ${catkin_LIBRARIES} -# ) - -############# -## 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 jsk_pepper_startup jsk_pepper_startup_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_jsk_pepper_startup.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +install(DIRECTORY launch nodes sample + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) diff --git a/jsk_pepper_robot/jsk_pepper_startup/launch/jsk_pepper_startup.launch b/jsk_pepper_robot/jsk_pepper_startup/launch/jsk_pepper_startup.launch index c8212e4ea7..d19acbf429 100644 --- a/jsk_pepper_robot/jsk_pepper_startup/launch/jsk_pepper_startup.launch +++ b/jsk_pepper_robot/jsk_pepper_startup/launch/jsk_pepper_startup.launch @@ -4,10 +4,10 @@ - - - - + + + + @@ -43,4 +43,4 @@ --> - \ No newline at end of file + diff --git a/jsk_pepper_robot/jsk_pepper_startup/package.xml b/jsk_pepper_robot/jsk_pepper_startup/package.xml index 08afd3bcb6..6a3a44ad3b 100644 --- a/jsk_pepper_robot/jsk_pepper_startup/package.xml +++ b/jsk_pepper_robot/jsk_pepper_startup/package.xml @@ -1,65 +1,29 @@ jsk_pepper_startup - 0.0.0 + 0.0.6 The jsk_pepper_startup package - - - Kei Okada - - - - BSD + Kei Okada - - - - - - - - - - - - - - - - - - - - - - - catkin - roseus - rostwitter - + nao_apps nao_interaction_launchers nao_interaction_msgs pepper_bringup + pepper_description topic_tools turtlebot_teleop image_view roseus rostwitter - - - - - - - - \ No newline at end of file + + diff --git a/jsk_pepper_robot/jsk_pepper_startup/sample/sample.l b/jsk_pepper_robot/jsk_pepper_startup/sample/sample.l old mode 100644 new mode 100755 index 3baeabf0b1..29064533ff --- a/jsk_pepper_robot/jsk_pepper_startup/sample/sample.l +++ b/jsk_pepper_robot/jsk_pepper_startup/sample/sample.l @@ -14,17 +14,17 @@ (defun get-installed-behaviors () (let ((ret)) - (setq ret (ros::service-call "get_installed_behaviors" (instance nao_msgs::GetInstalledBehaviorsRequest :init))) + (setq ret (ros::service-call "get_installed_behaviors" (instance naoqi_msgs::GetInstalledBehaviorsRequest :init))) (send ret :behaviors) )) ;; (run-behavior "pepper_tongue_twister_sample") ;; (run-behavior "pepper_dialog_sample") -(setq *run-behavior* (instance ros::simple-action-client :init "run_behavior" nao_msgs::RunBehaviorAction)) +(setq *run-behavior* (instance ros::simple-action-client :init "run_behavior" naoqi_msgs::RunBehaviorAction)) (defun run-behavior (behavior &key (wait nil)) (let (goal) (ros::ros-info "running ~A" behavior) - (setq goal (instance nao_msgs::RunBehaviorActionGoal :init)) + (setq goal (instance naoqi_msgs::RunBehaviorActionGoal :init)) (send goal :goal :behavior behavior) (send *run-behavior* :send-goal goal) (if wait (send *run-behavior* :wait-for-result)) @@ -53,7 +53,7 @@ (call-empty-service "start_recognition") (setq *word-recognized* nil) - (ros::subscribe "word_recognized" nao_msgs::WordRecognized + (ros::subscribe "word_recognized" naoqi_msgs::WordRecognized #'(lambda (msg) (ros::ros-info "Recognized ~A (~A)" (send msg :words) (send msg :confidence_values)) (if (> (elt (send msg :confidence_values) 0) threshold) diff --git a/jsk_pepper_robot/pepper.rosinstall b/jsk_pepper_robot/pepper.rosinstall index f8f8ccee33..5e6c1ab457 100644 --- a/jsk_pepper_robot/pepper.rosinstall +++ b/jsk_pepper_robot/pepper.rosinstall @@ -1,15 +1,3 @@ -- git: - local-name: naoqi_bridge - uri: https://github.com/ros-naoqi/naoqi_bridge.git - version: master -- git: - local-name: nao_interaction - uri: https//github.com/ros-naoqi/nao_interaction.git - version: master -- git: - local-name: nao_robot - uri: https://github.com/ros-naoqi/nao_robot.git - version: master - git: local-name: jsk_robot uri: http://github.com/jsk-ros-pkg/jsk_robot.git diff --git a/jsk_pepper_robot/pepper_bringup/CATKIN_IGNORE b/jsk_pepper_robot/pepper_bringup/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/jsk_pepper_robot/pepper_bringup/CHANGELOG.rst b/jsk_pepper_robot/pepper_bringup/CHANGELOG.rst new file mode 100644 index 0000000000..5d1f5cee55 --- /dev/null +++ b/jsk_pepper_robot/pepper_bringup/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pepper_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* add pepper_bringup, add CMakeLists.txt package.xml launch/pepper.launch +* Contributors: Kei Okada diff --git a/jsk_pepper_robot/pepper_bringup/CMakeLists.txt b/jsk_pepper_robot/pepper_bringup/CMakeLists.txt index 35b6d25aef..5a1e68f6a0 100644 --- a/jsk_pepper_robot/pepper_bringup/CMakeLists.txt +++ b/jsk_pepper_robot/pepper_bringup/CMakeLists.txt @@ -5,3 +5,7 @@ find_package(catkin REQUIRED) catkin_package() +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + diff --git a/jsk_pepper_robot/pepper_bringup/package.xml b/jsk_pepper_robot/pepper_bringup/package.xml index c577652664..e803f418d0 100644 --- a/jsk_pepper_robot/pepper_bringup/package.xml +++ b/jsk_pepper_robot/pepper_bringup/package.xml @@ -1,7 +1,7 @@ pepper_bringup - 0.0.0 + 0.0.6 The pepper_bringup package Kei Okada diff --git a/jsk_pepper_robot/pepper_description/CATKIN_IGNORE b/jsk_pepper_robot/pepper_description/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/jsk_pepper_robot/pepper_description/CHANGELOG.rst b/jsk_pepper_robot/pepper_description/CHANGELOG.rst new file mode 100644 index 0000000000..f13627f4b3 --- /dev/null +++ b/jsk_pepper_robot/pepper_description/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pepper_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* add pepper_description/.gitignore +* add pepper_description, just cmakefile to convert xml model file simulator to urdf, you need latest assimp +* Contributors: Kei Okada diff --git a/jsk_pepper_robot/pepper_description/CMakeLists.txt b/jsk_pepper_robot/pepper_description/CMakeLists.txt index e02e9f0a6a..54feeed42d 100644 --- a/jsk_pepper_robot/pepper_description/CMakeLists.txt +++ b/jsk_pepper_robot/pepper_description/CMakeLists.txt @@ -50,3 +50,7 @@ else() endif() +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + diff --git a/jsk_pepper_robot/pepper_description/package.xml b/jsk_pepper_robot/pepper_description/package.xml index ebb5a57d5e..2bf81344e8 100644 --- a/jsk_pepper_robot/pepper_description/package.xml +++ b/jsk_pepper_robot/pepper_description/package.xml @@ -1,7 +1,7 @@ pepper_description - 0.0.0 + 0.0.6 The pepper_description package Kei Okada diff --git a/jsk_pepper_robot/peppereus/.gitignore b/jsk_pepper_robot/peppereus/.gitignore index 67df623ae7..9d1ff69a35 100644 --- a/jsk_pepper_robot/peppereus/.gitignore +++ b/jsk_pepper_robot/peppereus/.gitignore @@ -1,2 +1,4 @@ pepper.l +pepper.dae + diff --git a/jsk_pepper_robot/peppereus/CHANGELOG.rst b/jsk_pepper_robot/peppereus/CHANGELOG.rst new file mode 100644 index 0000000000..2b376c0ffa --- /dev/null +++ b/jsk_pepper_robot/peppereus/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package peppereus +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ +* pepper-init added +* Contributors: kochigami + +0.0.5 (2015-04-08) +------------------ +* change nao_msgs to naoqi_msgs +* Contributors: Jiang Jun + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* use package:// for pepper.l +* add .gitignore +* add CMakeLists.txt package.xml pepper-interface.l +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ diff --git a/jsk_pepper_robot/peppereus/CMakeLists.txt b/jsk_pepper_robot/peppereus/CMakeLists.txt index 3895b98d6c..3922e2e0d8 100644 --- a/jsk_pepper_robot/peppereus/CMakeLists.txt +++ b/jsk_pepper_robot/peppereus/CMakeLists.txt @@ -1,7 +1,31 @@ cmake_minimum_required(VERSION 2.8.3) project(peppereus) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED naoqi_driver naoqi_sensors nao_pose pr2eus diagnostic_aggregator naoqi_msgs roseus pepper_description euscollada) catkin_package() +### +### pepper.l generation +### +if(EXISTS ${pepper_description_PREFIX}/share/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf) + set(pepper_urdf ${pepper_description_PREFIX}/share/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf) +elseif(EXISTS ${pepper_description_SOURCE_PREFIX}/urdf/pepper1.0_generated_urdf/pepper.urdf) + set(pepper_urdf ${pepper_description_SOURCE_PREFIX}/urdf/pepper1.0_generated_urdf/pepper.urdf) +else() + message(FATAL_ERROR "Could not found pepper.urdf in ${pepper_description_PREFIX}/share/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf and ${pepper_description_SOURCE_PREFIX}/urdf/pepper1.0_generated_urdf/pepper.urdf") +endif() +message(STATUS "Found pepper.urdf at ${pepper_urdf}") + +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/pepper.l + COMMAND rosrun euscollada collada2eus pepper.dae pepper.yaml pepper.l + COMMAND sed -i 's@julietteY20MP@pepper@g' pepper.l + COMMAND sed -i 's@juliettey20mp@pepper@g' pepper.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + DEPENDS pepper.dae) +add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/pepper.dae + COMMAND rosrun collada_urdf urdf_to_collada ${pepper_urdf} pepper.dae || echo "ok?" # urdf_to_collada fail to exit program, but generated dae is ok. + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + DEPENDS ${pepper_urdf}) + +add_custom_target(generate_pepper_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/pepper.l) diff --git a/jsk_pepper_robot/peppereus/package.xml b/jsk_pepper_robot/peppereus/package.xml index 150304b277..d0691e11ce 100644 --- a/jsk_pepper_robot/peppereus/package.xml +++ b/jsk_pepper_robot/peppereus/package.xml @@ -1,7 +1,7 @@ peppereus - 0.0.1 + 0.0.6 The pepper_bringup package Kei Okada @@ -11,11 +11,22 @@ catkin + diagnostic_aggregator + naoqi_driver + naoqi_sensors + nao_pose + roseus + pr2eus + pepper_description + euscollada + diagnostic_aggregator naoqi_driver naoqi_sensors nao_pose - + roseus + pr2eus + pepper_description diff --git a/jsk_pepper_robot/peppereus/pepper-interface.l b/jsk_pepper_robot/peppereus/pepper-interface.l index 17d568d639..b29ab81d2c 100644 --- a/jsk_pepper_robot/peppereus/pepper-interface.l +++ b/jsk_pepper_robot/peppereus/pepper-interface.l @@ -17,7 +17,7 @@ (ros::advertise "cmd_pose" geometry_msgs::Pose2D 1) (ros::advertise "cmd_vel" geometry_msgs::Twist 1) (ros::advertise "speech" std_msgs::String 1) - (ros::advertise "joint_angles" nao_msgs::JointAnglesWithSpeed 1) + (ros::advertise "joint_angles" naoqi_msgs::JointAnglesWithSpeed 1) (setq joint-stiffness-trajectory-action (instance ros::simple-action-client :init "joint_stiffness_trajectory" @@ -31,7 +31,7 @@ (cons :controller-action "joint_trajectory") ;;(cons :controller-state "joint_trajectory") (cons :controller-state "dummy_state") ;; this is dummy - (cons :action-type nao_msgs::JointTrajectoryAction) + (cons :action-type naoqi_msgs::JointTrajectoryAction) (cons :joint-names (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) (send-all (send robot :joint-list) :name)))) )) ;; @@ -56,7 +56,7 @@ ;; (:move-hand (value &optional (arm :arms)) - (let ((start_grasp_msg (instance nao_msgs::JointAnglesWithSpeed :init))) + (let ((start_grasp_msg (instance naoqi_msgs::JointAnglesWithSpeed :init))) (send start_grasp_msg :header :stamp (ros::time-now)) (send start_grasp_msg :header :seq 1) (send start_grasp_msg :speed 0.5) @@ -115,6 +115,18 @@ (:go-stop () (send self :go-velocity 0 0 0 0 :stop nil)) + (:play-audio-file (file) + (let (ret) + (ros::wait-for-service "nao_audio/play_file") + (setq ret (instance nao_interaction_msgs::AudioPlaybackRequest :init)) + (send ret :file_path :data file) + (setq ret (ros::service-call "nao_audio/play_file" ret)))) + (:set-master-volume (volume) + (let (ret) + (ros::wait-for-service "nao_audio/master_volume") + (setq ret (instance nao_interaction_msgs::AudioMasterVolumeRequest :init)) + (send ret :master_volume :data volume) + (setq ret (ros::service-call "nao_audio/master_volume" ret)))) ) ;; @@ -134,10 +146,21 @@ (:reset-pose () (send self :angle-vector #f(2 -2 -5 85 10 -70 -20 -40 85 -10 70 20 40 0 -0))) ) +(defun pepper-init (&optional (create-viewer)) + (unless (boundp '*pepper*) (pepper)) + (unless (ros::ok) (ros::roseus "pepper_eus_interface")) + (unless (boundp '*ri*) (setq *ri* (instance pepper-interface :init))) + + (ros::spin-once) + (send *ri* :spin-once) + + (send *pepper* :angle-vector (send *ri* :state :potentio-vector)) + (when create-viewer (objects (list *pepper*))) + ) #| (setq *ri* (instance pepper-interface :init)) (setq *pepper* (pepper)) (send *pepper* :reset-pose) (send *ri* :angle-vector (send *pepper* :angle-vector) 2000) -|# \ No newline at end of file +|# diff --git a/jsk_pepper_robot/peppereus/pepper.yaml b/jsk_pepper_robot/peppereus/pepper.yaml new file mode 100644 index 0000000000..de2640dc2b --- /dev/null +++ b/jsk_pepper_robot/peppereus/pepper.yaml @@ -0,0 +1,28 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## +rleg: + - KneePitch : knee-p + - HipRoll : hip-r + - HipPitch : hip-p +larm: + - LShoulderPitch : larm-shoulder-p + - LShoulderRoll : larm-shoulder-r + - LElbowYaw : larm-elbow-y + - LElbowRoll : larm-elbow-p + - LWristYaw : larm-wrist-y +rarm: + - RShoulderPitch : rarm-shoulder-p + - RShoulderRoll : rarm-shoulder-r + - RElbowYaw : rarm-elbow-y + - RElbowRoll : rarm-elbow-p + - RWristYaw : rarm-wrist-y +head: + - HeadYaw : head-neck-y + - HeadPitch : head-neck-p + +angle-vector: + reset-pose: [2, -2, -5, 85, 10, -70, -20, -40, 85, -10, 70, 20, 40, 0, -0] + +## TODO: end-coords tokuni base +## FIXME: base_linkが浮いてる \ No newline at end of file diff --git a/jsk_pr2_robot/jsk_pr2_calibration/CHANGELOG.rst b/jsk_pr2_robot/jsk_pr2_calibration/CHANGELOG.rst new file mode 100644 index 0000000000..82b2cfeee8 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_calibration/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_pr2_calibration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ + +0.0.1 (2014-12-25) +------------------ +* Remove generate file +* Add utility package to generate JSK-style pr2 urdf +* Contributors: Ryohei Ueda diff --git a/jsk_pr2_robot/jsk_pr2_calibration/package.xml b/jsk_pr2_robot/jsk_pr2_calibration/package.xml index 9b837574ae..42ad4c3f96 100644 --- a/jsk_pr2_robot/jsk_pr2_calibration/package.xml +++ b/jsk_pr2_robot/jsk_pr2_calibration/package.xml @@ -1,7 +1,7 @@ jsk_pr2_calibration - 0.0.0 + 0.0.6 The jsk_pr2_calibration package Ryohei Ueda BSD diff --git a/jsk_pr2_robot/jsk_pr2_startup/CHANGELOG.rst b/jsk_pr2_robot/jsk_pr2_startup/CHANGELOG.rst new file mode 100644 index 0000000000..0945b0f4bd --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/CHANGELOG.rst @@ -0,0 +1,85 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_pr2_startup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ +* [jsk_pr2_startup] Add rossetpr1012 and rossetpr1040 automatically by env-hooks +* add deps jsk_interactive_marker for jsk_pr2_startup +* add pr2 deps package for build test +* use only catkin; add deps for running pr2.launch +* add dwa_local_planner to build/run dependencies +* add move_base_msgs, roseus to build dependencies +* update readme for launching mongodb by multi users +* [jsk_pr2_startup] Remove collider related roslaunch +* launch mongodb when robot starts +* add action_result_db to record action result/goal and joint_states +* add tilt_scan_interpolated topic +* add openni_cloud_self_filter to launch as default and publish color pointclouds +* tested objectdetection for all camera on PR2 +* tested on PR2 +* fix option of db_client launch +* add debug message to objectdetection_db.py +* [jsk_pr2_robot] Use jsk_network_tools' euslisp code to + compress/decompress joint angles +* migrate pr2 move_base, objectdetection db from postgre to mongodb +* Contributors: Ryohei Ueda, Yuki Furuta, Yuto Inagaki + +0.0.4 (2015-01-30) +------------------ +* [jsk_pr2_startup] Remove unrequired return-from in pr2-compressed-angle-vector-interface +* rename pr2-compressed-angle-vector-interface.l +* use string to set data +* fix typo +* update to work +* add jsk_pr2_teleop + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* [jsk_pr2_startup] Disable collider node, it's out of date +* Merge pull request #232 from garaemon/rename-hydro-recognition + [jsk_pr2_startup] rename hydro_recognition.launch to people_detection.launch and start it up default +* [jsk_pr2_startup] Remove torso_lift_link from self filtering of + tilt laser to avoid too much filtering of points. And update padding + of shoulder links to remove veiling noise +* [jsk_pr2_startup] rename hydro_recognition.launch to people_detection.launch + and start it up in default. +* Merge pull request #230 from garaemon/move-image-processing-to-c2 + [jsk_pr2_startup] Move several image processing to c2 to avoid heavy network communication between c1 and c2 +* [jsk_pr2_startup] Move several image processing to c2 to avoid heavy + network communication between c1 and c2 +* [jsk_pr2_startup] Throttle before applying image_view2 to decrease + CPU load +* use robot-actions.l +* Fix parameter namespace to slow down pr2_gripper_sensor_action +* Use longer priod to check openni soundness +* use rostwitter and python_twoauth +* Contributors: Kei Okada, Ryohei Ueda, Yusuke Furuta + +0.0.1 (2014-12-25) +------------------ +* Restarting kinect paranoiac + 1) usb reset + 2) kill nodelet manager + 3) kill child processing + 4) restart openni.launch (hardcoded!) +* Add rviz_mouse_point_to_tablet.py to pr2.launch +* Use larger value to detect gound object by PR2 to avoid small noises +* Add sound when launching pr2.launch +* kill nodelet manager and processes rather than killing openni/driver +* Say something at the end of pr2.launch +* Use low framerate for gripper sensors to avoid high load +* move twitter related program to robot_common from jsk_pr2_startup +* modify launch file for gazebo +* add yaml file for gazebo +* delete LaserScanIntensityFilter +* modify sensors_kinect and add sensors +* move pr2 related package under jsk_pr2_robot +* Contributors: Ryohei Ueda, Yuto Inagaki, Yusuke Furuta diff --git a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt index b00b34ceb2..75c26c4fe6 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt +++ b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt @@ -1,35 +1,22 @@ -if(NOT USE_ROSBUILD) - include(catkin.cmake) - return() -endif() +cmake_minimum_required(VERSION 2.8.3) +project(jsk_pr2_startup) +find_package(catkin REQUIRED COMPONENTS + jsk_network_tools + move_base_msgs + roseus) -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +catkin_package( + CATKIN_DEPENDS jsk_network_tools jsk_robot_startup pr2_mannequin_mode pr2_gripper_sensor_action +) -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) +catkin_add_env_hooks(99.jsk_pr2_startup SHELLS bash zsh + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) -rosbuild_init() +install(DIRECTORY config jsk_pr2_image_transport + jsk_pr2_joy jsk_pr2_lifelog jsk_pr2_move_base jsk_pr2_moveit + jsk_pr2_sensors jsk_pr2_warning src + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +install(FILES install_pr1040_description.sh jsk_pr2.machine plugin.xml + pr2.launch pr2_bringup.launch pr2_jsk_interactive.launch rviz.launch startup.app + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jsk_pr2_robot/jsk_pr2_startup/README.md b/jsk_pr2_robot/jsk_pr2_startup/README.md index 6eb102ca5c..58def21551 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/README.md +++ b/jsk_pr2_robot/jsk_pr2_startup/README.md @@ -1,6 +1,7 @@ # jsk_pr2_startup ## setup + ###. rewrite `/etc/ros/robot.launch` Please rewrite `/etc/ros/robot.launch` like following: @@ -34,4 +35,18 @@ Please rewrite `/etc/ros/robot.launch` like following: -``` \ No newline at end of file +``` + +### launch mongodb for multiple users + +Different users in same unix group can't run mongod against single db owned by that group. +This is because `mongod` opens database files using the `O_NOATIME` flag to the open system call. +Open with `O_NOATIME` only works if the UID completelly matchs or the caller is priviledged (`CAP_FOWNER`) for security reasons. +So if you want to launch mongodb with shared database resouces, it's better to use POSIX Capabilities in Linux. + +```bash +# In Ubuntu +$ sudo aptitude install libcap2-bin +$ sudo setcap cap_fowner+ep /usr/bin/mongod +``` + diff --git a/jsk_pr2_robot/jsk_pr2_startup/catkin.cmake b/jsk_pr2_robot/jsk_pr2_startup/catkin.cmake deleted file mode 100644 index 811f9db89b..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/catkin.cmake +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(jsk_pr2_startup) -find_package(catkin REQUIRED) -catkin_package() diff --git a/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.bash b/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.bash new file mode 100644 index 0000000000..936170450b --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.bash @@ -0,0 +1,20 @@ +#!/bin/bash +# -*- mode: shell-script -*- + +function rossetpr1040() { + rossetmaster pr1040.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=PR2 + rossetip +} + +function rossetpr1012() { + rossetmaster pr1012.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=PR2 + rossetip +} + +function rossetc1() { + rossetmaster c1 + export ROBOT=PR2 + rossetip +} diff --git a/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.zsh b/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.zsh new file mode 100644 index 0000000000..9a65583f59 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/env-hooks/99.jsk_pr2_startup.zsh @@ -0,0 +1,20 @@ +#!/bin/zsh +# -*- mode: shell-script -*- + +function rossetpr1040() { + rossetmaster pr1040.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=PR2 + rossetip +} + +function rossetpr1012() { + rossetmaster pr1012.jsk.imi.i.u-tokyo.ac.jp + export ROBOT=PR2 + rossetip +} + +function rossetc1() { + rossetmaster c1 + export ROBOT=PR2 + rossetip +} diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_image_transport/pr2_roi_transport.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_image_transport/pr2_roi_transport.launch index f19876fc8c..b710626992 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_image_transport/pr2_roi_transport.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_image_transport/pr2_roi_transport.launch @@ -13,12 +13,14 @@ diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db.py index a24468f7e4..85981a173a 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db.py @@ -1,210 +1,175 @@ #!/usr/bin/python # -# This script stores All actionlib goals and results by PostgreSQL +# This script stores actionlib goals and results by MongoDB # And store the current robot joints when recieve result. # # table configuration is bottom of this script # # parameters: -# dbname,hostname,port,username,passwd: for DB connection -# except action type: # joint_tolerance: store the past joint_states (sec) -import roslib; roslib.load_manifest('jsk_pr2_startup') import rospy -import pgdb import tf -import cStringIO -import thread import std_msgs.msg import actionlib_msgs.msg import sensor_msgs.msg from sensor_msgs.msg import JointState -def message_serialize(msg): - buf = cStringIO.StringIO() - msg.serialize(buf) - return buf.getvalue().encode('hex_codec') +from mongodb_store.message_store import MessageStoreProxy -class ActionResultDB: + +class ActionResultDB(object): loaded_types = [] - useless_types = ['roslib.msg.Header'] # message types not but action (goal|result) + useless_types = ['std_msgs/Header'] # message types not but action (goal|result) subscribers = {} # topicname:subscriber - def __init__(self,connection=None,db_lock=None): # TODO - # args dbname, host, port, opt, tty, user, passwd - db_name = rospy.get_param('~db_name','pr2db') - host = rospy.get_param('~host_name','c1') - port = rospy.get_param('~port',5432) - username = rospy.get_param('~user_name','pr2admin') - passwd = rospy.get_param('~passwd','') - self.con = pgdb.connect(database=db_name, host=host, user=username, password=passwd) - self.lockobj = thread.allocate_lock() - # initialize + def __init__(self): # TODO + self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog') + self.col_name = rospy.get_param('~col_name', 'action_result_db') + self.update_cycle = rospy.get_param('~update_cycle', 1.0) + self.joint_tolerance = 1.0 self.joint_states = [] self.joint_states_inserted = [] # list of time stamp - self.joint_sub = rospy.Subscriber('/joint_states', JointState, self.joint_states_cb) - return + self.joint_sub = rospy.Subscriber('/joint_states', JointState, self._joint_states_cb) + + self._load_params() + + self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) + rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) + + def _load_params(self): + try: + self.action_name_white_list = rospy.get_param('~white_list')['name'] + rospy.loginfo("whitelist_name: %s", self.action_name_white_list) + except: + self.action_name_white_list = None + try: + self.action_name_black_list = rospy.get_param('~black_list')['name'] + rospy.loginfo("blacklist_name: %s", self.action_name_black_list) + except: + self.action_name_black_list = None + try: + self.action_type_white_list = rospy.get_param('~white_list')['type'] + rospy.loginfo("whitelist_type: %s", self.action_type_white_list) + except: + self.action_type_white_list = None + try: + self.action_type_black_list = rospy.get_param('~black_list')['type'] + rospy.loginfo("blacklist_name: %s", self.action_type_black_list) + except: + self.action_type_black_list = None # callback functions - def action_goal_cb(self, topic, type_name, msg): - self.lockobj.acquire() - self.insert_action_goal(topic,type_name,msg) - self.lockobj.release() - return - - def action_result_cb(self, topic, type_name, msg): - self.lockobj.acquire() - self.insert_action_result(topic,type_name,msg) + def _action_goal_cb(self, topic, type_name, msg): + self._insert_action_goal(topic,type_name,msg) + + def _action_result_cb(self, topic, type_name, msg): + self._insert_action_result(topic,type_name,msg) key_func = (lambda js: abs(js.header.stamp-msg.header.stamp)) nearest_state = min(self.joint_states,key=key_func) - self.insert_joint_states(nearest_state) - self.lockobj.release() - return + self._insert_joint_states(nearest_state) - def joint_states_cb(self, msg): - self.lockobj.acquire() + def _joint_states_cb(self, msg): self.joint_states = [m for m in self.joint_states if (msg.header.stamp - m.header.stamp).to_sec() < self.joint_tolerance] + [msg] self.joint_states_inserted = [s for s in self.joint_states_inserted if (msg.header.stamp - s).to_sec() < self.joint_tolerance] - self.lockobj.release() - return # insert functions - def insert_action_goal(self,topic,type_name,msg): - rospy.loginfo("insert action_goal message"); - goal_string = message_serialize(msg.goal) - cursor = self.con.cursor() - cursor.execute("INSERT INTO action_goal (topic,type,header_stamp, header_frame_id, goal_id_stamp, goal_id_id, goal) VALUES ('%s','%s',%d,'%s',%d,'%s','%s');" % (topic,type_name,msg.header.stamp.to_nsec(), msg.header.frame_id, msg.goal_id.stamp.to_nsec(), msg.goal_id.id, goal_string)) - cursor.close() - self.con.commit() - return - - def insert_action_result(self,topic,type_name,msg): - rospy.loginfo("insert action_result message"); - result_string = message_serialize(msg.result) - cursor = self.con.cursor() - cursor.execute("INSERT INTO action_result (topic,type,header_stamp, header_frame_id, status_goal_id_stamp, status_goal_id_id, status_status, status_text, result) VALUES ('%s','%s',%d,'%s',%d,'%s',%d,'%s','%s');" % (topic,type_name, msg.header.stamp.to_nsec(), msg.header.frame_id, msg.status.goal_id.stamp.to_nsec(), msg.status.goal_id.id, msg.status.status, msg.status.text, result_string)) - cursor.close() - self.con.commit() - return - - def insert_joint_states(self,msg): - if msg.header.stamp in self.joint_states_inserted: - return - rospy.loginfo("insert joint_states message"); - msg_string = message_serialize(msg) - cursor = self.con.cursor() - cursor.execute("INSERT INTO joint_states (header_stamp, header_frame_id, msg) VALUES (%d,'%s','%s');" % (msg.header.stamp.to_nsec(), msg.header.frame_id, msg_string)) - cursor.close() - self.con.commit() - return + def _insert_action_goal(self,topic,type_name,msg): + try: + res = self.msg_store.insert(msg) + rospy.loginfo("inserted action_goal message: %s (%s) -> %s", topic, type_name, res) + except Exception as e: + rospy.logerr("failed to insert action goal: %s (%s) -> %s", topic, type_name, e) + + def _insert_action_result(self,topic,type_name,msg): + try: + res = self.msg_store.insert(msg) + rospy.loginfo("inserted action_result message: %s (%s) -> %s", topic, type_name, res) + except Exception as e: + rospy.logerr("failed to insert action result: %s (%s) -> %s", topic, type_name, e) + + + def _insert_joint_states(self, msg): + try: + if msg.header.stamp in self.joint_states_inserted: + return + res = self.msg_store.insert(msg) + rospy.loginfo("inserted joint_states message: %s", res) + except Exception as e: + rospy.logerr("failed to insert joint states: %s", e) # if the message type is goal or result, return the callback - def message_callback_type(self,name,type_name,type_obj): - if not hasattr(type_obj,'header'): return None - if type(type_obj.header) != std_msgs.msg.Header: return None - if type_name in ['JointTrajectoryActionGoal', - 'JointTrajectoryActionResult', - 'PointHeadActionGoal', - 'PointHeadActionResult', - 'SingleJointPositionActionGoal', - 'SingleJointPositionActionResult', - 'Pr2GripperCommandActionGoal', - 'Pr2GripperCommandActionResult']: - return None + def _message_callback_type(self,name,type_name,type_obj): + if not hasattr(type_obj,'header'): return None # no header message + if type(type_obj.header) != std_msgs.msg.Header: return None # custom header message if hasattr(type_obj,'goal_id') and hasattr(type_obj,'goal') and type(type_obj.goal_id) == actionlib_msgs.msg.GoalID: - return (lambda msg: self.action_goal_cb(name,type_name,msg)) + return (lambda msg: self._action_goal_cb(name,type_name,msg)) if hasattr(type_obj,'status') and hasattr(type_obj,'result') and type(type_obj.status) == actionlib_msgs.msg.GoalStatus: - return (lambda msg: self.action_result_cb(name,type_name,msg)) - return None + return (lambda msg: self._action_result_cb(name,type_name,msg)) + else: + return None + + def sleep_one_cycle(self): + rospy.sleep(self.update_cycle) # subscriber updater def update_subscribers(self): - - # check connections -# for name in self.subscribers.keys(): -# sub = self.subscribers[name] -# if sub.get_num_connections() == 0: -# sub.unregister() -# self.subscribers.pop(name) -# rospy.loginfo('unsubscribe (%s)',name) - # check new publishers - current_subscribers = rospy.client.get_published_topics() - for topic_info in current_subscribers: - name = topic_info[0] - typ_tuple = tuple(topic_info[1].split('/')) - typ = '%s.msg.%s'%typ_tuple - if typ in self.useless_types: + topics = rospy.client.get_published_topics() + if self.action_name_white_list: + topics = [t for t in topics if t[0] in self.action_name_white_list] + if self.action_type_white_list: + topics = [t for t in topics if t[1].split('/')[1] in self.action_type_white_list] + if self.action_name_black_list: + topics = [t for t in topics if not t[0] in self.action_name_black_list] + if self.action_type_black_list: + topics = [t for t in topics if not t[1].split('/')[1] in self.action_type_black_list] + + for topic_name, topic_type in topics: + (pkg_name, msg_type) = topic_type.split('/') + py_topic_class = '%s.msg.%s' % (pkg_name, msg_type) + if py_topic_class in self.useless_types: continue - if name in self.subscribers.keys(): + if topic_name in self.subscribers.keys(): continue # Import and Check try: - type_obj = eval(typ)() - except (AttributeError, NameError), e: - try: - rospy.loginfo("try to load [%s]", typ_tuple[0]); - roslib.load_manifest(typ_tuple[0]) - exec('import ' + typ_tuple[0] + '.msg') - except SyntaxError, e: - rospy.loginfo('please rosmake %s', typ_tuple[0]) + pypkg = __import__('%s.msg' % pkg_name) + rospy.loginfo('imported %s.msg', pkg_name) + except Exception as e: + rospy.logerr('failed to import %s.msg: %s', pkg_name, e) + rospy.logerr('please catkin_make %s', pkg_name) + continue try: - type_class = eval(typ) - cb_obj = self.message_callback_type(name,typ_tuple[1],type_class()) + type_class = getattr(getattr(pypkg, 'msg'), msg_type) + type_instance = type_class() + except Exception as e: + rospy.logerr('failed to instantiate %s.msg.%s: %s', pkg_name, msg_type, e) + continue + + try: + cb_obj = self._message_callback_type(topic_name, msg_type, type_instance) if cb_obj == None: - self.useless_types += [typ] + self.useless_types += [py_topic_class] continue - self.subscribers[name] = rospy.Subscriber(name,type_class,cb_obj) - rospy.loginfo("start subscribe (topic=%s type=%s)", name, typ); - except Exception, e: - self.useless_types += [typ] - rospy.loginfo('error in checking '+typ_tuple+e) + + self.subscribers[topic_name] = rospy.Subscriber(topic_name, type_class, cb_obj) + rospy.loginfo("start subscribe (topic=%s type=%s)", topic_name, msg_type) + except Exception as e: + self.useless_types += [py_topic_class] + rospy.logerr('error registering subscriber: %s', e) continue - return + if __name__ == "__main__": rospy.init_node('action_result_db') obj = ActionResultDB() - looprate = rospy.Rate(1.0) + while not rospy.is_shutdown(): obj.update_subscribers() - looprate.sleep() - -''' -CREATE TABLE action_goal( - id SERIAL PRIMARY KEY UNIQUE, - topic TEXT, - type TEXT, - header_stamp BIGINT, - header_frame_id TEXT, - goal_id_stamp BIGINT, - goal_id_id TEXT, - goal TEXT); -''' -''' -CREATE TABLE action_result( - id SERIAL PRIMARY KEY UNIQUE, - topic TEXT, - type TEXT, - header_stamp BIGINT, - header_frame_id TEXT, - status_goal_id_stamp BIGINT, - status_goal_id_id TEXT, - status_status INTEGER, - status_text TEXT, - result TEXT); -''' -''' -CREATE TABLE joint_states( - id SERIAL PRIMARY KEY UNIQUE, - header_stamp BIGINT, - header_frame_id TEXT, - msg TEXT); -''' -# goal,result,msg is a serialized message object and coded in hex + obj.sleep_one_cycle() diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db_config.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db_config.yaml new file mode 100644 index 0000000000..52c753b815 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/action_result_db_config.yaml @@ -0,0 +1,12 @@ +black_list: + type: + - JointTrajectoryActionGoal + - JointTrajectoryActionResult + - PointHeadActionGoal + - PointHeadActionResult + - SingleJointPositionActionGoal + - SingleJointPositionActionResult + - Pr2GripperCommandActionGoal + - Pr2GripperCommandActionResult + - LookupTransformActionGoal + - LookupTransformActionResult \ No newline at end of file diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch index ca70c60ecd..8c29b2f782 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch @@ -1,12 +1,42 @@ - + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/move_base_db.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/move_base_db.py index f698a159eb..6c12fe9f16 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/move_base_db.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/move_base_db.py @@ -1,143 +1,113 @@ #!/usr/bin/python -# -# This uses PostgreSQL and tf from /map_frame to /robot_frame -# - -# parameters: -# dbname,hostname,port,username,passwd: for DB connection -# robot_frame: current robot base frame_id -# map_frame: map origin frmae_id - -# Table "public.tf" -# Column | Type | Modifiers -#-------------------------+---------+------------------------------------------------- -# id | integer | not null default nextval('tf_id_seq'::regclass) -# header_stamp | bigint | -# header_frame_id | text | -# child_frame_id | text | -# transform_translation_x | real | -# transform_translation_y | real | -# transform_translation_z | real | -# transform_rotation_x | real | -# transform_rotation_y | real | -# transform_rotation_z | real | -# transform_rotation_w | real | -#Indexes: -# "tf_stamp_idx" btree (header_stamp) -### command for creating table -# con = pgdb.connect(database='pr2db', host='c1', user='pr2admin') -# cur = con.cursor() -# cur.execute("CREATE TABLE tf (id serial, header_stamp bigint, header_frame_id text, child_frame_id text, transform_translation_x real, transform_translation_y real, transform_translation_z real, transform_rotation_x real, transform_rotation_y real, transform_rotation_z real, transform_rotation_w real);") -# cur.execute("CREATE INDEX tf_stamp_idx ON tf (header_stamp);") -# cur.close() -# con.commit() import rospy -import pgdb import tf2_ros + try: - import tf2 # groovy + import tf2 # groovy except: import tf2_py as tf2 import geometry_msgs +from mongodb_store.message_store import MessageStoreProxy -class MoveBaseDB: - def __init__(self,connection=None,db_lock=None): - db_name = rospy.get_param('~db_name','pr2db') - host = rospy.get_param('~host_name','c1') - port = rospy.get_param('~port',5432) - username = rospy.get_param('~user_name','pr2admin') - passwd = rospy.get_param('~passwd','') - self.update_cycle = rospy.get_param('update_cycle', 1) - # args dbname, host, port, opt, tty, user, passwd - self.con = pgdb.connect(database=db_name, host=host, user=username, password=passwd) +class MoveBaseDB(object): + def __init__(self): + self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog') + self.col_name = rospy.get_param('~col_name', 'move_base_db') + self.update_cycle = rospy.get_param('~update_cycle', 1.0) self.map_frame = rospy.get_param('~map_frame','map') self.robot_frame = rospy.get_param('~robot_frame','base_link') self.tf_listener = tf2_ros.BufferClient("tf2_buffer_server") self.initialpose_pub = rospy.Publisher('/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped) + self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) + rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) + rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame)) + self.current_pose = None self.latest_pose = None self.latest_stamp = rospy.Time(0) - self.load_latest_pose() + self._load_latest_pose() self.pub_latest_pose() - return + self.latest_exception = None - # DB Insertion function - def insert_pose_to_db(self, table, stamp, source, target, pose): - ##rospy.loginfo("insert robot_base pose"+str(pose)); - (trans,rot) = pose - cursor = self.con.cursor() - cursor.execute("INSERT INTO %s (header_stamp,header_frame_id,child_frame_id,transform_translation_x, transform_translation_y, transform_translation_z, transform_rotation_x, transform_rotation_y, transform_rotation_z, transform_rotation_w) VALUES (%d,'%s','%s',%f,%f,%f,%f,%f,%f,%f);" % (table,stamp.to_nsec(),source,target,trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3])) - cursor.close() - self.con.commit() + def _insert_pose_to_db(self, map_to_robot): + try: + res = self.msg_store.insert(map_to_robot) + rospy.loginfo("inserted %s : %s" % (res, map_to_robot)) + except Exception as e: + rospy.logerr('failed to insert current robot pose to db: %s', e) - def load_latest_pose(self): - cursor = self.con.cursor() - pos_rot = cursor.execute("SELECT transform_translation_x, transform_translation_y, transform_translation_z, transform_rotation_x, transform_rotation_y, transform_rotation_z, transform_rotation_w FROM tf ORDER BY header_stamp DESC LIMIT 1") - result = cursor.fetchall() - if len(result) != 0: - rospy.loginfo("latest position: pos: %f %f %f, rot: %f %f %f %f" % (result[0][0], result[0][1], result[0][2], result[0][3], result[0][4], result[0][5], result[0][6],)) - self.current_pose = (result[0][0:3],result[0][3:]) - self.latest_pose = (result[0][0:3],result[0][3:]) - return + def _load_latest_pose(self): + updated = None + try: + updated = self.msg_store.query('geometry_msgs/TransformStamped', single=True, sort_query=[("$natural", -1)]) + except Exception as e: + rospy.logerr('failed to load latest pose from db: %s' % e) + if 'master has changed' in str(e): + self._load_latest_pose() + else: + return + if updated is not None: + rospy.loginfo('found latest pose %s' % updated) + self.current_pose = updated[0] + self.latest_pose = updated[0] + + def _need_update_db(self, t): + if self.current_pose is None: + if self.latest_pose is None: + return True + else: + return False + diffthre = 0.1 + 1.0 / (rospy.Time.now() - self.latest_stamp).to_sec() + diffpos = sum([(t.transform.translation.x - self.current_pose.transform.translation.x) ** 2, + (t.transform.translation.y - self.current_pose.transform.translation.y) ** 2, + (t.transform.translation.z - self.current_pose.transform.translation.z) ** 2]) ** 0.5 + diffrot = sum([(t.transform.rotation.x - self.current_pose.transform.rotation.x) ** 2, + (t.transform.rotation.y - self.current_pose.transform.rotation.y) ** 2, + (t.transform.rotation.z - self.current_pose.transform.rotation.z) ** 2, + (t.transform.rotation.w - self.current_pose.transform.rotation.w) ** 2]) ** 0.5 + rospy.loginfo("diffthre: %f, diffpos: %f, diffrot: %f" % (diffthre, diffpos, diffrot)) + if diffthre < diffpos or diffthre / 2.0 < diffrot: + return True + else: + return False def insert_current_pose(self): try: transform = self.tf_listener.lookup_transform(self.map_frame,self.robot_frame,rospy.Time(0)) - trans = [transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z] - rot = [transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w] - stamp = rospy.Time.now() - pose = (list(trans),list(rot)) - - diffthre = 0.1 + 1.0 / (stamp - self.latest_stamp).to_sec() - if (self.current_pose == None - or diffthre < (sum([(trans[i]-self.current_pose[0][i])**2 for i in [0,1,2]]) ** 0.5) - or diffthre/2 < (sum([(rot[i]-self.current_pose[1][i])**2 for i in [0,1,2,3]]) ** 0.5)): - self.insert_pose_to_db("tf",stamp,self.map_frame,self.robot_frame,pose) - self.current_pose = (trans,rot) - except (tf2.LookupException, tf2.ConnectivityException, \ - tf2.ExtrapolationException, tf2.TimeoutException, \ - tf2.TransformException): - return +# rospy.loginfo("current pr2 location: %s" % transform) + if self._need_update_db(transform): + self._insert_pose_to_db(transform) + self.current_pose = transform + except Exception as e: + if not self.latest_exception or str(e) is self.latest_exception: + rospy.logwarn('failed to get current robot pose from tf: %s' % e) + self.latest_exception = str(e) def sleep_one_cycle(self): - self.pub_latest_pose() - rospy.sleep(self.update_cycle); + rospy.sleep(self.update_cycle) def pub_latest_pose(self): - if (self.latest_pose != None \ - and self.initialpose_pub.get_num_connections() > 0): + if(self.latest_pose is not None and self.initialpose_pub.get_num_connections() > 0): + pub_msg = geometry_msgs.msg.PoseWithCovarianceStamped() + pub_msg.header.stamp = rospy.Time(0) + pub_msg.header.frame_id = '/map' + pub_msg.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942] + pub_msg.pose.pose.position.x = self.latest_pose.transform.translation.x + pub_msg.pose.pose.position.y = self.latest_pose.transform.translation.y + pub_msg.pose.pose.position.z = self.latest_pose.transform.translation.z + pub_msg.pose.pose.orientation = self.latest_pose.transform.rotation try: - transform = self.tf_listener.lookup_transform(self.map_frame, 'map', rospy.Time(0)) - trans = [transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z] - rot = [transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w] - (ctrans, crot) = self.latest_pose - - rospy.loginfo("set pos: %f %f %f, rot: %f %f %f %f" % (trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3])) - rospy.loginfo(" pos: %f %f %f, rot: %f %f %f %f" % (ctrans[0], ctrans[1], ctrans[2], crot[0], crot[1], crot[2], crot[3])) - - ps = geometry_msgs.msg.PoseWithCovarianceStamped() - ps.header.stamp = rospy.Time(0) - ps.header.frame_id = '/map' - ps.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942] - ps.pose.pose.position.x = ctrans[0] - trans[0] - ps.pose.pose.position.y = ctrans[1] - trans[1] - ps.pose.pose.position.z = ctrans[2] - trans[2] - ps.pose.pose.orientation.x = crot[0] - ps.pose.pose.orientation.y = crot[1] - ps.pose.pose.orientation.z = crot[2] - ps.pose.pose.orientation.w = crot[3] - self.initialpose_pub.publish (ps); + self.initialpose_pub.publish(pub_msg) self.latest_pose = None - except (tf2.LookupException, tf2.ConnectivityException, \ - tf2.ExtrapolationException, tf2.TimeoutException, \ - tf2.TransformException): - return + rospy.loginfo('published latest pose %s' % pub_msg) + except Exception as e: + rospy.logerr('failed to publish initial robot pose: %s' % e) if __name__ == "__main__": rospy.init_node('move_base_db') obj = MoveBaseDB() while not rospy.is_shutdown(): - obj.insert_current_pose(); - obj.sleep_one_cycle(); - exit(1) + obj.insert_current_pose() + obj.pub_latest_pose() + obj.sleep_one_cycle() diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/objectdetection_db.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/objectdetection_db.py index f122f60dc3..a5bef271bc 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/objectdetection_db.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/objectdetection_db.py @@ -3,80 +3,76 @@ # Store the ObjectDetection message # -# parameters: -# dbname,hostname,port,username,passwd: for DB connection -# table_name(default "tf"): DB table name +try: + import roslib; roslib.load_manifest('jsk_pr2_startup') +except: + pass -# fore example, table definition of "tf" -# Table "public.tf" -# Column | Type | Modifiers -#-------------------------+---------+------------------------------------------------- -# id | integer | not null default nextval('tf_id_seq'::regclass) -# header_stamp | bigint | -# header_frame_id | text | -# child_frame_id | text | -# transform_translation_x | real | -# transform_translation_y | real | -# transform_translation_z | real | -# transform_rotation_x | real | -# transform_rotation_y | real | -# transform_rotation_z | real | -# transform_rotation_w | real | -#Indexes: -# "tf_stamp_idx" btree (header_stamp) - -import roslib; roslib.load_manifest('jsk_pr2_startup') import rospy -import pgdb import tf -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, TransformStamped from posedetection_msgs.msg import ObjectDetection -class ObjectDetectionDB: - def __init__(self,connection=None,db_lock=None): # TODO - db_name = rospy.get_param('~db_name','pr2db') - host = rospy.get_param('~host_name','c1') - port = rospy.get_param('~port',5432) - username = rospy.get_param('~user_name','pr2admin') - passwd = rospy.get_param('~passwd','') - # args dbname, host, port, opt, tty, user, passwd - if connection == None: - self.con = pgdb.connect(database=db_name, host=host, - user=username, password=passwd) - else: - self.con = connection +from mongodb_store.message_store import MessageStoreProxy + +class ObjectDetectionDB(object): + def __init__(self): + self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog') + self.col_name = rospy.get_param('~col_name', 'objectdetection_db') + self.update_cycle = rospy.get_param('update_cycle', 1) + self.map_frame = rospy.get_param('~map_frmae', 'map') + self.robot_frame = rospy.get_param('~robot_frame','base_footprint') self.tf_listener = tf.TransformListener() - self.robot_frame = rospy.get_param('~base_frame_id','/base_link') - self.table_name = rospy.get_param('~table_name','tf') + self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) + rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) + rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame)) + self.subscribers = [] - return # DB Insertion function - def insert_pose_to_db(self, table, stamp, source, target, pose): - rospy.loginfo("insert ObjectDetection message"); - (trans,rot) = (pose.position, pose.orientation) - cursor = self.con.cursor() - cursor.execute("INSERT INTO %s (header_stamp,header_frame_id,child_frame_id,transform_translation_x, transform_translation_y, transform_translation_z, transform_rotation_x, transform_rotation_y, transform_rotation_z, transform_rotation_w) VALUES (%d,'%s','%s',%f,%f,%f,%f,%f,%f,%f);" % (table,stamp.to_nsec(),source,target,trans.x,trans.y,trans.z,rot.x,rot.y,rot.z,rot.w)) - cursor.close() - self.con.commit() + def _insert_pose_to_db(self, map_to_robot, robot_to_obj): + try: + self.msg_store.insert(map_to_robot) + self.msg_store.insert(robot_to_obj) + rospy.loginfo('inserted map2robot: %s, robot2obj: %s' % (map_to_robot, robot_to_obj)) + except Exception as e: + rospy.logwarn('failed to insert to db' + e) + + def _lookup_transform(self, target_frame, source_frame, time=rospy.Time(0), timeout=rospy.Duration(0.0)): + self.tf_listener.waitForTransform(target_frame, source_frame, time, timeout) + res = self.tf_listener.lookupTransform(target_frame, source_frame, time) + ret = TransformStamped() + ret.header.frame_id = target_frame + ret.header.stamp = time + ret.child_frame_id = source_frame + ret.transform.translation.x = res[0][0] + ret.transform.translation.y = res[0][1] + ret.transform.translation.z = res[0][2] + ret.transform.rotation.x = res[1][0] + ret.transform.rotation.y = res[1][1] + ret.transform.rotation.z = res[1][2] + ret.transform.rotation.w = res[1][3] + return ret + + def _objectdetection_cb(self, msg): + try: + self.tf_listener.waitForTransform(self.robot_frame, self.map_frame, msg.header.stamp, rospy.Duration(1.0)) + map_to_robot = self._lookup_transform(self.robot_frame, self.map_frame, msg.header.stamp) + except Exception as e: + rospy.logwarn("failed to lookup tf: %s", e) + return - def objectdetection_cb(self, msg): try: - self.tf_listener.waitForTransform(self.robot_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(1.0)) for obj in msg.objects: spose = PoseStamped(header=msg.header,pose=obj.pose) - tpose = self.tf_listener.transformPose(self.robot_frame,spose) - pose = tpose.pose - - trans = (pose.position.x, pose.position.y, pose.position.z) - rot = (pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w) - self.insert_pose_to_db(self.table_name, msg.header.stamp, - msg.header.frame_id, obj.type, pose) - except (tf.Exception, tf.LookupException, tf.ConnectivityException): - return + tpose = self.tf_listener.transformPose(self.robot_frame, spose) + obj.pose = tpose.pose + self._insert_pose_to_db(map_to_robot, obj) + except Exception as e: + rospy.logwarn("failed to object pose transform: %s", e) - def update_subscribers(self): + def _update_subscribers(self): current_subscribers = rospy.client.get_published_topics() targets = [x for x in current_subscribers if x[1]=='posedetection_msgs/ObjectDetection' and ('_agg' in x[0])] for sub in self.subscribers: @@ -88,15 +84,16 @@ def update_subscribers(self): if topic_info[0] in [x.name for x in self.subscribers]: continue sub = rospy.Subscriber(topic_info[0], ObjectDetection, - obj.objectdetection_cb) + self._objectdetection_cb) self.subscribers += [sub] rospy.loginfo('start subscribe (%s)',sub.name) + def sleep_one_cycle(self): + self._update_subscribers() + rospy.sleep(self.update_cycle) + if __name__ == "__main__": - rospy.init_node('pbjectdetecton_db') + rospy.init_node('objectdetecton_db') obj = ObjectDetectionDB() - looprate = rospy.Rate(1.0) while not rospy.is_shutdown(): - obj.update_subscribers() - looprate.sleep() - + obj.sleep_one_cycle() diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py index cd2500de5c..88641bff2b 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py @@ -17,7 +17,7 @@ def __init__(self): self.image_sub = None def callback(self,data): - if data.header.stamp < self.prev_time + rospy.Duration(5): + if data.header.stamp < self.prev_time + rospy.Duration(60): return self.image_sub.unregister() self.prev_time = data.header.stamp diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch index ab057b4261..e305933ada 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch @@ -125,4 +125,6 @@ machine="c2" args="c2"/> + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml index 2be2fef20b..b9158d2375 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml @@ -14,6 +14,12 @@ + + + + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter.launch deleted file mode 100644 index 1cafd02afd..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter_color.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter_color.launch index a0cf888238..881e84741a 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter_color.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/openni_cloud_self_filter_color.launch @@ -1,24 +1,13 @@ - - - - - - - - - + type="self_filter" clear_params="true" + name="openni_cloud_self_filter" respawn="true" output="screen" machine="c2"> + + - + + + - - - - \ No newline at end of file diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hydro_recognition.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/people_detection.launch similarity index 92% rename from jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hydro_recognition.launch rename to jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/people_detection.launch index 8266d5f801..77f312c3bb 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hydro_recognition.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/people_detection.launch @@ -1,10 +1,6 @@ - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_teleop_robot.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_teleop_robot.launch index 7c19e6c7ca..83836cf817 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_teleop_robot.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_teleop_robot.launch @@ -26,8 +26,9 @@ + machine="c2" pkg="roseus" type="roseus" name="robot_actions" + args="$(find jsk_interactive_marker)/euslisp/robot-actions.l" + output="screen"> \ No newline at end of file diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tablet_startup.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tablet_startup.launch index 341f068698..97761b76b3 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tablet_startup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tablet_startup.launch @@ -15,9 +15,18 @@ machine="c2" type="emergency_stop.l" output="screen" args="(execute-main)" /> + + + + + update_rate: 5 + + + - + @@ -32,10 +41,16 @@ - + + + + + update_rate: 5 + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_cloud.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_cloud.launch deleted file mode 100644 index 46544d6192..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_cloud.launch +++ /dev/null @@ -1,70 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_self_filter.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_self_filter.yaml index 841e84dc8c..559667f8fc 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_self_filter.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_self_filter.yaml @@ -17,14 +17,14 @@ self_see_links: padding: .04 - name: l_forearm_roll_link padding: .04 - - name: l_forearm_cam_frame - padding: .01 + # - name: l_forearm_cam_frame + # padding: .01 - name: l_wrist_flex_link padding: .01 - name: l_wrist_roll_link padding: .01 - name: l_gripper_palm_link - padding: .04 #globe +0.03 + padding: .04 #globe +0.n03 - name: l_gripper_l_finger_link padding: .04 #globe +0.03 - name: l_gripper_l_finger_tip_link @@ -34,7 +34,7 @@ self_see_links: - name: l_gripper_r_finger_tip_link padding: .01 - name: l_shoulder_pan_link - padding: .01 + padding: .1 - name: l_shoulder_lift_link padding: .1 - name: r_upper_arm_link @@ -47,8 +47,8 @@ self_see_links: padding: .04 - name: r_forearm_roll_link padding: .04 - - name: r_forearm_cam_frame - padding: .01 + # - name: r_forearm_cam_frame + # padding: .01 - name: r_wrist_flex_link padding: .01 - name: r_wrist_roll_link @@ -64,15 +64,15 @@ self_see_links: - name: r_gripper_r_finger_tip_link padding: .01 - name: r_shoulder_pan_link - padding: .01 + padding: .1 - name: r_shoulder_lift_link padding: .1 - - name: base_laser_link - padding: .01 + # - name: base_laser_link + # padding: .01 - name: base_link padding: .01 - - name: torso_lift_link - padding: .05 + # - name: torso_lift_link + # padding: .05 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-bridge.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-bridge.l new file mode 100644 index 0000000000..82865ed1a1 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-bridge.l @@ -0,0 +1,65 @@ +(ros::roseus-add-msgs "jsk_pr2_startup") + +(defun compress-angle-vector (robot av &optional tms) + (if tms + (error "time is not impremented!")) + (let ((initial-pose (send robot :angle-vector)) + msg) + (send robot :angle-vector av) + (setq msg (instance jsk_pr2_startup::AngleVectorCompressed :init + :angles (coerce + (mapcar #'(lambda (jt) + (ros::ros-info "~A ~A ~A" + (send jt :joint-angle) + (send jt :min-angle) + (send jt :max-angle)) + (if (infinite-joint-p jt) + (round (* (/ + (abs (mod (send jt :joint-angle) 360)) + 360.0) + 255)) + (round (* (/ (- (send jt :joint-angle) + (send jt :min-angle)) + (- (send jt :max-angle) + (send jt :min-angle))) + 255)))) + (send robot :joint-list)) + string))) + (send robot :angle-vector initial-pose) + msg)) + +(defun decompress-angle-vector (robot msg) + (ros::ros-info "decompress ~A ~A" robot msg) + (coerce + (mapcar #'(lambda (jt-uint-ang) + (let ((jt (car jt-uint-ang)) + (uint-ang (cdr jt-uint-ang))) + (if (infinite-joint-p jt) + (* (/ uint-ang 255.0) 360.0) + (+ (send jt :min-angle) + (* (/ uint-ang 255.0) + (- (send jt :max-angle) + (send jt :min-angle))))))) + (pairlis + (send robot :joint-list) + (coerce (send msg :angles) cons))) + float-vector)) + +(defun infinite-joint-p (jt) + (or (> (send jt :max-angle) 360) + (< (send jt :min-angle) -360))) + +#| + (list (mapcar #'(lambda (av-uint-msg) + (let ((uint-av (send av-uint-msg :data))) + (mapcar #'(lambda (jt-ang) + (+ (send (car jt-ang) :min-angle) + (* (cdr jt-ang) + (- (send (car jt-ang) :max-angle)) + (send (car jt-ang) :min-angle))))) + (pairlst (send robot :joint-list) + uint-av))) + (send msg :angles)) + (send (send msg :times) :data))) +|# +(provide :pr2-compressed-angle-vector-bridge) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-client.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-client.l new file mode 100644 index 0000000000..001289af9d --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-client.l @@ -0,0 +1,31 @@ +#!/usr/bin/env roseus + +(ros::roseus "pr2_compressed_angle_vector_client") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :compressed-angle-vector "package://jsk_network_tools/euslisp/angle-vector-compress.l") +(ros::roseus-add-msgs "jsk_network_tools") +(pr2-init) + +(ros::subscribe "angle_vector_compressed" jsk_network_tools::CompressedAngleVectorPR2 + #'(lambda (msg) + (let ((av (decompress-angle-vector *pr2* (send msg :angles)))) + (ros::ros-info "angle-vector comming: ~A" av) + (send *pr2* :angle-vector av) + (send *ri* :angle-vector av)))) + +(ros::advertise "potentio_vector_compressed" jsk_network_tools::CompressedAngleVectorPR2 1) + +(ros::rate 10) + +(defun publish-compressed-joint-state () + (let ((msg (instance jsk_network_tools::CompressedAngleVectorPR2 :init))) + (send msg :angles + (compress-angle-vector *pr2* (send *ri* :state :potentio-vector))) + (ros::ros-info "compressed msg: ~A" (send *ri* :state :potentio-vector)) + (ros::publish "potentio_vector_compressed" msg))) + +(while (ros::ok) + (ros::spin-once) + (publish-compressed-joint-state) + (ros::sleep)) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-interface.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-interface.l new file mode 100644 index 0000000000..b44e9d9ea1 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_teleop/pr2-compressed-angle-vector-interface.l @@ -0,0 +1,70 @@ +#!/usr/bin/env roseus + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :pr2-compressed-angle-vector-bridge "pr2-compressed-angle-vector-bridge.l") +(ros::roseus-add-msgs "jsk_network_tools") +(defclass pr2-compressed-angle-vector-interface + :super pr2-interface + :slots (current-angle-vector groupname-compressed updating)) + +(defmethod pr2-compressed-angle-vector-interface + (:init + (&rest args &key (type :default-controller) + (move-base-action-name "move_base") &allow-other-keys) + (setq groupname-compressed "compressed") + (ros::create-nodehandle groupname-compressed) + (ros::advertise "angle_vector_compressed" jsk_network_tools::CompressedAngleVectorPR2 1) + (ros::subscribe "potentio_vector_compressed" jsk_network_tools::CompressedAngleVectorPR2 + #'send self :potentio-vector-compressed-callback :groupname groupname-compressed) + (send-super* :init :type type :move-base-action-name move-base-action-name args)) + (:angle-vector + (av &optional tm &rest args) + (if tm (ros::ros-warn "time is ignored")) + (ros::publish "angle_vector_compressed" + (instance jsk_network_tools::CompressedAngleVectorPR2 + :init :angles (compress-angle-vector robot av))) + (send robot :angle-vector av)) + (:angle-vector-sequence + (avs &optional tms &rest args) + (if tms (ros::ros-warn "time-lst is ignored")) + (dolist (av avs) + (send self :angle-vector av) + (unix:sleep 3))) + (:state + (&rest args) + (case (car args) + (:potentio-vector + (send self :update-current-angle-vector) + current-angle-vector) + (:reference-vector + (send self :update-current-angle-vector) + (while updating) + current-angle-vector) + (t + (send-super* :state args)))) + (:update-current-angle-vector + (&key (timeout 1000)) + (let ((start-time (ros::time-now))) + (setq updating t) + (while (and updating (< (send (ros::time- (ros::time-now) start-time) :to-nsec) (* timeout 1000 1000))) + (ros::spin-once groupname-compressed)))) + (:potentio-vector-compressed-callback + (msg) + (setq current-angle-vector (decompress-angle-vector robot (send msg :angles))) + (send robot :angle-vector current-angle-vector) + (ros::ros-info "current-angle-vector: ~A" current-angle-vector) + (setq updating nil))) + + +(unless (fboundp 'pr2-init-org) + (setf (symbol-function 'pr2-init-org) (symbol-function 'pr2-init))) + +(defun pr2-init (&rest args) + (unless (boundp '*ri*) + (setq *ri* (instance pr2-compressed-angle-vector-interface :init))) + (apply #'pr2-init-org args)) + +(ros::roseus "hoge") +(pr2-init) + +(provide :pr2-compressed-angle-vector-interface) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.l index a0934b302d..4f7b3943b2 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.l +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.l @@ -5,10 +5,14 @@ (ros::roseus-add-msgs "pr2_msgs") (ros::roseus-add-msgs "actionlib_msgs") (ros::roseus-add-srvs "std_srvs") +(ros::roseus-add-msgs "diagnostic_msgs") (load "package://pr2eus/speak.l") -(setq *ac* nil *ac-tm* (ros::time 0)) ;; ac_present > 0 +(ros::roseus "battery_warning") + +(setq *ac* nil *ac-tm* (ros::time-now)) ;; ac_present > 0 (setq *motor* t) ;; if motors working +(setq *warning-percentage* 5) (defun play_sound (sound) (let ((msg (instance sound_play::SoundRequest :init))) @@ -47,8 +51,46 @@ (t )))) -(ros::roseus "battery_warning") +(defun diag-cb (msg) + (when (> 180.0 (send (ros::time- (ros::time-now) *ac-tm*) :to-sec)) + (return-from diag-cb nil)) + (let ((battery-statuses (remove-if-not #'(lambda (d) + (substringp "/Power System/Smart Battery" + (send d :name))) + (send msg :status)))) + ;; check low capacity battery + (when (not *ac*) + (let ((warn-batt-names "")) + (dolist (b battery-statuses) + (dolist (kv (send b :values)) + (when (and + (substringp "Relative State Of Charge" (send kv :key)) + (> *warning-percentage* (read-from-string (send kv :value)))) + (setq warn-batt-names + (concatenate string + (subseq (send b :name) 28 31) + "と、" + warn-batt-names))))) + (ros::ros-info (format nil "ばってりー~Aが~A％以下です。" warn-batt-names *warning-percentage*)))) + + ;; check dischaged battery when plugged + (when *ac* + (let ((warn-batt-names "")) + (dolist (b battery-statuses) + (dolist (kv (send b :values)) + (when (and + (substringp "Charge Inhibited" (send kv :key)) + (string= "True" (send kv :value)) + (concatenate string + (subseq (send b :name) 28 31) + "と、" + warn-batt-names))))) + (ros::ros-info (format nil "ばってりー~Aが充電できていません" warn-batt-names)))))) + + +(ros::rate 1) (ros::subscribe "/power_state" pr2_msgs::PowerState #'check-power-state-cb) +(ros::subscribe "/diagnostics_agg" diagnostic_msgs::DiagnosticArray #'diag-cb) (ros::advertise "/robotsound" sound_play::SoundRequest 10) (ros::advertise "/robotsound_jp" sound_play::SoundRequest 5) (ros::spin) diff --git a/jsk_pr2_robot/jsk_pr2_startup/manifest.xml b/jsk_pr2_robot/jsk_pr2_startup/manifest.xml deleted file mode 100644 index 865e899115..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/manifest.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - - jsk_pr2_startup - - - Manabu Saito - BSD - - http://ros.org/wiki/jsk_pr2_startup - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/package.xml b/jsk_pr2_robot/jsk_pr2_startup/package.xml index 5a2d31064d..fc53483b97 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/package.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/package.xml @@ -1,6 +1,6 @@ jsk_pr2_startup - 0.0.0 + 0.0.6 jsk_pr2_startup @@ -14,17 +14,20 @@ openni_camera imagesift move_base + dwa_local_planner + move_base_msgs amcl map_server + jsk_network_tools - + pr2_base_trajectory_action - + pr2_gripper_sensor_action topic_tools mjpeg_server dynamic_tf_publisher @@ -32,15 +35,26 @@ jsk_pcl_ros jsk_topic_tools - python_twoauth + rostwitter jsk_perception resized_image_transport rospy + roseus rqt_gui rqt_gui_py rqt_py_common tf2 tf2_ros + std_msgs + jsk_robot_startup + pr2_mannequin_mode + jsk_interactive_marker + jsk_network_tools + dwa_local_planner + jsk_robot_startup + pr2_mannequin_mode + pr2_gripper_sensor_action + jsk_interactive_marker diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index 6e95df590b..a01863b13c 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -14,8 +14,8 @@ + - @@ -96,20 +96,13 @@ - - - - - + - - - + @@ -144,6 +137,8 @@ + diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index 4abbd163e9..ea4d20661a 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -160,4 +160,10 @@ + + + + + + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/CHANGELOG.rst b/jsk_pr2_robot/pr2_base_trajectory_action/CHANGELOG.rst new file mode 100644 index 0000000000..cb9e53328b --- /dev/null +++ b/jsk_pr2_robot/pr2_base_trajectory_action/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pr2_base_trajectory_action +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ +* add install commands to cmake +* Contributors: Kei Okada + +0.0.1 (2014-12-25) +------------------ +* fix version number +* move pr2 related package under jsk_pr2_robot +* Contributors: Kei Okada, Yuto Inagaki diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/catkin.cmake b/jsk_pr2_robot/pr2_base_trajectory_action/catkin.cmake index 5d984b39eb..8102f62632 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/catkin.cmake +++ b/jsk_pr2_robot/pr2_base_trajectory_action/catkin.cmake @@ -14,4 +14,13 @@ catkin_package( ) add_executable(pr2_base_trajectory_action src/pr2_base_trajectory_action.cpp) -target_link_libraries(pr2_base_trajectory_action ${catkin_LIBRARIES} ${Boost_LIBRARIES}) \ No newline at end of file +target_link_libraries(pr2_base_trajectory_action ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +install(DIRECTORY config include launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(TARGETS pr2_base_trajectory_action + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/package.xml b/jsk_pr2_robot/pr2_base_trajectory_action/package.xml index ebb96c3980..02d0197ec2 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/package.xml +++ b/jsk_pr2_robot/pr2_base_trajectory_action/package.xml @@ -1,6 +1,6 @@ pr2_base_trajectory_action - 0.0.1 + 0.0.6 pr2_base_trajectory_action is a node that exposes and action interface to move robot base along a trajectory. saito diff --git a/jsk_robot_common/jsk_robot_startup/CHANGELOG.rst b/jsk_robot_common/jsk_robot_startup/CHANGELOG.rst new file mode 100644 index 0000000000..55a84a8f55 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/CHANGELOG.rst @@ -0,0 +1,41 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package jsk_robot_startup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2015-04-10) +------------------ + +0.0.5 (2015-04-08) +------------------ +* [jsk_baxter_startup] update to add position diff paramter for tweet +* [jsk_baxter_startup] modify to prevent baxter.launch fail +* [jsk_robot_startup/package.xml: add diagnostic_msgs, pr2_mechanism_controllers, sensor_msgs to build dependencies +* [sk_robot_startup/CMakeLists.txt] update to set permission for installed script files +* [jsk_robot_startup] modfiy CMakeLists.txt to install jsk_robot_startup correctly +* [jsk_robot_startup/lifelog/active_user.l] repair tweet lifelog +* [jsk_robot_startup/lifelog/mongodb.launch] fix typo of option in launch +* [jsk_robot_startup/lifelog/mongodb.launch: add mongodb launch; mongod kill watcher +* Contributors: Yuki Furuta, Yuto Inagaki + +0.0.4 (2015-01-30) +------------------ + +0.0.3 (2015-01-09) +------------------ + +0.0.2 (2015-01-08) +------------------ + +0.0.1 (2014-12-25) +------------------ +* check joint state and set movep for odom disable robot +* Add sound when launching pr2.launch +* Say something at the end of pr2.launch +* move twitter related program to robot_common from jsk_pr2_startup +* add ros-info +* robot time signal +* add tweet.l, see jsk_nao_startup.launch for example +* repiar mongodb.launch +* repair mongodb.launch and add param +* add jsk_robot_common/jsk_robot_startup +* Contributors: Kanae Kochigami, Ryohei Ueda, Yuto Inagaki, Yusuke Furuta diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index 49d45fe03b..06fa2d7aa5 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -5,6 +5,10 @@ find_package(catkin REQUIRED COMPONENTS mongodb_store roscpp rospy + pr2_mechanism_controllers + diagnostic_msgs + sensor_msgs + roseus ) catkin_package( @@ -13,3 +17,6 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} ) + +install(DIRECTORY lifelog util + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l index 95fceb31cc..47b0288523 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l @@ -5,8 +5,8 @@ (ros::load-ros-manifest "sensor_msgs") (ros::load-ros-manifest "diagnostic_msgs") -(setq *start-distance* 0) -(setq *start-angle* 0) +(setq *start-distance* nil) +(setq *start-angle* nil) (setq *servo-on* nil) (ros::roseus "active_user_statistics") @@ -17,15 +17,14 @@ (defun odom-cb (msg) (ros::ros-debug "odom_state -> distance ~A, angle ~A" (send msg :distance) (send msg :angle)) - (if (or (= *start-distance* 0) (= *start-angle* 0)) + (if (or (equal *start-distance* nil) (equal *start-angle* nil)) (setq *start-distance* (send msg :distance) - *start-angle* (send msg :angle))) - (setq *distance* (- (send msg :distance) *start-distance*) - *angle* (- (send msg :angle) *start-angle*))) + *start-angle* (send msg :angle)) + (setq *distance* (- (send msg :distance) *start-distance*) + *angle* (- (send msg :angle) *start-angle*)))) (defun joint-cb (msg) (setq *position* (concatenate float-vector (subseq (send msg :position) 12 16) (subseq (send msg :position) 17))) - (ros::ros-debug "/joint_states -> ~A" *position*) (update-activeness) ) @@ -48,12 +47,18 @@ ) ) +(when (ros::has-param "/active_user/position_diff_threshold") + (setq *position-diff-threshold* (ros::get-param "/active_user/position_diff_threshold"))) + + + (ros::advertise "diagnostics" diagnostic_msgs::DiagnosticArray 1) (setq *prev-distance* nil *prev-angle* nil *prev-position* nil) (setq *distance* nil *angle* nil *position* nil) (setq *status* 'stop) +(setq *movingp* nil) (setq *start-time* (ros::time 0)) (setq *elapsed-sec* 0) (setq *seq* 0) @@ -67,14 +72,17 @@ (unix::system (format nil "getent passwd `whoami` | cut -d ':' -f 5 | cut -d ',' -f 1 > /tmp/username_~d.txt" (unix::getpid))) + (warn "Get PID ~A" (unix::getpid)) (with-open-file (f (format nil "/tmp/username_~d.txt" (unix::getpid))) - (setq *user-name* (read-line f)))) + (setq *user-name* (read-line f))) + (ros::set-param "/active_user/launch_user_name" *user-name*) + ) (warn "~%;; start user_name = ~A~%" *user-name*) (if (and (ros::has-param "/active_user/elapsed_time") - (ros::has-param "/active_user/user_name") - (string= (ros::get-param "/active_user/user_name") *user-name*)) + (ros::has-param "/active_user/launch_user_name") + (string= (ros::get-param "/active_user/launch_user_name") *user-name*)) (setq *elapsed* (ros::time (ros::get-param "/active_user/elapsed_time"))) (setq *elapsed* (ros::time 0))) (warn "~%;; start elapsed_time with ~A sec~%~%" *elapsed*);; @@ -90,13 +98,15 @@ )) (defun update-activeness() - (ros::ros-debug "user -> ~A" *user-name*) + (ros::ros-debug "user -> ~A" *user-name*) + (ros::ros-debug "status-> ~A" *status*) + (ros::ros-debug "moving-> ~A" *movingp*) (setq *odom-disable* (not (ros::get-param "/active_user/odom_subscribe"))) ;; check if the robot is moving (when (and *user-name* (or (and *prev-distance* *prev-angle* *prev-position*) *odom-disable*)) (let ((diff-distance (if *odom-disable* nil (- *distance* *prev-distance*))) (diff-angle (if *odom-disable* nil (- *angle* *prev-angle*))) - (diff-position (if *odom-disable* nil (norm (v- *position* *prev-position*))))) + (diff-position (if (not *prev-position*) nil (norm (v- *position* *prev-position*))))) ;; check servo on (ros::ros-debug " servo on -> ~A" *servo-on*) ;; check move_base @@ -104,14 +114,13 @@ (ros::ros-debug " move base -> ~A ~A" diff-distance diff-angle)) ;; check arms (ros::ros-debug " joint-angle -> ~A" diff-position) - ;;for odom-enable machine (if (and *servo-on* - (or (> diff-distance 0.001) (> diff-angle 0.001) (> diff-position 0.001)) + (or (> diff-distance 0.001) (> diff-angle 0.001) (> diff-position *position-diff-threshold*)) (not *odom-disable*)) - (setq *movingp* t) + (setq *movingp* t) ;;for odom-disable machine - (if (and (> diff-position 0.001) + (if (and (and (numberp diff-position) (> diff-position *position-diff-threshold*) ) *odom-disable*) (setq *movingp* t) (setq *movingp* nil)) @@ -165,9 +174,9 @@ *prev-position* *position*) (setq *prev-position* *position*) ) - (when (and *user-name* *elapsed-sec*) + (when (and (> (length *user-name*) 0) *elapsed-sec*) (ros::set-param "/active_user/user_name" *user-name*) (ros::set-param "/active_user/elapsed_time" *elapsed-sec*)) ) -(ros::spin) \ No newline at end of file +(ros::spin) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch index eb5d0d6d03..080ddcd797 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch @@ -1,21 +1,24 @@ - - + - - - - + - - - + + + + + + + + - - + + + + - - - - - \ No newline at end of file + diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/time-signal.l b/jsk_robot_common/jsk_robot_startup/lifelog/time-signal.l old mode 100644 new mode 100755 diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch index 82e13c34ed..43c533a9ed 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch @@ -8,6 +8,7 @@ + @@ -35,6 +36,7 @@ + diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l index e607719987..9425ec1f56 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l @@ -14,7 +14,7 @@ (ros::rate 0.1) (do-until-key - (setq *user-name* (ros::get-param "/active_user/user_name") + (setq *user-name* (ros::get-param "/active_user/launch_user_name") *elapsed-time* (ros::get-param "/active_user/elapsed_time")) ;; tweet depend on up time (let ((st (ros::get-param "/active_user/start_time"))) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l index 762c26375e..398334bd73 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l @@ -11,9 +11,9 @@ (ros::rate 0.1) (do-until-key - (setq *user-name* (ros::get-param "/active_user/user_name") + (setq *user-name* (ros::get-param "/active_user/launch_user_name") *elapsed-time* (ros::get-param "/active_user/elapsed_time")) - (ros::ros-info "user -> ~A, time -> ~A (~A) " + (ros::ros-debug "user -> ~A, time -> ~A (~A) " *user-name* *elapsed-time* *target-second*) ;; tweet depend on working time (when (> *elapsed-time* *target-second*) diff --git a/jsk_robot_common/jsk_robot_startup/package.xml b/jsk_robot_common/jsk_robot_startup/package.xml index 58b7f4f887..b39c2d6206 100644 --- a/jsk_robot_common/jsk_robot_startup/package.xml +++ b/jsk_robot_common/jsk_robot_startup/package.xml @@ -1,7 +1,7 @@ jsk_robot_startup - 0.0.0 + 0.0.6 The jsk_robot_startup package inagaki BSD @@ -10,6 +10,10 @@ mongodb_store roscpp rospy + roseus + diagnostic_msgs + pr2_mechanism_controllers + sensor_msgs mongodb_store roscpp rospy diff --git a/jsk_robot_common/jsk_robot_startup/util/mongod_watcher.py b/jsk_robot_common/jsk_robot_startup/util/mongod_watcher.py new file mode 100755 index 0000000000..bd30726440 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/util/mongod_watcher.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# when this node is killed, kill mongod. + +import rospy +import signal +import os + +db_path=None + +def kill_mongod(): + pid = None + try: + with open(os.path.join(db_path, 'mongod.lock')) as f: + pid = int(f.readline()) + except Exception as e: + rospy.logfatal('cannot find mongod.lock: %s' % e) + return + rospy.sleep(5.0) + try: + os.kill(pid, signal.SIGINT) + rospy.loginfo('killed mongod: %d' % pid) + except Exception as e: + rospy.logwarn('escallating to send SIGTERM to mongod') + os.kill(pid, signal.SIGTERM) + +if __name__ == '__main__': + rospy.init_node('mongodb_killer') + db_path = rospy.get_param('~db_path', '/var/lib/robot/mongodb_store') + rospy.on_shutdown(kill_mongod) + rospy.spin()