diff --git a/.travis.rosinstall.melodic b/.travis.rosinstall.melodic
index d9add05bce..278b28a7f6 100644
--- a/.travis.rosinstall.melodic
+++ b/.travis.rosinstall.melodic
@@ -54,3 +54,15 @@
local-name: DENSORobot/denso_cobotta_ros
uri: https://github.com/DENSORobot/denso_cobotta_ros.git
version: bb60e75adb8477ed3402561b4ec3ba687af3f397
+# switchbot_ros is not correctly released.
+# see: https://github.com/jsk-ros-pkg/jsk_3rdparty/issues/356
+- git:
+ local-name: jsk-ros-pkg/jsk_3rdparty
+ uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
+ version: f0ab7bba54523b8f9945ed4a3e9c0efec0c8dde9
+# for jsk_spot_teleop in jsk_robot
+# spot_msgs is not released
+- git:
+ local-name: clearpathrobotics/spot_ros
+ uri: https://github.com/clearpathrobotics/spot_ros.git
+ version: 8e1554b95cf2875a53f14fda8165957559b0ffb7
\ No newline at end of file
diff --git a/jsk_aero_robot/aeroeus/CMakeLists.txt b/jsk_aero_robot/aeroeus/CMakeLists.txt
index 38590b4cd4..8f3d7c7127 100644
--- a/jsk_aero_robot/aeroeus/CMakeLists.txt
+++ b/jsk_aero_robot/aeroeus/CMakeLists.txt
@@ -8,4 +8,5 @@ catkin_package(
## rosrun euscollada collada2eus_urdfmodel $(rospack find aero_description)/robots/aero.urdf $(rospack find aero_description)/robots/aero.yaml aero.l
-install(FILES aero-interface.l aero-hand.l aero-wheels.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt b/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt
index 94a9bdf257..9e060e3e0e 100644
--- a/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt
+++ b/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
find_package(catkin REQUIRED)
catkin_package()
-install(DIRECTORY launch raw_maps scripts
+install(DIRECTORY launch raw_maps scripts config test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_baxter_robot/baxtereus/CMakeLists.txt b/jsk_baxter_robot/baxtereus/CMakeLists.txt
index d75a0e0bc8..5a4e1fe79a 100644
--- a/jsk_baxter_robot/baxtereus/CMakeLists.txt
+++ b/jsk_baxter_robot/baxtereus/CMakeLists.txt
@@ -65,7 +65,8 @@ install(DIRECTORY euslisp test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES baxter.l baxter-interface.l baxter-util.l baxter.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} baxter.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)
diff --git a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt
index 51ff998466..f4eb3b6789 100644
--- a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt
+++ b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt
@@ -17,9 +17,10 @@ include_directories(
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 jsk_baxter_tools
+install(DIRECTORY config jsk_baxter_joy jsk_baxter_lifelog jsk_baxter_moveit jsk_baxter_sensors jsk_baxter_tools jsk_baxter_description jsk_baxter_machine jsk_baxter_udev
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES baxter.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+file(GLOB LAUNCH_FILES *.launch)
+install(FILES ${LAUNCH_FILES} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt
index 9c7072a085..a61eefcd87 100644
--- a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt
+++ b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt
@@ -29,6 +29,6 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
-install(DIRECTORY launch
+install(DIRECTORY launch www
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_denso_robot/cobottaeus/CMakeLists.txt b/jsk_denso_robot/cobottaeus/CMakeLists.txt
index e59ae6d705..ac455f9fb2 100644
--- a/jsk_denso_robot/cobottaeus/CMakeLists.txt
+++ b/jsk_denso_robot/cobottaeus/CMakeLists.txt
@@ -43,11 +43,12 @@ add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/cobotta.l
add_custom_target(compile_cobotta ALL DEPENDS ${PROJECT_SOURCE_DIR}/cobotta.l)
-install(DIRECTORY euslisp test
+install(DIRECTORY sample test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES cobotta.l cobotta-interface.l cobotta-util.l cobotta.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} cobotta.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)
diff --git a/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt b/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt
index 50ceb95ec7..805e919076 100644
--- a/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt
+++ b/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt
@@ -33,12 +33,12 @@ catkin_package()
#############
## Install ##
#############
-install(DIRECTORY config launch scripts data
+install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES jsk_cobotta.machine
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+# install(FILES jsk_cobotta.machine
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
#############
## Testing ##
diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt
index f8ab04eead..319eb6314f 100644
--- a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt
+++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt
@@ -4,7 +4,7 @@ project(jsk_fetch_gazebo_demo)
find_package(catkin)
catkin_package()
-install(DIRECTORY euslisp launch config
+install(DIRECTORY euslisp launch config test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS
)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt
index be9259dac6..2e813fb0f8 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt
+++ b/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt
@@ -23,7 +23,15 @@ catkin_add_env_hooks(99.jsk_fetch_startup SHELLS bash zsh sh
#############
## Install ##
#############
-install(DIRECTORY config launch scripts data
+file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts/)
+ endif()
+endforeach()
+install(DIRECTORY config launch data apps euslisp robots supervisor_scripts tmuxinator_yaml udev_rules upstart_scripts
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt
index c0e3d84fbd..c50bda2127 100644
--- a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt
+++ b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt
@@ -14,7 +14,7 @@ catkin_package()
#############
## Install ##
#############
-install(DIRECTORY config launch
+install(DIRECTORY config launch scripts
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt
index b6f51a120b..2ef0ba696f 100644
--- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt
+++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt
@@ -67,7 +67,7 @@ install(DIRECTORY test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES kinova.l kinova-interface.l kinova-util.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(FILES kinova.l kinova-interface.l kinova-util.l gen3_robotiq_2f_85.yaml gen3_robotiq_2f_140.yaml gen3_lite_gen3_lite_2f.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)
diff --git a/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt b/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt
index 33876c3948..cbf50ad329 100644
--- a/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt
+++ b/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt
@@ -14,7 +14,7 @@ catkin_package()
#############
## Install ##
#############
-install(DIRECTORY launch
+install(DIRECTORY launch data
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt b/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt
index 4a945f7df0..a708f3dbb8 100644
--- a/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt
+++ b/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS)
## 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()
+catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
@@ -130,6 +130,22 @@ include_directories(
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
+file(GLOB_RECURSE SCRIPT_PROGRAMS nodes/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes)
+ endif()
+endforeach()
+file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts)
+ endif()
+endforeach()
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
@@ -137,6 +153,9 @@ include_directories(
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
diff --git a/jsk_naoqi_robot/jsk_nao_startup/setup.py b/jsk_naoqi_robot/jsk_nao_startup/setup.py
new file mode 100644
index 0000000000..5a423aaf02
--- /dev/null
+++ b/jsk_naoqi_robot/jsk_nao_startup/setup.py
@@ -0,0 +1,9 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup()
+
+setup(**setup_args)
diff --git a/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt b/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt
index 4b9e09ae9f..e25a9339f1 100644
--- a/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt
+++ b/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt
@@ -3,11 +3,21 @@ project(jsk_pepper_startup)
find_package(catkin REQUIRED COMPONENTS)
+catkin_python_setup()
+
catkin_package()
-install(DIRECTORY launch nodes sample
+install(DIRECTORY launch nodes sample apps config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
+file(GLOB_RECURSE SCRIPT_PROGRAMS script/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/script)
+ endif()
+endforeach()
if(CATKIN_ENABLE_TESTING AND NOT $ENV{ROS_DISTRO} STREQUAL "hydro")
find_package(catkin REQUIRED COMPONENTS rostest roslaunch)
diff --git a/jsk_naoqi_robot/jsk_pepper_startup/setup.py b/jsk_naoqi_robot/jsk_pepper_startup/setup.py
new file mode 100644
index 0000000000..5a423aaf02
--- /dev/null
+++ b/jsk_naoqi_robot/jsk_pepper_startup/setup.py
@@ -0,0 +1,9 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup()
+
+setup(**setup_args)
diff --git a/jsk_naoqi_robot/naoeus/CMakeLists.txt b/jsk_naoqi_robot/naoeus/CMakeLists.txt
index 9457324d22..5a1e382a0c 100644
--- a/jsk_naoqi_robot/naoeus/CMakeLists.txt
+++ b/jsk_naoqi_robot/naoeus/CMakeLists.txt
@@ -19,3 +19,13 @@ compile_naoqi_model(nao naoV50_generated_urdf)
if(nao_meshes_FOUND)
add_rostest(test/naoeus.test)
endif()
+
+###
+### install
+###
+install(DIRECTORY patch test launch sample webots euslisp
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} nao.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
diff --git a/jsk_naoqi_robot/peppereus/CMakeLists.txt b/jsk_naoqi_robot/peppereus/CMakeLists.txt
index bf179714e7..1889e522e5 100644
--- a/jsk_naoqi_robot/peppereus/CMakeLists.txt
+++ b/jsk_naoqi_robot/peppereus/CMakeLists.txt
@@ -20,4 +20,11 @@ if(pepper_meshes_FOUND AND CATKIN_ENABLE_TESTING)
add_rostest(test/peppereus.test)
endif()
-
+###
+### install
+###
+install(DIRECTORY doc test
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} pepper.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt b/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt
index 73fdf60e27..bc76135f60 100644
--- a/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt
+++ b/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt
@@ -4,3 +4,7 @@ project(jsk_pr2_calibration)
find_package(catkin REQUIRED COMPONENTS)
catkin_package()
+
+install(DIRECTORY scripts
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
diff --git a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt
index b1981968a9..77c3d71a12 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt
+++ b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt
@@ -9,7 +9,9 @@ catkin_add_env_hooks(99.jsk_pr2_startup SHELLS bash zsh
install(DIRECTORY config jsk_pr2_image_transport
jsk_pr2_joy jsk_pr2_lifelog jsk_pr2_move_base jsk_pr2_moveit
+ jsk_pr2_perception jsk_pr2_rocon jsk_pr2_teleop
jsk_pr2_sensors jsk_pr2_warning src sample
+ apps
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
file(GLOB _LAUNCH_FILES ${PROJECT_SOURCE_DIR}/ *.launch) # add / to avoid to get current directory
@@ -18,5 +20,5 @@ foreach(_LAUNCH_FILE ${_LAUNCH_FILES})
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
endforeach()
-install(FILES install_pr1040_description.sh jsk_pr2.machine plugin.xml startup.app
+install(FILES iai_kitchen.rosinstall jsk_pr2.rosinstall install_pr1040_description.sh jsk_pr2.machine plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt
index 048aa8ed00..43769001c5 100644
--- a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt
+++ b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt
@@ -28,7 +28,12 @@ add_executable(pr2_base_trajectory_action
src/pr2_base_trajectory_action_controller_node.cpp)
target_link_libraries(pr2_base_trajectory_action ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(DIRECTORY config include launch
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+)
+
+install(DIRECTORY config launch test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(TARGETS pr2_base_trajectory_action
diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
index bf9d8a55cb..a38a661a86 100644
--- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
+++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
@@ -138,18 +138,35 @@ if($ENV{ROS_DISTRO} STRGREATER "indigo") # quadruped_joystick_teleop.cpp uses c+
)
endif()
-install(DIRECTORY lifelog util launch images config cfg
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h")
+
+install(DIRECTORY util launch images config cfg euslisp apps
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
+
+install(FILES lifelog_nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
file(GLOB_RECURSE SCRIPT_PROGRAMS lifelog/*)
foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
else()
- install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/lifelog)
+ endif()
+endforeach()
+file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts)
endif()
endforeach()
+
if(mongodb_store_FOUND)
install(TARGETS jsk_robot_lifelog
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb_replication_params.yaml b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb_replication_params.yaml
index 966a90f87d..2a8b99f9ea 100644
--- a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb_replication_params.yaml
+++ b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb_replication_params.yaml
@@ -1,4 +1,4 @@
-mongodb_store_extras: [["robot-database.jsk.imi.i.u-tokyo.ac.jp", 27017],["musca.jsk.imi.i.u-tokyo.ac.jp",27017]]
+mongodb_store_extras: [["robot-database.jsk.imi.i.u-tokyo.ac.jp", 27017]]
replication_client:
interval: 3600
delete_after_move: true
diff --git a/jsk_robot_common/jsk_robot_startup/package.xml b/jsk_robot_common/jsk_robot_startup/package.xml
index 8b1e644b85..c008fbdf44 100644
--- a/jsk_robot_common/jsk_robot_startup/package.xml
+++ b/jsk_robot_common/jsk_robot_startup/package.xml
@@ -19,6 +19,7 @@
sensor_msgs
dynamic_reconfigure
urdf
+
actionlib_msgs
app_manager
dynamic_reconfigure
diff --git a/jsk_robot_common/jsk_robot_startup/scripts/head_teleop_joy_completion.py b/jsk_robot_common/jsk_robot_startup/scripts/head_teleop_joy_completion.py
index cc323c7ee1..a1637a517a 100755
--- a/jsk_robot_common/jsk_robot_startup/scripts/head_teleop_joy_completion.py
+++ b/jsk_robot_common/jsk_robot_startup/scripts/head_teleop_joy_completion.py
@@ -58,9 +58,9 @@ def callback(self,msg):
rospy.loginfo("raw: x={:5.2f}, y={:5.2f}, v={:5.2f}, r={:5.2f} -> cmd: x={:5.2f}, y={:5.2f}, r={:5.2f}".format(x, y, v, r, xx, yy, rr))
msg.axes = list(msg.axes) # tuple to list
- msg.axes[self.axis_linear_x] = xx
- msg.axes[self.axis_linear_y] = yy
- msg.axes[self.axis_angular] = rr
+ msg.axes[self.axis_linear_x] = max(min(xx, self.axis_linear_x_max), self.axis_linear_x_min)
+ msg.axes[self.axis_linear_y] = max(min(yy, self.axis_linear_y_max), self.axis_linear_y_min)
+ msg.axes[self.axis_angular] = max(min(rr, self.axis_angular_max), self.axis_angular_min)
self.pub.publish(msg)
@@ -74,6 +74,13 @@ def spin(self):
if rospy.Time.now() - self.last_publish_time > rospy.Duration(3.0):
self.pub.publish(msg)
+ def get_param(self, key1, key2, value):
+ p = rospy.get_param(key1)
+ if type(p) == dict and p.has_key(key2):
+ return p[key2]
+ else:
+ return value
+
def __init__(self):
rospy.init_node('joy_topic_completion')
self.pass_through = rospy.get_param('~pass_through', True)
@@ -81,6 +88,12 @@ def __init__(self):
self.axis_linear_x = int(rospy.get_param('~axis_linear', {'x': 1})['x'])
self.axis_linear_y = int(rospy.get_param('~axis_linear', {'y': 2})['y'])
self.axis_angular = int(rospy.get_param('~axis_angular', {'yaw': 0})['yaw'])
+ self.axis_linear_x_min = float(self.get_param('~axis_linear', 'x_min', -1))
+ self.axis_linear_x_max = float(self.get_param('~axis_linear', 'x_max', 1))
+ self.axis_linear_y_min = float(self.get_param('~axis_linear', 'y_min', -1))
+ self.axis_linear_y_max = float(self.get_param('~axis_linear', 'y_max', 1))
+ self.axis_angular_min = float(self.get_param('~axis_angular', 'yaw_min', -0.5))
+ self.axis_angular_max = float(self.get_param('~axis_angular', 'yaw_max', 0.5))
self.last_publish_time = rospy.Time.now()
diff --git a/jsk_robot_common/jsk_robot_startup/src/quadruped_joystick_teleop.cpp b/jsk_robot_common/jsk_robot_startup/src/quadruped_joystick_teleop.cpp
index 9a35c227a7..d0077eec62 100644
--- a/jsk_robot_common/jsk_robot_startup/src/quadruped_joystick_teleop.cpp
+++ b/jsk_robot_common/jsk_robot_startup/src/quadruped_joystick_teleop.cpp
@@ -67,7 +67,7 @@ class TeleopManager
sound_play::SoundRequest msg;
msg.sound = sound_play::SoundRequest::SAY;
msg.command = sound_play::SoundRequest::PLAY_ONCE;
- msg.volume = 1.0;
+ msg.volume = 0.3;
msg.arg = message;
pub_sound_play_.publish(msg);
}
@@ -92,7 +92,7 @@ class TeleopManager
if ( client_estop_hard_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_estop_hard_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_estop_hard_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_estop_hard_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_estop_hard_] = true;
}
@@ -115,7 +115,7 @@ class TeleopManager
if ( client_estop_gentle_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_estop_gentle_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_estop_gentle_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_estop_gentle_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_estop_gentle_] = true;
}
@@ -138,7 +138,7 @@ class TeleopManager
if ( client_power_off_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_power_off_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_power_off_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_power_off_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_power_off_] = true;
}
@@ -161,7 +161,7 @@ class TeleopManager
if ( client_power_on_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_power_on_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_power_on_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_power_on_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_power_on_] = true;
}
@@ -184,7 +184,7 @@ class TeleopManager
if ( client_self_right_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_self_right_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_self_right_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_self_right_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_self_right_] = true;
}
@@ -207,7 +207,7 @@ class TeleopManager
if ( client_sit_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_sit_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_sit_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_sit_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_sit_] = true;
}
@@ -230,7 +230,7 @@ class TeleopManager
if ( client_stand_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_stand_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_stand_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_stand_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_stand_] = true;
}
@@ -253,7 +253,7 @@ class TeleopManager
if ( client_stop_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_stop_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_stop_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_stop_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_stop_] = true;
}
@@ -276,7 +276,7 @@ class TeleopManager
if ( client_release_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_release_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_release_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_release_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_release_] = true;
}
@@ -299,7 +299,7 @@ class TeleopManager
if ( client_claim_.call(srv) && srv.response.success ) {
ROS_INFO_STREAM("Service " << client_claim_.getService() << " succeeded.");
} else {
- ROS_ERROR_STREAM("Service " << client_claim_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_claim_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_claim_] = true;
}
@@ -328,7 +328,7 @@ class TeleopManager
ROS_INFO_STREAM("Service " << client_stair_mode_.getService() << " succeeded. set to " << req_next_stair_mode_.data);
req_next_stair_mode_.data = not req_next_stair_mode_.data;
} else {
- ROS_ERROR_STREAM("Service " << client_stair_mode_.getService() << " failed.");
+ ROS_ERROR_STREAM("Service " << client_stair_mode_.getService() << " failed. (" << srv.response.message << ")");
}
pressed_[button_stair_mode_] = true;
}
@@ -350,7 +350,7 @@ class TeleopManager
if (client_dock_.call(srv) && srv.response.success ){
ROS_INFO_STREAM("Service '" << client_dock_.getService() << "' succeeded.");
} else {
- ROS_ERROR_STREAM("Service '" << client_dock_.getService() << "' failed.");
+ ROS_ERROR_STREAM("Service '" << client_dock_.getService() << "' failed. (" << srv.response.message << ")");
}
pressed_axes_[axe_dock_] = true;
}
@@ -358,9 +358,9 @@ class TeleopManager
if (not pressed_axes_[axe_dock_]){
this->say("undock calling");
if (client_undock_.call(srv) && srv.response.success ){
- ROS_INFO("Service 'undock' succeeded.");
+ ROS_INFO_STREAM("Service '" << client_undock_.getService() << "' succeeded.");
} else {
- ROS_ERROR("Service 'undock' failed.");
+ ROS_ERROR_STREAM("Service '" << client_undock_.getService() << "' failed. (" << srv.response.message << ")");
}
pressed_axes_[axe_dock_] = true;
}
@@ -384,7 +384,7 @@ class TeleopManager
if (client_tuck_.call(srv) && srv.response.success ){
ROS_INFO_STREAM("Service '" << client_tuck_.getService() << "' succeeded.");
} else {
- ROS_ERROR_STREAM("Service '" << client_tuck_.getService() << "' failed.");
+ ROS_ERROR_STREAM("Service '" << client_tuck_.getService() << "' failed. (" << srv.response.message << ")");
}
pressed_axes_[axe_tuck_] = true;
}
@@ -394,7 +394,7 @@ class TeleopManager
if (client_untuck_.call(srv) && srv.response.success ){
ROS_INFO_STREAM("Service '" << client_untuck_.getService() << "' succeeded.");
} else {
- ROS_ERROR_STREAM("Service '" << client_untuck_.getService() << "' failed.");
+ ROS_ERROR_STREAM("Service '" << client_untuck_.getService() << "' failed. (" << srv.response.message << ")");
}
pressed_axes_[axe_tuck_] = true;
}
diff --git a/jsk_robot_common/jsk_robot_utils/CMakeLists.txt b/jsk_robot_common/jsk_robot_utils/CMakeLists.txt
index 5c80baa63b..acb7e736bf 100644
--- a/jsk_robot_common/jsk_robot_utils/CMakeLists.txt
+++ b/jsk_robot_common/jsk_robot_utils/CMakeLists.txt
@@ -10,7 +10,15 @@ if(CATKIN_ENABLE_TESTING)
add_rostest(test/joint_state_compressor.test)
endif()
-install(DIRECTORY euslisp
+file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts)
+ endif()
+endforeach()
+install(DIRECTORY euslisp test
DESTINATION
${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS
diff --git a/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt b/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt
index 1e9db5e7f2..a29626d496 100644
--- a/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt
+++ b/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt
@@ -53,6 +53,9 @@ install(DIRECTORY include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h"
)
+install(DIRECTORY launch test
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
install(FILES speak_and_wait_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
diff --git a/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt b/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt
index c684447081..7e3eefc877 100644
--- a/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt
+++ b/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt
@@ -61,6 +61,9 @@ install(DIRECTORY include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h"
)
+install(DIRECTORY config launch tests worlds
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
install(FILES update_move_base_parameter_recovery_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md
new file mode 100644
index 0000000000..9f642f4988
--- /dev/null
+++ b/jsk_spot_robot/README.md
@@ -0,0 +1,122 @@
+jsk_spot_robot
+==============
+
+## Manuals
+
+- [Supported Documents of Boston Dynamics](https://www.bostondynamics.com/spot/training/documentation)
+- [Spot ROS User Documentation](http://www.clearpathrobotics.com/assets/guides/melodic/spot-ros/ros_usage.html#taking-control-of-the-robot)
+
+## Installation
+
+To setup a new internal PC and spot user, Please see [this page](./SetupInternalPCAndSpotUser.md).
+
+### How to set up a catkin workspace for a remote PC
+
+Create a workspace
+
+```bash
+source /opt/ros/melodic/setup.bash
+mkdir ~/spot_ws/src -p
+cd ~/spot_ws/src
+wstool init .
+wstool set jsk-ros-pkg/jsk_robot https://github.com/k-okada/jsk_robot.git --git -v spot_arm
+wstool update
+wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_user.rosinstall
+wstool update
+rosdep update
+rosdep install -y -r --from-paths . --ignore-src
+cd $HOME/spot_ws
+catkin init
+catkin build -j4 jsk_spot_startup spoteus
+```
+
+## How to run
+
+### Bringup spot
+
+First, please turn on spot and turn on motors according to [the OPERATION section of spot user guide](https://www.bostondynamics.com/sites/default/files/inline-files/spot-user-guide.pdf) and power on the internal PC.
+
+Basically, ros systemd services will start automatically. So you can use spot now.
+
+
+#### superviosur
+
+URI: http://:9001
+
+#### rwt_app_chooser
+
+URI: http://:8000/rwt_app_chooser
+
+### Development with a remote PC
+
+Please create `spot_ws` to your PC.
+
+First, connect your development PC's wifi adapter to Access point of the robot.
+
+And for every terminals in this section, Set your ROS_IP and ROS_MASTER_URI and source spot_ws.
+
+```
+rossetmaster
+rossetip
+source ~/spot_ws/devel/setup.bash
+```
+
+##### spoteus
+
+spoteus is roseus client for ROS Interface of spot_ros.
+
+Open a terminal, setting up it for spot and run roseus repl. ( emacs shell or euslime are recommended. )
+
+Then initialize it for spot
+
+```
+(load "package://spoteus/spot-interface.l")
+(spot-init)
+```
+
+You can now control spot from roseus.
+for example, when you want to move spot 1m forward, type.
+
+```
+(send *ri* :undock) ;; undock robot
+(send *spot* :reset-pose) ;; change robot pose, use (objects (list *spot*)) to visuailze
+(send *ri* :angle-vector (send *spot* :angle-vector) 2000 :default-controller) ;; send to real robot
+(send *ri* :stow-arm) ;; contorl onbody-api
+(send *ri* :sit)
+```
+
+### How to set up a catkin workspace in on-bodyPC
+
+First create user account into internal PC
+```
+ssh spotcore -l spot
+sudo adduser k-okada
+sudo adduser k-okada spot
+sudo adduser k-okada sudo
+```
+
+Create a workspace. Make sure that you need to login to spotcore with your account
+
+```bash
+ssh spotcore -l k-okada
+source /opt/ros/melodic/setup.bash
+source ~spot/spot_driver_ws/devel/setup.bash
+mkdir ~/spot_ws/src -p
+cd ~/spot_ws/src
+cd $HOME/spot_ws
+catkin init
+catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/include/python3.6m -DPYTHON_LIBRARY=/usr/lib/x86_64-linux-gnu/libpython3.6m.so
+catkin build -j4 jsk_spot_startup spoteus
+echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+echo "source ~/spot_ws/devel/setup.bash" >> ~/.bashrc
+```
+
+#### Run apps
+
+You can run apps manually, this is good for debugging your applications.
+```bash
+roscd jsk_spot_startup/apps/head_lead_demo roslaunch head_lead_demo.xml
+```
+
+#### Install apps
+If you would like to call your apps from rwt_app_chooser, you can
diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md
new file mode 100644
index 0000000000..230505513e
--- /dev/null
+++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md
@@ -0,0 +1,307 @@
+# How to setup a internal PC and spot user
+
+This page describes how to setup a internal pc and spot user.
+
+## How to setup a [CORE I/O](https://dev.bostondynamics.com/docs/payload/coreio_documentation)
+
+The CORE I/O is desgined to use filesystem is read only and user Docekr or Spot Extensions for user applications.
+
+
+### Setup core-io users
+
+login with default password (ex: ssh -p 20022 10.0.0.3 -el spot)
+
+```
+# better to reboot on every command ????
+core-io$ passwd ... # change password
+core-io$ sudo usermod -a -G docker spot # add spot to docker group
+```
+
+# Build Docker containers for Development environment
+
+Software development on core-is uses Docker containers and we provided two containers.
+
+1. spot_dev_env
+2. `_dev_env`.
+
+The `spot_dev_env` runs as `spot` user, which will provide basic functions like `roscore` and `jsk_spot_startup` and should be started at boot time.
+
+Users are expected to run `_dev_env` and create an overay workspace (https://wiki.ros.org/catkin/Tutorials/workspace_overlaying) on top of the workspace of `spot` user.
+
+## Build `spot_dev_env` (for admin)
+
+1. Login to spot as `spot` user or any ararch64 machine as user with uid `1000`. Using an aarch64 machine is recommended because `core-io` has less CPU power.
+2. Create a ROS workspace in `~/spot_dev_env`. Then change to the `jsk_spot_robot/coreio/base` directory.
+3. Run `make build`. This will build the docker image and compile all packages needed for `jsk_spot_startup`.
+
+
+If you have built `spot_dev_env` on a machine other than `core-io,
+
+1. Run `docker save spot_dev_env:latest -o spot_dev_env.tar` on your aarch64 machine.
+2. copy it to core-io, e.g. `scp spot_dev_env.tar spot@core-io:/tmp'.
+3. Run `docker load -i /tmp/spot_dev_env.tar' on `core-io` machine.
+4. As this docker image requires `~/spot_dev_env`. Please set up the ROS workspace under `~/spot_dev_env` as the same as your aarch64 machine.
+5. Run `make build` in the `jsk_spot_robot/coreio/base` directory.
+
+## Build `_dev_env`
+
+1. Create your ROS workspace on `core-io` as your own userid.
+2. go to `jsk_spot_robot/coreio/base` and run `make build`.
+
+# Run Docker container as your development environment
+
+## Run the startup program (for admin)
+
+1.Login to `core-io` as `spot` user, run `~/bash.sh` to start `spot_dev_env` container2. run `roslaunch jsk_spot_startup jsk_spot_bringup.launch credential_config:=$(rospack find jsk_spot_startup)/auth/spot_credential.yaml use_gps:=false`.
+
+Note:
+- This process should be run as upstart or supervisor.
+- Please do not start multiple `spot_dev_env` containers, it will restart multiple system services including bluetooth, so it will disconnect your joystick.
+
+## Run your custom development environment
+
+1. Login to `core-io` as your local user and run `~/bash.sh` to start your build environment.
+2. For the first time, you don't have an overlay package, so if you run `rospack list | grep $HOME`, you will get nothing. If you have a package to modify, run `catkin ` and `source devel/setup.bash`, you will have your package on your overlay workspace.
+
+
+# Tips
+
+### Setup development environment
+
+```
+core-io$ mkdir ~/spot_dev_env/src -p
+core-io$ cd ~/spot_dev_env/src
+core-io$ git clone https://github.com/k-okada/jsk_robot.git -b spot_arm
+core-io$ cd ~/spot_dev_env/src/jsk_robot/jsk_spot_robot/coreio/base
+core-io$ make build bash
+```
+This procedure create `~/bash.sh`. Please use this script when you start development.
+
+### Setup wifi interfaces
+
+Use [TP-Link AC600 wireless Realtek RTL8811AU Archer T2U Nano](https://www.amazon.co.jp/gp/product/B07MXHJ6KB/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&psc=1)
+```
+$ dmesg
+[ 7.305827] usb 1-2.1.1: new high-speed USB device number 5 using tegra-xusb
+[ 7.326404] usb 1-2.1.1: New USB device found, idVendor=2357, idProduct=011e
+[ 7.326553] usb 1-2.1.1: New USB device strings: Mfr=1, Product=2, SerialNumber=3
+[ 7.326681] usb 1-2.1.1: Product: 802.11ac WLAN Adapter
+[ 7.326781] usb 1-2.1.1: Manufacturer: Realtek
+$ lsusb
+Bus 001 Device 005: ID 2357:011e
+```
+
+Make sure that the roming configuration works correctly (from core-io)
+```
+core-io$ sudo wpa_cli get_network 0 bgscan
+Selected interface 'wlxac15a246e382'
+"simple:30:-80:86400"
+```
+This scan every 30 seconds, when signal is below -80, and 86400 seconds (24 hours) otherwise.
+
+### Setup Joystick
+
+[Push PS and (left-top small) create button to enter pairing mode](https://www.playstation.com/en-us/support/hardware/pair-dualsense-controller-bluetooth/#blue)
+```
+$ hcitool dev
+Devices:
+ hci0 00:1B:DC:0D:D0:AD
+$ hcitool -i hci0 scan
+Scanning ...
+ D0:BC:C1:CB:48:37 Wireless Controller
+$ sudo bluetoothctl
+[bluetooth]# pair D0:BC:C1:CB:48:37
+[bluetooth]# trust D0:BC:C1:CB:48:37
+[bluetooth]# connect D0:BC:C1:CB:48:37
+
+```
+
+
+## Settings for SPOT CORE (x86_64)
+
+## wifi settings [Wi-Fi roaming aggressiveness configuration](https://github.com/jsk-ros-pkg/jsk_robot/issues/1598#issuecomment-1247533330)
+
+```
+$ git clone https://github.com/cilynx/rtl88x2bu.git
+cd rtl88x2bu
+./deply.sh
+echo 88x2bu | sudo tee /etc/modules-load.d/88x2bu.conf # to startup on boot time
+echo 'install 88x2bu /sbin/modprobe -i 88x2bu && { /sbin/wpa_cli set_network 0 bgscan "\\"simple:5:-50:3000\\"";}' | sudo tee /etc/modprobe.d/88x2bu.conf # run set_network when load module
+
+sudo nmtui # to configure network
+```
+
+### Setup timezone
+
+```
+sudo timedatectl set-timezone Asia/Tokyo
+```
+
+
+## How to set up the spot user
+
+First, create your user account to internal PC.
+
+```
+ssh spot-BD-xxxxxxxx-p.jsk.imi.i.u-tokyo.ac.jp -l spot -p 20022
+sudo adduser k-okada
+sudo adduser k-okada spot
+
+```
+
+If you are the first user to use spotcore, add `spot` user to sudo group. If someone already using the spotcore, you can skip this section
+
+```
+sudo gpasswd -a spot sudo
+```
+
+To install systemd service, run following commands. Note that this script start launch file of user's workspace, so usually we expect to run from spot users.
+```
+rosrun robot_upstart install --provider supervisor --supervisor-priority 10 --roscore
+rosrun robot_upstart install --provider supervisor --supervisor-priority 300 --symlink --wait --job jsk_spot_startup jsk_spot_startup/launch/jsk_spot_bringup.launch credential_config:=$(rospack find jsk_spot_startup)/auth/spot_credential.yaml use_app_manager:=false
+rosrun robot_upstart install --provider supervisor --supervisor-priority 400 --symlink --wait --job app_manager jsk_robot_startup/lifelog/app_manager.launch use_applist:=false respawn:=false
+```
+
+To check output of roslaunch output, please try
+```
+sudo supervisorctl tail -f jsk_spot_startup stdout
+sudo supervisorctl tail -f jsk_spot_startup stderr
+```
+
+You can connect to the supervisor console from https://spotcore.jsk.imi.i.u-tokyo.ac.jp:9001/
+
+systemd services of JSK Spot system use workspaces in `spot` user.
+
+- `spot_driver_ws`: a workspace to run driver.launch. which requires python3 build version of geometry3
+
+### Prerequities
+
+Install necessary packages for workspace building
+
+```
+sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy python3-opencv python3-sip-dev python3-defusedxml
+sudo apt-get install ros-melodic-catkin ros-melodic-vision-msgs
+```
+
+
+### setting up `spot_driver_ws`
+
+Create a workspace for `spot_driver`.
+
+```bash
+source /opt/ros/melodic/setup.bash
+mkdir ~/spot_driver_ws/src -p
+cd ~/spot_driver_ws/src
+wstool init .
+wstool set jsk-ros-pkg/jsk_robot https://github.com/jsk-ros-pkg/jsk_robot.git --git
+wstool update
+wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_driver.rosinstall
+wstool update
+rosdep update
+rosdep install -y -r --from-paths . --ignore-src
+pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt
+cd ~/spot_driver_ws
+catkin init
+catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/include/python3.6m -DPYTHON_LIBRARY=/usr/lib/x86_64-linux-gnu/libpython3.6m.so
+catkin build -j4 tf2_ros cv_bridge jsk_spot_startup spoteus robot_upstart
+```
+
+After this, please modify the credential files for spot_driver.
+
+```bash
+roscd jsk_spot_startup
+# modify auth/credential_config.yaml
+git update-index --skip-worktree auth/spot_credential.yaml
+```
+
+Note that `rosdep install ...` did not install all dependencies, you need to install `python3-` modules manualy. or run `ROS_PYTHON_VERSION=3 rosdep install ...`. But this will install `python3-catkin-pkg` which remove `python2-catkin-pkg` package and all `ros-melodic-*` packages.
+
+Workaround is to install only jsk_robot direcotry. (But you still re-install `ros-melodic-jsk-tools`)
+```
+ROS_PYTHON_VERSION=3 rosdep install -r --from-paths ~/spot_driver_ws/src/jsk_robot --ignore-src
+```
+or create dummy package that did not conflict each other.
+
+```
+#!/bin/bash
+
+set -x
+set -e
+
+TMPDIR=/tmp/tmp-$$
+
+mkdir ${TMPDIR}
+cd ${TMPDIR}
+apt download python3-catkin-pkg
+dpkg-deb -R python3-catkin-pkg_0.5.2-100_all.deb ${TMPDIR}/src-python3-catkin-pkg
+rm -fr ${TMPDIR}/src-python3-catkin-pkg/usr/bin/
+sed -i /^Conflicts:/d ${TMPDIR}/src-python3-catkin-pkg/DEBIAN/control
+sed -i '/^Version:/ s/$/0/' ${TMPDIR}/src-python3-catkin-pkg/DEBIAN/control
+cat ${TMPDIR}/src-python3-catkin-pkg/DEBIAN/control
+# edit DEBIAN/postinst
+dpkg-deb -b ${TMPDIR}/src-python3-catkin-pkg python3-catkin-pkg-dummy.deb
+
+apt download python-catkin-pkg
+dpkg-deb -R python-catkin-pkg_0.5.2-100_all.deb ${TMPDIR}/src-python2-catkin-pkg
+rm -fr ${TMPDIR}/src-python2-catkin-pkg/usr/bin/
+sed -i 's/, python3-catkin-pkg//' ${TMPDIR}/src-python2-catkin-pkg/DEBIAN/control
+sed -i '/^Version:/ s/$/0/' ${TMPDIR}/src-python2-catkin-pkg/DEBIAN/control
+cat ${TMPDIR}/src-python2-catkin-pkg/DEBIAN/control
+# edit DEBIAN/postinst
+dpkg-deb -b ${TMPDIR}/src-python2-catkin-pkg python2-catkin-pkg-no-conflict.deb
+
+apt download python3-rosdep
+dpkg-deb -R python3-rosdep_0.22.1-1_all.deb ${TMPDIR}/src-python3-rosdep
+rm -fr ${TMPDIR}/src-python3-rosdep/usr/bin/
+sed -i /^Conflicts:/d ${TMPDIR}/src-python3-rosdep/DEBIAN/control
+sed -i '/^Version:/ s/$/0/' ${TMPDIR}/src-python3-rosdep/DEBIAN/control
+cat ${TMPDIR}/src-python3-rosdep/DEBIAN/control
+# edit DEBIAN/postinst
+dpkg-deb -b ${TMPDIR}/src-python3-rosdep python3-rosdep-dummy.deb
+
+apt download python-rosdep
+dpkg-deb -R python-rosdep_0.22.1-1_all.deb ${TMPDIR}/src-python-rosdep
+rm -fr ${TMPDIR}/src-python-rosdep/usr/bin/
+sed -i 's/, python3-rosdep//' ${TMPDIR}/src-python-rosdep/DEBIAN/control
+sed -i '/^Version:/ s/$/0/' ${TMPDIR}/src-python-rosdep/DEBIAN/control
+cat ${TMPDIR}/src-python-rosdep/DEBIAN/control
+# edit DEBIAN/postinst
+dpkg-deb -b ${TMPDIR}/src-python-rosdep python-rosdep-no-conflict.deb
+
+set +x
+echo "scp ${TMPDIR}/python2-catkin-pkg-no-conflict.deb to your environment"
+echo "scp ${TMPDIR}/python3-catkin-pkg-dummy.deb to your environment"
+echo "scp ${TMPDIR}/python-rosdep-no-conflict.deb to your environment"
+echo "scp ${TMPDIR}/python3-rosdep-dummy.deb to your environment"
+```
+
+## Troubleshootig
+
+### Joystick did not respond
+
+Check `Setup Joystick` section and reconnet bluetooth
+
+### Wifi did not connec
+
+Check [roaming aggressiveness configuration](https://github.com/jsk-ros-pkg/jsk_robot/issues/1598)
+```
+$ wpa_cli get_network 0 bgscan
+```
+
+This tells roaming configuation, for example below setting means it scan every 5 seconds when the signal is weak (below -50), and every 3600 seconds otherwise.
+```
+$ wpa_cli set_network 0 bgscan "\"simple:5:-50:3000\""
+```
+
+You can also check the output of wpa_supplicant.
+```
+$ journalctl -u wpa_supplicant -f
+```
+
+### rwt_app_chooser did not respond
+
+http://spotcore:8000/rwt_app_chooser/#!task/ did not show any apps, and following command did not returns any apps, make sure that you have run `rosdep update`.
+
+```
+$ rosservice call /SpotCORE/list_apps
+```
diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile
new file mode 100644
index 0000000000..be53b0674f
--- /dev/null
+++ b/jsk_spot_robot/coreio/base/Dockerfile
@@ -0,0 +1,121 @@
+#####
+## Setup development environment
+#####
+##### Use sha256 as of 2023/03/16
+FROM arm64v8/ros:melodic-robot@sha256:c0a0a8a25dc822ea48b40c45d74b267ddf4dbe22b8accc9fd66bb146c34eb974 AS base_build
+ENV DEBIAN_FRONTEND noninteractive
+
+# development tools
+RUN apt-get update && apt install -y -q python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-pip python3-empy python3-opencv python3-sip-dev python3-defusedxml cython3
+
+# network tools
+RUN apt-get update && apt install -y -q net-tools iputils-ping dnsutils traceroute curl
+
+# development tools
+RUN apt-get update && apt install -y -q python-catkin-tools ros-melodic-catkin-virtualenv
+
+# editors
+RUN apt-get update && apt install -y -q less emacs vim
+
+# bluetooth
+RUN apt-get update && apt install -y -q bluez bluez-tools
+
+# ROS packages
+RUN apt-get update && apt install -y -q ros-melodic-catkin ros-melodic-vision-msgs
+
+# setup user space
+RUN sed -i 's@%sudo\s*ALL=(ALL:ALL)\s*ALL@%sudo ALL=(ALL) NOPASSWD: ALL@' /etc/sudoers
+RUN cat /etc/sudoers
+RUN useradd -ms /bin/bash spot
+RUN echo spot:spot | chpasswd
+RUN adduser spot sudo
+USER spot
+WORKDIR /home/spot
+RUN rosdep update -y --include-eol-distros
+
+# get dummy workspace to install dpeendent packages
+# setup catkin develeopment environment with mounted workspace
+RUN mkdir -p /tmp/ws
+COPY package.tar /tmp/
+RUN tar -C /tmp/ws/ -xvf /tmp/package.tar
+
+# install pip3 dependencies
+RUN pip3 install pip==21.3.1
+RUN pip3 install wrapt==1.12.1 six==1.15.0 PyJWT==2.0.0 numpy==1.19.4 grpcio==1.34.0
+
+# install python requirements
+RUN pip3 install -r /tmp/ws/src/jsk_robot/jsk_spot_robot/requirements.txt
+
+# # rosdep install
+# RUN sudo apt-get update && rosdep install -y -r --from-paths /tmp/ws/src --ignore-src --rosdistro melodic || echo "OK"
+
+#####
+## Initialize catkin workspace after wstool merge and update with jsk_spot_robot/jsk_spot_driver.rosinstall
+#####
+FROM base_build AS pre_build
+
+# setup catkin develeopment environment with mounted workspace
+COPY package.tar /tmp/
+RUN tar -C /tmp/ws/ -xvf /tmp/package.tar
+RUN sudo apt-get update && rosdep install -y -r --from-paths /tmp/ --ignore-src --rosdistro melodic || echo "OK"
+# rosdep install for python3
+RUN sudo apt-get update && ROS_PYTHON_VERSION=3 rosdep install -y -r --skip-keys="python3-catkin-pkg python3-catkin-tools python3-rosinstall python3-rosdep" --from-paths /tmp/ --ignore-src --rosdistro melodic || echo "OK"
+
+##
+## 'RUN --mount' called everytime when conttnes of spot/ws changed, so use 'COPY'
+#RUN --mount=type=bind,source=/,target=/home/spot/ws cd ws && rosdep install -y -r --from-paths src --ignore-src --rosdistro melodic || echo "OK"
+
+
+#####
+## User preferences for developent tools
+#####
+FROM pre_build AS dev_build
+
+RUN echo "#!/bin/bash" > /home/spot/ros_entrypoint.sh && \
+ echo "set -e" >> /home/spot/ros_entrypoint.sh && \
+ echo "source /opt/ros/melodic/setup.bash --" >> /home/spot/ros_entrypoint.sh && \
+ echo "[ -e /home/spot/ws/install/setup.bash ] && source /home/spot/ws/install/setup.bash" >> /home/spot/ros_entrypoint.sh && \
+ echo "sudo service dbus start" >> /home/spot/ros_entrypoint.sh && \
+ echo "sudo bluetoothd &" >> /home/spot/ros_entrypoint.sh && \
+ echo "exec \"\$@\"" >> /home/spot/ros_entrypoint.sh
+
+RUN chmod 0777 /home/spot/ros_entrypoint.sh
+
+### FIXME ros_entrypoint did not work with roscd
+RUN echo "source /opt/ros/melodic/share/rosbash/rosbash" >> ~/.bashrc
+
+ENV TZ=Asia/Tokyo
+
+ENTRYPOINT ["/home/spot/ros_entrypoint.sh"]
+CMD ["bash"]
+
+FROM dev_build AS user_build
+
+ARG USER
+ENV USER ${USER:-spot}
+ARG UID
+ENV UID ${UID:-1000}
+ARG GID
+ENV GID ${GID:-1000}
+RUN sudo addgroup --gid ${GID} ${USER}
+RUN sudo adduser --disabled-password --gecos '' --uid ${UID} --gid ${GID} ${USER}
+RUN sudo adduser ${USER} sudo
+USER ${USER}
+
+RUN echo "#!/bin/bash" > /home/${USER}/ros_entrypoint.sh && \
+ echo "set -e" >> /home/${USER}/ros_entrypoint.sh && \
+ echo "source /opt/ros/melodic/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \
+ echo "source /home/spot/ws/install/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \
+ echo "[ -e /home/${USER}/ws/devel/setup.bash ] && source /home/${USER}/ws/devel/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \
+ echo "exec \"\$@\"" >> /home/${USER}/ros_entrypoint.sh
+
+RUN chmod 0777 /home/${USER}/ros_entrypoint.sh
+
+RUN echo "source /opt/ros/melodic/share/rosbash/rosbash" >> ~/.bashrc
+
+## add your favorite package
+RUN sudo apt install -y clisp
+
+## FIXME @@HOME@@ is replaced by create_src_tree_tar in Makefile
+ENTRYPOINT ["@@HOME@@/ros_entrypoint.sh"]
+CMD ["bash"]
diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile
new file mode 100644
index 0000000000..fe107c6e32
--- /dev/null
+++ b/jsk_spot_robot/coreio/base/Makefile
@@ -0,0 +1,128 @@
+HOME=$(shell realpath ~/)
+
+all: $(HOME)/bash.sh
+ @echo "make build"
+
+$(HOME)/bash.sh:
+ echo "#!/bin/bash\n\ncd $(CURDIR)\nmake shell\n" > $(HOME)/bash.sh
+ chmod u+x $(HOME)/bash.sh
+
+
+WS_ROOT=$(abspath $(CURDIR)/../../../../../)
+
+HOSTNAME=$(shell hostname)
+IPADDRESS=$(shell dig +short $(HOSTNAME).jsk.imi.i.u-tokyo.ac.jp)
+IPADDRESS=$(shell hostname -I | sed 's/ /\n/g' | awk '/133.11/')
+
+ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting
+DOCKER_USER=spot
+DOCKER_TARGET_NAME=spot_dev_env
+$(info "This is dev setting, create $(DOCKER_TARGET_NAME)")
+ifneq ($(WS_ROOT),$(HOME)/spot_dev_env)
+$(info "We assume WS_ROOT to $(HOME)/spot_dev_env but current WS_ROOT is '$(WS_ROOT)'. Please setup as follows")
+$(info "$ mkdir -p ~/spot_dev_env/src")
+$(info "$ cd ~/spot_dev_env/src")
+$(info "$ git clone $(shell git ls-remote --get-url) -b $(shell git branch | grep ^* | sed s/\*\ // )")
+$(error "")
+endif
+else
+DOCKER_USER=$(USER)
+DOCKER_TARGET_NAME=$(USER)_dev_env
+$(info "This is user setting, create $(DOCKER_TARGET_NAME)")
+endif
+
+define run_uid_not_equal
+ifeq ($(shell id -u $$USER),${1})
+$$(info "${2}")
+$$(error "Target '$@' need to run except uid ${1} ($(shell id -un ${1})), your current uid is $(shell id -u $$USER) ($(shell echo $$USER))")
+endif
+endef
+define run_uid_equal
+ifneq ($(shell id -u $$USER),${1})
+$$(info "${2}")
+$$(error "Target '$@' need to run with uid ${1} ($(shell id -un ${1})), your current uid is $(shell id -u $$USER) ($(shell echo $$USER))")
+endif
+endef
+
+define run
+docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo --device=/dev/input -v /home/$(shell id -un 1000)/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws -ti $(DOCKER_TARGET_NAME) ${1};
+endef
+
+define create_src_tree_tar
+$(info "Create source tree tar file")
+$(eval TMP_FILE:=$(shell mktemp))
+$(eval PKG_FILE:=$(WS_ROOT)/package.tar)
+cp Dockerfile $(WS_ROOT); sed -i 's+@@HOME@@+$(HOME)+' $(WS_ROOT)/Dockerfile; (cd $(WS_ROOT); find src -iname 'package.xml' -o -path '*jsk_spot_robot/requirements.txt' | tar cf $(TMP_FILE) -T -); chmod 644 $(TMP_FILE)
+if test "$$(md5sum $(TMP_FILE) | awk '{print $$1}')" = "$$(md5sum $(PKG_FILE) | awk '{print $$1}')" ; then echo "- We have latest $(PKG_FILE)"; else echo "- source tree is different from $(PKG_FILE), create new source tree tar"; cp $(TMP_FILE) $(PKG_FILE); tar -tf $(PKG_FILE); fi
+rm $(TMP_FILE)
+endef
+define delete_src_tree_tar
+rm $(WS_ROOT)/Dockerfile $(PKG_FILE);
+endef
+
+# setup workspace ( clone source code from jsk_spot_draiver.rosinstall, rosdep install, wstool )
+base_build:
+ $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000))
+ # make .ros file
+ mkdir -p ~/.ros
+ # create tar file to pass package.xml under jsk_spot_robot, so that pip3 install requirements.txt
+ $(call create_src_tree_tar)
+ # build base file
+ cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target base_build --progress=plain --network=host -t spot_dev_env -f Dockerfile .
+ # run rosdep update
+ $(call run, bash -c '[ -e /etc/ros/rosdep/sources.list.d/ ] || rosdep update -y --include-eol-distros')
+ # create workspace from jsk_spot_driver.rosinstall
+ if [ ! -e $(WS_ROOT)/src/.rosinstall ]; then $(call run, wstool init src) fi
+ $(call run, wstool merge -t src src/jsk_robot/jsk_spot_robot/jsk_spot_driver.rosinstall)
+ $(call run, wstool update -t src)
+ # delete package.tar
+ $(call delete_src_tree_tar)
+
+# run rosdep install
+pre_build: base_build
+ $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000))
+ # create tarfile to pass package.xm under $($WS_ROOT), so that rosdep install finds necessary dependency list
+ $(call create_src_tree_tar)
+ # rosdep install
+ cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target pre_build --progress=plain --network=host -t spot_dev_env -f Dockerfile .
+ # delete package.tar
+ $(call delete_src_tree_tar)
+
+# install dev tools, ros_entrypoint.sh
+dev_build: pre_build
+ $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000))
+ # dev_build does not use package.xml
+ $(call create_src_tree_tar)
+ cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target dev_build --progress=plain --network=host -t spot_dev_env -f Dockerfile .
+ # delete package.tar
+ $(call delete_src_tree_tar)
+
+# run user build
+user_build:
+ $(eval $(call run_uid_not_equal,1000,This target requires user setting, except uid 1000))
+ $(call create_src_tree_tar)
+ cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target user_build --progress=plain --network=host -t $(shell echo $$USER)_dev_env --build-arg USER=$$USER --build-arg UID=$(shell id -u $$USER) --build-arg GID=$(shell id -g $$USER) -f Dockerfile .
+
+# run catkin build
+catkin_config:
+ $(call run, catkin init)
+ $(call run, catkin config $(CATKIN_CONFIG) --cmake-args -DCMAKE_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/include/python3.6m -DPYTHON_LIBRARY=/usr/lib/aarch64-linux-gnu/libpython3.6m.so.1)
+
+catkin_build:
+ $(call run, catkin build)
+
+# run all build
+build:
+ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting
+ make dev_build
+ make catkin_config CATKIN_CONFIG='--merge-install --blacklist test_tf2'
+ make catkin_build
+else
+ make user_build
+ make catkin_config
+endif
+shell:
+ $(call run, bash)
+
+emacs:
+ $(call run, emacs -nw)
diff --git a/jsk_spot_robot/coreio/network_compute_server/0_setup.sh b/jsk_spot_robot/coreio/network_compute_server/0_setup.sh
new file mode 100755
index 0000000000..4b555ad992
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/0_setup.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+## see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1
+
+. ~/my_spot_env/bin/activate
diff --git a/jsk_spot_robot/coreio/network_compute_server/1_capture.sh b/jsk_spot_robot/coreio/network_compute_server/1_capture.sh
new file mode 100755
index 0000000000..e96128ac60
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/1_capture.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+## see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1
+
+set -x
+rm -fr dogtoy/images/*
+
+trap "exit" INT TERM ERR
+trap "kill 0" EXIT
+
+python capture_images.py 10.0.0.3 --image-source frontleft_fisheye_image --folder dogtoy/images &
+python capture_images.py 10.0.0.3 --image-source frontright_fisheye_image --folder dogtoy/images &
+python capture_images.py 10.0.0.3 --image-source left_fisheye_image --folder dogtoy/images &
+python capture_images.py 10.0.0.3 --image-source right_fisheye_image --folder dogtoy/images &
+python capture_images.py 10.0.0.3 --image-source back_fisheye_image --folder dogtoy/images &
+
+wait
diff --git a/jsk_spot_robot/coreio/network_compute_server/2_split.sh b/jsk_spot_robot/coreio/network_compute_server/2_split.sh
new file mode 100755
index 0000000000..201830d8b1
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/2_split.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+mv dogtoy/images/*.xml dogtoy/annotations/
+python split_dataset.py --labels-dir dogtoy/annotations/ --output-dir dogtoy/annotations/ --ratio 0.9
diff --git a/jsk_spot_robot/coreio/network_compute_server/3_convert.sh b/jsk_spot_robot/coreio/network_compute_server/3_convert.sh
new file mode 100755
index 0000000000..155949ff77
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/3_convert.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+## see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1
+
+python generate_tfrecord.py --xml_dir dogtoy/annotations/train --image_dir dogtoy/images --labels_path dogtoy/annotations/label_map.pbtxt --output_path dogtoy/annotations/train.record
+python generate_tfrecord.py --xml_dir dogtoy/annotations/test --image_dir dogtoy/images --labels_path dogtoy/annotations/label_map.pbtxt --output_path dogtoy/annotations/test.record
+
diff --git a/jsk_spot_robot/coreio/network_compute_server/4_train.sh b/jsk_spot_robot/coreio/network_compute_server/4_train.sh
new file mode 100755
index 0000000000..ece266b850
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/4_train.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+## see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1
+
+python model_main_tf2.py --model_dir=dogtoy/models/dogtoy_ssd_resnet50_v1_fpn --pipeline_config_path=dogtoy/models/dogtoy_ssd_resnet50_v1_fpn/pipeline.config --num_train_steps=20000
+# CUDA_VISIBLE_DEVICES="-1" python model_main_tf2.py --model_dir=dogtoy/models/dogtoy_ssd_resnet50_v1_fpn --pipeline_config_path=dogtoy/models/dogtoy_ssd_resnet50_v1_fpn/pipeline.config --checkpoint_dir=dogtoy/models/dogtoy_ssd_resnet50_v1_fpn
+mkdir -p dogtoy/exported-models/dogtoy-model
+python exporter_main_v2.py --input_type image_tensor --pipeline_config_path dogtoy/models/dogtoy_ssd_resnet50_v1_fpn/pipeline.config --trained_checkpoint_dir dogtoy/models/dogtoy_ssd_resnet50_v1_fpn/ --output_directory dogtoy/exported-models/dogtoy-model
diff --git a/jsk_spot_robot/coreio/network_compute_server/5_run.sh b/jsk_spot_robot/coreio/network_compute_server/5_run.sh
new file mode 100755
index 0000000000..03b51fd747
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/5_run.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+## see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1
+
+python network_compute_server.py -m dogtoy/exported-models/dogtoy-model/saved_model dogtoy/annotations/label_map.pbtxt 10.0.0.3
+
diff --git a/jsk_spot_robot/coreio/network_compute_server/6_build_ext.sh b/jsk_spot_robot/coreio/network_compute_server/6_build_ext.sh
new file mode 100755
index 0000000000..ed260c2d1b
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/6_build_ext.sh
@@ -0,0 +1,31 @@
+#!/bin/bash -ex
+
+## https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch6
+## https://dev.bostondynamics.com/docs/python/fetch_tutorial/files/coreio_extension/create_extension.sh
+
+mkdir -p data
+rm -rf data/*
+# Dogtoy model
+cp -r ./dogtoy/exported-models/dogtoy-model data/.
+# and its label map
+cp ./dogtoy/annotations/label_map.pbtxt data/dogtoy-model/.
+# coco model (includes its label map)
+cp -r ./dogtoy/pre-trained-models/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8 data/.
+cp ./models-with-protos/research/object_detection/data/mscoco_label_map.pbtxt data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/
+
+# Build the image
+docker build -t object_detection_coco:l4t -f Dockerfile.l4t .
+
+# Exports the image, uses pigz
+docker save object_detection_coco:l4t | pigz > object_detection_coco_image.tar.gz
+
+# Built the Spot Extension by taring all the files together
+tar -cvzf object_detection_coco.spx \
+ object_detection_coco_image.tar.gz \
+ manifest.json \
+ docker-compose.yml \
+ data
+
+# Cleanup intermediate image
+rm object_detection_coco_image.tar.gz
+rm -fr data
diff --git a/jsk_spot_robot/coreio/network_compute_server/Dockerfile.l4t b/jsk_spot_robot/coreio/network_compute_server/Dockerfile.l4t
new file mode 100644
index 0000000000..f5ba4ae1bc
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/Dockerfile.l4t
@@ -0,0 +1,22 @@
+# Use a base image provided by nvidia that already contains tensorflow 2.5
+FROM nvcr.io/nvidia/l4t-tensorflow:r32.7.1-tf2.7-py3
+
+# Do some basic apt and pip updating
+RUN apt-get update && \
+ apt-get install -y --no-install-recommends python3-pip && \
+ apt-get clean
+
+# Copy over the python requirements file and our prebuilt models API library
+COPY docker-requirements.txt prebuilt/*.whl ./
+COPY models-with-protos models-with-protos
+
+# Install the python requirements
+RUN python3 -m pip install pip==21.3.1 setuptools==59.6.0 wheel==0.37.1 && \
+ python3 -m pip install -r docker-requirements.txt --find-links .
+
+# Copy over our main script
+COPY network_compute_server.py /app/
+WORKDIR /app
+
+# Set our script as the main entrypoint for the container
+ENTRYPOINT ["python3", "network_compute_server.py"]
diff --git a/jsk_spot_robot/coreio/network_compute_server/docker-compose.yml b/jsk_spot_robot/coreio/network_compute_server/docker-compose.yml
new file mode 100644
index 0000000000..72a9437ea1
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/docker-compose.yml
@@ -0,0 +1,26 @@
+version: "3.5"
+services:
+ object_detection_coco:
+ image: object_detection_coco:l4t
+ network_mode: host
+ restart: unless-stopped
+ environment:
+ # This package couldn't be installed, but putting it on the path allows required access to the protos.
+ - PYTHONPATH=/models-with-protos/research/
+ volumes:
+ # Mount payload credentials.
+ - /opt/payload_credentials/payload_guid_and_secret:/opt/payload_credentials/payload_guid_and_secret
+ # and ML models
+ - /data/.extensions/object_detection_coco/data:/data
+ # The command below is partial because the docker image is already configured with an entrypoint.
+ command: >
+ -m /data/dogtoy-model/saved_model /data/dogtoy-model/label_map.pbtxt
+ -m /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt
+ --payload-credentials-file /opt/payload_credentials/payload_guid_and_secret
+ 192.168.50.3
+ deploy:
+ resources:
+ reservations:
+ devices:
+ - driver: nvidia
+ capabilities: [gpu]
diff --git a/jsk_spot_robot/coreio/network_compute_server/docker-requirements.txt b/jsk_spot_robot/coreio/network_compute_server/docker-requirements.txt
new file mode 100644
index 0000000000..12c96e14fe
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/docker-requirements.txt
@@ -0,0 +1,40 @@
+bosdyn-api==3.2.0
+ # via
+ # bosdyn-client
+ # bosdyn-core
+bosdyn-client==3.2.0
+ # via -r requirements.txt
+bosdyn-core==3.2.0
+ # via bosdyn-client
+certifi==2022.5.18.1
+ # via requests
+charset-normalizer==2.0.12
+ # via requests
+deprecated==1.2.13
+ # via
+ # bosdyn-client
+ # bosdyn-core
+grpcio
+ #==1.46.3 (conflicts with base image)
+ # via bosdyn-client
+idna==3.3
+ # via requests
+numpy==1.19.4
+ # via bosdyn-client
+protobuf==3.19.4
+ # via bosdyn-api
+pyjwt==2.4.0
+ # via bosdyn-client
+requests==2.27.1
+ # via bosdyn-client
+six
+ #==1.16.0 (conflicts with base image)
+ # via grpcio
+urllib3==1.26.9
+ # via requests
+wrapt
+ #==1.14.1 (conflicts with base image)
+ # via deprecated
+Pillow==8.4.0
+opencv-python==4.6.0.66
+ # via network_compute_server.py
\ No newline at end of file
diff --git a/jsk_spot_robot/coreio/network_compute_server/manifest.json b/jsk_spot_robot/coreio/network_compute_server/manifest.json
new file mode 100644
index 0000000000..d762e573d0
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/manifest.json
@@ -0,0 +1,7 @@
+{
+ "description": "object_detection_coco",
+ "version": "3.2.0",
+ "images": [
+ "object_detection_coco_image.tar.gz"
+ ]
+}
diff --git a/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py b/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py
new file mode 100644
index 0000000000..d600d51577
--- /dev/null
+++ b/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py
@@ -0,0 +1,312 @@
+# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+import argparse
+import io
+import os
+import sys
+import time
+import logging
+
+import cv2
+from PIL import Image
+import numpy as np
+
+from bosdyn.api import network_compute_bridge_service_pb2_grpc
+from bosdyn.api import network_compute_bridge_pb2
+from bosdyn.api import image_pb2
+from bosdyn.api import header_pb2
+import bosdyn.client
+import bosdyn.client.util
+import grpc
+from concurrent import futures
+import tensorflow as tf
+
+import queue
+import threading
+from google.protobuf import wrappers_pb2
+from object_detection.utils import label_map_util
+
+kServiceAuthority = "object-detection-coco-worker.spot.robot"
+
+
+class TensorFlowObjectDetectionModel:
+ def __init__(self, model_path, label_path):
+ self.detect_fn = tf.saved_model.load(model_path)
+ self.category_index = label_map_util.create_category_index_from_labelmap(label_path, use_display_name=True)
+ self.name = os.path.basename(os.path.dirname(model_path))
+
+ def predict(self, image):
+ input_tensor = tf.convert_to_tensor(image)
+ input_tensor = input_tensor[tf.newaxis, ...]
+ detections = self.detect_fn(input_tensor)
+
+ return detections
+
+def process_thread(args, request_queue, response_queue):
+ # Load the model(s)
+ models = {}
+ for model in args.model:
+ this_model = TensorFlowObjectDetectionModel(model[0], model[1])
+ models[this_model.name] = this_model
+
+ print('')
+ print('Service ' + args.name + ' running on port: ' + str(args.port))
+
+ print('Loaded models:')
+ for model_name in models:
+ print(' ' + model_name)
+
+ while True:
+ request = request_queue.get()
+
+ if isinstance(request, network_compute_bridge_pb2.ListAvailableModelsRequest):
+ out_proto = network_compute_bridge_pb2.ListAvailableModelsResponse()
+ for model_name in models:
+ out_proto.available_models.append(model_name)
+ if models[model_name].category_index is not None:
+ labels_msg = out_proto.labels.add()
+ labels_msg.model_name = model_name
+ for n in models[model_name].category_index.keys():
+ labels_msg.available_labels.append(models[model_name].category_index[n]['name'])
+ response_queue.put(out_proto)
+ continue
+ else:
+ out_proto = network_compute_bridge_pb2.NetworkComputeResponse()
+
+ # Find the model
+ if request.input_data.model_name not in models:
+ err_str = 'Cannot find model "' + request.input_data.model_name + '" in loaded models.'
+ print(err_str)
+
+ # Set the error in the header.
+ out_proto.header.error.code = header_pb2.CommonError.CODE_INVALID_REQUEST
+ out_proto.header.error.message = err_str
+ response_queue.put(out_proto)
+ continue
+
+ model = models[request.input_data.model_name]
+
+ # Unpack the incoming image.
+ if request.input_data.image.format == image_pb2.Image.FORMAT_RAW:
+ pil_image = Image.open(io.BytesIO(request.input_data.image.data))
+ if request.input_data.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8:
+ # If the input image is grayscale, convert it to RGB.
+ image = cv2.cvtColor(pil_image, cv2.COLOR_GRAY2RGB)
+
+ elif request.input_data.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_RGB_U8:
+ # Already an RGB image.
+ image = pil_image
+
+ else:
+ print('Error: image input in unsupported pixel format: ', request.input_data.image.pixel_format)
+ response_queue.put(out_proto)
+ continue
+
+ elif request.input_data.image.format == image_pb2.Image.FORMAT_JPEG:
+ dtype = np.uint8
+ jpg = np.frombuffer(request.input_data.image.data, dtype=dtype)
+ image = cv2.imdecode(jpg, -1)
+
+ if len(image.shape) < 3:
+ # If the input image is grayscale, convert it to RGB.
+ image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB)
+
+ image_width = image.shape[0]
+ image_height = image.shape[1]
+
+ detections = model.predict(image)
+
+ num_objects = 0
+
+ # All outputs are batches of tensors.
+ # Convert to numpy arrays, and take index [0] to remove the batch dimension.
+ # We're only interested in the first num_detections.
+ num_detections = int(detections.pop('num_detections'))
+ detections = {key: value[0, :num_detections].numpy()
+ for key, value in detections.items()}
+
+ boxes = detections['detection_boxes']
+ classes = detections['detection_classes']
+ scores = detections['detection_scores']
+
+
+ for i in range(boxes.shape[0]):
+ if scores[i] < request.input_data.min_confidence:
+ continue
+
+ box = tuple(boxes[i].tolist())
+
+ # Boxes come in with normalized coordinates. Convert to pixel values.
+ box = [box[0] * image_width, box[1] * image_height, box[2] * image_width, box[3] * image_height]
+
+ score = scores[i]
+
+ if classes[i] in model.category_index.keys():
+ label = model.category_index[classes[i]]['name']
+ else:
+ label = 'N/A'
+
+ num_objects += 1
+
+ print('Found object with label: "' + label + '" and score: ' + str(score))
+
+
+ point1 = np.array([box[1], box[0]])
+ point2 = np.array([box[3], box[0]])
+ point3 = np.array([box[3], box[2]])
+ point4 = np.array([box[1], box[2]])
+
+ # Add data to the output proto.
+ out_obj = out_proto.object_in_image.add()
+ out_obj.name = "obj" + str(num_objects) + "_label_" + label
+
+ vertex1 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex1.x = point1[0]
+ vertex1.y = point1[1]
+
+ vertex2 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex2.x = point2[0]
+ vertex2.y = point2[1]
+
+ vertex3 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex3.x = point3[0]
+ vertex3.y = point3[1]
+
+ vertex4 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex4.x = point4[0]
+ vertex4.y = point4[1]
+
+ # Pack the confidence value.
+ confidence = wrappers_pb2.FloatValue(value=score)
+ out_obj.additional_properties.Pack(confidence)
+
+ if not args.no_debug:
+ polygon = np.array([point1, point2, point3, point4], np.int32)
+ polygon = polygon.reshape((-1, 1, 2))
+ cv2.polylines(image, [polygon], True, (0, 255, 0), 2)
+
+ caption = "{}: {:.3f}".format(label, score)
+ left_x = min(point1[0], min(point2[0], min(point3[0], point4[0])))
+ top_y = min(point1[1], min(point2[1], min(point3[1], point4[1])))
+ cv2.putText(image, caption, (int(left_x), int(top_y)), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
+ (0, 255, 0), 2)
+
+ print('Found ' + str(num_objects) + ' object(s)')
+
+ if not args.no_debug:
+ debug_image_filename = 'network_compute_server_output.jpg'
+ cv2.imwrite(debug_image_filename, image)
+ print('Wrote debug image output to: "' + debug_image_filename + '"')
+
+ response_queue.put(out_proto)
+
+
+class NetworkComputeBridgeWorkerServicer(
+ network_compute_bridge_service_pb2_grpc.NetworkComputeBridgeWorkerServicer):
+
+ def __init__(self, thread_input_queue, thread_output_queue):
+ super(NetworkComputeBridgeWorkerServicer, self).__init__()
+
+ self.thread_input_queue = thread_input_queue
+ self.thread_output_queue = thread_output_queue
+
+ def NetworkCompute(self, request, context):
+ print('Got NetworkCompute request')
+ self.thread_input_queue.put(request)
+ out_proto = self.thread_output_queue.get()
+ return out_proto
+
+ def ListAvailableModels(self, request, context):
+ print('Got ListAvailableModels request')
+ self.thread_input_queue.put(request)
+ out_proto = self.thread_output_queue.get()
+ return out_proto
+
+
+def register_with_robot(options):
+ """ Registers this worker with the robot's Directory."""
+ ip = bosdyn.client.common.get_self_ip(options.hostname)
+ print('Detected IP address as: ' + ip)
+
+ sdk = bosdyn.client.create_standard_sdk("tensorflow_server")
+
+ robot = sdk.create_robot(options.hostname)
+
+ # Authenticate robot before being able to use it
+ if options.payload_credentials_file:
+ robot.authenticate_from_payload_credentials(
+ *bosdyn.client.util.get_guid_and_secret(options))
+ else:
+ bosdyn.client.util.authenticate(robot)
+
+ directory_client = robot.ensure_client(
+ bosdyn.client.directory.DirectoryClient.default_service_name)
+ directory_registration_client = robot.ensure_client(
+ bosdyn.client.directory_registration.DirectoryRegistrationClient.default_service_name)
+
+ # Check to see if a service is already registered with our name
+ services = directory_client.list()
+ for s in services:
+ if s.name == options.name:
+ print("WARNING: existing service with name, \"" + options.name + "\", removing it.")
+ directory_registration_client.unregister(options.name)
+ break
+
+ # Register service
+ print('Attempting to register ' + ip + ':' + options.port + ' onto ' + options.hostname + ' directory...')
+ directory_registration_client.register(options.name, "bosdyn.api.NetworkComputeBridgeWorker", kServiceAuthority, ip, int(options.port))
+
+
+
+def main(argv):
+ default_port = '50051'
+
+ parser = argparse.ArgumentParser()
+ parser.add_argument('-m', '--model', help='[MODEL_DIR] [LABELS_FILE.pbtxt]: Path to a model\'s directory and path to its labels .pbtxt file', action='append', nargs=2, required=True)
+ parser.add_argument('-p', '--port', help='Server\'s port number, default: ' + default_port,
+ default=default_port)
+ parser.add_argument('-d', '--no-debug', help='Disable writing debug images.', action='store_true')
+ parser.add_argument('-n', '--name', help='Service name', default='object-detection-coco')
+ bosdyn.client.util.add_payload_credentials_arguments(parser, required=False)
+ bosdyn.client.util.add_base_arguments(parser)
+
+ options = parser.parse_args(argv)
+
+ print(options.model)
+
+ for model in options.model:
+ if not os.path.isdir(model[0]):
+ print('Error: model directory (' + model[0] + ') not found or is not a directory.')
+ sys.exit(1)
+
+ # Perform registration.
+ register_with_robot(options)
+
+ # Thread-safe queues for communication between the GRPC endpoint and the ML thread.
+ request_queue = queue.Queue()
+ response_queue = queue.Queue()
+
+ # Start server thread
+ thread = threading.Thread(target=process_thread, args=([options, request_queue, response_queue]))
+ thread.start()
+
+ # Set up GRPC endpoint
+ server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
+ network_compute_bridge_service_pb2_grpc.add_NetworkComputeBridgeWorkerServicer_to_server(
+ NetworkComputeBridgeWorkerServicer(request_queue, response_queue), server)
+ server.add_insecure_port('[::]:' + options.port)
+ server.start()
+
+ print('Running...')
+ thread.join()
+
+ return True
+
+if __name__ == '__main__':
+ logging.basicConfig()
+ if not main(sys.argv[1:]):
+ sys.exit(1)
diff --git a/jsk_spot_robot/coreio_extension/Dockerfile b/jsk_spot_robot/coreio_extension/Dockerfile
new file mode 100644
index 0000000000..bb60defe90
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/Dockerfile
@@ -0,0 +1,42 @@
+# Use a base image provided by nvidia that already contains tensorflow 2.5
+## FROM nvcr.io/nvidia/l4t-tensorflow:r32.6.1-tf2.5-py3
+## https://docs.nvidia.com/deeplearning/frameworks/support-matrix/index.html#framework-matrix-2019
+FROM nvcr.io/nvidia/tensorflow:19.10-py3
+
+
+# Do some basic apt and pip updating
+RUN apt-get update && \
+ apt-get install -y --no-install-recommends python3-pip libgl1 &&\
+ apt-get clean
+
+# Copy over the python requirements file and our prebuilt models API library
+COPY docker-requirements.txt prebuilt/*.whl ./
+COPY models-with-protos models-with-protos
+
+
+# Install the python requirements
+RUN python3 -m pip install pip==21.3.1 setuptools==59.6.0 wheel==0.37.1 && \
+ python3 -m pip install -r docker-requirements.txt --find-links .
+
+# https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch2
+RUN python3 -m pip install opencv-python==4.5.*
+RUN python3 -m pip install --upgrade pip && \
+ python3 -m pip install tensorflow-gpu==2.3.1 tensorflow==2.3.1 tensorboard==2.3.0 tf-models-official==2.3.0 pycocotools lvis && \
+ python3 -m pip uninstall -y opencv-python-headless
+
+
+RUN python3 -m pip install models-with-protos/research
+
+# copy pre-trained model
+COPY ssd_resnet50_v1_fpn_640x640_coco17_tpu-8 /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8
+RUN cp models-with-protos/research/object_detection/data/mscoco_label_map.pbtxt /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/
+
+# copy trained model
+COPY dogtoy-model /data/dogtoy-model
+
+# Copy over our main script
+COPY network_compute_server.py /app/
+WORKDIR /app
+
+# Set our script as the main entrypoint for the container
+ENTRYPOINT ["python3", "network_compute_server.py"]
diff --git a/jsk_spot_robot/coreio_extension/Makefile b/jsk_spot_robot/coreio_extension/Makefile
new file mode 100644
index 0000000000..ab1675d527
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/Makefile
@@ -0,0 +1,29 @@
+HOST_IP=192.168.50.3 # Or 10.0.0.3
+
+all:
+ [ -e models-with-protos.zip ] || wget https://dev.bostondynamics.com/docs/python/fetch_tutorial/files/models-with-protos.zip
+ [ -e models-with-protos ] || unzip models-with-protos.zip
+ [ -e ssd_resnet50_v1_fpn_640x640_coco17_tpu-8.tar.gz ] || wget http://download.tensorflow.org/models/object_detection/tf2/20200711/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8.tar.gz
+ [ -e ssd_resnet50_v1_fpn_640x640_coco17_tpu-8 ] || tar -xvzf ssd_resnet50_v1_fpn_640x640_coco17_tpu-8.tar.gz
+ [ -e dogtoy-model ] || cp -r ../dogtoy/exported-models/dogtoy-model .
+ [ -e dogtoy-model/label_map.pbtxt ] || cp ../dogtoy/annotations/label_map.pbtxt dogtoy-model/
+ docker build -t object_detection_coco -f Dockerfile .
+
+run:
+ docker run --network host \
+ -v /opt/payload_credentials/payload_guid_and_secret:/opt/payload_credentials/payload_guid_and_secret \
+ --rm object_detection_coco \
+ -m /data/dogtoy-model/saved_model /data/dogtoy-model/label_map.pbtxt \
+ -m /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model \
+ /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt \
+ --payload-credentials-file /opt/payload_credentials/payload_guid_and_secret \
+ --name ssd-resnet-coco-server \
+ $(HOST_IP)
+
+export:
+ [ -e object_detection_coco.tar.gz ] || docker save object_detection_coco | pigz > object_detection_coco.tar.gz
+ # scp -P 20022 *.tgz to spot@10.0.0.3:/opt
+ # scp -P 20022 *.service spot@10.0.0.3:/lib/systemd/system/
+
+bash:
+ docker run --gpus all --rm -it --entrypoint /bin/bash object_detection_coco
diff --git a/jsk_spot_robot/coreio_extension/README.md b/jsk_spot_robot/coreio_extension/README.md
new file mode 100644
index 0000000000..aae571ffdc
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/README.md
@@ -0,0 +1,23 @@
+coreio extensions
+-----------------
+
+Please follow https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch1 to get images, annotate target, training network. After that, we expect following directory structure.
+
+```
+- dogtoy/
+ |- annotations/
+ |- label_map.pbtxt
+ |- exported-models/
+ |- dogtoy-model/
+ |- checkpoint/
+ |- pipeline.config
+ |- saved_model/
+ |- models/
+ |- pre-trained-models/
+```
+
+
+Then copy `dogtoy-model` directory within `dogtoy/exported-models/` to this directory and put `annotations/label_map.pbtxt` under `dogtoy-model`. See `Makefile` for detail.
+
+To build docker run `make all`. To export docker image, run `make export`. After that copy `object_detection_coco.tar.gz` to `/opt/` directory of SpotCORE. To copy the data, we recommend to connect the ethernet cable to the robot or dock and use `scp -P 20022 object_detection_coco.tar.gz spot@10.0.0.3:/tmp/` to copy data and then, login to to spotcore and run `sudo cp /tmp/object_detection_coco.tar.gz /opt`. Please keep backup before you overwrite `tar.gz` file.
+
diff --git a/jsk_spot_robot/coreio_extension/docker-compose.yml b/jsk_spot_robot/coreio_extension/docker-compose.yml
new file mode 100644
index 0000000000..3d194245f2
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/docker-compose.yml
@@ -0,0 +1,20 @@
+version: "3.5"
+services:
+ object_detection_coco:
+ image: object_detection_coco
+ network_mode: host
+ restart: unless-stopped
+ environment:
+ # This package couldn't be installed, but putting it on the path allows required access to the protos.
+ - PYTHONPATH=/models-with-protos/research/
+ volumes:
+ # Mount payload credentials.
+ - /opt/payload_credentials:/opt/payload_credentials
+ # and ML models
+ - /data/.extensions/object_detection_coco/data:/data
+ # The command below is partial because the docker image is already configured with an entrypoint.
+ command: >
+ -m /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt
+ --payload-credentials-file /opt/payload_credentials/payload_guid_and_secret
+ 10.0.0.3
+# 192.168.50.3
diff --git a/jsk_spot_robot/coreio_extension/docker-requirements.txt b/jsk_spot_robot/coreio_extension/docker-requirements.txt
new file mode 100644
index 0000000000..12c96e14fe
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/docker-requirements.txt
@@ -0,0 +1,40 @@
+bosdyn-api==3.2.0
+ # via
+ # bosdyn-client
+ # bosdyn-core
+bosdyn-client==3.2.0
+ # via -r requirements.txt
+bosdyn-core==3.2.0
+ # via bosdyn-client
+certifi==2022.5.18.1
+ # via requests
+charset-normalizer==2.0.12
+ # via requests
+deprecated==1.2.13
+ # via
+ # bosdyn-client
+ # bosdyn-core
+grpcio
+ #==1.46.3 (conflicts with base image)
+ # via bosdyn-client
+idna==3.3
+ # via requests
+numpy==1.19.4
+ # via bosdyn-client
+protobuf==3.19.4
+ # via bosdyn-api
+pyjwt==2.4.0
+ # via bosdyn-client
+requests==2.27.1
+ # via bosdyn-client
+six
+ #==1.16.0 (conflicts with base image)
+ # via grpcio
+urllib3==1.26.9
+ # via requests
+wrapt
+ #==1.14.1 (conflicts with base image)
+ # via deprecated
+Pillow==8.4.0
+opencv-python==4.6.0.66
+ # via network_compute_server.py
\ No newline at end of file
diff --git a/jsk_spot_robot/coreio_extension/ncb_object_detection_coco.service b/jsk_spot_robot/coreio_extension/ncb_object_detection_coco.service
new file mode 100644
index 0000000000..40f869e6d8
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/ncb_object_detection_coco.service
@@ -0,0 +1,18 @@
+[Unit]
+Description=Coco object detection container
+After=docker.service
+Wants=network-online.target docker.socket
+Requires=docker.socket
+
+[Service]
+Restart=always
+User=root
+ExecStartPre=/bin/bash -c "/usr/bin/docker container inspect object_detection_coco 2> /dev/null || /usr/bin/docker load < /opt/object_detection_coco.tar.gz;"
+ExecStartPre=-/usr/bin/docker stop object_detection_coco
+ExecStartPre=-/usr/bin/docker rm object_detection_coco
+ExecStart=/usr/bin/docker run --name object_detection_coco --network host --rm -v /opt/payload_credentials/payload_guid_and_secret:/opt/payload_credentials/payload_guid_and_secret object_detection_coco -m /data/dogtoy-model/saved_model /data/dogtoy-model/label_map.pbtxt -m /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/saved_model /data/ssd_resnet50_v1_fpn_640x640_coco17_tpu-8/mscoco_label_map.pbtxt --payload-credentials-file /opt/payload_credentials/payload_guid_and_secret --name ssd-resnet-coco-server --port 50055 192.168.50.3
+ExecStop=/usr/bin/docker stop -t 10 object_detection_coco
+TimeoutSec=900
+
+[Install]
+WantedBy=multi-user.target
diff --git a/jsk_spot_robot/coreio_extension/network_compute_server.py b/jsk_spot_robot/coreio_extension/network_compute_server.py
new file mode 100644
index 0000000000..9b697b8383
--- /dev/null
+++ b/jsk_spot_robot/coreio_extension/network_compute_server.py
@@ -0,0 +1,312 @@
+# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+import argparse
+import io
+import os
+import sys
+import time
+import logging
+
+import cv2
+from PIL import Image
+import numpy as np
+
+from bosdyn.api import network_compute_bridge_service_pb2_grpc
+from bosdyn.api import network_compute_bridge_pb2
+from bosdyn.api import image_pb2
+from bosdyn.api import header_pb2
+import bosdyn.client
+import bosdyn.client.util
+import grpc
+from concurrent import futures
+import tensorflow as tf
+
+import queue
+import threading
+from google.protobuf import wrappers_pb2
+from object_detection.utils import label_map_util
+
+kServiceAuthority = "fetch-tutorial-worker.spot.robot"
+
+
+class TensorFlowObjectDetectionModel:
+ def __init__(self, model_path, label_path):
+ self.detect_fn = tf.saved_model.load(model_path)
+ self.category_index = label_map_util.create_category_index_from_labelmap(label_path, use_display_name=True)
+ self.name = os.path.basename(os.path.dirname(model_path))
+
+ def predict(self, image):
+ input_tensor = tf.convert_to_tensor(image)
+ input_tensor = input_tensor[tf.newaxis, ...]
+ detections = self.detect_fn(input_tensor)
+
+ return detections
+
+def process_thread(args, request_queue, response_queue):
+ # Load the model(s)
+ models = {}
+ for model in args.model:
+ this_model = TensorFlowObjectDetectionModel(model[0], model[1])
+ models[this_model.name] = this_model
+
+ print('')
+ print('Service ' + args.name + ' running on port: ' + str(args.port))
+
+ print('Loaded models:')
+ for model_name in models:
+ print(' ' + model_name)
+
+ while True:
+ request = request_queue.get()
+
+ if isinstance(request, network_compute_bridge_pb2.ListAvailableModelsRequest):
+ out_proto = network_compute_bridge_pb2.ListAvailableModelsResponse()
+ for model_name in models:
+ out_proto.available_models.append(model_name)
+ if models[model_name].category_index is not None:
+ labels_msg = out_proto.labels.add()
+ labels_msg.model_name = model_name
+ for n in models[model_name].category_index.keys():
+ labels_msg.available_labels.append(models[model_name].category_index[n]['name'])
+ response_queue.put(out_proto)
+ continue
+ else:
+ out_proto = network_compute_bridge_pb2.NetworkComputeResponse()
+
+ # Find the model
+ if request.input_data.model_name not in models:
+ err_str = 'Cannot find model "' + request.input_data.model_name + '" in loaded models.'
+ print(err_str)
+
+ # Set the error in the header.
+ out_proto.header.error.code = header_pb2.CommonError.CODE_INVALID_REQUEST
+ out_proto.header.error.message = err_str
+ response_queue.put(out_proto)
+ continue
+
+ model = models[request.input_data.model_name]
+
+ # Unpack the incoming image.
+ if request.input_data.image.format == image_pb2.Image.FORMAT_RAW:
+ pil_image = Image.open(io.BytesIO(request.input_data.image.data))
+ if request.input_data.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8:
+ # If the input image is grayscale, convert it to RGB.
+ image = cv2.cvtColor(pil_image, cv2.COLOR_GRAY2RGB)
+
+ elif request.input_data.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_RGB_U8:
+ # Already an RGB image.
+ image = pil_image
+
+ else:
+ print('Error: image input in unsupported pixel format: ', request.input_data.image.pixel_format)
+ response_queue.put(out_proto)
+ continue
+
+ elif request.input_data.image.format == image_pb2.Image.FORMAT_JPEG:
+ dtype = np.uint8
+ jpg = np.frombuffer(request.input_data.image.data, dtype=dtype)
+ image = cv2.imdecode(jpg, -1)
+
+ if len(image.shape) < 3:
+ # If the input image is grayscale, convert it to RGB.
+ image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB)
+
+ image_width = image.shape[0]
+ image_height = image.shape[1]
+
+ detections = model.predict(image)
+
+ num_objects = 0
+
+ # All outputs are batches of tensors.
+ # Convert to numpy arrays, and take index [0] to remove the batch dimension.
+ # We're only interested in the first num_detections.
+ num_detections = int(detections.pop('num_detections'))
+ detections = {key: value[0, :num_detections].numpy()
+ for key, value in detections.items()}
+
+ boxes = detections['detection_boxes']
+ classes = detections['detection_classes']
+ scores = detections['detection_scores']
+
+
+ for i in range(boxes.shape[0]):
+ if scores[i] < request.input_data.min_confidence:
+ continue
+
+ box = tuple(boxes[i].tolist())
+
+ # Boxes come in with normalized coordinates. Convert to pixel values.
+ box = [box[0] * image_width, box[1] * image_height, box[2] * image_width, box[3] * image_height]
+
+ score = scores[i]
+
+ if classes[i] in model.category_index.keys():
+ label = model.category_index[classes[i]]['name']
+ else:
+ label = 'N/A'
+
+ num_objects += 1
+
+ print('Found object with label: "' + label + '" and score: ' + str(score))
+
+
+ point1 = np.array([box[1], box[0]])
+ point2 = np.array([box[3], box[0]])
+ point3 = np.array([box[3], box[2]])
+ point4 = np.array([box[1], box[2]])
+
+ # Add data to the output proto.
+ out_obj = out_proto.object_in_image.add()
+ out_obj.name = "obj" + str(num_objects) + "_label_" + label
+
+ vertex1 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex1.x = point1[0]
+ vertex1.y = point1[1]
+
+ vertex2 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex2.x = point2[0]
+ vertex2.y = point2[1]
+
+ vertex3 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex3.x = point3[0]
+ vertex3.y = point3[1]
+
+ vertex4 = out_obj.image_properties.coordinates.vertexes.add()
+ vertex4.x = point4[0]
+ vertex4.y = point4[1]
+
+ # Pack the confidence value.
+ confidence = wrappers_pb2.FloatValue(value=score)
+ out_obj.additional_properties.Pack(confidence)
+
+ if not args.no_debug:
+ polygon = np.array([point1, point2, point3, point4], np.int32)
+ polygon = polygon.reshape((-1, 1, 2))
+ cv2.polylines(image, [polygon], True, (0, 255, 0), 2)
+
+ caption = "{}: {:.3f}".format(label, score)
+ left_x = min(point1[0], min(point2[0], min(point3[0], point4[0])))
+ top_y = min(point1[1], min(point2[1], min(point3[1], point4[1])))
+ cv2.putText(image, caption, (int(left_x), int(top_y)), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
+ (0, 255, 0), 2)
+
+ print('Found ' + str(num_objects) + ' object(s)')
+
+ if not args.no_debug:
+ debug_image_filename = 'network_compute_server_output.jpg'
+ cv2.imwrite(debug_image_filename, image)
+ print('Wrote debug image output to: "' + debug_image_filename + '"')
+
+ response_queue.put(out_proto)
+
+
+class NetworkComputeBridgeWorkerServicer(
+ network_compute_bridge_service_pb2_grpc.NetworkComputeBridgeWorkerServicer):
+
+ def __init__(self, thread_input_queue, thread_output_queue):
+ super(NetworkComputeBridgeWorkerServicer, self).__init__()
+
+ self.thread_input_queue = thread_input_queue
+ self.thread_output_queue = thread_output_queue
+
+ def NetworkCompute(self, request, context):
+ print('Got NetworkCompute request')
+ self.thread_input_queue.put(request)
+ out_proto = self.thread_output_queue.get()
+ return out_proto
+
+ def ListAvailableModels(self, request, context):
+ print('Got ListAvailableModels request')
+ self.thread_input_queue.put(request)
+ out_proto = self.thread_output_queue.get()
+ return out_proto
+
+
+def register_with_robot(options):
+ """ Registers this worker with the robot's Directory."""
+ ip = bosdyn.client.common.get_self_ip(options.hostname)
+ print('Detected IP address as: ' + ip)
+
+ sdk = bosdyn.client.create_standard_sdk("tensorflow_server")
+
+ robot = sdk.create_robot(options.hostname)
+
+ # Authenticate robot before being able to use it
+ if options.payload_credentials_file:
+ robot.authenticate_from_payload_credentials(
+ *bosdyn.client.util.get_guid_and_secret(options))
+ else:
+ bosdyn.client.util.authenticate(robot)
+
+ directory_client = robot.ensure_client(
+ bosdyn.client.directory.DirectoryClient.default_service_name)
+ directory_registration_client = robot.ensure_client(
+ bosdyn.client.directory_registration.DirectoryRegistrationClient.default_service_name)
+
+ # Check to see if a service is already registered with our name
+ services = directory_client.list()
+ for s in services:
+ if s.name == options.name:
+ print("WARNING: existing service with name, \"" + options.name + "\", removing it.")
+ directory_registration_client.unregister(options.name)
+ break
+
+ # Register service
+ print('Attempting to register ' + ip + ':' + options.port + ' onto ' + options.hostname + ' directory...')
+ directory_registration_client.register(options.name, "bosdyn.api.NetworkComputeBridgeWorker", kServiceAuthority, ip, int(options.port))
+
+
+
+def main(argv):
+ default_port = '50051'
+
+ parser = argparse.ArgumentParser()
+ parser.add_argument('-m', '--model', help='[MODEL_DIR] [LABELS_FILE.pbtxt]: Path to a model\'s directory and path to its labels .pbtxt file', action='append', nargs=2, required=True)
+ parser.add_argument('-p', '--port', help='Server\'s port number, default: ' + default_port,
+ default=default_port)
+ parser.add_argument('-d', '--no-debug', help='Disable writing debug images.', action='store_true')
+ parser.add_argument('-n', '--name', help='Service name', default='fetch-server')
+ bosdyn.client.util.add_payload_credentials_arguments(parser, required=False)
+ bosdyn.client.util.add_base_arguments(parser)
+
+ options = parser.parse_args(argv)
+
+ print(options.model)
+
+ for model in options.model:
+ if not os.path.isdir(model[0]):
+ print('Error: model directory (' + model[0] + ') not found or is not a directory.')
+ sys.exit(1)
+
+ # Perform registration.
+ register_with_robot(options)
+
+ # Thread-safe queues for communication between the GRPC endpoint and the ML thread.
+ request_queue = queue.Queue()
+ response_queue = queue.Queue()
+
+ # Start server thread
+ thread = threading.Thread(target=process_thread, args=([options, request_queue, response_queue]))
+ thread.start()
+
+ # Set up GRPC endpoint
+ server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
+ network_compute_bridge_service_pb2_grpc.add_NetworkComputeBridgeWorkerServicer_to_server(
+ NetworkComputeBridgeWorkerServicer(request_queue, response_queue), server)
+ server.add_insecure_port('[::]:' + options.port)
+ server.start()
+
+ print('Running...')
+ thread.join()
+
+ return True
+
+if __name__ == '__main__':
+ logging.basicConfig()
+ if not main(sys.argv[1:]):
+ sys.exit(1)
diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall
new file mode 100644
index 0000000000..021b6953c2
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_driver.rosinstall
@@ -0,0 +1,89 @@
+# spot-ros is required for spot
+# This is a develop branch for jsk version.
+# We need to use it until it is merged to master
+# - git:
+# local-name: spot-ros
+# uri: https://github.com/sktometometo/spot_ros.git
+# version: develop/spot
+## use https://github.com/clearpathrobotics/spot_ros/pull/67 (Commands for the Arm and gripper, and URDF and simulation of the arm added)
+# - git:
+# local-name: spot-ros
+# uri: https://github.com/estherRay/spot_ros.git
+# version: b5fe5e5c4a732c1a724e4f86abc2be6395088d02
+## use https://github.com/cst0/spot_ros/pull/4 for Arm and gripper
+- git:
+ local-name: spot-ros
+ uri: https://github.com/k-okada/spot_ros-arm.git
+ version: 0a8c4b29768ae946d7c84aec9a2ae2d185ed97a9
+## to support custom limb name, we need https://github.com/jsk-ros-pkg/jsk_model_tools/pull/249
+- git:
+ local-name: jsk_model_tools
+ uri: https://github.com/k-okada/jsk_model_tools.git
+ version: custom_limb
+#
+# geometry2 have to be built for python3
+- git:
+ local-name: geometry2
+ uri: https://github.com/ros/geometry2.git
+ version: 0.6.5
+# vision_opencv have to be build for python3
+- git:
+ local-name: vision_opencv
+ uri: https://github.com/ros-perception/vision_opencv.git
+ version: 1.13.0
+#
+# pykdl have to be built for python3
+- git:
+ local-name: orocos_kinematics_dynamics
+ uri: https://github.com/orocos/orocos_kinematics_dynamics.git
+ version: v1.4.0
+#
+# mongodb_store need to be built for python3
+- git:
+ local-name: mongodb_store
+ uri: https://github.com/strands-project/mongodb_store.git
+ version: noetic-devel
+#
+# to avtivate python_catkin_install, we need newer catkin
+- git:
+ local-name: catkin
+ uri: https://github.com/ros/catkin.git
+ version: 0.8.10
+#
+# use robot_upstart to install supervisor (https://github.com/clearpathrobotics/robot_upstart/pull/113)
+- git:
+ local-name: robot_upstart
+ uri: https://github.com/k-okada/robot_upstart.git
+ version: develop
+### aques_talk does not have arm64 libraries
+# # aques_talk needs to be compiled from source
+# - tar:
+# local-name: aques_talk
+# uri: https://github.com/tork-a/jsk_3rdparty-release/archive/refs/tags/release/melodic/aques_talk/2.1.24-2.tar.gz
+# use parallel app_manager
+- git:
+ local-name: app_manager
+ uri: https://github.com/PR2/app_manager.git
+ version: kinetic-devel
+# wait for https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/443 (add volume key arg for robot-interface :speak) to be released
+- git:
+ local-name: jsk_pr2eus
+ uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
+ version: master
+#
+#
+# need to add following code to ublox_gps/config/zed_f9p.yaml
+#config_on_startup: true
+#
+#rate: 0.016
+## TMODE3 Config
+#tmode3: 1 # Survey-In Mode
+#sv_in:
+# reset: false # True: disables and re-enables survey-in (resets)
+# # False: Disables survey-in only if TMODE3 is
+# # disabled
+# min_dur: 300 # Survey-In Minimum Duration [s]
+# acc_lim: 3.0 # Survey-In Accuracy Limit [m]
+- git:
+ local-name: ublox
+ uri: https://github.com/KumarRobotics/ublox.git
diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt
new file mode 100644
index 0000000000..a9e4f14e83
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt
@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_spot_startup)
+
+find_package(catkin REQUIRED)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package()
+
+
+#############
+## Install ##
+#############
+install(DIRECTORY auth config launch scripts services udev_rules template
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
+
+file(GLOB_RECURSE SCRIPT_PROGRAMS node_scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts)
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts)
+ endif()
+endforeach()
+
+
+#############
+## Testing ##
+#############
+if(CATKIN_ENABLE_TESTING)
+ find_package(catkin REQUIRED COMPONENTS roslaunch roslint)
+ file(GLOB LAUNCH_FILES launch/*.launch)
+ foreach(LAUNCH_FILE ${LAUNCH_FILES})
+ roslaunch_add_file_check(${LAUNCH_FILE})
+ endforeach()
+
+ set(ROSLINT_PYTHON_OPTS --max-line-length=180 --ignore=E221,E222,E241) # skip multiple spaces before/after operator
+ roslint_python()
+ roslint_add_test()
+endif()
+
+#############
+## Install ##
+#############
+file(GLOB_RECURSE SCRIPT_PROGRAMS node_scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts)
+ endif()
+endforeach()
+
+install(DIRECTORY app config launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
+
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} nao.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/app.installed b/jsk_spot_robot/jsk_spot_startup/apps/app.installed
new file mode 100644
index 0000000000..2bf50ef955
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/app.installed
@@ -0,0 +1,6 @@
+apps:
+- app: jsk_spot_startup/hello_world
+ dispay: Hello World
+- app: jsk_spot_startup/head_lead_demo
+ dispay: Head Lead Demo
+
\ No newline at end of file
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l
new file mode 100755
index 0000000000..8548d7f372
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l
@@ -0,0 +1,297 @@
+#!/usr/bin/env roseus
+
+(ros::roseus-add-msgs "std_msgs")
+
+(setq *continue* t)
+;; control head-lead
+(setq *head-lead* t)
+;; (setq *head-lead* nil)
+(load "package://spoteus/spot-interface.l")
+(spot-init)
+(if (eq (send *ri* :state :power-state-shore-power-state) 'on-shore-power)
+ (send *ri* :undock))
+;; (send *ri* :set-impedance-param :linear-stiffness #f(250 250 250) :rotational-stiffness #f(30 30 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5))
+;; (send *ri* :set-impedance-param :linear-stiffness #f(250 25 25) :rotational-stiffness #f(30 5 5) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5))
+
+(setq *default-av* #f(0.0 -130.0 120.0 0.0 10.0 0.0))
+
+;; sample
+(setq *person* nil)
+(setq *ball* nil)
+(setq *ball-name* nil)
+(setq *last-detected-time* (ros::time-now))
+(defclass ncb-result-synchronizer
+ :super exact-time-message-filter)
+
+(defmethod ncb-result-synchronizer
+ (:callback (bbox rects result)
+ (setq *last-detected-time* (ros::time-now))
+ (let (len ret)
+ (setq len (length (send bbox :boxes)))
+ (dotimes (i len)
+ (ros::ros-info (format nil "found ~A (~A) in ~A sec ago in ~A"
+ (elt (send result :label_names) i)
+ (elt (send result :label_proba) i)
+ (send (ros::time- (ros::time-now) (send bbox :header :stamp)) :to-sec)
+ (cdr (assoc "topic" (send result :connection-header) :test #'string=))))
+ (cond ((string= (elt (send result :label_names) i) "person")
+ (send *ri* :speak "p")
+ (push (list (send result :header :frame_id) (elt (send rects :rects) i) (elt (send bbox :boxes) i)) *person*))
+ ((or (string= (elt (send result :label_names) i) "soccer ball")
+ (string= (elt (send result :label_names) i) "pokemon ball")
+ (string= (elt (send result :label_names) i) "dice"))
+ (send *ri* :speak "ball")
+ (push (list (send result :header :frame_id)
+ (elt (send rects :rects) i)
+ (elt (send bbox :boxes) i)
+ (elt (send result :label_names) i))
+ *ball*))))
+ ;; results are (list image rect box); to get box user (elt x 2) or (third x)
+ (setq *person* (sort *person* #'< #'(lambda (x) (send (elt x 2) :value))))
+ (setq *ball* (sort *ball* #'< #'(lambda (x) (send (elt x 2) :value))))
+ )) ;; :callback
+ )
+(ros::roseus-add-msgs "jsk_recognition_msgs")
+
+(setq synchronizers nil)
+(dolist (image (list "left_fisheye_image" "right_fisheye_image"
+ "frontleft_fisheye_image" "frontright_fisheye_image"
+ "hand_color_image" "back_fisheye_image"))
+ (push (instance ncb-result-synchronizer :init
+ (list (list (format nil "/spot/ncb_provider/~A/bbox_array" image) jsk_recognition_msgs::BoundingBoxArray)
+ (list (format nil "/spot/ncb_provider/~A/rects" image) jsk_recognition_msgs::RectArray)
+ (list (format nil "/spot/ncb_provider/~A/class" image) jsk_recognition_msgs::ClassificationResult))) synchronizers))
+
+(defun init-walk-arm-setting ()
+ (send *spot* :arm :angle-vector #f(0.0 -130.0 120.0 0.0 10.0 0.0))
+ (send *ri* :angle-vector (send *spot* :angle-vector) 1000 :default-controller)
+ (send *ri* :stop-grasp)
+ (send *ri* :wait-interpolation)
+ (send *ri* :set-impedance-param
+ :linear-stiffness #f(250 25 75) ;; z(up) y(side) x(front)
+ :rotational-stiffness #f(3 30 30)
+ :linear-damping #f(1.5 1.0 1.0)
+ :rotational-damping #f(0.1 0.5 0.5))
+ (setq hand-pos (ros::pos->tf-point (send *spot* :hand :end-coords :worldpos)))
+ (ros::set-param "end_effector_to_joy/center_x" (send hand-pos :x))
+ (ros::set-param "end_effector_to_joy/center_y" (send hand-pos :y))
+ (unless (ros::wait-for-service "end_effector_to_joy/set_enabled" 10)
+ (ros::ros-error "end_effector_to_joy/set_enabled not found, so quitting")
+ (return-from init-walk-arm-setting :finished))
+ (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data *head-lead*))
+ )
+
+(defun start-func (args)
+ (when (not (eq (send *ri* :state :power-state-shore-power-state) 'off-shore-power))
+ (send *ri* :speak "Robot is on dock")
+ (ros::ros-error "Robot is on dock or powered, quit from apps")
+ ;(return-from start-func :finished)
+ )
+ (send *ri* :speak "Hello, let's start walking")
+ (if *head-lead*
+ (init-walk-arm-setting)
+ (progn
+ (send *spot* :arm :angle-vector #f(0 -180 180 90 0 -90))
+ (send *ri* :angle-vector (send *spot* :angle-vector) 2000)
+ (send *ri* :stow-arm)
+ (send *ri* :stop-grasp)
+ (send *ri* :wait-interpolation)))
+ :started)
+
+(defun end-func (args)
+ (ros::ros-info "end-func")
+ (send *ri* :speak "Thank You")
+ (send *ri* :stow-arm)
+ (unless (ros::wait-for-service "end_effector_to_joy/set_enabled" 10)
+ (ros::ros-error "end_effector_to_joy/set_enabled not found, so quitting")
+ (return-from end-func :finished))
+ (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data nil))
+ :finished)
+
+;; *cmd-vel* is (cons (ros::time-now) msg)
+(defun walk-func (args)
+ (let ((still-sec 0)
+ (countdown 0))
+ (ros::ros-info "walk-func")
+ (do-until-key
+ (ros::rate 2)
+ (ros::spin-once)
+ (if *cmd-vel*
+ (setq still-sec (send (ros::time- (ros::time-now) (car *cmd-vel*)) :to-sec)))
+ (ros::ros-info "continue... ~A, still-sec ~A" *continue* still-sec)
+ (if (null *continue*) (return-from walk-func :finished))
+ (if *cmd-vel*
+ (ros::ros-info "cmd-vel ... ~A sec ago ~A (~A)"
+ still-sec
+ (float-vector (send (cdr *cmd-vel*) :linear :x) (send (cdr *cmd-vel*) :linear :y) (send (cdr *cmd-vel*) :angular :z))
+ (norm (float-vector (send (cdr *cmd-vel*) :linear :x) (send (cdr *cmd-vel*) :linear :y) (send (cdr *cmd-vel*) :angular :z)))))
+ (when (> still-sec countdown)
+ (send *ri* :speak (format nil "~A" (case countdown (0 "z") (2 "fu") (4 "shi") (6 "mu") (8 "ya") (10 "tou") (t countdown))))
+ (setq countdown (+ (ceiling still-sec) 1)))
+ (if (and *cmd-vel* (> still-sec 10))
+ (return-from walk-func :not-pull))
+ (ros::sleep))
+ :finished))
+
+(setq *distance-thre* 2000)
+(defun ball-play-func (args)
+ (ros::ros-info "ball-play")
+ (ros::spin-once)
+ (block
+ :cond
+ (cond ((send *ri* :interpolatingp)
+ (ros::ros-info "robot is moving..."))
+ (*ball*
+ (send *ri* :speak "found ball")
+ (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data nil))
+ (ros::ros-info "found ball ~A" *ball*)
+ (ros::ros-info "found ~A" (mapcar #'ros::tf-pose->coords (send-all (mapcar #'third *ball*) :pose)))
+ (ros::ros-info " ~A" (mapcar #'second *ball*))
+ (ros::ros-info " ~A" (mapcar #'fourth *ball*))
+ (send *ri* :stow-arm)
+ (let ((image-source (first (car *ball*)))
+ (bbox (third (car *ball*)))
+ (rect (second (car *ball*)))
+ (ball-name (fourth (car *ball*)))
+ result)
+ (ros::ros-info "distance ~A (< ~A)" (norm (send (ros::tf-pose->coords (send bbox :pose)) :worldpos)) *distance-thre*)
+ (when (> (norm (send (ros::tf-pose->coords (send bbox :pose)) :worldpos)) *distance-thre*)
+ (send *ri* :speak "too far")
+ (setq *ball* nil)
+ (return-from :cond nil))
+ ;; look at the ball
+ (send *spot* :head :look-at
+ (v+ (float-vector 0 0 (/ (send bbox :dimensions :z) 2))
+ (send (ros::tf-pose->coords (send bbox :pose)) :worldpos)))
+ (send *ri* :angle-vector (send *spot* :angle-vector) 1000)
+ (send *ri* :wait-interpolation)
+ (unix::sleep 1)
+ (send *ri* :pick-object-in-image image-source
+ (+ (send rect :x) (/ (send rect :width) 2))
+ (+ (send rect :y) (/ (send rect :height) 2)))
+ (ros::publish "/spot/pick_object_in_image/pick_object"
+ (instance std_msgs::String :init :data ball-name))
+ (while (= (send *ri* :pick-object-in-image-get-state) actionlib_msgs::GoalStatus::*active*)
+ (ros::ros-info "wait for pick... ~A"
+ (if (send *ri* :pick-object-in-image-feedback-msg)
+ (send *ri* :pick-object-in-image-feedback-msg :feedback :status)))
+ (ros::duration-sleep 1))
+ (setq result (send *ri* :pick-object-in-image-wait-for-result))
+ (if (send result :success)
+ (progn
+ (send *ri* :speak "good")
+ (ros::ros-info "succeeded to pick object"))
+ (progn
+ (send *ri* :speak "too bad")
+ (ros::ros-info "failed to pick object")
+ (send *ri* :stop-grasp)
+ ;;(unix::sleep 2) ;; ?? prevent from walking????
+ ))
+ (if *head-lead*
+ (progn
+ (send *spot* :arm :angle-vector *default-av*)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 1000)
+ (send *ri* :wait-interpolation)
+ )
+ (progn
+ (send *spot* :arm :angle-vector #f(0 -180 180 90 0 -90))
+ (send *ri* :angle-vector (send *spot* :angle-vector) 2000)
+ (send *ri* :wait-interpolation)
+ (send *ri* :stow-arm)
+ ))
+ (when (send result :success)
+ (send *ri* :go-pos -0.5 0 0)
+ (when *head-lead*
+ (send *spot* :arm :wrist-p :joint-angle 30)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 500))
+ (send *ri* :stop-grasp))
+ (setq *ball* nil)
+ (ros::rate 1)
+ (if *head-lead* (init-walk-arm-setting))
+ ))
+ (*person*
+ (send *ri* :speak "found person")
+ (ros::ros-info "found person ~A" *person*)
+ (ros::ros-info "found person ~A" (mapcar #'ros::tf-pose->coords (send-all (mapcar #'third *person*) :pose)))
+ (ros::ros-info " ~A" (mapcar #'second *person*))
+ (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data nil))
+ (send *spot* :arm :angle-vector *default-av*)
+ (send *spot* :head :look-at (v+
+ (float-vector 0 0 (/ (send (third (car *person*)) :dimensions :z) 2))
+ (send (ros::tf-pose->coords (send (third (car *person*)) :pose)) :worldpos)))
+ (send *ri* :angle-vector (send *spot* :angle-vector) 500)
+ (setq *person* nil)
+ (send *ri* :wait-interpolation)
+ (if *head-lead* (init-walk-arm-setting)))
+ (t
+ (ros::ros-info (format nil "nothing found ..... last detected object is ~A sec ago" (send (ros::time- (ros::time-now) *last-detected-time*) :to-sec)))
+ (when (and
+ *head-lead*
+ (> (send (ros::time- (ros::time-now) *last-detected-time*) :to-sec) 10)
+ (> (norm (v- (coerce (mapcar #'(lambda (x) (elt (send *ri* :state :angle-vector) x)) (mapcar #'(lambda (x) (position x (send *spot* :joint-list))) (send *spot* :arm :joint-list))) float-vector) *default-av*)) 10))
+ (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data nil))
+ (send *spot* :arm :angle-vector *default-av*)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 500)
+ (send *ri* :wait-interpolation)
+ (init-walk-arm-setting)
+ ))
+ )) ;; cond
+ :walk)
+;;
+(load "package://roseus_smach/src/state-machine-ros.l")
+(defun walk-sm ()
+ (let (sm)
+ (setq sm
+ (make-state-machine
+ '((:start :started :walk) ;; transitions (node transition node)
+ (:start :finished :end)
+ (:walk :finished :end)
+ (:walk :not-pull :ball-play)
+ (:ball-play :walk :walk)
+ (:ball-play :finished :end)
+ (:end :finished :goal)
+ )
+ '((:start 'start-func) ;; node-to-function maps
+ (:end 'end-func)
+ (:walk 'walk-func)
+ (:ball-play 'ball-play-func)
+ )
+ '(:start) ;; initial node
+ '(:goal) ;; goal node
+ ))
+ (send sm :arg-keys 'description)
+ sm))
+
+;; create robot interface
+(unless (boundp '*ri*) (spot-init))
+(objects (list *spot*))
+
+;;
+
+;; this does not work...
+;; (unix:signal unix::sigint '(lambda-closure nil 0 0 (sig code) (setq *continue* nil)))
+
+;; https://github.com/jsk-ros-pkg/jsk_roseus/pull/717
+;; did not work, when we subscribe image data ???
+
+(defun ros::roseus-sigint-handler (sig code)
+ (ros::ros-warn (format nil "ros::roseus-sigint-handler ~A" sig))
+ (setq *continue* nil))
+(unix:signal unix::sigint 'ros::roseus-sigint-handler)
+
+;; send start-func (set-impedance) when R Stick button is pressed
+(ros::subscribe "/joy_dualsense" sensor_msgs::Joy
+ #'(lambda (msg)
+ (if (and (> (length (send msg :buttons)) 11)
+ (= (elt (send msg :buttons) 11) 1))
+ (start-func nil)))
+ 20)
+(setq *cmd-vel* (cons (ros::time-now) (instance geometry_msgs::Twist :init)))
+(ros::subscribe "/spot/cmd_vel" geometry_msgs::Twist
+ #'(lambda (msg) (setq *cmd-vel* (cons (ros::time-now) msg))) 20)
+
+(ros::advertise "/spot/pick_object_in_image/pick_object" std_msgs::String 1 t)
+
+;; state machine
+(exec-state-machine (walk-sm) '((description . "お散歩しました！")(image . "")))
+(exit)
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.app b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.app
new file mode 100644
index 0000000000..500d55e80a
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.app
@@ -0,0 +1,5 @@
+display: Head Lead Demo
+platform: spot
+launch: jsk_spot_startup/head_lead_demo.xml
+interface: jsk_spot_startup/head_lead_demo.interface
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.interface b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.interface
new file mode 100644
index 0000000000..c27c9c296e
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.interface
@@ -0,0 +1,3 @@
+published_topics: {}
+subscribed_topics: {}
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml
new file mode 100644
index 0000000000..bee6b6b5e4
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml
@@ -0,0 +1,42 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello-world.l b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello-world.l
new file mode 100755
index 0000000000..3f6ad67077
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello-world.l
@@ -0,0 +1,78 @@
+#!/usr/bin/env roseus
+
+(setq *continue* t)
+(load "package://spoteus/spot-interface.l")
+(spot-init)
+(if (eq (send *ri* :state :power-state-shore-power-state) 'on-shore-power)
+ (send *ri* :undock))
+
+(defun init-pose ()
+ (send *spot* :reset-pose)
+ (send *spot* :arm :move-end-pos #f(0 0 250) :world)
+ )
+
+(defun start-func (args)
+ (init-pose)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 500 :default-controller)
+ (send *ri* :wait-interpolation)
+ :started)
+
+(defun end-func (args)
+ (send *ri* :stow-arm)
+ :finished)
+
+(defun hello-func (args)
+ (send *ri* :speak "Hello" :volume 1.0)
+ (init-pose)
+ (send *spot* :arm :move-end-rot -20 :z :parent)
+ (send *spot* :arm :move-end-rot -10 :x :parent)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 150 :default-controller)
+ (send *ri* :wait-interpolation)
+ ;;
+ (send *ri* :speak "World" :volume 1.0)
+ (init-pose)
+ (send *spot* :arm :move-end-rot 20 :z :parent)
+ (send *spot* :arm :move-end-rot -10 :x :parent)
+ (send *ri* :angle-vector (send *spot* :angle-vector) 150 :default-controller)
+ (send *ri* :wait-interpolation)
+ :finished)
+;;
+(load "package://roseus_smach/src/state-machine-ros.l")
+(defun hello-sm ()
+ (let (sm)
+ (setq sm
+ (make-state-machine
+ '((:start :started :hello) ;; transitions (node transition node)
+ (:hello :finished :end)
+ (:end :finished :goal)
+ )
+ '((:start 'start-func) ;; node-to-function maps
+ (:end 'end-func)
+ (:hello 'hello-func)
+ )
+ '(:start) ;; initial node
+ '(:goal) ;; goal node
+ ))
+ (send sm :arg-keys 'description)
+ sm))
+
+;; create robot interface
+(unless (boundp '*ri*) (spot-init))
+(objects (list *spot*))
+
+;;
+
+;; this does not work...
+;; (unix:signal unix::sigint '(lambda-closure nil 0 0 (sig code) (setq *continue* nil)))
+
+;; https://github.com/jsk-ros-pkg/jsk_roseus/pull/717
+;; did not work, when we subscribe image data ???
+
+(defun ros::roseus-sigint-handler (sig code)
+ (ros::ros-warn (format nil "ros::roseus-sigint-handler ~A" sig))
+ (setq *continue* nil))
+(unix:signal unix::sigint 'ros::roseus-sigint-handler)
+
+;; state machine
+(exec-state-machine (hello-sm) '((description . "Hello World")(image . "")))
+(exit)
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.app b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.app
new file mode 100644
index 0000000000..ae8dc86a34
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.app
@@ -0,0 +1,5 @@
+display: Hello World
+platform: spot
+launch: jsk_spot_startup/hello_world.xml
+interface: jsk_spot_startup/hello_world.interface
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.interface b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.interface
new file mode 100644
index 0000000000..c27c9c296e
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.interface
@@ -0,0 +1,3 @@
+published_topics: {}
+subscribed_topics: {}
+
diff --git a/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.xml b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.xml
new file mode 100644
index 0000000000..15a5b233e7
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml b/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml
new file mode 100644
index 0000000000..a9e90c9bf1
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml
@@ -0,0 +1,4 @@
+username: "dummyusername"
+password: "dummypassword"
+hostname: "192.168.50.3"
+dock_id: 521
diff --git a/jsk_spot_robot/jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml b/jsk_spot_robot/jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml
new file mode 100644
index 0000000000..bfd30e14b0
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml
@@ -0,0 +1,17 @@
+axis_linear:
+ x: 1
+ y: 0
+scale_linear:
+ x: 1.0
+ y: 0.8
+scale_linear_turbo:
+ x: 1.5
+ y: 1.2
+axis_angular:
+ yaw: 2
+scale_angular:
+ yaw: 1.0
+scale_angular_turbo:
+ yaw: 2.0
+enable_button: 4
+enable_turbo_button: 10
diff --git a/jsk_spot_robot/jsk_spot_startup/config/head_teleop_twist_joy.yaml b/jsk_spot_robot/jsk_spot_startup/config/head_teleop_twist_joy.yaml
new file mode 100644
index 0000000000..364aaf2941
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/config/head_teleop_twist_joy.yaml
@@ -0,0 +1,13 @@
+axis_linear:
+ x: 0
+ y: 1
+ x_min: -0.5
+ x_max: 1.25
+ y_min: -0.5
+ y_max: 0.5
+axis_angular:
+ yaw: 2
+ yaw_min: -0.75
+ yaw_max: 0.75
+enable_button: 0
+enable_turbo_button: -1
diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz
new file mode 100644
index 0000000000..c732bf2766
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz
@@ -0,0 +1,505 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /DepthClouds1/FrontLeftDepthCloud1/Autocompute Value Bounds1
+ - /DepthClouds1/FrontRightDepthCloud1/Autocompute Value Bounds1
+ - /DepthClouds1/LeftDepthCloud1/Autocompute Value Bounds1
+ - /DepthClouds1/RightDepthCloud1/Autocompute Value Bounds1
+ - /DepthClouds1/RearDepthCloud1/Autocompute Value Bounds1
+ Splitter Ratio: 0.6832844614982605
+ Tree Height: 441
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ - /Current View1/Focal Point1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: FrontLeftDepthCloud
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ body:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_hip:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_rail:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ front_right_hip:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ insta360_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ jsk_head_mount_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ jsk_payload_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ rear_left_hip:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_rail:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ rear_right_hip:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 0.25
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 2.072983741760254
+ Min Value: 0.4096323251724243
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: AxisColor
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /spot/depth/frontleft/image
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: FrontLeftDepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 3.060708999633789
+ Min Value: 0.31034958362579346
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: AxisColor
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /spot/depth/frontright/image
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: FrontRightDepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 1.9569889307022095
+ Min Value: 0.40128064155578613
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: AxisColor
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /spot/depth/left/image
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: LeftDepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 2.684413433074951
+ Min Value: 0.4553413391113281
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: AxisColor
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /spot/depth/right/image
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: RightDepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 4.3172607421875
+ Min Value: 0.1980423927307129
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: AxisColor
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /spot/depth/back/image
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: RearDepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Name: DepthClouds
+ - Class: jsk_rviz_plugin/OverlayImage
+ Enabled: true
+ Name: PanoramaImage
+ Topic: /dual_fisheye_to_panorama/output
+ Value: true
+ alpha: 0.800000011920929
+ height: 128
+ keep aspect ratio: true
+ left: 0
+ overwrite alpha value: false
+ top: 0
+ transport hint: raw
+ width: 700
+ - Class: jsk_rviz_plugin/OverlayImage
+ Enabled: true
+ Name: PanoramaDetection
+ Topic: /spot_recognition/object_detection_image
+ Value: true
+ alpha: 0.800000011920929
+ height: 128
+ keep aspect ratio: true
+ left: 0
+ overwrite alpha value: false
+ top: 0
+ transport hint: raw
+ width: 700
+ - Class: jsk_rviz_plugin/TFTrajectory
+ Enabled: true
+ Name: BodyTrajectory
+ Value: true
+ color: 25; 255; 240
+ duration: 1000
+ frame: base_link
+ line_width: 0.009999999776482582
+ - Class: jsk_rviz_plugin/PieChart
+ Enabled: true
+ Name: SpotBattery
+ Topic: /spot/status/battery_percentage
+ Value: true
+ auto color change: false
+ background color: 0; 0; 0
+ backround alpha: 0
+ clockwise rotate direction: false
+ foreground alpha: 0.699999988079071
+ foreground alpha 2: 0.4000000059604645
+ foreground color: 25; 255; 240
+ left: 750
+ max color: 255; 0; 0
+ max value: 100
+ min value: 0
+ show caption: true
+ size: 128
+ text size: 10
+ top: 50
+ - Class: jsk_rviz_plugin/PieChart
+ Enabled: true
+ Name: LaptopBattery
+ Topic: /spot/status/laptop_battery_percentage
+ Value: true
+ auto color change: false
+ background color: 0; 0; 0
+ backround alpha: 0
+ clockwise rotate direction: false
+ foreground alpha: 0.699999988079071
+ foreground alpha 2: 0.4000000059604645
+ foreground color: 25; 255; 240
+ left: 900
+ max color: 255; 0; 0
+ max value: 100
+ min value: 0
+ show caption: true
+ size: 128
+ text size: 10
+ top: 50
+ - Class: jsk_rviz_plugin/BoundingBoxArray
+ Enabled: true
+ Name: DetectedObjects
+ Topic: /spot_recognition/bbox_array
+ Unreliable: false
+ Value: true
+ alpha: 0.800000011920929
+ color: 25; 255; 0
+ coloring: Label
+ line width: 0.05000000074505806
+ only edge: true
+ show coords: false
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: odom
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 8.482810974121094
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.3449980318546295
+ Target Frame: base_link
+ Value: Orbit (rviz)
+ Yaw: 4.94684362411499
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 1025
+ Hide Left Dock: true
+ Hide Right Dock: true
+ QMainWindow State: 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
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1920
+ X: 0
+ Y: 27
diff --git a/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml
new file mode 100644
index 0000000000..f5df2a4c8c
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml
@@ -0,0 +1,13 @@
+topics:
+#
+# priority 9: high <--> 1: low
+#
+- name : bt_joy
+ topic : bluetooth_teleop/cmd_vel
+ timeout : 0.5
+ priority: 9
+- name : head_joy
+ topic : joy_head/cmd_vel
+ timeout : 0.5
+ priority: 7
+locks: []
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch
new file mode 100644
index 0000000000..6da02babdd
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch
@@ -0,0 +1,184 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch
new file mode 100644
index 0000000000..d4f6ae4788
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch
new file mode 100644
index 0000000000..3e6195e067
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch
@@ -0,0 +1,224 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ approximate_sync: true
+ topics:
+ - /publish_trigger_mongodb_event
+ - /spot/camera/hand_color/image/compressed
+ - /spot/camera/frontleft/image/compressed
+ - /spot/camera/frontright/image/compressed
+ - /spot/camera/left/image/compressed
+ - /spot/camera/right/image/compressed
+ - /spot/camera/back/image/compressed
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ topics:
+ - /spot/ncb_provider/back_fisheye_image/class
+ - /spot/ncb_provider/frontleft_fisheye_image/class
+ - /spot/ncb_provider/frontright_fisheye_image/class
+ - /spot/ncb_provider/hand_color_image/class
+ - /spot/ncb_provider/left_fisheye_image/class
+ - /spot/ncb_provider/right_fisheye_image/class
+
+
+
+
+
+
+ topics:
+ - /spot/ncb_provider/bbox_array
+
+
+
+
+
+ topics:
+ - /ublox/fix
+ - /ublox/fix_velocity
+
+
+
+
+ periodic: false
+ threshold: 0.01
+
+
+
+ white_list:
+ type:
+ - control_msgs/FollowJointTrajectoryActionFeedback
+ - control_msgs/FollowJointTrajectoryActionGoal
+ - control_msgs/FollowJointTrajectoryActionResult
+ - pr2_common_action_msgs/TuckArmsActionFeedback
+ - pr2_common_action_msgs/TuckArmsActionGoal
+ - pr2_common_action_msgs/TuckArmsActionResult
+ - pr2_controllers_msgs/PointHeadActionFeedback
+ - pr2_controllers_msgs/PointHeadActionGoal
+ - pr2_controllers_msgs/PointHeadActionResult
+ - pr2_controllers_msgs/Pr2GripperCommandActionFeedback
+ - pr2_controllers_msgs/Pr2GripperCommandActionGoal
+ - pr2_controllers_msgs/Pr2GripperCommandActionResult
+ - sound_play/SoundRequestActionResult
+ - sound_play/SoundRequestActionGoal
+ - spot_msgs/PickObjectInImageActionGoal
+ - spot_msgs/PickObjectInImageActionResult
+
+
+
+ topics:
+ - /sound_play/goal
+ - /robotsound/goal
+ - /robotsound_jp/goal
+ - /sound_play
+ - /robotsound
+ - /robotsound_jp
+
+
+
+ topics:
+ - /faces_name
+ - /aws_auto_checkin_app/output/class
+ - /aws_detect_faces/attributes
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog_rgb_image.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog_rgb_image.launch
new file mode 100644
index 0000000000..6f0f1e4a28
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog_rgb_image.launch
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+ enable_monitor: false
+ vital_check: false
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/perception.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/perception.launch
new file mode 100644
index 0000000000..1772e298c1
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/perception.launch
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ image_sources: $(arg image_sources)
+ ml_service: $(arg ml_service)
+ ml_models: $(arg ml_models)
+ ml_confidence: $(arg ml_confidence)
+ use_gui: $(arg use_gui)
+
+
+
+
+
+
+
+
+ use_window: $(arg use_gui)
+ aws_credentials_path: $(arg aws_credential_config)
+ always_publish: false
+ attributes: ALL
+
+
+
+
+
+
+
+ use_window: $(arg use_gui)
+ env_path: $(arg aws_credential_config)
+ aws_credentials_path: $(arg aws_credential_config)
+ always_publish: false
+ approximate_sync: true
+ queue_size: 1
+ image_transport: compressed
+
+
+
+
+
+
+
+
+ font_path: $(find jsk_recognition_utils)/font_data/NotoSansJP-Regular.otf
+ use_classification_result: true
+ label_size: 16
+ approximate_sync: true
+ queue_size: 30
+ image_transport: compressed
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch
new file mode 100644
index 0000000000..6446752c6b
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch
@@ -0,0 +1,2 @@
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch
new file mode 100644
index 0000000000..16234a445b
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch
new file mode 100644
index 0000000000..d3e8eee15a
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch
@@ -0,0 +1,140 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch
new file mode 100644
index 0000000000..b28009e057
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch
@@ -0,0 +1,176 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch
new file mode 100644
index 0000000000..2d9fce1f16
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/sample_pick_object_in_image.launch b/jsk_spot_robot/jsk_spot_startup/launch/sample_pick_object_in_image.launch
new file mode 100644
index 0000000000..103827e8e4
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/sample_pick_object_in_image.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch b/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch
new file mode 100644
index 0000000000..a22020a9c7
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py
new file mode 100755
index 0000000000..e35b6ceae2
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from std_msgs.msg import Bool
+from std_srvs.srv import Trigger, TriggerRequest
+from spot_msgs.msg import BatteryStateArray
+from sensor_msgs.msg import BatteryState
+
+from sound_play.libsoundplay import SoundClient
+
+
+class SpotBatteryNotifier(object):
+
+ def __init__(self):
+
+ self._battery_spot = None
+ self._battery_temperature = 0
+ self._is_connected = False
+ self.last_warn_bat_temp_time = rospy.get_time()
+
+ self._sub_spot = rospy.Subscriber(
+ '/spot/status/battery_states',
+ BatteryStateArray,
+ self._cb_spot)
+ self._sub_connected = rospy.Subscriber(
+ '/spot/status/cable_connected',
+ Bool,
+ self._cb_connected)
+
+ sound_client = SoundClient(
+ blocking=False,
+ sound_action='/robotsound_jp',
+ sound_topic='/robotsound_jp'
+ )
+
+ threshold_warning_battery_temperature =\
+ float(rospy.get_param('~threshold_warning_battery_temperature', 45))
+
+ threshold_warning_spot = float(
+ rospy.get_param('~threshold_warning_spot', 20))
+
+ threshold_return_spot = float(
+ rospy.get_param('~threshold_return_spot', 15))
+
+ threshold_estop_spot = float(
+ rospy.get_param('~threshold_estop_spot', 5))
+
+ notification_duration = float(
+ rospy.get_param('~notification_duration', 300))
+
+ rate = rospy.Rate(0.1)
+ last_notified = rospy.Time.now()
+ while not rospy.is_shutdown():
+
+ rate.sleep()
+
+ if not self._is_connected:
+
+ if (rospy.Time.now() - last_notified).to_sec() > notification_duration:
+ sound_client.say('バッテリー残量、{}、パーセントです。'.format(
+ int(self._battery_spot)), volume=0.3)
+ last_notified = rospy.Time.now()
+
+ if (self._battery_spot is not None and self._battery_spot < threshold_estop_spot):
+ rospy.logerr('Battery is low. Estop.')
+ sound_client.say('バッテリー残量が少ないため、動作を停止します')
+ rospy.ServiceProxy('/spot/estop/gentle', Trigger)(TriggerRequest())
+ rospy.ServiceProxy('/spot/estop/hard', Trigger)(TriggerRequest())
+
+ elif (self._battery_spot is not None and self._battery_spot < threshold_return_spot):
+ rospy.logerr('Battery is low. Returning to home.')
+ sound_client.say('バッテリー残量が少ないため、ドックに戻ります')
+ self._call_go_back_home()
+
+ elif (self._battery_spot is not None and self._battery_spot < threshold_warning_spot):
+ rospy.logwarn('Battery is low. Spot: {}'.format(self._battery_spot))
+ sound_client.say('バッテリー残量が少ないです。本体が、{}、パーセントです。'.format(
+ int(self._battery_spot)))
+
+ if self._battery_temperature > threshold_warning_battery_temperature\
+ and (rospy.get_time() - self.last_warn_bat_temp_time) > 180:
+ rospy.logerr('Battery temperature is high. Battery temp:{:.2f} > threshold:{}'
+ .format(self._battery_temperature, threshold_warning_battery_temperature))
+ sound_client.say('バッテリー温度が高いです。')
+ self.last_warn_bat_temp_time = rospy.get_time()
+
+ def _cb_spot(self, msg):
+
+ self._battery_spot = msg.battery_states[0].charge_percentage
+ self._battery_temperature = max(msg.battery_states[0].temperatures)
+
+ def _cb_connected(self, msg):
+
+ self._is_connected = msg.data
+
+ def _call_estop_gentle(self):
+
+ try:
+ trigger = rospy.ServiceProxy('/spot/estop/gentle', Trigger)
+ ret = trigger()
+ rospy.loginfo('Call %s and received %s(%s)'%(trigger.resolved_name, ret.sucess, ret.message))
+ except rospy.ServiceException as e:
+ rospy.logerr('Service call failed: %s'%e)
+
+ def _call_estop_hard(self):
+
+ try:
+ trigger = rospy.ServiceProxy('/spot/estop/hard', Trigger)
+ ret = trigger()
+ rospy.loginfo('Call %s and received %s(%s)'%(trigger.resolved_name, ret.sucess, ret.message))
+ except rospy.ServiceException as e:
+ rospy.logerr('Service call failed: %s'%e)
+
+ def _call_go_back_home(self):
+
+ try:
+ trigger = rospy.ServiceProxy('/spot/dock_fixed_id', Trigger)
+ ret = trigger()
+ rospy.loginfo('Call %s and received %s(%s)'%(trigger.resolved_name, ret.sucess, ret.message))
+ except rospy.ServiceException as e:
+ rospy.logerr('Service call failed: %s'%e)
+
+
+def main():
+
+ rospy.init_node('battery_notifier')
+ battery_notifier = SpotBatteryNotifier()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py
new file mode 100755
index 0000000000..95be4670c5
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py
@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+
+import rospy
+from spot_msgs.msg import BatteryStateArray
+from std_msgs.msg import Float32
+
+
+class SpotBatteryVisualizer(object):
+
+ def __init__(self):
+
+ rospy.init_node('spot_battery_visualizer')
+
+ self._sub = rospy.Subscriber(
+ '/spot/status/battery_states', BatteryStateArray, self._cb)
+ self._pub = rospy.Publisher(
+ '/spot/status/battery_percentage', Float32, queue_size=1)
+
+ def _cb(self, msg):
+
+ pub_msg = Float32()
+ pub_msg.data = msg.battery_states[0].charge_percentage
+ self._pub.publish(pub_msg)
+
+
+def main():
+
+ battery_visualizer = SpotBatteryVisualizer()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/cable_connection_detector.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/cable_connection_detector.py
new file mode 100755
index 0000000000..606c5abf36
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/cable_connection_detector.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import rospy
+import spot_msgs.msg
+import sensor_msgs.msg
+from std_msgs.msg import Bool
+
+
+class CalbeConnectionDetector:
+
+ def __init__(self):
+
+ self.pub = rospy.Publisher(
+ '/spot/status/cable_connected', Bool, queue_size=1)
+
+ self.msg_battery_spot = None
+ self.msg_battery_laptop = None
+
+ self.sub_battery_spot = rospy.Subscriber(
+ '/spot/status/power_state',
+ spot_msgs.msg.PowerState,
+ self.callback_battery_spot)
+
+ rate = rospy.Rate(1)
+ while not rospy.is_shutdown():
+ rate.sleep()
+ if self.msg_battery_spot == None and self.msg_battery_laptop == None:
+ continue
+ self.pub.publish(Bool(self.check_connection()))
+
+ def check_connection(self):
+
+ if self.msg_battery_spot is not None and\
+ self.msg_battery_spot.shore_power_state == spot_msgs.msg.PowerState.STATE_ON_SHORE_POWER:
+ return True
+ else:
+ return False
+
+ def callback_battery_spot(self, msg):
+
+ self.msg_battery_spot = msg
+
+
+
+def main():
+
+ rospy.init_node('cable_connection_detector')
+ node = CalbeConnectionDetector()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/call_spot_dock_with_id.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/call_spot_dock_with_id.py
new file mode 100755
index 0000000000..c21e1fec7e
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/call_spot_dock_with_id.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from std_srvs.srv import Trigger, TriggerResponse
+from spot_msgs.srv import Dock
+
+class SpotDockWithID(object):
+
+ def __init__(self):
+
+ self._dock_id = int(rospy.get_param('~dock_id', 1))
+
+ self._server = rospy.Service('/spot/dock_fixed_id', Trigger, self._cb)
+ self._client = rospy.ServiceProxy('/spot/dock', Dock)
+
+ def _cb(self, req):
+
+ resp = self._client(self._dock_id)
+
+ rospy.loginfo("Call /spot/dock(dock_id={}) returns {}".format(self._dock_id, resp))
+ return TriggerResponse(success=resp.success, message=resp.message)
+
+def main():
+ rospy.init_node('spot_dock_with_id')
+ end_effector_to_joy = SpotDockWithID()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/end_effector_to_joy.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/end_effector_to_joy.py
new file mode 100755
index 0000000000..e62715641d
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/end_effector_to_joy.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+import tf2_ros
+from spot_msgs.msg import ManipulatorState
+from sensor_msgs.msg import Joy
+from std_srvs.srv import SetBool, SetBoolResponse
+
+# python3 does noth have cmp...
+def cmp(a, b):
+ return (a > b) - (a < b)
+
+class EndEffectorToJoy(object):
+
+ def __init__(self):
+ self._tf_buffer = tf2_ros.Buffer()
+ self._listener = tf2_ros.TransformListener(self._tf_buffer)
+ self._pub = rospy.Publisher(
+ '/joy_head/joy_raw',
+ Joy)
+ self._sub = None
+ self._enabled = rospy.Service('~set_enabled', SetBool, self._set_enabled)
+
+ self._deadzone_x = float(rospy.get_param('~deadzone_x', 0.01))
+ self._deadzone_y = float(rospy.get_param('~deadzone_y', 0.01))
+ self._maxrange_y = float(rospy.get_param('~maxrange_x', 0.2))
+ self._maxrange_x = float(rospy.get_param('~maxrange_y', 0.2))
+ self._offset_x = float(rospy.get_param('~offset_x', 0.0))
+ self._offset_y = float(rospy.get_param('~offset_y', 0.0))
+
+ def _set_enabled(self, req):
+ if req.data:
+ self._center_x = float(rospy.get_param('~center_x', 0.576))
+ self._center_y = float(rospy.get_param('~center_0', 0))
+ rospy.logwarn("Enabled end_effector_to_joy with center ({}, {})".format(self._center_x, self._center_y))
+ self._sub = rospy.Subscriber('/spot/status/manipulator_state', ManipulatorState, self._cb)
+ else:
+ rospy.logwarn("Disabled end_effector_to_joy with center")
+ if self._sub:
+ self._sub.unregister()
+ self._sub = None
+ return SetBoolResponse(True, 'Success')
+
+ def _cb(self, msg):
+ joy = Joy()
+ joy.header.frame_id = rospy.get_name()
+ joy.header.stamp = rospy.Time.now()
+ x = msg.estimated_end_effector_force_in_hand.x
+ y = msg.estimated_end_effector_force_in_hand.y
+ z = msg.estimated_end_effector_force_in_hand.z
+ try:
+ trans = self._tf_buffer.lookup_transform('body', 'hand', rospy.Time(0))
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+ rospy.logerr(e)
+ return None
+ x = trans.transform.translation.x - self._center_x
+ y = trans.transform.translation.y - self._center_y
+ rospy.loginfo_throttle(10, "end_effector_froce_in_hand = ({:4.1f}, {:4.1f}, {:4.1f}), stow_state = {}".format(x, y, z, msg.stow_state))
+ if msg.stow_state == ManipulatorState.STOWSTATE_STOWED:
+ x = 0
+ y = 0
+ else:
+ sign_x = cmp(x, 0)
+ sign_y = cmp(y, 0)
+ x = abs(x) + self._offset_x
+ y = abs(y) + self._offset_y
+ if x > self._maxrange_x: x = self._maxrange_x
+ if y > self._maxrange_y: y = self._maxrange_y
+ x = 0 if x < self._deadzone_x else x - self._deadzone_x
+ y = 0 if y < self._deadzone_y else y - self._deadzone_y
+ x = sign_x * x / (self._maxrange_x - self._deadzone_x)
+ y = sign_y * y / (self._maxrange_y - self._deadzone_y)
+ joy.axes = [x, y, 0, 0, 0, 0]
+ rospy.loginfo_throttle(10, " ({:4.1f}, {:4.1f}, {:4.1f})".format(x, y, 0))
+ joy.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] # dummy button data
+ self._pub.publish(joy)
+
+def main():
+ rospy.init_node('end_effector_to_joy')
+ end_effector_to_joy = EndEffectorToJoy()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/laptop_battery_visualizer.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/laptop_battery_visualizer.py
new file mode 100755
index 0000000000..2969614532
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/laptop_battery_visualizer.py
@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import BatteryState
+from std_msgs.msg import Float32
+
+
+class LaptopBatteryVisualizer(object):
+
+ def __init__(self):
+
+ rospy.init_node('spot_battery_visualizer')
+
+ self._sub = rospy.Subscriber('/laptop_charge', BatteryState, self._cb)
+ self._pub = rospy.Publisher(
+ '/spot/status/laptop_battery_percentage', Float32, queue_size=1)
+
+ def _cb(self, msg):
+
+ pub_msg = Float32()
+ pub_msg.data = msg.percentage
+ self._pub.publish(pub_msg)
+
+
+def main():
+
+ battery_visualizer = LaptopBatteryVisualizer()
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_pick_object_in_image.l b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_pick_object_in_image.l
new file mode 100755
index 0000000000..9aec1452cf
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_pick_object_in_image.l
@@ -0,0 +1,58 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "roseus")
+(ros::load-ros-manifest "geometry_msgs")
+(ros::load-ros-manifest "spot_msgs")
+
+(ros::roseus "sample_pick_object_in_image")
+(setq *c* (instance ros::simple-action-client :init
+ "/spot/pick_object_in_image" spot_msgs::PickObjectInImageAction))
+(send *c* :wait-for-server)
+
+;; call-trigger-service reqrueis roseus>1.7.5
+;; https://github.com/jsk-ros-pkg/jsk_roseus/pull/705
+;;
+(defun ros::call-trigger-service (srvname &key (wait nil) (timeout -1) (persistent nil))
+ "Call std_srv/Trigger service. Use (setq r (call-trigger-service \"/test\" t)) (and r (send r :success)) to check if it succeed. If r is nil, it fail to find service."
+ (let (r)
+ (when (ros::wait-for-service srvname (if wait timeout 0))
+ (setq r (ros::service-call srvname (instance std_srvs::TriggerRequest :init) persistent))
+ (ros::ros-debug "Call \"~A\" returns \"~A\"" srvname (send r :message))
+ (unless (send r :success) (ros::ros-warn "Call \"~A\" fails, it returns \"~A\"" srvname (send r :message)))
+ r)))
+
+(ros::subscribe "screenpoint" geometry_msgs::PointStamped
+ #'(lambda (msg)
+ (let ((goal (instance spot_msgs::PickObjectInImageActionGoal :init)))
+ (send goal :goal :image_source (ros::get-param "~image_source")) ;; required ex) "hand_color_image"
+ (send goal :goal :center (instance geometry_msgs::Point :init :x (send msg :point :x) :y (send msg :point :y)))
+ (send goal :goal :max_duration (instance ros::duration :init 15))
+ (send *c* :send-goal goal))))
+
+(do-until-key
+ (ros::rate 3)
+ (while (memq (send (send (*c* . ros::comm-state) :latest-goal-status) :status) ;; bugs in actionlib.l??
+ (list actionlib_msgs::GoalStatus::*pending*
+ actionlib_msgs::GoalStatus::*preempted*
+ actionlib_msgs::GoalStatus::*succeeded*
+ actionlib_msgs::GoalStatus::*aborted*))
+ (ros::ros-warn "waiting to select target object in image_view ...")
+ (ros::sleep)
+ (ros::spin-once))
+ (ros::ros-warn ";; wait-for-result")
+ (send *c* :wait-for-result)
+ (if (send (send *c* :get-result) :success)
+ ;; if robot grab object, unstow then open gripper
+ (progn
+ (ros::call-trigger-service "/spot/unstow_arm")
+ (ros::call-trigger-service "/spot/gripper_open"))
+ (progn
+ ;; if robot failed to grab object, just open gripper
+ (ros::call-trigger-service "/spot/gripper_open")))
+ (ros::call-trigger-service "/spot/stow_arm")
+ )
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py
new file mode 100755
index 0000000000..4ef0566282
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import rospy
+import actionlib
+from geometry_msgs.msg import Point, PointStamped
+from spot_msgs.msg import WalkToObjectInImageAction, WalkToObjectInImageGoal
+
+def cb(msg):
+ rospy.loginfo('screenpoint callback x={}, y={}, distance={}'.format(msg.point.x, msg.point.y, distance))
+ timeout = 15
+ goal = WalkToObjectInImageGoal(
+ image_source = image_source,
+ center = msg.point,
+ distance = distance,
+ max_duration = rospy.Duration(timeout)
+ )
+ client.send_goal(goal)
+ client.wait_for_result(timeout=rospy.Duration(timeout+5))
+ if client.get_result().success:
+ rospy.loginfo("succeeded")
+ else:
+ rospy.logerr("failed to walkt ot object")
+
+if __name__ == '__main__':
+ try:
+ rospy.init_node('sample_walk_to_object_in_image')
+ image_source = rospy.get_param('~image_source', 'hand_color')
+ distance = rospy.get_param('~distance', 0.5)
+ # subscribe point in screen
+ rospy.loginfo('start subscribe screenpoint at {}'.format(image_source))
+ rospy.Subscriber('screenpoint', PointStamped, cb)
+ # action client for walk to point API
+ rospy.loginfo('conncting to walk_to_point_in_image action')
+ client = actionlib.SimpleActionClient('/spot/walk_to_object_in_image', WalkToObjectInImageAction)
+ client.wait_for_server()
+ #
+ rospy.loginfo('start main loop')
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ ropsy.logerr("program interrupted before completion")
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/start-spot.l b/jsk_spot_robot/jsk_spot_startup/node_scripts/start-spot.l
new file mode 100755
index 0000000000..204ccfaef5
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/start-spot.l
@@ -0,0 +1,23 @@
+#!/usr/bin/env roseus
+
+(load "package://spoteus/spot-interface.l")
+(unless (boundp '*spot*) (spot) (send *spot* :reset-pose))
+(unless (ros::ok) (ros::roseus "spot_eus_interface"))
+(unless (boundp '*ri*) (setq *ri* (instance spot-interface :init)))
+(send *ri* :claim)
+
+(setq ip "unknown")
+(setq host (unix::gethostbyname "spotcore.jsk.imi.i.u-tokyo.ac.jp"))
+(if (listp host)
+ (setq ip (format nil "~A . ~A . ~A . ~A" (elt (car host) 0) (elt (car host) 1) (elt (car host) 2) (elt (car host) 3))))
+
+(setq info-txt (format nil "Hello, my name is ~A, my internet address is ~A, my owner is ~A and my dock id is ~A"
+ (ros::get-param "/robot/name")
+ ip
+ (unix::getenv "USER")
+ (round (ros::get-param "/spot/spot_ros/dock_id"))))
+(ros::ros-info info-txt)
+(send *ri* :speak info-txt :wait t)
+
+
+(ros::exit)
diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/static_tf_republisher.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/static_tf_republisher.py
new file mode 100755
index 0000000000..ff5363d6c0
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/static_tf_republisher.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+
+import sys
+import argparse
+
+import rospy
+import rosbag
+import tf2_ros
+
+from geometry_msgs.msg import TransformStamped
+from tf2_msgs.msg import TFMessage
+
+
+def main():
+
+ rospy.init_node('static_tf_republisher')
+
+ topicname = '/tf_static'
+
+ myargv = rospy.myargv(argv=sys.argv)
+ if len(myargv) > 1:
+ bagfilename = myargv[1]
+ else:
+ bagfilename = rospy.get_param("~file")
+
+ broadcaster = tf2_ros.StaticTransformBroadcaster()
+
+ list_messages = []
+ transforms = []
+ with rosbag.Bag(bagfilename, 'r') as inputbag:
+ for topic, msg, t in inputbag.read_messages('/tf_static'):
+ transforms.extend(msg.transforms)
+
+ rospy.loginfo(
+ 'republish /tf_static with {} TransformStamped messages'.format(len(transforms)))
+
+ broadcaster.sendTransform(transforms)
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml
new file mode 100644
index 0000000000..fefef06d3d
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_startup/package.xml
@@ -0,0 +1,55 @@
+
+
+ jsk_spot_startup
+ 1.1.0
+ The jsk_spot_startup package
+
+ Kei Okada
+ Koki Shinjo
+ Koki Shinjo
+
+ BSD
+
+ catkin
+
+ roslaunch
+ roslint
+
+ app_manager
+ app_scheduler
+ aques_talk
+ audio_capture
+ cmd_vel_smoother
+ compressed_image_transport
+ image_transport
+ interactive_marker_twist_server
+ jsk_perception
+ jsk_robot_startup
+ jsk_tools
+ julius_ros
+ laptop_battery_monitor
+ robot_state_publisher
+ ros_speech_recognition
+ rosbag
+ roseus
+ roseus_smach
+ roslaunch
+ rosserial_python
+ rviz
+ rwt_app_chooser
+ sound_play
+ spot_description
+ spot_driver
+ spoteus
+ topic_tools
+ twist_mux
+ voice_text
+ xacro
+ xterm
+
+ rostest
+
+
+
+
+
diff --git a/jsk_spot_robot/jsk_spot_user.rosinstall b/jsk_spot_robot/jsk_spot_user.rosinstall
new file mode 100644
index 0000000000..1fb7336910
--- /dev/null
+++ b/jsk_spot_robot/jsk_spot_user.rosinstall
@@ -0,0 +1,16 @@
+# spot-ros is required for spot
+## use https://github.com/cst0/spot_ros/pull/4 for Arm and gripper
+- git:
+ local-name: spot-ros
+ uri: https://github.com/k-okada/spot_ros-arm.git
+ version: arm
+## to support custom limb name, we need https://github.com/jsk-ros-pkg/jsk_model_tools/pull/249
+- git:
+ local-name: jsk_model_tools
+ uri: https://github.com/k-okada/jsk_model_tools.git
+ version: custom_limb
+# wait for https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/443 (add volume key arg for robot-interface :speak) to be released
+- git:
+ local-name: jsk_pr2eus
+ uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
+ version: master
diff --git a/jsk_spot_robot/requirements.txt b/jsk_spot_robot/requirements.txt
new file mode 100644
index 0000000000..6306e82a90
--- /dev/null
+++ b/jsk_spot_robot/requirements.txt
@@ -0,0 +1,6 @@
+protobuf == 3.19.4 # protobuf > 3.20 requres Python3.7 (https://github.com/protocolbuffers/protobuf/pull/9480)
+bosdyn-client == 3.2.0
+bosdyn-mission == 3.2.0
+bosdyn-api == 3.2.0
+bosdyn-core == 3.2.0
+bosdyn-choreography-client == 3.2.0
diff --git a/jsk_spot_robot/spoteus/.gitignore b/jsk_spot_robot/spoteus/.gitignore
new file mode 100644
index 0000000000..a1fe640a20
--- /dev/null
+++ b/jsk_spot_robot/spoteus/.gitignore
@@ -0,0 +1,3 @@
+spot.l
+spot.urdf
+spot.dae
diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt
new file mode 100644
index 0000000000..23b23f0bcb
--- /dev/null
+++ b/jsk_spot_robot/spoteus/CMakeLists.txt
@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spoteus)
+
+## 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
+ euscollada
+)
+find_package(spot_description) # Just in case when description is not released. Avoid compile failing
+
+catkin_package()
+
+if(NOT spot_description_FOUND)
+ message(WARNING "spot_description is not found, so skip generating spot.l")
+ message(WARNING "Install spot_description from https://github.com/clearpathrobotics/spot_ros")
+ return()
+endif()
+
+
+###########
+## Build ##
+###########
+if(EXISTS ${spot_description_SOURCE_PREFIX}/urdf)
+ set(_spot_urdf ${spot_description_SOURCE_PREFIX}/urdf)
+else()
+ set(_spot_urdf ${spot_description_PREFIX}/share/spot_description/urdf)
+endif()
+set(_collada2eus ${euscollada_PREFIX}/lib/euscollada/collada2eus)
+
+message("spot_urdf: ${_spot_urdf}")
+message("urdf_to_collada: ${_urdf_to_collada}")
+message("collada2eus: ${_collada2eus}")
+
+add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.urdf
+ COMMAND xacro ${_spot_urdf}/spot.urdf.xacro > ${PROJECT_SOURCE_DIR}/spot.urdf
+ DEPENDS ${_spot_urdf}/spot.urdf.xacro)
+
+add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.l
+ COMMAND echo "${_collada2eus} spot.urdf spot.l"
+ COMMAND ${_collada2eus} ${PROJECT_SOURCE_DIR}/spot.urdf ${PROJECT_SOURCE_DIR}/spot.yaml ${PROJECT_SOURCE_DIR}/spot.l
+ DEPENDS ${PROJECT_SOURCE_DIR}/spot.urdf ${PROJECT_SOURCE_DIR}/spot.yaml ${_collada2eus})
+add_custom_target(compile_spot ALL DEPENDS ${PROJECT_SOURCE_DIR}/spot.l)
+
+
+install(DIRECTORY demo test
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS)
+
+file(GLOB LISP_FILES *.l)
+install(FILES ${LISP_FILES} spot.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(catkin REQUIRED COMPONENTS rostest)
+ add_rostest(test/test-spot.test)
+endif()
diff --git a/jsk_spot_robot/spoteus/demo/sample_basics.l b/jsk_spot_robot/spoteus/demo/sample_basics.l
new file mode 100755
index 0000000000..fcd8910854
--- /dev/null
+++ b/jsk_spot_robot/spoteus/demo/sample_basics.l
@@ -0,0 +1,80 @@
+#!/usr/bin/env roseus
+
+(load "package://spoteus/spot-interface.l")
+
+(spot-init nil) ;; do not create-viewer
+
+;; power on
+(send *ri* :claim)
+(send *ri* :power-on)
+
+(send *ri* :stand)
+(ros::duration-sleep 1)
+
+;; states
+(format t "(send *ri* :state :metrics) : ~A~%" (send *ri* :state :metrics))
+(format t "(send *ri* :state :leases) : ~A~%" (send *ri* :state :leases))
+(format t "(send *ri* :state :estop) : ~A~%" (send *ri* :state :estop))
+(format t "(send *ri* :state :wifi) : ~A~%" (send *ri* :state :wifi))
+(format t "(send *ri* :state :power-state) : ~A~%" (send *ri* :state :power-state))
+(format t "(send *ri* :state :battery-faults) : ~A~%" (send *ri* :state :battery-faults))
+(format t "(send *ri* :state :system-faults) : ~A~%" (send *ri* :state :system-faults))
+(format t "(send *ri* :state :feedback) : ~A~%" (send *ri* :state :feedback))
+
+;; basic services
+(format t "Testing Basic Functionality~%")
+(send *ri* :sit)
+(ros::duration-sleep 3)
+
+(send *ri* :power-off)
+(ros::duration-sleep 3)
+
+(send *ri* :release)
+(ros::duration-sleep 3)
+
+(send *ri* :claim)
+(ros::duration-sleep 3)
+
+(send *ri* :power-on)
+(ros::duration-sleep 3)
+
+(send *ri* :stand)
+(ros::duration-sleep 5)
+
+;; cmd-vel
+(format t "Testing cmd-vel~%")
+(setq end-time (ros::time+ (ros::time 10) (ros::time-now)))
+(while (ros::time< (ros::time-now) end-time)
+ (send *ri* :go-velocity 0 0 0.8)
+ )
+
+(setq end-time (ros::time+ (ros::time 10) (ros::time-now)))
+(while (ros::time< (ros::time-now) end-time)
+ (send *ri* :go-velocity 0 0 -0.8)
+ )
+
+;; body-pose
+(format t "Testing body-pose~%")
+(send *ri* :body-pose '(0 0.2 0))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(0 -0.2 0))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(0 0 0.2))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(0 0 -0.2))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(0.1 0 0))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(-0.1 0 0))
+(ros::duration-sleep 1)
+(send *ri* :body-pose '(0 0 0))
+(ros::duration-sleep 1)
+
+;; go pos
+(format t "Testing go pos")
+(send *ri* :go-pos 1 0)
+(send *ri* :go-pos 0 0 90)
+(send *ri* :go-pos 0 1 -90)
+
+;;
+(exit 1)
diff --git a/jsk_spot_robot/spoteus/demo/sample_go_to_spot.l b/jsk_spot_robot/spoteus/demo/sample_go_to_spot.l
new file mode 100755
index 0000000000..e623800d64
--- /dev/null
+++ b/jsk_spot_robot/spoteus/demo/sample_go_to_spot.l
@@ -0,0 +1,19 @@
+#!/usr/bin/env roseus
+
+(load "package://spoteus/spot-interface.l")
+
+(spot-init)
+(send *ri* :stand)
+;; if dock is used
+;; (setq dock-id (ros::get-param "~dock_id" 520))
+;; (send *ri* :undock)
+
+;;(send *ri* :go-to-spot "eng2_7FElevator")
+(send *ri* :go-to-spot "eng2_2FElevator")
+(send *ri* :speak-jp "こんにちは" :wait t)
+(send *ri* :speak-en "hello" :wait t)
+(send *ri* :go-to-spot "eng2_73B2")
+
+(send *ri* :sit)
+(send *ri* :stand)
+;; (send *ri* :dock dock-id)
diff --git a/jsk_spot_robot/spoteus/demo/sample_navigate_to.l b/jsk_spot_robot/spoteus/demo/sample_navigate_to.l
new file mode 100755
index 0000000000..81a9b58e1e
--- /dev/null
+++ b/jsk_spot_robot/spoteus/demo/sample_navigate_to.l
@@ -0,0 +1,38 @@
+#!/usr/bin/env roseus
+
+;;
+;; This script is for demonstration of GraphNav interface with euslisp.
+;; By default, it is assumed that Spot is at the entrance of 73B2 and headed to the AR marker on the door.
+;;
+
+(load "package://spoteus/spot-interface.l")
+
+(spot-init nil) ;; do not create-viewer
+
+(setq *path* (ros::get-param "~path" (format nil "~A/autowalk/eng2_73B2_to_81C1.walk" (ros::rospack-find "spot_autowalk_data"))))
+(setq *init-waypoint* (floor (ros::get-param "~init_waypoint" 0)))
+(setq *upload* (ros::get-param "~upload" t))
+
+;; Upload graphnav files to the robot.
+(if *upload* (send *ri* :upload-path *path*))
+
+;; Localize the robot in the map
+(ros::ros-info "initialize position with waypoint of ~A" *init-waypoint*)
+(send *ri* :initial-localization-waypoint *init-waypoint*)
+;; you can also use following command to initialize localization if you start from 73B2
+;; (send *ri* :initial-localization-fiducial)
+;; the difference is, :initial-localization-waypoint can initialize with any waypoint, meaning you can start from arbitary wapoint, e.g., -1 -> 0, 1 -> -1
+
+(ros::ros-info "ready go to 81C1?")
+(if (y-or-n-p) t (exit))
+
+;; go to 81C1
+(send *ri* :navigate-to -1)
+
+(ros::ros-info "ready go back to 73B2?")
+(if (y-or-n-p) t (exit))
+
+;; go back to 73B2
+(send *ri* :navigate-to 0)
+
+(send *ri* :sit)
diff --git a/jsk_spot_robot/spoteus/demo/sample_visualization.l b/jsk_spot_robot/spoteus/demo/sample_visualization.l
new file mode 100755
index 0000000000..1d5e539adc
--- /dev/null
+++ b/jsk_spot_robot/spoteus/demo/sample_visualization.l
@@ -0,0 +1,23 @@
+#!/usr/bin/env roseus
+
+;;
+;; This script is for demonstration of visualization of spot's angle vector and global pose with irtviewer
+;;
+
+(load "package://spoteus/spot-interface.l")
+
+(spot-init t) ;; create-viewer
+(send *irtviewer* :draw-floor t)
+
+;; if not power-on, power on
+;; if not stand, stand
+
+(do-until-key
+ ;; update body posture
+ (send *spot* :angle-vector (send *ri* :state :angle-vector))
+ ;;
+ (send *spot* :move-to (send *ri* :state :worldcoords) :world)
+ (send *irtviewer* :look1 (send *spot* :worldpos))
+ (send *irtviewer* :draw-objects)
+ (x::window-main-one)
+ )
diff --git a/jsk_spot_robot/spoteus/package.xml b/jsk_spot_robot/spoteus/package.xml
new file mode 100644
index 0000000000..258253ca59
--- /dev/null
+++ b/jsk_spot_robot/spoteus/package.xml
@@ -0,0 +1,29 @@
+
+
+ spoteus
+ 1.1.0
+ The spoteus package
+
+ Kei Okada
+ Yoshiki Obinata
+ BSD
+ Kei Okada
+ Yoshiki Obinata
+
+ catkin
+
+ collada_urdf
+ euscollada
+ rostest
+ roseus
+ pr2eus
+ spot_description
+
+ pr2eus
+ roseus
+ spot_msgs
+ std_srvs
+
+
+
+
diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l
new file mode 100644
index 0000000000..c384aa122e
--- /dev/null
+++ b/jsk_spot_robot/spoteus/spot-interface.l
@@ -0,0 +1,553 @@
+(ros::roseus "spot")
+
+(require "package://spoteus/spot-utils.l")
+(require "package://pr2eus/robot-interface.l")
+
+(ros::roseus-add-srvs "std_srvs")
+(ros::roseus-add-msgs "spot_msgs")
+(ros::roseus-add-srvs "spot_msgs")
+(ros::roseus-add-srvs "control_msgs")
+
+(defun call-trigger-service (srvname &key (wait nil))
+ "Call std_srv/Trigger service"
+ (let (r)
+ (when (send *ri* :simulation-modep)
+ (ros::ros-warn "call-trigger-service (~A)" srvname)
+ (return-from call-trigger-service t))
+ (if wait (ros::wait-for-service srvname))
+ (setq r (ros::service-call srvname (instance std_srvs::TriggerRequest :init)))
+ (ros::ros-debug "Call \"~A\" returns \"~A\"" srvname (send r :message))
+ (send r :success)))
+
+(defun call-set-bool-service (srvname data &key (wait nil))
+ "Call std_srv/Trigger service"
+ (let (r)
+ (when (send *ri* :simulation-modep)
+ (ros::ros-warn "call-set-bool-service (~A) ~A" srvname data)
+ (return-from call-set-bool-service t))
+ (if wait (ros::wait-for-service srvname))
+ (setq r (ros::service-call srvname (instance std_srvs::SetBoolRequest :init :data data)))
+ (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message))
+ (send r :success)))
+
+(defun call-set-locomotion-service (srvname locomotion_hint &key (wait nil))
+ "Call spot_msgs/SetLocomotion service"
+ (let (r)
+ (when (send *ri* :simulation-modep)
+ (ros::ros-warn "call-set-locomotion-service (~A) ~A" srvname locomotion_hint)
+ (return-from call-set-locomotion-service t))
+ (if wait (ros::wait-for-service srvname))
+ (setq r (ros::service-call srvname (instance spot_msgs::SetLocomotionRequest :init :locomotion_mode locomotion_hint)))
+ (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message))
+ (send r :success)))
+
+(defun call-dock-service (srvname dock_id &key (wait nil))
+ "Call spot_msgs/Dock service"
+ (let (r)
+ (when (send *ri* :simulation-modep)
+ (ros::ros-warn "call-dock-service (~A) ~A" srvname dock_id)
+ (return-from call-dock-service t))
+ (if wait (ros::wait-for-service srvname))
+ (setq r (ros::service-call srvname (instance spot_msgs::DockRequest :init :dock_id dock_id)))
+ (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message))
+ (send r :success)))
+
+(defclass spot-interface
+ :super robot-move-base-interface
+ :slots (trajectory-cmd-action pick-object-in-image-client pick-object-in-image-feedback-msg)
+ )
+
+(defmethod spot-interface
+ (:init
+ (&rest args &key (trajectory-cmd-action-name "/spot/trajectory") &allow-other-keys)
+ (prog1
+ (send-super* :init :robot spot-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args)
+ ;; check if spot_ros/driver.launch started
+ (unless (or (send self :simulation-modep) (ros::wait-for-service "/spot/claim" 5))
+ (ros::ros-error "could not communicate with robot, may be forget to roslaunch spot_driver driver.launch, or did not power on the robot"))
+ ;; http://www.clearpathrobotics.com/assets/guides/melodic/spot-ros/ros_usage.html#view-the-robot
+ ;; spot_msgs version 0.0.0
+ (ros::subscribe "/spot/status/metrics" spot_msgs::Metrics #'send self :spot-status-metrics-callback :groupname groupname)
+ (ros::subscribe "/spot/status/leases" spot_msgs::LeaseArray #'send self :spot-status-leases-callback :groupname groupname)
+ (ros::subscribe "/spot/status/feet" spot_msgs::FootStateArray #'send self :spot-status-feet-callback :groupname groupname)
+ (ros::subscribe "/spot/status/estop" spot_msgs::EStopStateArray #'send self :spot-status-estop-callback :groupname groupname)
+ (ros::subscribe "/spot/status/wifi" spot_msgs::WiFiState #'send self :spot-status-wifi-callback :groupname groupname)
+ (ros::subscribe "/spot/status/manipulator_state" spot_msgs::ManipulatorState #'send self :spot-status-manipulator-state-callback :groupname groupname)
+ (ros::subscribe "/spot/status/power_state" spot_msgs::PowerState #'send self :spot-status-power-state-callback :groupname groupname)
+ (ros::subscribe "/spot/status/battery_states" spot_msgs::BatteryStateArray #'send self :spot-status-battery-states-callback :groupname groupname)
+ (ros::subscribe "/spot/status/behavior_faults" spot_msgs::BehaviorFaultState #'send self :spot-status-behavior-faults-callback :groupname groupname)
+ (ros::subscribe "/spot/status/system_faults" spot_msgs::SystemFaultState #'send self :spot-status-system-faults-callback :groupname groupname)
+ (ros::subscribe "/spot/status/feedback" spot_msgs::Feedback #'send self :spot-feedback-callback :groupname groupname)
+ (setq trajectory-cmd-action (instance ros::simple-action-client :init
+ trajectory-cmd-action-name spot_msgs::TrajectoryAction
+ :groupname groupname))
+ (setq pick-object-in-image-client (instance ros::simple-action-client :init
+ "/spot/pick_object_in_image" spot_msgs::PickObjectInImageAction))
+ ))
+ ;; (:default-controller ()
+ ;; (append
+ ;; (send self :arm-controller)
+ ;; (send self :dummy-controller)
+ ;; ))
+ (:default-controller () (send self :arm-controller))
+ (:arm-controller ()
+ (list
+ (list
+ (cons :controller-action "spot/arm_controller/follow_joint_trajectory")
+ (cons :controller-state "spot/arm_controller/follow_joint_trajectory/state")
+ (cons :action-type control_msgs::FollowJointTrajectoryAction)
+ (cons :joint-names (send-all (send robot :arm :joint-list) :name))
+ )))
+ (:set-impedance-param
+ (&key (linear-stiffness #f(500 500 500))
+ (rotational-stiffness #f(60 60 60))
+ (linear-damping #f(2.5 2.5 2.5))
+ (rotational-damping #f(1.0 1.0 1.0)))
+ "X is up, Z is forward"
+ (let (m r)
+ (labels
+ ((v2v (v) (instance geometry_msgs::Vector3 :init :x (elt v 0) :y (elt v 1) :z (elt v 2))))
+ (setq m (instance spot_msgs::SetArmImpedanceParamsRequest :init
+ :linear_stiffness (v2v linear-stiffness)
+ :rotational_stiffness (v2v rotational-stiffness)
+ :linear_damping (v2v linear-damping)
+ :rotational_damping (v2v rotational-damping)))
+ (setq r (ros::service-call "/spot/arm_impedance_parameters" m))
+ (send r :success))))
+ (:unstow-arm () (call-trigger-service "/spot/unstow_arm"))
+ (:stow-arm () (call-trigger-service "/spot/stow_arm"))
+ (:gripper-open () (call-trigger-service "/spot/gripper_open"))
+ (:gripper-close () (call-trigger-service "/spot/gripper_close"))
+ (:start-grasp
+ ()
+ (let (p s)
+ (send self :gripper-close)
+ (setq p (send *ri* :state :manipulator-state-gripper-open-percentage)
+ s (send *ri* :state :manipulator-state-is-gripper-holding-item))
+ (ros::ros-info "gripper-open-percentage ~A" p)
+ (ros::ros-info "manipulator-state-is-gripper-holding-item ~A" s)
+ s))
+ (:stop-grasp
+ ()
+ (let (p s)
+ (send self :gripper-open)
+ (setq p (send *ri* :state :manipulator-state-gripper-open-percentage)
+ s (send *ri* :state :manipulator-state-is-gripper-holding-item))
+ (ros::ros-info "gripper-open-percentage ~A" p)
+ (ros::ros-info "manipulator-state-is-gripper-holding-item ~A" s)
+ s))
+ (:pick-object-in-image
+ (image-source x y &optional (max-duration 15))
+ (let ((goal (instance spot_msgs::PickObjectInImageActionGoal :init)))
+ (print (list image-source x y max-duration))
+ (send goal :goal :image_source image-source)
+ (send goal :goal :center (instance geometry_msgs::Point :init :x x :y y))
+ (send goal :goal :max_duration (instance ros::duration :init max-duration))
+ (send pick-object-in-image-client :send-goal goal :feedback-cb `(lambda (msg) (send ,self :pick-object-in-image-feedback-cb msg)))
+ ))
+ (:pick-object-in-image-feedback-cb
+ (msg)
+ (setq pick-object-in-image-feedback-msg msg))
+ (:pick-object-in-image-feedback-msg
+ (&rest args)
+ (forward-message-to pick-object-in-image-feedback-msg args))
+ (:pick-object-in-image-wait-for-result
+ ()
+ (send pick-object-in-image-client :wait-for-result)
+ (send pick-object-in-image-client :get-result))
+ (:pick-object-in-image-get-state
+ ()
+ (send pick-object-in-image-client :get-state))
+ ;; (:dummy-controller ()
+ ;; (list
+ ;; (list
+ ;; (cons :controller-action "spot/dummy_controller/follow_joint_trajectory")
+ ;; (cons :controller-state "spot/dummy_controller/follow_joint_trajectory/state")
+ ;; (cons :action-type control_msgs::FollowJointTrajectoryAction)
+ ;; (cons :joint-names (send-all (set-difference (send robot :joint-list) (send robot :arm :joint-list)) :name))
+ ;; )))
+ (:spot-status-metrics-callback
+ (msg)
+ (send self :set-robot-state1 :metrics-distance (send msg :distance))
+ (send self :set-robot-state1 :metrics-gaint-cycles (send msg :gait_cycles))
+ (send self :set-robot-state1 :metrics-time-moving (send (send msg :time_moving) :to-sec))
+ (send self :set-robot-state1 :metrics-electric-power (send (send msg :electric_power) :to-sec)))
+ (:spot-status-leases-callback
+ (msg)
+ (dolist (resource (send msg :resources))
+ (let ((r (string-upcase (send resource :resource))))
+ (send self :set-robot-state1 (intern (format nil "LEASES-~A-RESOURCE" r) *keyword-package*)
+ (send resource :lease :resource))
+ (send self :set-robot-state1 (intern (format nil "LEASES-~A-EPOCH" r) *keyword-package*)
+ (send resource :lease :epoch))
+ (send self :set-robot-state1 (intern (format nil "LEASES-~A-SEQUENCE" r) *keyword-package*)
+ (send resource :lease :sequence))
+ (send self :set-robot-state1 (intern (format nil "LEASES-~A-CLIENT-NAME" r) *keyword-package*)
+ (send resource :lease_owner :client_name))
+ (send self :set-robot-state1 (intern (format nil "LEASES-~A-USER-NAME" r) *keyword-package*)
+ (send resource :lease_owner :user_name)))))
+ (:spot-status-feet-callback
+ (msg)
+ (send self :set-robot-state1 :feet
+ (mapcar #'(lambda (state)
+ (list (cons :foot-position-rt-body (ros::tf-point->pos (send state :foot_position_rt_body)))
+ (cons :contact (case (send state :contact) (0 'unknown) (1 'made) (2 'lost))))) (send msg :states))))
+ (:spot-status-estop-callback
+ (msg)
+ (dolist (state (send msg :estop_states))
+ (let ((s (string-upcase (substitute #\- #\_ (send state :name)))))
+ (send self :set-robot-state1 (intern (format nil "ESTOP-~A-NAME" s) *keyword-package*)
+ (send state :name))
+ (send self :set-robot-state1 (intern (format nil "ESTOP-~A-TYPE" s) *keyword-package*)
+ (case (send state :type) (0 'unknown) (1 'hardware) (2 'software)))
+ (send self :set-robot-state1 (intern (format nil "ESTOP-~A-STATE" s) *keyword-package*)
+ (case (send state :state) (0 'unknown) (1 'estopped) (2 'not-estopped)))
+ (send self :set-robot-state1 (intern (format nil "ESTOP-~A-STATE-DESCRIPTION" s) *keyword-package*)
+ (send state :state_description)))))
+ (:spot-status-wifi-callback
+ (msg)
+ (send self :set-robot-state1 :wifi-current-mode
+ (case (send msg :current_mode)
+ (0 'unknown) (1 'access-point) (2 'client)))
+ (send self :set-robot-state1 :wifi-essid (send msg :essid)))
+ (:spot-status-manipulator-state-callback
+ (msg)
+ (send self :set-robot-state1 :manipulator-state-gripper-open-percentage (send msg :gripper_open_percentage))
+ (send self :set-robot-state1 :manipulator-state-is-gripper-holding-item (send msg :is_gripper_holding_item))
+ (send self :set-robot-state1 :manipulator-state-estimated-end-effector-force-in-hand
+ (let ((m (send msg :estimated_end_effector_force_in_hand)))
+ (float-vector (send m :x) (send m :y) (send m :z))))
+ (send self :set-robot-state1 :manipulator-state-stow-state
+ (case (send msg :stow_state) (0 'unknown) (1 'stowed) (2 'deployed)))
+ (send self :set-robot-state1 :manipulator-state-velocity-of-hand-in-vision
+ (let ((m (send msg :velocity_of_hand_in_vision)))
+ (float-vector (send m :linear :x) (send m :linear :y) (send m :linear :z)
+ (send m :angular :x) (send m :angular :y) (send m :angular :z))))
+ (send self :set-robot-state1 :manipulator-state-velocity-of-hand-in-odom
+ (let ((m (send msg :velocity_of_hand_in_odom)))
+ (float-vector (send m :linear :x) (send m :linear :y) (send m :linear :z)
+ (send m :angular :x) (send m :angular :y) (send m :angular :z))))
+ (send self :set-robot-state1 :manipulator-carry-state
+ (case (send msg :carry_state) (0 'unknown) (1 'not-carriable) (2 'carriable) (3 'carriable-and-stowable)))
+ )
+ (:spot-status-power-state-callback
+ (msg)
+ (send self :set-robot-state1 :power-state-motor-power-state
+ (case (send msg :motor_power_state)
+ (0 'unknown) (1 'off) (2 'on) (3 'powering-on) (4 'powering-off) (5 'error)))
+ (send self :set-robot-state1 :power-state-shore-power-state
+ (case (send msg :shore_power_state)
+ (0 'unknown-shore-power) (1 'on-shore-power) (2 'off-shore-power)))
+ (send self :set-robot-state1 :power-state-locomotion-charge-percentage (send msg :locomotion_charge_percentage))
+ (send self :set-robot-state1 :power-state-locomotion-estimated-runtime (send (send msg :locomotion_estimated_runtime) :to-sec)))
+ (:spot-status-battery-states-callback
+ (msg)
+ (dolist (state (send msg :battery_states))
+ (let ((i 1))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-IDENTIFIER" i) *keyword-package*)
+ (send state :identifier))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-CHARGE-PERCENTAGE" i) *keyword-package*)
+ (send state :charge_percentage))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-ESTIMATED-RUNTIME" i) *keyword-package*)
+ (send (send state :estimated_runtime) :to-sec))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-CURRENT" i) *keyword-package*)
+ (send state :current))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-VOLTAGE" i) *keyword-package*)
+ (send state :voltage))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-TEMPERATURES" i) *keyword-package*)
+ (send state :temperatures))
+ (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-STATUS" i) *keyword-package*)
+ (case (send state :status) (0 'unknown) (1 'missing) (2 'charging) (3 'discharging) (4 'booting)))
+ (setq i (+ i 1))
+ )))
+ (:spot-status-behavior-faults-callback
+ (msg)
+ (send self :set-robot-state1 :behavior-faults
+ (mapcar #'(lambda (fault)
+ (list (cons :behavior-fault-id (send fault :behavior_fault_id))
+ (cons :cause (case (send fault :cause) (0 'unknown) (1 'fall) (2 'hardware)))
+ (cons :status (case (send fault :status) (0 'unknown) (1 'clearable) (2 'unclearable)))))
+ (send msg :faults))))
+ (:spot-status-system-faults-callback
+ (msg)
+ (dolist (fault (send msg :faults))
+ (let ((s (string-upcase (substitute #\- #\_ (send fault :name)))))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-NAME" s) *keyword-package*)
+ (send fault :NAME))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-DURATION" s) *keyword-package*)
+ (send (send fault :duration) :to-sec))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-CODE" s) *keyword-package*)
+ (send fault :code))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-UID" s) *keyword-package*)
+ (send fault :uid))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-ERROR_MESSAGE" s) *keyword-package*)
+ (send fault :error_message))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-ATTRIBUTES" s) *keyword-package*)
+ (send fault :attributes))
+ (send self :set-robot-state1 (intern (format nil "SYSTEM-FAULTS-~A-SEVERITY" s) *keyword-package*)
+ (case (send fault :severity) (0 'unknown) (1 'info) (2 'warn) (3 'critical))))))
+ (:spot-feedback-callback
+ (msg)
+ (send self :set-robot-state1 :feedback-standing (send msg :standing))
+ (send self :set-robot-state1 :feedback-sitting (send msg :sitting))
+ (send self :set-robot-state1 :feedback-moving (send msg :moving))
+ (send self :set-robot-state1 :feedback-serial-number (send msg :serial_number))
+ (send self :set-robot-state1 :feedback-species (send msg :species))
+ (send self :set-robot-state1 :feedback-version (send msg :version))
+ (send self :set-robot-state1 :feedback-nickname (send msg :nickname))
+ (send self :set-robot-state1 :feedback-computer-serial-number(send msg :computer_serial_number)))
+ (:state
+ (&rest args)
+ "use :metrics, :leases :feet, :estop, :wifi, :power-state, :battery-states, :behvior-faults, :system-fault, :feedback to get spot status, you can also acess to the specific data by concatenating these method name + key value, for example :metrics-time-moving"
+ (prog1
+ (send-message* self robot-interface :state args)
+ (flet ((gen-status
+ (key)
+ (mapcan #'(lambda (x) (if (substringp (string key) (string (car x))) (list (cons (intern (subseq (string (car x)) (1+ (length (string key)))) *keyword-package*) (cdr x))))) robot-state)))
+ (case (car args)
+ (:metrics (return-from :state (gen-status :metrics)))
+ (:leases (return-from :state (gen-status :leases)))
+ ;; (:feet (return-from :state (gen-status :feet)))
+ (:estop (return-from :state (gen-status :estop)))
+ (:wifi (return-from :state (gen-status :wifi)))
+ (:manipulator-state (return-from :state (gen-status :manipulator-state)))
+ (:power-state (return-from :state (gen-status :power-state)))
+ (:battery-states (return-from :state (gen-status :battery-states)))
+ ;; (:behavior-faults (return-from :state (gen-status :behavior-faults)))
+ (:system-faults (return-from :state (gen-status :system-faults)))
+ (:feedback (return-from :state (gen-status :feedback)))
+ (:angle-vector
+ (return-from :state (send robot :angle-vector)))
+ (:worldcoords
+ (unless joint-action-enable
+ (return-from :state (send self :worldcoords)))
+ (return-from :state (send *tfl* :lookup-transform "vision" base-frame-id (ros::time 0))))))))
+ ;;
+ (:estop-gentle () (call-trigger-service "/spot/estop/gentle"))
+ (:estop-hard () (call-trigger-service "/spot/estop/hard"))
+ (:claim ()
+ "Claim the robot control"
+ (let ((client (send self :state :leases-body-client-name)))
+ (if (or (null client) (string= client ""))
+ (call-trigger-service "/spot/claim")
+ (ros::ros-warn "robot is already claimed by ~A" client))))
+ (:release () "Relase the robot control" (call-trigger-service "/spot/release"))
+ (:power-on ()
+ "Power on the robot"
+ (if (send *ri* :simulation-modep)
+ (send self :set-robot-state1 :power-state-motor-power-state 'on)
+ (call-trigger-service "/spot/power_on")))
+ (:power-off
+ ()
+ "Power off the robot"
+ (if (send *ri* :simulation-modep)
+ (send self :set-robot-state1 :power-state-motor-power-state 'off)
+ (call-trigger-service "/spot/power_off")))
+ (:self-right () (call-trigger-service "/spot/self_right"))
+ (:stand () "Stand the robot up" (call-trigger-service "/spot/stand"))
+ (:sit () "Sit the robot down" (call-trigger-service "/spot/sit"))
+ (:stop () "Stop the robot in place with minimal motion" (call-trigger-service "/spot/stop"))
+ (:set-locomotion-mode (locomotion-hint) "Set locomotion mode" (call-set-locomotion-service "/spot/locomotion_mode" locomotion-hint))
+ (:set-stair-mode (is-stair-mode) "Set stair mode" (call-set-bool-service "/spot/stair_mode" is-stair-mode))
+ ;;
+ (:dock (dock-id) "Dock the robot" (call-dock-service "/spot/dock" dock-id))
+ (:undock
+ ()
+ "Undock the robot"
+ (send self :power-on)
+ (call-trigger-service "/spot/undock"))
+ ;;
+ (:send-cmd-vel-raw
+ (x y d &key (topic-name "/spot/cmd_vel"))
+ (when (send self :simulation-modep)
+ (return-from :send-cmd-vel-raw t))
+ (unless (ros::get-topic-publisher topic-name)
+ (ros::advertise topic-name geometry_msgs::Twist 1)
+ (unix:sleep 1))
+ (let ((msg (instance geometry_msgs::Twist :init)))
+ (send msg :linear :x x)
+ (send msg :linear :y y)
+ (send msg :angular :z d)
+ (ros::publish topic-name msg)))
+ (:go-velocity
+ (x y d ;; [m/sec] [m/sec] [rad/sec]
+ &optional (msec 1000) ;; msec is total animation time [msec]
+ &key (stop t) (wait t))
+ "control the robot velocity x([m/sec]) y([m/sec]) d([rad/sec]) msec([msec]). msec is the time to travel."
+ (unless wait
+ (ros::ros-error ":go-velocity without wait is unsupported")
+ (return-from :go-velocity nil))
+ (ros::rate 100)
+ (let ((start-time (ros::time-now)))
+ (while (and (ros::ok)
+ (< (* 1000.0 (send (ros::time- (ros::time-now) start-time) :to-sec)) msec))
+ (send self :spin-once)
+ (send self :send-cmd-vel-raw x y d)
+ (ros::sleep)))
+ (when stop
+ (send self :send-cmd-vel-raw 0 0 0))
+ (ros::rate 10)
+ t)
+ (:go-pos
+ (x y &optional (d 0) &key (timeout 10) (wait t)) ;; [m] [m] [degree]
+ "move robot toward (x, y, d) (units are m, m and degrees respectively)."
+ ;;
+ (setq trajectory-cmd-goal-msg (instance spot_msgs::TrajectoryGoal :init))
+ (send trajectory-cmd-goal-msg :target_pose :header :stamp (ros::time-now))
+ (send trajectory-cmd-goal-msg :target_pose :header :frame_id "body")
+ (send trajectory-cmd-goal-msg :target_pose :pose :position :x x)
+ (send trajectory-cmd-goal-msg :target_pose :pose :position :y y)
+ (send trajectory-cmd-goal-msg :target_pose :pose :orientation :z (sin (/ (deg2rad d) 2)))
+ (send trajectory-cmd-goal-msg :target_pose :pose :orientation :w (cos (/ (deg2rad d) 2)))
+ (send trajectory-cmd-goal-msg :duration :data (ros::time timeout))
+ ;;
+ (send trajectory-cmd-action :send-goal trajectory-cmd-goal-msg)
+ (if wait
+ (send trajectory-cmd-action :wait-for-result)
+ )
+ )
+ (:go-pos-wait
+ (x y &optional (d 0) (timeout 10)) ;; [m] [m] [degree]
+ "move robot toward (x, y, d) (units are m, m and degrees respectively). and wait"
+ (send self :go-pos x y d :timeout timeout :wait t)
+ )
+ (:go-pos-no-wait
+ (x y &optional (d 0) (timeout 10)) ;; [m] [m] [degree]
+ "move robot toward (x, y, d) (units are m, m and degrees respectively)."
+ (send self :go-pos x y d :timeout timeout :wait nil)
+ )
+ (:body-pose
+ ;; pose expected to get 3 elements float-vector (r p y), or eus coords.
+ (pose &key (topic-name "/spot/body_pose"))
+ (when (send self :simulation-modep)
+ (return-from :body-pose t))
+ (unless (ros::get-topic-publisher topic-name)
+ (ros::advertise topic-name geometry_msgs::Pose 1)
+ (unix:sleep 1))
+ (if (or (vectorp pose) (listp pose))
+ (progn
+ (ros::ros-debug "Got r p y float-vector or list as args.")
+ (let ((pose-msg (instance geometry_msgs::Pose :init))
+ (quaternion-msg (send self :create-quaternion-msg-from-rpy (elt pose 0) (elt pose 1) (elt pose 2))))
+ (send pose-msg :orientation quaternion-msg)
+ (ros::publish topic-name pose-msg)))
+ (progn
+ (ros::ros-debug "Got coords variable as args.")
+ (let ((pose-msg (ros::coords->tf-pose pose)))
+ (ros::publish topic-name pose-msg))))
+ (send self :stand))
+ (:find-waypoint-position-from-id
+ (id ids)
+ (let (ret)
+ (setq ret (position id ids :test #'string=))
+ ret))
+ (:find-waypoint-id-from-position
+ (index ids)
+ "return waypint id from position, if you set -1 to index, it returns last waypoint"
+ (let (ret)
+ (if (< index 0)
+ (setq index (+ (length ids) index)))
+ (if (< index (length ids))
+ (setq ret (elt ids index)))
+ ret))
+ (:initial-localization-fiducial
+ ()
+ "initial the localization for autowalk based on the fiducial marker"
+ (let (r)
+ (setq r (ros::service-call "/spot/set_localization_fiducial"
+ (instance spot_msgs::SetLocalizationFiducialRequest :init)))
+ (ros::ros-info "Call \"/spot/set_localization_fiducial\" returns \"~A\"" (send r :message))
+ t))
+ (:initial-localization-waypoint
+ (init-waypoint)
+ "initial the localization for waypoint id in the graph"
+ (let (r)
+ (if (numberp init-waypoint)
+ (prog1
+ (setq ids (send self :list-graph))
+ (setq init-waypoint (send self :find-waypoint-id-from-position init-waypoint ids))))
+ (setq r (ros::service-call "/spot/set_localization_waypoint"
+ (instance spot_msgs::SetLocalizationWaypointRequest :init :waypoint_id init-waypoint)))
+ (ros::ros-info "Call \"/spot/set_localization_waypoint\" returns \"~A\"" (send r :message))
+ t))
+ (:upload-path
+ (filepath &key (initial-localization-fiducial t) (wait nil))
+ "upload graph for autowalk"
+ (let (r)
+ (if wait (ros::wait-for-service srvname))
+ (setq r (ros::service-call "/spot/upload_graph"
+ (instance spot_msgs::UploadGraphRequest :init :upload_filepath filepath)))
+ (ros::ros-info "Call \"/spot/upload_graph\" returns .. \"~A\"" (send r :message))
+ (if initial-localization-fiducial (send self :initial-localization-fiducial))
+ (send r :message)))
+ (:list-graph
+ ()
+ "list up the waypoint (a string type hash id) list in the uploaded graph"
+ (let (r)
+ (setq r (ros::service-call "/spot/list_graph"
+ (instance spot_msgs::ListGraphRequest :init)))
+ (ros::ros-info "Call \"/spot/list_graph\" returns ..")
+ (dolist (id (send r :waypoint_ids))
+ (ros::ros-info " \"~A\"" id))
+ (send r :waypoint_ids)))
+ (:navigate-to
+ (navigate-to &key (initial-localization-waypoint nil))
+ (let (ids c goal ret)
+ (setq ids (send self :list-graph))
+ (if (numberp navigate-to)
+ (setq navigate-to (send self :find-waypoint-id-from-position navigate-to ids)))
+ (if (numberp initial-localization-waypoint)
+ (setq initial-localization-waypoint (send self :find-waypoint-id-from-position initial-localization-waypoint ids)))
+ (setq c (instance ros::simple-action-client :init
+ "/spot/navigate_to" spot_msgs::NavigateToAction))
+ (send c :wait-for-server)
+ (ros::ros-info "initial waypoit ~3D/~3D ~A"
+ (if initial-localization-waypoint
+ (position initial-localization-waypoint ids :test #'string=)
+ -1)
+ (length ids)
+ initial-localization-waypoint)
+ (ros::ros-info " goal waypoit ~3D/~3D ~A"
+ (position navigate-to ids :test #'string=) (length ids) navigate-to)
+ (setq goal (instance spot_msgs::NavigateToActionGoal :init))
+ (send goal :goal :id_navigate_to navigate-to)
+ (send c :send-goal goal :feedback-cb #'(lambda (msg) (let ((id (send msg :feedback :waypoint_id))) (ros::ros-info "~A/~A ~A" (position id ids :test #'string=) (length ids) id))))
+ (setq ret (send c :wait-for-result))
+ (ros::ros-info "~A ~A" ret (send (send c :get-result) :message))
+ ret))
+ ;; These are the methods to calculate rpy -> quaternion.
+ ;; You can use ros::create-quaternion-from-rpy after https://github.com/jsk-ros-pkg/jsk_roseus/pull/662 was merged and released.
+ (:create-quaternion-from-rpy
+ (roll pitch yaw) ;; return [w x y z]
+ (let ((sin-roll (sin (* roll 0.5))) (cos-roll (cos (* roll 0.5)))
+ (sin-pitch (sin (* pitch 0.5))) (cos-pitch (cos (* pitch 0.5)))
+ (sin-yaw (sin (* yaw 0.5))) (cos-yaw (cos (* yaw 0.5))))
+ (float-vector (+ (* cos-roll cos-pitch cos-yaw) (* sin-roll sin-pitch sin-yaw))
+ (- (* sin-roll cos-pitch cos-yaw) (* cos-roll sin-pitch sin-yaw))
+ (+ (* cos-roll sin-pitch cos-yaw) (* sin-roll cos-pitch sin-yaw))
+ (- (* cos-roll cos-pitch sin-yaw) (* sin-roll sin-pitch cos-yaw)))))
+ (:create-quaternion-msg-from-rpy
+ (roll pitch yaw)
+ (let* ((q (send self :create-quaternion-from-rpy roll pitch yaw))
+ (qx (elt q 1)) (qy (elt q 2)) (qz (elt q 3)) (qw (elt q 0)))
+ (instance geometry_msgs::quaternion :init :x qx :y qy :z qz :w qw)))
+ )
+
+(defun spot-init (&optional (create-viewer))
+ (unless (boundp '*spot*) (spot) (send *spot* :reset-pose))
+ (unless (ros::ok) (ros::roseus "spot_eus_interface"))
+ (unless (boundp '*ri*) (setq *ri* (instance spot-interface :init)))
+
+ (ros::spin-once)
+ (send *ri* :spin-once)
+ (send *ri* :claim)
+ (while (member (send *ri* :state :power-state-motor-power-state) (list 'off nil))
+ (unix::sleep 1)
+ (ros::ros-info "powering on...")
+ (send *ri* :power-on))
+ ;;
+ (unless (every #'(lambda (x) (eq x 'made)) (mapcar #'(lambda (x) (cdr (assoc :contact x))) (send *ri* :state :feet)))
+ (ros::ros-info "run (send *ri* :stand) to stand the robot"))
+
+ (when create-viewer (objects (list *spot*)))
+ )
diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l
new file mode 100644
index 0000000000..86e7b92b1b
--- /dev/null
+++ b/jsk_spot_robot/spoteus/spot-utils.l
@@ -0,0 +1,143 @@
+(require :spot "package://spoteus/spot.l")
+
+(unless (assoc :init-orig (send spot-robot :methods))
+ (rplaca (assoc :init (send spot-robot :methods)) :init-orig))
+
+(defmethod spot-robot
+ (:init
+ (&rest args) ;; fix colors
+ (dolist (b (list :_make_instance_body_geom0
+ :_make_instance_front_left_upper_leg_geom0
+ :_make_instance_front_left_lower_leg_geom0
+ :_make_instance_front_right_upper_leg_geom0
+ :_make_instance_front_right_lower_leg_geom0
+ :_make_instance_rear_left_upper_leg_geom0
+ :_make_instance_rear_left_lower_leg_geom0
+ :_make_instance_rear_right_upper_leg_geom0
+ :_make_instance_rear_right_lower_leg_geom0))
+ (rplacd (assoc b (send (class self) :methods))
+ (cdr (subst '(list :diffuse #f(1.00 0.84 0.32 0))
+ '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000))
+ (assoc b (send (class self) :methods)))))
+ )
+ (dolist (b (list :_make_instance_arm0.link_sh0_geom0
+ :_make_instance_arm0.link_sh1_geom0
+ :_make_instance_arm0.link_hr0_geom0
+; :_make_instance_arm0.link_el0_geom0
+ :_make_instance_arm0.link_el1_geom0
+ :_make_instance_arm0.link_wr0_geom0
+ :_make_instance_arm0.link_wr1_geom0
+ :_make_instance_arm0.link_fngr_geom0
+ ))
+ (rplacd (assoc b (send (class self) :methods))
+ (cdr (subst '(list :diffuse #f(1.00 0.84 0.32 0))
+ '(list :diffuse (float-vector 0.200000 0.200000 0.200000 1.000000))
+ (assoc b (send (class self) :methods)))))
+ )
+ (dolist (b (list :_make_instance_front_left_hip_geom0
+ :_make_instance_front_right_hip_geom0
+ :_make_instance_rear_left_hip_geom0
+ :_make_instance_rear_right_hip_geom0))
+ (rplacd (assoc b (send (class self) :methods))
+ (cdr (subst '(list :diffuse #f(0.1 0.1 0.1 0))
+ '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000))
+ (assoc b (send (class self) :methods)))))
+ )
+ (prog1
+ (send* self :init-orig args)
+ ;; add head-end-coords (camera coords)
+ (let ((head-end-coords (make-cascoords :coords (send arm-end-coords :copy-worldcoords))))
+ (send head-end-coords :rotate pi/2 :y)
+ (send (send arm-end-coords :parent) :assoc head-end-coords)
+ (send self :put :head-end-coords head-end-coords))))
+ ;; add head method so that (send *spot* :head :look-at) works
+ (:head
+ (&rest args)
+ (unless args (setq args (list nil)))
+ (case (car args)
+ (:end-coords
+ (user::forward-message-to (send self :get :head-end-coords) (cdr args)))
+ (:look-at
+ (let ((target-coords
+ (orient-coords-to-axis ;; orient target-coords to look-at direction
+ (make-coords :pos (cadr args))
+ (v- (cadr args) (send self :head :end-coords :worldpos))))
+ (head-coords (send self :head :end-coords :copy-worldcoords)))
+ ;; align target-coords x (image-y) direction to head direction
+ (send target-coords :rotate
+ (acos (v. (send target-coords :axis :x)
+ (send head-coords :axis :x))) :z)
+ (send* self :head :inverse-kinematics target-coords
+ :move-target (send self :head :end-coords)
+ :rotation-axis t
+ :translation-axis nil
+ :target-coords target-coords ;; for debug-view
+ (cddr args))))
+ ;;(send* self :inverse-kinematics-loop-for-look-at :head (cdr args)))
+ (t
+ (send* self :limb :arm args))))
+ (:legs ;; support legs for all limbs
+ (&rest args)
+ (case (car args)
+ (:crotch-r
+ (forward-message-to front_left_hip_x_jt (cdr args))
+ (forward-message-to front_right_hip_x_jt (cdr args))
+ (forward-message-to rear_left_hip_x_jt (cdr args))
+ (forward-message-to rear_right_hip_x_jt (cdr args)))
+ (:crotch-p
+ (forward-message-to front_left_hip_y_jt (cdr args))
+ (forward-message-to front_right_hip_y_jt (cdr args))
+ (forward-message-to rear_left_hip_y_jt (cdr args))
+ (forward-message-to rear_right_hip_y_jt (cdr args)))
+ (:knee-p
+ (forward-message-to front_left_knee_jt (cdr args))
+ (forward-message-to front_right_knee_jt (cdr args))
+ (forward-message-to rear_left_knee_jt (cdr args))
+ (forward-message-to rear_right_knee_jt (cdr args)))))
+ (:body-inverse-kinematics
+ (target-coords
+ &rest args
+ &key (move-target) (link-list)
+ (min (float-vector -500 -500 -500 -200 -200 -100))
+ (max (float-vector 500 500 500 200 200 100))
+ (target-centroid-pos)
+ (rotation-axis)
+ (root-link-virtual-joint-weight #f(0.0 0.0 0.1 0.1 0.5 0.5))
+ &allow-other-keys)
+ "The purpose of this function is to use :fullbody-inverse-kinematics by only specifying body target-coords.
+Example:
+(send *spot* :body-inverse-kinematics (make-coords :pos #f(0 0 50)))
+"
+ (let ((body-coords (make-cascoords :coords (send (send self :coords) :copy-worldcoords))))
+ (send body_lk :assoc body-coords)
+ (setq target-coords
+ (list target-coords
+ (send self :larm :end-coords :copy-worldcoords)
+ (send self :rarm :end-coords :copy-worldcoords)
+ (send self :lleg :end-coords :copy-worldcoords)
+ (send self :rleg :end-coords :copy-worldcoords)))
+ (unless move-target
+ (setq move-target
+ (list body-coords
+ (send self :larm :end-coords)
+ (send self :rarm :end-coords)
+ (send self :lleg :end-coords)
+ (send self :rleg :end-coords))))
+ (unless link-list
+ (setq link-list
+ (mapcar #'(lambda (limb) (send self :link-list (send limb :parent))) move-target)))
+ (unless rotation-axis
+ (setq rotation-axis (list t nil nil nil nil)))
+ (prog1
+ (send-super*
+ :fullbody-inverse-kinematics target-coords
+ :move-target move-target
+ :link-list link-list
+ :min min
+ :max max
+ :target-centroid-pos target-centroid-pos
+ :rotation-axis rotation-axis
+ :root-link-virtual-joint-weight root-link-virtual-joint-weight
+ args)
+ (send body_lk :dissoc body-coords))))
+ )
diff --git a/jsk_spot_robot/spoteus/spot.yaml b/jsk_spot_robot/spoteus/spot.yaml
new file mode 100644
index 0000000000..1157a69b53
--- /dev/null
+++ b/jsk_spot_robot/spoteus/spot.yaml
@@ -0,0 +1,54 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+lfleg:
+ - front_left_hip_x : lfleg-crotch-r
+ - front_left_hip_y : lfleg-crotch-p
+ - front_left_knee : lfleg-knee-p
+rfleg:
+ - front_right_hip_x : rfleg-crotch-r
+ - front_right_hip_y : rfleg-crotch-p
+ - front_right_knee : rfleg-knee-p
+lrleg:
+ - rear_left_hip_x : lrleg-crotch-r
+ - rear_left_hip_y : lrleg-crotch-p
+ - rear_left_knee : lrleg-knee-p
+rrleg:
+ - rear_right_hip_x : rrleg-crotch-r
+ - rear_right_hip_y : rrleg-crotch-p
+ - rear_right_knee : rrleg-knee-p
+arm:
+ - arm0.sh0 : arm-shoulder-y
+ - arm0.sh1 : arm-shoulder-p
+ - arm0.el0 : arm-elbow-p
+ - arm0.el1 : arm-elbow-r
+ - arm0.wr0 : arm-wrist-p
+ - arm0.wr1 : arm-wrist-r
+hand:
+ - arm0.f1x : hand-gripper-p
+
+
+angle-vector:
+ reset-pose : [0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, -170.0, 160.0, 0.0, 10.0, 0.0, 0]
+
+lfleg-end-coords:
+ parent : front_left_lower_leg
+ translate : [0, 0, -0.38]
+ rotate : [0, 1, 0, 0]
+rfleg-end-coords:
+ parent : front_right_lower_leg
+ translate : [0, 0, -0.38]
+ rotate : [0, 1, 0, 0]
+lrleg-end-coords:
+ parent : rear_left_lower_leg
+ translate : [0, 0, -0.38]
+ rotate : [0, 1, 0, 0]
+rrleg-end-coords:
+ parent : rear_right_lower_leg
+ translate : [0, 0, -0.38]
+ rotate : [0, 1, 0, 0]
+arm-end-coords:
+ parent: arm0.link_wr1
+ translate : [0.175, 0, -0.03]
+ rotate : [1, 0, 0, 0]
diff --git a/jsk_spot_robot/spoteus/test/test-spot.l b/jsk_spot_robot/spoteus/test/test-spot.l
new file mode 100755
index 0000000000..1d378779a4
--- /dev/null
+++ b/jsk_spot_robot/spoteus/test/test-spot.l
@@ -0,0 +1,37 @@
+#!/usr/bin/env roseus
+(require :unittest "lib/llib/unittest.l")
+(require "package://spoteus/spot-utils.l")
+(require "package://spoteus/spot-interface.l")
+
+(init-unit-test)
+
+(deftest test-pose
+ (let (robot)
+ (setq robot (instance spot-robot :init))
+ ;; move robot arm to #f(600 0 600) relative to body
+ (send robot :arm :inverse-kinematics (send (make-coords :pos #f(600 0 600)) :transform robot))
+ ))
+
+(defun look-at (robot)
+ (let ((targets (list (make-cube 100 100 100 :pos #f(2000 0 0))
+ (make-cube 100 100 100 :pos #f(2000 1000 2000))
+ (make-cube 100 100 100 :pos #f(2000 -1000 1000))
+ (make-cube 100 100 100 :pos #f(0 2000 2000))
+ (make-cube 100 100 100 :pos #f(0 -2000 1000)))))
+ (objects (append (list robot) targets))
+ (dolist (target targets)
+ (send robot :head :look-at (send target :worldpos) :debug-view :no-mesage)
+ (send *irtviewer* :draw-objects))))
+
+(deftest test-spot-look-at
+ (let (robot)
+ (setq robot (instance spot-robot :init))
+ (look-at robot)
+ (send robot :arm :inverse-kinematics (send (make-coords :pos #f(600 0 600)) :transform robot))
+ (look-at robot)))
+
+(deftest test-spot-init
+ (spot-init))
+
+(run-all-tests)
+(exit)
diff --git a/jsk_spot_robot/spoteus/test/test-spot.test b/jsk_spot_robot/spoteus/test/test-spot.test
new file mode 100644
index 0000000000..b2e8fb2164
--- /dev/null
+++ b/jsk_spot_robot/spoteus/test/test-spot.test
@@ -0,0 +1,3 @@
+
+
+
diff --git a/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt b/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt
index e7595dd01f..6b1c3d909a 100644
--- a/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt
+++ b/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt
@@ -14,7 +14,15 @@ catkin_package()
#############
## Install ##
#############
-install(DIRECTORY config launch
+file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*)
+foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS})
+ if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$")
+ catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+ else()
+ install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts)
+ endif()
+endforeach()
+install(DIRECTORY config launch apps autostart ino
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
diff --git a/jsk_unitree_robot/unitreeeus/CMakeLists.txt b/jsk_unitree_robot/unitreeeus/CMakeLists.txt
index ad64f874f6..f541e2a509 100644
--- a/jsk_unitree_robot/unitreeeus/CMakeLists.txt
+++ b/jsk_unitree_robot/unitreeeus/CMakeLists.txt
@@ -44,12 +44,14 @@ foreach(ROBOT_TYPE go1)
add_custom_target(compile_${ROBOT_TYPE} ALL DEPENDS ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}-simple.l)
endforeach()
-
+###
+### install
+###
install(DIRECTORY test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
-install(FILES go1.l unitree-interface.l go1-utils.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(FILES go1.l unitree-interface.l go1-utils.l go1.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)