Add Kashiwagi Interaction Programs

README.md

@@ -7,3 +7,5 @@ JSK demo programs
 ## [jsk_2013_04_pr2_610](https://github.com/jsk-ros-pkg/jsk_demos/tree/master/jsk_2013_04_pr2_610)
 
    ![bloom](https://gist.githubusercontent.com/k-okada/b3308c08ce31230e8947/raw/c14d6c52d8bf35fd5c244d989beccd35caa6fa8a/jsk_2013_04_pr2_irt_4.png =200x200)
+=======
+# Workspace for Semi2024 in JSK

jsk_2023_12_codesign/CMakeLists.txt

@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(jsk_2023_12_codesign)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES kashiwagi_kuromitsu
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/kashiwagi_kuromitsu.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/kashiwagi_kuromitsu_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_kashiwagi_kuromitsu.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

jsk_2023_12_codesign/README.md

@@ -0,0 +1,116 @@
+# 柏木さんとくろみつ（2023年12月のcodesignにより作成）を動かす方法（2025年1月時点）
+## 環境構築
+```
+repositories:
+  jsk_demos:
+    type: git
+    url: git@github.com:sawada10/jsk_demos.git
+    version: add-codesigned-robot-demos
+  modular_robot_model_zoo:
+    type: git
+    url: git@gitlab.jsk.imi.i.u-tokyo.ac.jp:sawada1/modular_robot_model_zoo.git
+    version: add-kashiwagi-kuromitsu-models
+  rcb4:
+    type: git
+    url: git@github.com:iory/rcb4.git
+    version: 930687bdba6f527b366b5cebb3c807ca2c9182e9
+```
+を repos.yamlとして`<ワークスペース>/src/repos.yaml` に保存する。
+その後以下を実行。
+```
+cd src
+source <ワークスペース>/devel/setup.bash
+vcs import < repos.yaml
+rosdep install -iry --from-paths .
+cd ..
+catkin build
+
+```
+## 任意動作の作成と保存方法
+前提：ロボットの電源を入れる（柏木さん/くろみつの場合は7.40v）
+### launchファイルを立ち上げる
+```
+source <ワークスペース>/devel/setup.bash
+roslaunch jsk_2023_12_codesign codesigned_robot.launch robot_name:=kuromitsu
+```
+※launchするときにrobot_nameに動かしたいロボットの名前（kashiwagi/kuromitsu）を指定する。
+※joyを使いたいときは、joyをlaunchファイルを立ち上げているPCにUSB接続したうえで、`joy`を`true`に設定する。
+e.g.
+```
+roslaunch jsk_2023_12_codesign codesigned_robot.launch joy:=true
+```
+
+### 動作を保存する
+```
+$ roscd jsk_2023_12_codesign
+$ cd scripts
+$ ipython -i robot_interface.py
+> from save_angle_vector import save_angle_vector_mode
+> import os
+> save_angle_vector_mode(ri,“path/to/<your file name>.json”) #ここで関節角度列を保存するjsonファイルのpathを指定
+> you : save # saveと打つことで動作の記録を開始
+> saving angles you: end # endと打つことで動作の記録を終了
+> you : playing motions # 動きの自動再生
+[ENTER] saving
+please input motion name : "保存動作の名前" # jsonファイルの中にshake_headで保存される（ここで保存した名前をact()関数で使える）
+```
+
+### 動作を再生する
+```
+$ roscd jsk_2023_12_codesign
+$ cd scripts
+$ ipython
+> from robotmodel import *
+> act("保存動作の名前",“path/to/<your file name>.json”)
+```
+
+### サーボのON/OFF
+```
+$ roscd jsk_2023_12_codesign
+$ cd scripts
+$ ipython
+> from robotmodel import *
+> servo_on()
+> servo_off()
+```
+
+## TODO(2025-01-31)
+### 目・ほっぺ・梅の光らせ方
+```
+source <ワークスペース>/devel/setup.bash
+roslaunch jsk_2023_12_codesign codesigned_module.launch
+```
+した上で、以下の表に示すトピックにpublishすることで変化する
+
+| 箇所  |　トピック | 型 | 機能 |
+| ------------- | ------------- |---| --- |
+| 目 | `/eye_status`  |UInt16| 表情変化 |
+| ほっぺ  | `/cheek/led_blink_time`  | UInt16 |点滅回数を指定|
+||`/cheek/led_duration`| UInt16 | 点滅時間を秒数で指定|
+||`/cheek/led_mode`| UInt16 | 光り方を1,2,3で指定 1:常時 2:点滅 3:虹色|
+||`/cheek/led_rainbow_delta_hue`| UInt16 | 虹色の光り方を1-50で指定(数字が大きいほど隣同士が別の色)|
+||`/cheek/led_rgb`| ColorRGBA |r:Red, g:Green, b:Blue, a:明るさ (rgbは0-255で指定, aは10くらいでOK) |
+| 梅（柏木さんのみ) | `/ume/led_blink_time` |UInt16|点滅回数を指定 |
+||`/ume/led_duration`| UInt16 | 点滅時間を秒数で指定|
+||`/ume/led_mode`| UInt16 |光り方を1,2,3で指定 1:常時 2:点滅 3:虹色 |
+||`/ume/led_rainbow_delta_hue`| UInt16 |虹色の光り方を1-50で指定(数字が大きいほど隣同士が別の色) |
+||`/ume/led_rgb`| ColorRGBA |r:Red, g:Green, b:Blue, a:明るさ (rgbは0-255で指定, aは10くらいでOK)|
+
+<表情の対応表>
+| 番号 | くろみつ |柏木さん|
+|---|---|---|
+|0| 通常|通常|
+|1| まばたき|まばたき|
+|2| キラキラ|おどろき|
+|3| 眠い |眠い|
+|4| 真剣 |怒る|
+|5| 悲しい|悲しい|
+|6| 嬉しい |楽しい|
+|7|悔しい１|困る|
+|8| 悔しい2|嬉しい|
+|9| 退屈|期待|
+|10| ハート ||
+|11| ぐるぐる ||
+|12| キラキラ2||
+
+### くろみつでの動作確認

jsk_2023_12_codesign/config/dualshock3_teleop_twist_joy.yaml

@@ -0,0 +1,17 @@
+axis_linear:
+  x: 1
+  y: 0
+scale_linear:
+  x: 0.5
+  y: 0.3
+scale_linear_turbo:
+  x: 1.2
+  y: 0.3
+axis_angular:
+  yaw: 3
+scale_angular:
+  yaw: 0.5
+scale_angular_turbo:
+  yaw: 0.7
+enable_button: 4
+enable_turbo_button: 7

jsk_2023_12_codesign/launch/codesigned_module.launch

@@ -0,0 +1,30 @@
+<launch>
+  <!-- Define the robot name argument -->
+  <arg name="robot_name" default="kashiwagi" />
+  <arg name="debug" default="false"/>
+
+  <!-- Launch serial_node for dual eyes -->
+  <include file="$(find eye_display)/launch/kashiwagi_dual.launch">
+    <arg name="port_right" value="/dev/ttyACM-righteye" />
+    <arg name="port_left" value="/dev/ttyACM-lefteye" />
+    <arg name="baud" value="57600" />
+    <arg name="debug" value="true" />
+  </include>
+
+  <!-- Launch serial_node for the ume if robot_name is "kashiwagi" -->
+  <group if="$(eval robot_name == 'kashiwagi')">
+    <node pkg="rosserial_python" name="led_ume" type="serial_node.py">
+      <param name="~port" value="/dev/ttyUSB-ume"/>
+      <param name="~baud" value="57600"/>
+    </node>
+  </group>
+
+  <group if="$(arg debug)">
+    <node
+        pkg="rosservice"
+        type="rosservice"
+        name="set_debug_mode"
+        args="call --wait /eye_display/set_logger_level rosout DEBUG"
+        />
+  </group>
+</launch>