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_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_spot_robot/README.md b/jsk_spot_robot/README.md new file mode 100644 index 0000000000..13696b4be1 --- /dev/null +++ b/jsk_spot_robot/README.md @@ -0,0 +1,149 @@ +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 + +## Connect Spot Robot + +There are 3 network interface in Spot Robot + +- Ethernet +- WiFi +- Payload + +### Ethernet + +Spot has ethernet port on its rear. its ip address is `10.0.0.3/24` by default. See [this article](https://support.bostondynamics.com/s/article/Spot-network-setup) for more details. + +You can open web console by accessing `https://-p.jsk.imi.i.u-tokyo.ac.jp` . You can connect `https://spot-BD--p.jsk.imi.i.u-tokyo.ac.jp`. + +### WiFi + +Spot has WiFi interface and it is usually used for table control. Its accesspoint is `spot-BD-` by default. +You can connect web console by connecting its AP and connect ip of robot. + +### Payload + +Payload has ethernet port `192.168.50.3` by default. So if you have access to SpotCore and do port foward to `192.168.50.3:443` with some way ( e.g. Local forward of SSH ) You can connect robot web console through payload. + +For examples, if you can connect SpotCore by `ssh spot@spotcore.local`, you can forward access of robot to your local machine by `ssh spot@sporcore.local -L 443:192.168.50.3:443` and you can open webconsole by accesing `https://localhost:443`. diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md new file mode 100644 index 0000000000..ab7f5a886a --- /dev/null +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -0,0 +1,258 @@ +# 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 +``` + +### Setup development environment + +``` +core-io$ mkdir ~/spot_driver_ws/src -p +core-io$ cd ~/spot_driver_ws/src +core-io$ git clone https://github.com/k-okada/jsk_robot.git -b spot_arm +core-io$ cd ~/spot_driver_ws/src/jsk_robot/jsk_spot_robot/coreio/base +core-io$ make pre_build catkin_build dev_build all +``` +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..0464b2e58d --- /dev/null +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -0,0 +1,80 @@ +##### +## 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 + +# 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:ALL) NOPASSWD: ALL@' /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 + +# get dummy workspace to install dpeendent packages +RUN mkdir -p /tmp/ws/src/jsk_robot/jsk_spot_robot/jsk_spot_startup /tmp/ws/src/jsk_robot/jsk_spot_robot/spoteus +COPY src/jsk_robot/jsk_spot_robot/jsk_spot_startup/package.xml /tmp/ws/src/jsk_robot/jsk_spot_robot/jsk_spot_startup/ +COPY src/jsk_robot/jsk_spot_robot/spoteus/package.xml /tmp/ws/src/jsk_robot/jsk_spot_robot/spoteus/ +COPY src/jsk_robot/jsk_spot_robot/requirements.txt /tmp/ws/src/jsk_robot/jsk_spot_robot/ + +# 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 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.xml.tar /tmp/ +RUN tar -C /tmp/ws/ -xvf /tmp/package.xml.tar +RUN rosdep install -y -r --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 sudo apt install -y -q python-catkin-tools less emacs vim + +RUN sudo apt install -y -q bluez bluez-tools + +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 "source /home/spot/ws/devel/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 u+x /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"] diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile new file mode 100644 index 0000000000..ed00da7df5 --- /dev/null +++ b/jsk_spot_robot/coreio/base/Makefile @@ -0,0 +1,49 @@ +all: $(HOME)/bash.sh + @echo "make build: build docker for spot user" + +$(HOME)/bash.sh: + echo "#!/bin/bash\n\ncd ~/spot_driver_ws/src/jsk_robot/jsk_spot_robot/coreio/base\nmake shell\n" > $(HOME)/bash.sh + chmod u+x $(HOME)/bash.sh + + +WS_ROOT=$(abspath $(CURDIR)/../../../../../) + +define run +docker run --rm --privileged --hostname strelka --add-host strelka:133.11.216.166 --add-host strelka.jsk.imi.i.u-tokyo.ac.jp:133.11.216.166 --network=host --device=/dev/input -v $(WS_ROOT):/home/spot/ws -v $(HOME)/.ros:/home/spot/.ros -w /home/spot/ws -ti dev_env ${1}; +endef +define docker_run +${1} +endef + +pre_build: + # make .ros file + mkdir -p ~/.ros + # build base file + cp Dockerfile $(WS_ROOT) + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target base_build --progress=plain --network=host -t dev_env -f Dockerfile . + $(call run, /bin/ls -al src) + # copy workspaces + 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) + # rosdep install all + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target pre_build --progress=plain --network=host -t dev_env -f Dockerfile . + rm $(WS_ROOT)/Dockerfile + +dev_build: + cp Dockerfile $(WS_ROOT) + cd $(WS_ROOT); [ -e package.xml.tar ] || find src -iname 'package.xml' | tar cf package.xml.tar -T - + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build --target dev_build --progress=plain --network=host -t dev_env -f Dockerfile . + rm $(WS_ROOT)/Dockerfile $(WS_ROOT)/package.xml.tar + +catkin_build: + $(call run, catkin init) + $(call run, 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) + $(call run, catkin build) + + +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_behavior_manager/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt new file mode 100644 index 0000000000..46b773ac6b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_spot_behavior_manager) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/README.md b/jsk_spot_robot/jsk_spot_behavior_manager/README.md new file mode 100644 index 0000000000..96d7c3e0fd --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/README.md @@ -0,0 +1,93 @@ +# jsk_spot_behaviors + +These packages enable for Spot to execute locomotoion behaviors to reach a desired position. + +## Concept + +In this framework, knowledge about positions and transtion behaviors between them are represented as a digraph like below. + +Each node represents specified positions and each edge represents a behavior to transition between them. + +![example_graph](https://user-images.githubusercontent.com/9410362/124147589-cc8ce700-dac9-11eb-930f-1c00c2a4777e.png) + +A graph is defined by a yaml file ( e.g. [node.yaml in spot_behavior_graph](./spot_behavior_graph/config/node.yaml) and [edge.yaml in spot_behavior_graph](./spot_behavior_graph/config/edge.yaml) ) +Please see nodes and edges format section below. + +Knowledge representation and execution process of behaviors are separated from actual behavior implementation. +The implementation of former is in spot_behavior_manager and spot_behavior_manager_server, but actual behaviors like walk and elevator are in spot_basic_behaviors. +behavior_manager_server node will dynamically load each behaviors defined in map.yaml so you can easilly add your behavior without editing these core implementation. +Please see spot_basic_behaviors package for behavior examples. + +## How to use it + +To run demo, please make Spot stand in front of the fiducial in 73B2 and run + +```bash +roslaunch spot_behavior_graph demo.launch +``` + +```bash +$ rostopic pub -1 /spot_behavior_manager_server/lead_person/execute_behaviors spot_behavior_manager_msgs/LeadPersonActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + target_node_id: 'eng2_2FElevator'" +``` + +Then Spot will go to 2FElevator of eng2 by walk behavior and elevator behavior implemented in spot_basic_behaviors packages. + +https://user-images.githubusercontent.com/9410362/124338016-aad25380-dbe0-11eb-962f-b9a27e1e08cb.mp4 + +For more details, please see [spot_behavior_manager](./spot_behavior_manager), [spot_behavior_manager_server](./spot_behavior_manager_server) and each behavior documentation. (e.g. [spot_basic_behaviors](./spot_basic_behaviors) ) + +## Behavior Graph + + +This package provides graph and node data for spot_behavior_manager. + +### scripts + +#### visualize_map.py + +Visualizer script of map file of spot_behavior_manager + +Befor using this script, you need to install + +``` +pip3 install graphviz xdot +``` + +##### Usage + +``` +rosrun spot_behavior_graph visualize_map.py --filename +``` + +##### Example Output + +![map](https://user-images.githubusercontent.com/9410362/132942120-4a4e652b-3d25-43df-a678-fd3c09782284.png) + +## behavior manager server + +This package provides ros nodes for spot_behavior_manager. + +### ROS Node + +#### behavior_manager_server.py + +ROS Node script for spot_behavior_manager. + +See [the source of behavior_manager_node](../spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py). + +#### interactive_behavior_executor.py + +ROS Node script to execute action of spot_behavior_manager by language. + diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml new file mode 100644 index 0000000000..13455ad03f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml @@ -0,0 +1,389 @@ +edges: + - from: 'eng2_JSKEntrance' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73A2' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73B1' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B1' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73A4' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73A4' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_belka_home' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2_belka_home' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_strelka_home' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2_strelka_home' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_shinjo_desk' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + - from: 'eng2_73B2_shinjo_desk' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + - from: 'eng2_73B2' + to: 'eng2_81C1' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + - from: 'eng2_81C1' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + - from: 'eng2_73B2' + to: 'eng2_83A2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + - from: 'eng2_83A2' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + - from: 'eng2_2FElevator' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng_TelephoneBox' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng_TelephoneBox' + to: 'eng8_Piloti_Center' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng8_Piloti_Center' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng2_3FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_2FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + - from: 'eng2_3FElevator' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_Mailbox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mailbox' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_CampusMail' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_CampusMail' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mailbox' + to: 'eng2_CampusMail' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_CampusMail' + to: 'eng2_Mailbox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_3FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_7FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_subway_pillar' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_subway_initial' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_pillar' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_00' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_00' + to: 'eng2_subway_initial' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_2FElevator' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_basic_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_basic_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'HongoMainGate' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'HongoMainGate' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_7FElevator' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + rest_waypoint_id: 'snubbed-puma-5v8XtjdA17XhPNLhsH2WEg==' + goal_waypoint_id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + - from: 'eng2_1FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + rest_waypoint_id: 'snubbed-puma-5v8XtjdA17XhPNLhsH2WEg==' + goal_waypoint_id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + - from: 'eng2_1FElevator' + to: 'eng2_GarbageCollectionArea_east' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + - from: 'eng2_1FElevator' + to: 'eng2_GarbageCollectionArea_south' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + - from: 'eng2_GarbageCollectionArea_east' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + - from: 'eng2_GarbageCollectionArea_south' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml new file mode 100644 index 0000000000..64632e982f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml @@ -0,0 +1,295 @@ +edges: + - from: 'eng2_73B2' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_73B2' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + - from: 'eng2_2FElevator' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_person_lead_behaviors.stair_behavior.StairBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + stair_nums: + - -4 + - -8 + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_person_lead_behaviors.stair_behavior.StairBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + stair_nums: + - 8 + - 4 + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_person_lead_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_person_lead_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'HongoMainGate' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'HongoMainGate' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_3FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_2FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + - from: 'eng2_3FElevator' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_Mech_Office' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_7FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_subway_pillar' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_pillar' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_00' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_00' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_01' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_01' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_02' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_02' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_03' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_03' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_04' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_04' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_10' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_10' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_11' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_11' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_12' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_12' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_13' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_13' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_14' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_14' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml new file mode 100644 index 0000000000..49006a7fde --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml @@ -0,0 +1,421 @@ +nodes: + 'dummy': + name_en: 'dummy' + name_jp: 'ダミー' + 'eng2_JSKEntrance': + name_en: '73B1' + name_jp: + - 'JSK' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'hated-salmon-+oAmVbVKzwfer6BexXKNog==' + localization_method: 'waypoint' + 'eng2_73B1': + name_en: '73B1' + name_jp: + - '73 B 1' + - '73B1' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'unkept-scarab-pfr5U1CBRCXx9EOt6eQDCw==' + localization_method: 'waypoint' + 'eng2_73B2': + name_en: '73B2' + name_jp: + - '73 B 2' + - '73B2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'arty-hyrax-hiViwXJwUrkJ2LR5UAuKyw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + id: 'goodly-hogg-cEjt+0aMeZdksVRXrP7.uw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + id: 'fuzzed-medusa-wyXWCEjmWas1kMo8EJW8Bw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + id: 'dyed-viper-HVQRLVeklEUaE2aXJW+d6Q==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_73B2_trash.walk' + id: 'tined-mouse-D.2ws5hStb8AE.C.J5lKEw==' + localization_method: 'fiducial' + 'eng2_73B2_belka_home': + name_en: 'belka' + name_jp: + - 'ベルカ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'uppity-cayman-rm8aTrilycp7soPeorOdUw==' + localization_method: 'waypoint' + 'eng2_73B2_strelka_home': + name_en: 'strelka' + name_jp: + - 'ストレルカ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'proof-llama-9slE0zml9BYcNFbpQkmNcg==' + localization_method: 'waypoint' + 'eng2_73B2_shinjo_desk': + name_en: 'shinjo' + name_jp: + - '新城' + - '新城の席' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + id: 'cogent-vole-wTZdgPPyjZIObFh2dr7EqA==' + localization_method: 'waypoint' + 'eng2_73A2': + name_en: '73A2' + name_jp: + - '73 A 2' + - '73A2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'enough-python-pqYBEhDZHuCEapcDxLK32w==' + localization_method: 'waypoint' + 'eng2_73A4': + name_en: '73A4' + name_jp: + - '73 A 4' + - '73A4' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dashed-kodiak-bKNHSR3S7fXE11UeXeeekA==' + localization_method: 'waypoint' + 'eng2_81C1': + name_en: '81C1' + name_jp: + - '81 C 1' + - '81C1' + - 'プロジェクト部屋' + - 'プロジェクト 部屋' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + id: 'sable-narwal-o7qEo7543BUTj4sNKWcQ6g==' + localization_method: 'waypoint' + 'eng2_83A2': + name_en: '83A2' + name_jp: + - '83 A 2' + - '83A2' + - '工作室' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + id: 'shaved-koala-aLy+bUIQpAHBQbi5DxeKJA==' + localization_method: 'waypoint' + 'eng2_7FElevator': + name_en: 'Elevator' + name_jp: + - '7階エレベーター' + - '7階 エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'looted-cougar-RWT2C0zLJXG.ezYnCuLUeQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'larger-mudcat-0x3bt3Dif.1QU9JfXpyQPw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_73B2_trash.walk' + id: 'spruce-frog-bNtHHEv469wvlNxlIxBV5w==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator_bot_down' + floor: 7 + floor_height: 24 + 'eng2_2FElevator': + name_en: 'Elevator' + name_jp: + - '2階 エレベーター' + - '2階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'rainy-collie-uwFW2KVYg2YCUi8FF6N00g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_2FEntrance.walk' + id: 'snaky-beagle-.fmRm2JflzlV6Dv5fTdnIQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainGate.walk' + id: 'banner-oxen-UrEwTUbZYL0IQnY5TCFwrA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'inland-remora-FqtX8ScP+.inEd4xFYKq4A==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + id: 'mythic-leech-Uslc00eTIhygFsCdH2q1Qg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + id: 'brumal-gerbil-NLVgQc0oBXbc8BCZev57LQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'felted-weasel-kFVXZpw1.DvEm6FCSgC5XQ==' + localization_method: 'fiducial' + switchbot_device: '/eng2/2f/elevator_bot_up' + floor: 2 + floor_height: 0 + 'eng2_3FElevator': + name_en: 'Elevator' + name_jp: + - '3階 エレベーター' + - '3階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'doughy-vulture-kMie2F0bAno9STnKFjNadA==' + localization_method: 'fiducial' + switchbot_device: '/eng2/3f/elevator_bot_up' + floor: 3 + floor_height: 4.5 + 'eng2_1FElevator': + name_en: 'Elevator' + name_jp: + - '1階 エレベーター' + - '1階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + id: 'brag-eagle-Of.kPg1gvRBhSWzrP+G3Yw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + id: 'sunlit-iguana-cXGwqooA99O95Hzqas19dw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + id: 'gilled-bee-ve3JJ8B2sMTnRL3s8p.SZA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk' + id: 'oafish-vervet-w.bCe7FjEILCP8oBQg6Miw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1fentrance_to_1FElevator.walk' + id: 'anemic-spider-TimPFBe0OMBdd+kWxYBW2Q==' + localization_method: 'fiducial' + switchbot_device: '/eng2/1f/elevator_bot_up' + floor: 1 + floor_height: -4.5 + 'eng2_Mech_Office': + name_en: 'Mech Office' + name_jp: + - '機械系 事務室' + - '機械系事務室' + - '事務室' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'foiled-ibis-dAio3Ld6rCSWQgy9ERDulw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'askant-beef-JQLXogu60cJgJiSXcyjKUQ==' + localization_method: 'fiducial' + 'eng2_Mailbox': + name_en: 'Mailbox' + name_jp: + - '郵便入れ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'sacked-tomcat-wpzX0UCSooUeKol.CShTqQ==' + localization_method: 'waypoint' + 'eng2_CampusMail': + name_en: 'CampusMail' + name_jp: + - '学内便' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'jovial-civet-OPcK4EyGMTkxjZg04nuO.Q==' + localization_method: 'waypoint' + 'eng2_3F_door_inside': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'silty-llama-gdnA1Y.5ugUuDuZ9RL9upQ==' + localization_method: 'waypoint' + 'eng2_3F_door_outside': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'plane-marlin-gaEqSGH8WX35X1619X0NwQ==' + localization_method: 'waypoint' + 'eng_TelephoneBox': + name_en: 'telephone box' + name_jp: '電話ボックス' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'slain-pika-VdtPz3lWcp5T+pEArUB3PA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng_TelephoneBox_to_HongoMainGate.walk' + id: 'aired-hyrax-BF+hXHaUufi.DLd3pEWm5Q==' + localization_method: 'fiducial' + 'eng8_Piloti_Center': + name_en: 'Piloti' + name_jp: '8号館 ピロティ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'azoic-lion-Iapbto7rzlzuFF0MvXdyPA==' + localization_method: 'fiducial' + 'eng2_subway_pillar': + name_en: 'Piloti' + name_jp: 'サブウェイ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + id: 'quack-snail-Zm7LgLSyql.xcoqi9OeCQA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'risen-bowfin-2he29MpsZLJRToJDhZ3.3g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + id: 'unread-donkey-2pdTiY6qfMnAfHD8RNH2ZA==' + localization_method: 'fiducial' + 'eng2_subway_initial': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'skimpy-vole-2plogvKFUrjXTCoxsCsNGA==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + id: 'fired-minnow-Pw+F4RFHFQwQolGeuHgkEQ==' + localization_method: 'waypoint' + 'eng2_subway_table_00': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'perky-crake-iZWT3e+cQtLKUCo8iVTu7Q==' + localization_method: 'waypoint' + 'eng2_subway_table_01': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'spacy-eland-WlC6wgdJeo5u.TY6T3hA8Q==' + localization_method: 'waypoint' + 'eng2_subway_table_02': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'apodal-drum-o2pzrnIBJdLVWMX.ElaQOQ==' + localization_method: 'waypoint' + 'eng2_subway_table_03': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'formed-gerbil-yZerC+XIahSrvCJK5Duk6A==' + localization_method: 'waypoint' + 'eng2_subway_table_04': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'ruled-otter-2zSijP2KPICZpqU80utSPw==' + localization_method: 'waypoint' + 'eng2_subway_table_10': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'spiral-falcon-bG1t8To8x.p7BWwOWjH2Lg==' + localization_method: 'waypoint' + 'eng2_subway_table_11': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'manky-mite-EvhV0ln+iiG+pTCfeEnArQ==' + localization_method: 'waypoint' + 'eng2_subway_table_12': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'brash-sheep-NYE7iE0HuaqR0i5xnzYE1A==' + localization_method: 'waypoint' + 'eng2_subway_table_13': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'swampy-jay-jEYEQ+FOe4eMKEhoGco9LA==' + localization_method: 'waypoint' + 'eng2_subway_table_14': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'terse-crest-Toj82ugJt2Y029d7Afhq0A==' + localization_method: 'waypoint' + 'eng2_MainEntrance_upper': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + id: 'violet-ibis-HtCPm39SIDSzoJFVX88S+A==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'about-goat-EHRLqkk7XWmMXAdfcXHP5g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'ahead-mink-Fml31k5vw.Y2XR.7v1BSeg==' + localization_method: 'fiducial' + 'eng2_MainEntrance_lower': + name_en: 'Entrance' + name_jp: + - '2号館 正面入口' + - '2号館正面入口' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'aft-tick-clWAq8obxtWm64bTthYwMQ==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'rested-grouse-GzVp6htKyt6kdIcS+Tu6aA==' + localization_method: 'waypoint' + 'eng2_MainEntrance_outer': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'tilled-weevil-slAZLtX3U2NcqaINkGY0Sw==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'sown-pincer-NcgfKhWf9H+B3yWIKA8W7A==' + localization_method: 'waypoint' + 'eng2_1FEntrance_outer': + name_en: '1F Entrance' + name_jp: + - '2号館1階入口' + - '2号館 1階入口' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1fentrance_to_1FElevator.walk' + id: 'madcap-racoon-WmDiG93hDjlU6eDOUX2Jbg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'jammed-deer-L6mHZb2lmu+iQwGexsoAvw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_outside.walk' + id: 'bully-walrus-xsZTCuUHJ0dvzFGGmEZJxQ==' + localization_method: 'fiducial' + 'eng2_corner_northwest': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_outside.walk' + id: 'setose-angus-zflUwvXh2LTV96LNrrAHFg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'honey-agouti-3CfK3qU9XiErSV9gttqSKA==' + localization_method: 'fiducial' + 'eng2_GarbageCollectionArea_east': + name_en: 'Garbage collection area' + name_jp: 'ゴミ捨て場' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + id: 'podgy-oxen-t+KIiX.QiVaFGcphBsrekA==' + localization_method: 'waypoint' + 'eng2_GarbageCollectionArea_south': + name_en: 'Garbage collection area' + name_jp: 'ゴミ捨て場' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + id: 'atonic-frog-uCPybjcZAN3fQZHNx71GRw==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk' + id: 'boxy-hound-2E2IbEf63lXIM91zP.P2Ww==' + localization_method: 'waypoint' + 'eng2_7F_trashcan': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_73B2_trash.walk' + id: 'faddy-cuckoo-ZSHi6wGNKFCQhYetW6iLew==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7f_around.walk' + id: '' + localization_method: 'waypoint' + 'HongoMainGate': + name_en: 'Maingate' + name_jp: '正門' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'aglow-gadfly-tSusAUqUNtqVaKFO49pDbQ==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'regal-bison-qmAA7fyZh10L2bzoTNqUFQ==' + localization_method: 'waypoint' diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml new file mode 100644 index 0000000000..cba2b070a0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml @@ -0,0 +1,28 @@ +edges: + - from: 'nodeA' + to: 'nodeB' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} + - from: 'nodeB' + to: 'nodeC' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} + - from: 'nodeC' + to: 'nodeB' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} + - from: 'nodeB' + to: 'nodeA' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} +nodes: + 'nodeA': + name_en: 'nodeA' + 'nodeB': + name_en: 'nodeB' + 'nodeC': + name_en: 'nodeC' diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch new file mode 100644 index 0000000000..8986335870 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + initial_node_id: '$(arg initial_node_id)' + silent_mode: true + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch new file mode 100644 index 0000000000..4cc59161ec --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch @@ -0,0 +1,22 @@ + + + + + + + + initial_node_id: 'nodeA' + silent_mode: true + dry_mode: true + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py new file mode 100755 index 0000000000..af70e08639 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import rospy +from jsk_spot_behavior_manager.behavior_manager_node import BehaviorManagerNode + +def main(): + + rospy.init_node('behavior_manager_node') + node = BehaviorManagerNode() + node.run() + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py new file mode 100755 index 0000000000..85333b6ba6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python +# -*- encoding: utf-8 -*- + +import actionlib +import rospy + +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationGoal + +from sound_play.libsoundplay import SoundClient +from ros_speech_recognition import SpeechRecognitionClient + + +def main(): + + rospy.init_node('interactive_behavior_executor') + + speech_recognition_client = SpeechRecognitionClient() + sound_client = SoundClient(sound_action='/robotsound_jp', sound_topic='/robotsound_jp') + action_server_lead_to = actionlib.SimpleActionClient('~execute_behaviors', NavigationAction) + + node_list = rospy.get_param('~nodes', {}) + + while not rospy.is_shutdown(): + + rospy.loginfo('Asking package information') + while not rospy.is_shutdown(): + sound_client.say('行き先を教えてください。', blocking=True) + recogntion_result = speech_recognition_client.recognize() + if len(recogntion_result.transcript) == 0: + rospy.logerr('No matching node found from spoken \'{}\''.format(recogntion_result)) + sound_client.say('行き先がわかりませんでした', blocking=True) + continue + recognized_destination = recogntion_result.transcript[0] + target_node_candidates = {} + for node_id, value in node_list.items(): + try: + if not value.has_key('name_jp'): + continue + if type(value['name_jp']) is list: + # DO HOGE + for name in value['name_jp']: + if name.encode('utf-8') == recognized_destination: + target_node_candidates[node_id] = value + else: + if value['name_jp'].encode('utf-8') == recognized_destination: + target_node_candidates[node_id] = value + except Exception as e: + rospy.logerr('Error: {}'.format(e)) + if len(target_node_candidates) == 0: + rospy.logerr('No matching node found from spoken \'{}\''.format(recogntion_result)) + sound_client.say('行き先がわかりませんでした', blocking=True) + else: + rospy.loginfo('target_node_candidates: {}'.format(target_node_candidates)) + break + + target_node_id = target_node_candidates.keys()[0] + target_node_name_jp = node_list[target_node_id]['name_jp'].encode('utf-8') + + sound_client.say('{} へ移動します'.format(target_node_name_jp)) + + rospy.loginfo('executing behaviors to {}'.format(target_node_name_jp)) + action_server_lead_to.send_goal_and_wait(NavigationGoal(target_node_id=target_node_id)) + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml new file mode 100644 index 0000000000..bc941c558b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml @@ -0,0 +1,39 @@ + + + jsk_spot_behavior_manager + 1.1.0 + The spot_behavior_manager package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + networkx + roslaunch + rospkg + rospy + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + spot_autowalk_data + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py b/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py new file mode 100755 index 0000000000..f26b7742da --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# -*- encoding: utf-8 -*- + +import argparse +import yaml + +from graphviz import Digraph + +import gi +gi.require_version('Gtk','3.0') +from gi.repository import Gtk + +import xdot + + +class MyDotwindow(xdot.DotWindow): + + def __init__(self): + xdot.DotWindow.__init__(self) + self.dotwidget.connect('clicked', self.on_url_clicked) + + def on_url_clicked(self, widget, url, event): + dialog = Gtk.MessageDialog( + parent=self, + buttons=Gtk.ButtonsType.OK, + message_format='{} clicked'.format(url)) + dialog.connect('response', lambda dialog, response: dialog.destroy()) + dialog.run() + return True + + +def main(): + + parser = argparse.ArgumentParser() + parser.add_argument('filename') + parser.add_argument('--output') + args = parser.parse_args() + + filename = args.filename + + with open(filename, 'r') as f: + map_data = yaml.load(f, Loader=yaml.FullLoader) + + edges = map_data['edges'] + + list_node = [] + list_behavior_type = [] + list_color_for_bt = [] + + for edge in edges: + + if edge['from'] not in list_node: + list_node.append(edge['from']) + + if edge['to'] not in list_node: + list_node.append(edge['to']) + + if edge['behavior_type'] not in list_behavior_type: + list_behavior_type.append(edge['behavior_type']) + + for index, behavior_type in enumerate(list_behavior_type): + list_color_for_bt.append('{} 1.0 1.0'.format(1.0 * index / len(list_behavior_type))) + + dg = Digraph(format='svg') + + for node in list_node: + dg.node(node) + + for edge in edges: + dg.attr('edge', color=list_color_for_bt[list_behavior_type.index(edge['behavior_type'])]) + dg.edge(edge['from'], edge['to'], label=edge['behavior_type'].split('.')[2]) + + + if args.output: + dg.render(args.output) + else: + window = MyDotwindow() + window.set_dotcode(dg.source.encode('utf-8')) + window.connect('delete-event', Gtk.main_quit) + Gtk.main() + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/setup.py b/jsk_spot_robot/jsk_spot_behavior_manager/setup.py new file mode 100644 index 0000000000..799b412b34 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['jsk_spot_behavior_manager'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py new file mode 100644 index 0000000000..5c3fa4266f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py @@ -0,0 +1,51 @@ +# -*- coding: utf-8 -*- + +import rospy +import roslaunch + + +def load_behavior_class(class_string): + package_name = class_string.split('.')[0] + module_name = class_string.split('.')[1] + class_name = class_string.split('.')[2] + behavior_class = getattr( + getattr(__import__(package_name), module_name), class_name) + return behavior_class + + +class BaseBehavior(object): + + def __init__(self, spot_client, sound_client): + + self.spot_client = spot_client + self.sound_client = sound_client + + def run_initial(self, start_node, end_node, edge, pre_edge): + pass + + def run_main(self, start_node, end_node, edge, pre_edge): + pass + + def run_final(self, start_node, end_node, edge, pre_edge): + pass + + +class SimpleBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_initial() called') + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_main() called') + rospy.loginfo('start_node: {}'.format(start_node)) + rospy.loginfo('end_node: {}'.format(end_node)) + rospy.loginfo('edge: {}'.format(edge)) + rospy.loginfo('pre_edge: {}'.format(pre_edge)) + return True + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py new file mode 100644 index 0000000000..861dcf3c00 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py @@ -0,0 +1,99 @@ +# -*- coding: utf-8 -*- + +import networkx as nx + + +class GraphEdge: + + def __init__(self, + node_id_from, + node_id_to, + behavior_type, + cost, + properties + ): + self.node_id_from = node_id_from + self.node_id_to = node_id_to + self.behavior_type = behavior_type + self.cost = cost + self.properties = properties + + +class GraphNode: + + def __init__(self, + node_id, + properties + ): + self.node_id = node_id + self.properties = properties + + +class BehaviorGraph: + + # 現在の BehaviorGraph の仕様 + # 重み付きの有向グラフ + # あるノードからあるノードまでのエッジの数は 0 or 1 + + def __init__(self, raw_edges=[], raw_nodes={}): + + self.edges = {} + self.nodes = {} + self.network = nx.DiGraph() + + for raw_edge in raw_edges: + self.add_edge(raw_edge['from'], + raw_edge['to'], + raw_edge['behavior_type'], + int(raw_edge['cost']), + raw_edge['args']) + + for key, raw_node in raw_nodes.items(): + self.add_node(key, raw_node) + + def calcPath(self, node_id_from, node_id_to): + + try: + node_id_list = nx.shortest_path( + self.network, node_id_from, node_id_to) + except nx.NetworkXNoPath as e: + return None + path = [] + for index in range(len(node_id_list)-1): + path.append(self.edges[node_id_list[index], node_id_list[index+1]]) + return path + + def add_node(self, + node_id, + properties): + # TODO: もしすでにnodeがあれば,適切に上書きするようにする + # NOTICE: node と edge で整合性を保つようには実装されていない + self.nodes[node_id] = GraphNode(node_id, properties) + + def add_edge(self, + node_id_from, + node_id_to, + behavior_type, + cost, + args): + # TODO: もしすでにedgeがあれば,適切に上書きするようにする + # NOTICE: node と edge で整合性を保つようには実装されていない + edge = GraphEdge(node_id_from, + node_id_to, + behavior_type, + cost, + args) + self.edges[node_id_from, node_id_to] = edge + self.network.add_edge(node_id_from, + node_id_to, + weight=cost) + + def remove_node(self, + node_id): + del self.nodes[node_id] + + def remove_edge(self, + node_id_from, + node_id_to): + del self.edges[node_id_from, node_id_to] + self.network.remove_edge(node_id_from, node_id_to) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py new file mode 100644 index 0000000000..abb4049ef8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -0,0 +1,236 @@ +# -*- coding: utf-8 -*- + +import copy +import sys +import traceback + +import actionlib +import rospy +import roslaunch +import PyKDL + +from sound_play.libsoundplay import SoundClient +from spot_ros_client.libspotros import SpotRosClient + +from jsk_spot_behavior_manager.behavior_graph import BehaviorGraph +from jsk_spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class + +from std_msgs.msg import String +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationFeedback, NavigationResult, NavigationActionFeedback +from jsk_spot_behavior_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse + + +class BehaviorManagerNode(object): + + def __init__(self): + + # navigation dictonary + raw_edges = rospy.get_param('~map/edges') + raw_nodes = rospy.get_param('~map/nodes') + self.graph = BehaviorGraph(raw_edges, raw_nodes) + self.current_node_id = rospy.get_param('~initial_node_id') + self.pre_edge = None + self.anchor_pose = None + + # silent mode / dry mode + self.silent_mode = rospy.get_param('~silent_mode', False) + self.dry_mode = rospy.get_param('~dry_mode', False) + + # get target action name list for synchronizer + self.list_action_name_synchronizer = rospy.get_param( + '~action_names_synchronizer', []) + + # action clients + if self.dry_mode: + self.spot_client = None + self.sound_client = None + else: + self.spot_client = SpotRosClient() + self.sound_client = SoundClient( + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) + + # publisher + self.pub_current_node_id = rospy.Publisher( + '~current_node_id', String, queue_size=1) + + # reset service + self.service_reset_current_node_id = rospy.Service( + '~reset_current_node_id', + ResetCurrentNode, + self.handler_reset_current_node_id + ) + + # + self.set_anchor_pose() + + # + roslaunch.pmon._init_signal_handlers() + + # subscribers + self.list_behaviors_execution_actions = [] + for action_name in self.list_action_name_synchronizer: + self.list_behaviors_execution_actions.append( + rospy.Subscriber('{}/feedback'.format(action_name), NavigationActionFeedback, self.callback_synchronizer) + ) + + # action server + self.server_execute_behaviors = actionlib.SimpleActionServer( + '~execute_behaviors', + NavigationAction, + execute_cb=self.handler_execute_behaviors, + auto_start=False + ) + self.server_execute_behaviors.start() + + rospy.loginfo('Initialized!') + + def say(self, message, blocking=True): + + rospy.loginfo('Saying: {}'.format(message)) + if not self.silent_mode and not self.dry_mode: + self.sound_client.say(message, blocking=blocking) + + def set_anchor_pose(self): + + rospy.loginfo('Set new anchor pose') + if not self.dry_mode: + self.anchor_pose = self.spot_client.get_robot_pose() + + def go_back_to_anchor_pose(self): + + rospy.loginfo('Going back to an anchor pose') + if not self.dry_mode: + current_pose = self.spot_client.get_robot_pose() + if current_pose is None or self.anchor_pose is None: + return False + frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose + self.spot_client.go_pose(frame_current_to_anchor) + + def callback_synchronizer(self, msg): + + rospy.loginfo('Current node is updated to {}'.format(msg.feedback.current_node_id)) + self.current_node_id = msg.feedback.current_node_id + self.pre_edge = None + + def run(self): + + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + self.pub_current_node_id.publish(String(data=self.current_node_id)) + + def handler_reset_current_node_id(self, req): + + rospy.loginfo('current_node_id is reset to {}'.format( + req.current_node_id)) + self.current_node_id = req.current_node_id + self.pre_edge = None + self.set_anchor_pose() + return ResetCurrentNodeResponse(success=True) + + def handler_execute_behaviors(self, goal): + + rospy.loginfo('Behavior Action started. goal: {}'.format(goal)) + + current_graph = copy.deepcopy(self.graph) + while True: + # path calculation + path = current_graph.calcPath( + self.current_node_id, goal.target_node_id) + if path is None: + rospy.logerr('No path from {} to {}'.format( + self.current_node_id, goal.target_node_id)) + self.say('パスが見つかりませんでした') + result = NavigationResult( + success=False, message='No path found') + self.server_execute_behaviors.set_aborted(result) + return + else: + # navigation of edges in the path + self.say('目的地に向かいます', blocking=True) + self.go_back_to_anchor_pose() + success_navigation = True + for edge in path: + rospy.loginfo('Navigating Edge {}...'.format(edge)) + try: + if self.navigate_edge(edge): + rospy.loginfo('Edge {} succeeded.'.format(edge)) + self.current_node_id = edge.node_id_to + self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) + self.pre_edge = edge + self.set_anchor_pose() + else: + rospy.logwarn('Edge {} failed'.format(edge)) + self.say( + '移動に失敗しました。経路を探索し直します。', blocking=True) + self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) + current_graph.remove_edge( + edge.node_id_from, edge.node_id_to) + success_navigation = False + break + except Exception as e: + rospy.logerr( + 'Got an error while navigating edge {}: {}'.format(edge, sys.exc_info())) + traceback.print_exc() + self.say('エラーが発生しました', blocking=True) + self.pre_edge = None + result = NavigationResult( + success=False, + message='Got an error while navigating edge {}: {}'.format(edge, e)) + self.server_execute_behaviors.set_aborted(result) + self.anchor_pose = None + return + + if success_navigation: + break + + rospy.loginfo('Goal Reached!') + self.say('目的地に到着しました.', blocking=True) + result = NavigationResult(success=True, message='Goal Reached.') + self.server_execute_behaviors.set_succeeded(result) + self.set_anchor_pose() + return + + def navigate_edge(self, edge): + + # start node id validation + if self.current_node_id != edge.node_id_from: + rospy.logwarn( + 'current_node_id {} does not match node_id_from of edge ({})'.format( + self.current_node_id, + edge.node_id_from) + ) + return False + + # load behavior class + try: + behavior_class = load_behavior_class(edge.behavior_type) + behavior = behavior_class( + self.spot_client, + self.sound_client + ) + except Exception as e: + rospy.logerr( + 'Failed to load and initialize behavior class: {}'.format(sys.exc_info())) + traceback.print_exc() + self.say('行動クラスを読み込めませんでした', blocking=True) + self.pre_edge = None + return False + + node_from = self.graph.nodes[edge.node_id_from] + node_to = self.graph.nodes[edge.node_id_to] + + # Exception from behavior will be caught in handler + success_initial = behavior.run_initial( + node_from, node_to, edge, self.pre_edge) + if success_initial is False: + behavior.run_final(node_from, node_to, edge, self.pre_edge) + return False + else: + success_main = behavior.run_main( + node_from, node_to, edge, self.pre_edge) + behavior.run_final(node_from, node_to, edge, self.pre_edge) + return success_main diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt new file mode 100644 index 0000000000..30e82ce82f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_spot_behavior_msgs) + +find_package(catkin REQUIRED COMPONENTS genmsg actionlib actionlib_msgs) + +add_action_files( + DIRECTORY action + FILES Navigation.action +) + +add_service_files( + DIRECTORY srv + FILES ResetCurrentNode.srv +) + +generate_messages(DEPENDENCIES actionlib_msgs) + +catkin_package( + LIBRARIES jsk_spot_behavior_msgs + CATKIN_DEPENDS message_runtime +) diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action b/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action new file mode 100644 index 0000000000..8f41dfdaf9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action @@ -0,0 +1,6 @@ +string target_node_id +--- +bool success +string message +--- +string current_node_id diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml new file mode 100644 index 0000000000..be66e24a55 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml @@ -0,0 +1,24 @@ + + + jsk_spot_behavior_msgs + 1.1.0 + The jsk_spot_behavior_msgs package + + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + actionlib + actionlib_msgs + message_generation + + actionlib + actionlib_msgs + message_runtime + + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv b/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv new file mode 100644 index 0000000000..8fa01bd4c0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv @@ -0,0 +1,3 @@ +string current_node_id +--- +bool success diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt new file mode 100644 index 0000000000..fd9652fb99 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_basic_behaviors) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) + +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md new file mode 100644 index 0000000000..fa341940b3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md @@ -0,0 +1,124 @@ +# spot_basic_behaviors + +This package includes exaple implementation of base_behavior for spot_behavior_manager. + +## walk behavior + +This behavior enables Spot to walk a specified route in a autowalk data from a given waypoint to another waypoint. + +behavior name: `spot_basic_behaviors.walk_behavior.WalkBehavior` + +https://user-images.githubusercontent.com/9410362/124337233-896f6880-dbdc-11eb-9588-30d4a5193bbd.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior +- edge and nodes configuration for the behavior + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data + +e.g. + +```yaml +- from: 'eng2_73B2' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behavior.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' + +e.g. + +```yaml + 'eng2_73B2': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dyed-bat-t00VKo5XixLihCvpsZPRqw==' + localization_method: 'fiducial' +``` + +## elevator behavior + +This behavior enables Spot to use an elevator to move to anothor floor. + +behavior name: `spot_basic_behaviors.elevator_behavior.ElevatorBehavior` + +https://user-images.githubusercontent.com/9410362/124337679-cd636d00-dbde-11eb-8db9-c5fedfda4ad9.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior. +- apriltag pose information for elevator door frame detection. +- edge and nodes configuration for the behavior + +#### autowalk data + +To use this behavior, you need to record autowalk data while Spot riding on the elevator and getting off. + +Here is an example. + +![124134162-86318b00-dabd-11eb-86e3-092fcc5e8719](https://user-images.githubusercontent.com/9410362/124337765-364ae500-dbdf-11eb-83d3-a99e4025102e.png) + +#### apriltag pose information + +To use this behavior, an Fiducial (april tag) must be placed on the wall near the elevator doors. +And for elevator door opening and closing detection, transform from elevator foor frame ( center point on the ground ) to the fiducial is required. +Please see https://github.com/sktometometo/jsk_robot/blob/b166ef04cb954b175bedd5653af808be35e42121/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml#L44-L54 for examples + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data +- `rest_waypoint_id`: waypoint of rest position in a elevator + +e.g. + +```yaml +- from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behavior.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' +- `switchbot_device`: switchbot device name for elevator button + +e.g. + +```yaml + 'eng2_7FElevator': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator/down/button' +``` diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml new file mode 100644 index 0000000000..9bd12af3d9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml @@ -0,0 +1,9 @@ +tag_family: "tag36h11" # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 +tag_threads: 2 # default: 2 +tag_decimate: 1.0 # default: 1.0 +tag_blur: 0.0 # default: 0.0 +tag_refine_edges: 1 # default: 1 +tag_debug: 0 # default: 0 +max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) +# Other parameters +publish_tf: true # default: false diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml new file mode 100644 index 0000000000..6828fdd222 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml @@ -0,0 +1,94 @@ +# # Definitions of tags to detect +# +# ## General remarks +# +# - All length in meters +# - Ellipsis (...) signifies that the previous element can be repeated multiple times. +# +# ## Standalone tag definitions +# ### Remarks +# +# - name is optional +# +# ### Syntax +# +# standalone_tags: +# [ +# {id: ID, size: SIZE, name: NAME}, +# ... +# ] +standalone_tags: [] +# ## Tag bundle definitions +# ### Remarks +# +# - name is optional +# - x, y, z have default values of 0 thus they are optional +# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional +# +# ### Syntax +# +# tag_bundles: +# [ +# { +# name: 'CUSTOM_BUNDLE_NAME', +# layout: +# [ +# {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL}, +# ... +# ] +# }, +# ... +# ] +tag_bundles: + [ + { + name: "elevator_door_frame_raw", + layout: + [ + { + id: 219, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.6, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 211, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 202, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 216, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + ], + }, + ] diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml new file mode 100644 index 0000000000..bfc61225f6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml @@ -0,0 +1 @@ +token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch new file mode 100644 index 0000000000..e5c9f2d921 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + max_duration: 5.0 + timeout_duration: 0.05 + timer_duration: 0.1 + reference_frame_id: elevator_door_frame_raw + output_frame_id: elevator_door_frame + fixed_frame_id: odom + + + + + + + + approximate_sync: true + output_frame: body + input_topics: + - $(arg TOPIC_DEPTH_FRONT_RIGHT)/points + - $(arg TOPIC_DEPTH_FRONT_LEFT)/points + - $(arg TOPIC_DEPTH_RIGHT)/points + - $(arg TOPIC_DEPTH_LEFT)/points + - $(arg TOPIC_DEPTH_BACK)/points + + + + + + + + initial_pos: [0, 0, 0.5] + initial_rot: [0, 0, 0] + dimension_x: 0.5 + dimension_y: 0.5 + dimension_z: 0.8 + frame_id: elevator_door_frame + + + + + + + + + + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml new file mode 100644 index 0000000000..d11b5b7d7f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -0,0 +1,32 @@ + + + spot_basic_behaviors + 1.1.0 + The spot_basic_behaviors package + + Kei Okada + Koki Shinjo + Koki Shinjo + BSD + + catkin + + spot_behavior_manager + + apriltag_ros + depth_image_proc + geometry_msgs + jsk_pcl_ros + jsk_recognition_msgs + message_filters + sensor_msgs + std_msgs + switchbot_ros + tf2_geometry_msgs + tf2_ros + tf_relay + elevator_operation + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py new file mode 100644 index 0000000000..5518911cd8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_basic_behaviors'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py new file mode 100644 index 0000000000..077d19e4ad --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -0,0 +1,2 @@ +import spot_basic_behaviors.walk_behavior as walk_behavior +import spot_basic_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py new file mode 100644 index 0000000000..68d212fcbe --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -0,0 +1,279 @@ +# -*- coding: utf-8 -*- + +from jsk_spot_behavior_manager.base_behavior import BaseBehavior + +import actionlib +import roslaunch +import rospkg +import rospy + +import math + +from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import Quaternion +from std_msgs.msg import Float32, Bool, Int16 + + +class ElevatorBehavior(BaseBehavior): + + def door_point_cloud_callback(self, msg): + if len(msg.data) == 0: + self.door_is_open = True + else: + self.door_is_open = False + + def current_floor_callback(self, msg): + self.current_floor = msg.data + + def rest_elevator_callback(self, msg): + self.rest_elevator = msg.data + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_initial() called') + + self.silent_mode = rospy.get_param('~silent_mode', True) + + start_floor = start_node.properties['floor'] + rospy.loginfo("Start floor: {}".format(start_floor)) + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) + roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ + '/launch/elevator_detection.launch' + cli_args = [roslaunch_path, "initial_floor:={}".format(start_floor)] + roslaunch_args = cli_args[1:] + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for door openring checker + self.door_is_open = False + self.subscriber_door_check = None + + # elevator state + self.current_floor = None + self.rest_elevator = None + + self.subscriber_current_floor = rospy.Subscriber("/elevator_state_publisher/current_floor", Int16, self.current_floor_callback) + self.subscriber_rest_elevator = rospy.Subscriber("/elevator_state_publisher/rest_elevator", Bool, self.rest_elevator_callback) + + # value for switchbot + self.action_client_switchbot = actionlib.SimpleActionClient( + '/switchbot_ros/switch', + SwitchBotCommandAction + ) + + if not self.action_client_switchbot.wait_for_server(rospy.Duration(30)): + rospy.logerr('switchbot server seems to fail.') + return False + + try: + end_time = rospy.Time.now() + rospy.Duration(30) + except rospy.ROSException: + rospy.logerr("Door points topics are not published.") + return False + + try: + rospy.wait_for_message("/elevator_state_publisher/current_floor", Int16, timeout=rospy.Duration(30)) + except rospy.ROSException: + rospy.logerr("Current floor topics are not published.") + return False + + try: + rospy.wait_for_message("/elevator_state_publisher/rest_elevator", Bool, timeout=rospy.Duration(30)) + except rospy.ROSException: + rospy.logerr("Elevator state topic are not published.") + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_main() called') + + start_floor = start_node.properties['floor'] + + graph_name = edge.properties['graph'] + start_id = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['id'] + end_id = edge.properties['goal_waypoint_id'] + rest_waypoint_id = edge.properties['rest_waypoint_id'] + localization_method = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['localization_method'] + + end_floor = end_node.properties['floor'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False, 'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start door opening check from outside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # push button with switchbot + rospy.loginfo('calling elevator when riding...') + success_calling = False + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else start_node.properties['switchbot_device_down'] + switchbot_goal.command = 'press' + count = 0 + while True: + self.action_client_switchbot.send_goal(switchbot_goal) + if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): + break + count += 1 + if count >= 3: + rospy.logerr('switchbot calling failed.') + return False + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') + return False + rospy.loginfo('elevator calling when riding on has succeeded') + + # wait for elevator + rate = rospy.Rate(1) + door_open_count = 0 + while not rospy.is_shutdown(): + rate.sleep() + if self.door_is_open: + door_open_count += 1 + else: + door_open_count = 0 + if door_open_count >= 2: + break + rospy.loginfo('elevator door opened.') + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + # start navigation to rest point + rate = rospy.Rate(10) + if not self.silent_mode: + self.sound_client.say('エレベータに乗り込みます。ご注意ください。', blocking=False) + self.spot_client.navigate_to(rest_waypoint_id, blocking=False) + # call elevator from destination floor while + rospy.loginfo('calling elevator when getting off...') + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = end_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal(switchbot_goal) + ## + if not self.action_client_switchbot.wait_for_result(timeout=rospy.Duration(20)): + rospy.logerr('Switchbot timeout') + self.spot_client.wait_for_navigate_to_result() + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + result_switchbot = self.action_client_switchbot.get_result() + while not rospy.is_shutdown(): + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal_and_wait(switchbot_goal, execute_timeout=rospy.Duration(10)) + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(5)): + break + result_navigation = self.spot_client.get_navigate_to_result() + # recovery when riding on + if not result_navigation.success or not result_switchbot.done: + rospy.logerr('Failed to ride on a elevator. result_navigation: {}, result_switchbot: {}'.format( + result_navigation, result_switchbot)) + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + else: + rospy.loginfo('Riding on succeded.') + + if not self.silent_mode: + self.sound_client.say('{}階に行きます'.format(end_floor), blocking=False) + + # start door openning check from inside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # check if the door is closed + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if not self.door_is_open: + break + rospy.loginfo('elevator door closed') + + # check if the door is open and at the target floor + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + rospy.loginfo('door_is_open: {}, is_target_floor: {}, elevator stop: {}'.format( + self.door_is_open, end_floor == self.current_floor, self.rest_elevator)) + if self.door_is_open and end_floor == self.current_floor and self.rest_elevator: + break + rospy.loginfo('elevator door opened and at the target_floor') + + # dance before starting to move + self.spot_client.pub_body_pose(0.0, Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(-0.2, Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(0.0, Quaternion(w=1)) + self.spot_client.stand() + + # get off the elevator + self.spot_client.navigate_to(end_id, blocking=True) + result = self.spot_client.get_navigate_to_result() + + return result.success + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_finalize() called') + + if self.subscriber_door_check != None: + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + if self.subscriber_current_floor != None: + self.subscriber_current_floor.unregister() + self.subscriber_current_floor = None + + if self.subscriber_rest_elevator != None: + self.subscriber_rest_elevator.unregister() + self.subscriber_rest_elevator = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py new file mode 100644 index 0000000000..9f595d0255 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -0,0 +1,94 @@ +# -*- coding: utf-8 -*- + +from jsk_spot_behavior_manager.base_behavior import BaseBehavior + +import rospy + + +class WalkBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_initial() called') + self.silent_mode = rospy.get_param('~silent_mode', True) + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['id'] + end_id = list(filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + ))[0]['id'] + localization_method = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False, 'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start navigation + success = False + rate = rospy.Rate(10) + velocity_limit_linear_x = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_x', 1.0) + velocity_limit_linear_y = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_y', 1.0) + velocity_limit_angular_z = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_angular_z', 1.0) + if not self.silent_mode: + self.sound_client.say('移動します', blocking=True) + self.spot_client.navigate_to( + end_id, + velocity_limit=( + velocity_limit_linear_x, + velocity_limit_linear_y, + velocity_limit_angular_z + ), + blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + # recovery on failure + if not success: + if not self.silent_mode: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall new file mode 100644 index 0000000000..49d25f998d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -0,0 +1,96 @@ +# 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 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 +# insta360 need latest jsk_perception +- git: + local-name: jsk-ros-pkg/jsk_recognition + uri: jsk-ros-pkg/jsk_recognition.git +# Panoram image recognition need coral_usb, which is not release as apt package. +- git: + local-name: jsk-ros-pkg/coral_usb_ros + uri: https://github.com/jsk-ros-pkg/coral_usb_ros.git diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall.noetic b/jsk_spot_robot/jsk_spot_driver.rosinstall.noetic new file mode 100644 index 0000000000..9ad27be062 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall.noetic @@ -0,0 +1,68 @@ +# 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 +# +# 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 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 +# insta360 need latest jsk_perception +- git: + local-name: jsk-ros-pkg/jsk_recognition + uri: jsk-ros-pkg/jsk_recognition.git + version: master +# Panoram image recognition need coral_usb, which is not release as apt package. +- git: + local-name: jsk-ros-pkg/coral_usb_ros + uri: https://github.com/jsk-ros-pkg/coral_usb_ros.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..7552ce0555 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -0,0 +1,44 @@ +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() 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..eb14ab712a --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/app.installed @@ -0,0 +1,7 @@ +apps: +- app: jsk_spot_startup/hello_world + dispay: Hello World +- app: jsk_spot_startup/head_lead_demo + dispay: Head Lead Demo +- app: jsk_spot_startup/sample_elevator + dispay: Sample Elevator 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/apps/sample_elevator/sample_elevator.app b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app new file mode 100644 index 0000000000..2fdff70260 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app @@ -0,0 +1,75 @@ +display: Sample Elevator +platform: spot +launch: jsk_spot_startup/sample_elevator.xml +interface: jsk_spot_startup/sample_elevator.interface +plugins: + - name: panorama_video_recorder_plugin + type: app_recorder/video_recorder_plugin + launch_args: + video_path: /tmp + video_title: sample_elevator_panorama.avi + video_topic_name: /dual_fisheye_to_panorama/output + video_fps: 1.0 + - name: rosbag_recorder_plugin + type: app_recorder/rosbag_recorder_plugin + launch_args: + rosbag_path: /tmp + rosbag_title: sample_elevator_rosbag.bag + compress: true + rosbag_topic_names: + - /rosout + - /tf + - /tf_static + - /joint_states + - /odom + - /elevator_accel + - /elevator_accel_filtered + - /elevator_altitude + - /elevator_state_publisher/current_floor + - /elevator_state_publisher/elevator_movement + - /elevator_state_publisher/rest_elevator + - /m5stack_core2_driver/imu + - /m5stack_core2_driver/pressure + - /m5stack_core2_driver/temperature + - name: result_recorder_plugin + type: app_recorder/result_recorder_plugin + plugin_args: + result_path: /tmp + result_title: sample_elevator_result.yaml + - name: gdrive_uploader_plugin + type: app_uploader/gdrive_uploader_plugin + plugin_args: + upload_file_paths: + - /tmp/sample_elevator_result.yaml + - /tmp/sample_elevator_panorama.avi + - /tmp/sample_elevator_rosbag.bag + upload_file_titles: + - sample_elevator_result.yaml + - sample_elevator_panorama.avi + - sample_elevator_rosbag.bag + upload_parents_path: spot_sample_elevator + upload_server_name: /gdrive_server + - name: speech_notifier_plugin + type: app_notifier/speech_notifier_plugin + plugin_args: + client_name: /sound_play + - name: mail_notifier_plugin + type: app_notifier/mail_notifier_plugin + plugin_args: + mail_title: Spot Sample Elevator demo + use_timestamp_title: true + sender_address: belka@jsk.imi.i.u-tokyo.ac.jp + receiver_address: spot@jsk.imi.i.u-tokyo.ac.jp +plugin_order: + start_plugin_order: + - panorama_video_recorder_plugin + - rosbag_recorder_plugin + - result_recorder_plugin + - gdrive_uploader_plugin + - mail_notifier_plugin + stop_plugin_order: + - panorama_video_recorder_plugin + - rosbag_recorder_plugin + - result_recorder_plugin + - gdrive_uploader_plugin + - mail_notifier_plugin diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface new file mode 100644 index 0000000000..c27c9c296e --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface @@ -0,0 +1,3 @@ +published_topics: {} +subscribed_topics: {} + diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py new file mode 100755 index 0000000000..00372e57e8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 + +import rospy +from spot_ros_client.libspotros import SpotRosClient +import actionlib +from jsk_spot_behavior_msgs.msg import NavigationAction +from jsk_spot_behavior_msgs.msg import NavigationGoal + + +if __name__ == '__main__': + + rospy.init_node("sample_elevator") + client = SpotRosClient() + + behavior_client = actionlib.SimpleActionClient("/spot_behavior_manager_server/execute_behaviors", NavigationAction) + behavior_client.wait_for_server() + + client.claim() + client.power_on() + client.undock() + + client.reset_current_node('eng2_73B2_dock') + client.execute_behaviors(target_node_id="eng2_3FElevator") + client.wait_execute_behaviors_result() + result = client.get_execute_behaviors_result() + rospy.loginfo("Result forward: {} {}".format(result.success, result.message)) + + client.execute_behaviors(NavigationGoal(target_node_id="eng2_73B2_dock")) + result = client.get_execute_behaviors_result() + rospy.loginfo("Result backward: {} {}".format(result.success, result.message)) + + client.dock(521) diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml new file mode 100644 index 0000000000..f3497f43d5 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml @@ -0,0 +1,9 @@ + + + + + + + + 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..26e4953b77 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/panorama_detection.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch new file mode 100644 index 0000000000..0def9ef903 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/panorama_detection.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + score_thresh: 0.6 + model_file: $(arg model_file) + label_file: $(arg label_file) + image_transport: raw + device_id: $(arg device_id) + n_split: $(arg n_split) + overlap: $(arg overlap) + enable_visualization: false + + + + + + + + + + + + frame_fixed: "odom" + dimensions_labels: + person: [0.5, 0.5, 1.5] + car: [4.0, 4.0, 2.0] + truck: [4.0, 4.0, 3.0] + + + 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..8a4bcc949f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + 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..494c2c6dac --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..7fe113e663 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch @@ -0,0 +1,179 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 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/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/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..c72d0da068 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -0,0 +1,60 @@ + + + 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 + coral_usb + 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 + + jsk_spot_behavior_manager + spot_basic_behaviors + esp_now_ros + + rostest + + + + + diff --git a/jsk_spot_robot/jsk_spot_startup/scripts/deploy-scripts-and-services.sh b/jsk_spot_robot/jsk_spot_startup/scripts/deploy-scripts-and-services.sh new file mode 100755 index 0000000000..0d702f4cac --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/scripts/deploy-scripts-and-services.sh @@ -0,0 +1,15 @@ +#!/bin/bash + +PACKAGE_PATH=$(rospack find jsk_spot_startup) +SERVICE_SOURCE_PATH=$PACKAGE_PATH/services +SERVICE_DESTINATION_PATH=/etc/systemd/system + +# install systemd unit files +cd $SERVICE_SOURCE_PATH +for service_file in $(ls ./*); +do + sudo cp $service_file $SERVICE_DESTINATION_PATH/$service_file + sudo chmod 644 $SERVICE_DESTINATION_PATH/$service_file + sudo chown root:root $SERVICE_DESTINATION_PATH/$service_file +done +sudo systemctl daemon-reload diff --git a/jsk_spot_robot/jsk_spot_startup/scripts/install_udev_rules.sh b/jsk_spot_robot/jsk_spot_startup/scripts/install_udev_rules.sh new file mode 100755 index 0000000000..bab4be1279 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/scripts/install_udev_rules.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +sudo cp $(rospack find jsk_spot_startup)/udev_rules/* /etc/udev/rules.d/ +sudo udevadm control --reload-rules diff --git a/jsk_spot_robot/jsk_spot_startup/scripts/update-network-connection.sh b/jsk_spot_robot/jsk_spot_startup/scripts/update-network-connection.sh new file mode 100755 index 0000000000..c5edb7f505 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/scripts/update-network-connection.sh @@ -0,0 +1,157 @@ +#!/bin/bash + +IF_ETH=enxa0cec875af37 +IF_WIFI=wlxc006c31b1a80 +IF_LTE=enxf8b7975c750a +WIFI_PROFILE=sanshiro +CURRENT_CONNECTION= + +function existIF() { + interface_name=$1 + for DEV in `find /sys/devices -name net | grep -v virtual` + do + if [ `ls $DEV/` = $interface_name ]; then + echo 0 + return 0 + fi + done + echo 1 + return 1 +} + +function restartWIFI() { + sudo nmcli connection down $WIFI_PROFILE + #sudo ip l set $IF_WIFI down + #sudo ip l set $IF_WIFI up + sudo nmcli connection up $WIFI_PROFILE +} + +function updateRouteToETH() { + sudo ifmetric $IF_ETH 100 + sudo ifmetric $IF_WIFI 101 + sudo ifmetric $IF_LTE 101 +} + +function updateRouteToWIFI() { + sudo ifmetric $IF_WIFI 100 + sudo ifmetric $IF_ETH 102 + sudo ifmetric $IF_LTE 101 +} + +function updateRouteToLTE() { + sudo ifmetric $IF_LTE 100 + sudo ifmetric $IF_WIFI 101 + sudo ifmetric $IF_ETH 102 +} + +# get default route interface name which has most priority +function get_default_route() { + ip route | grep default | awk '{print $5}' | head -n 1 +} + +function updateConnectionToETH() { + if [ $(existIF $IF_ETH) = 0 ]; then + ping -c 1 -W 1 1.1.1.1 -I $IF_ETH > /dev/null 2>&1 + if [ $? = 0 ]; then + echo "Ethernet is online. switched to ethernet." + CURRENT_CONNECTION=$IF_ETH + updateRouteToETH + else + echo "Ethernet is offline.: $?" + fi + else + echo "No ethernet device is found." + fi +} + +function updateConnectionToWiFi() { + if [ $(existIF $IF_WIFI) = 0 ]; then + ping -c 1 -W 1 1.1.1.1 -I $IF_WIFI > /dev/null 2>&1 + if [ $? = 0 ]; then + echo "Wifi is online. switched to wifi." + CURRENT_CONNECTION=$IF_WIFI + updateRouteToWIFI + else + echo "Wifi is offline.: $?" + fi + else + echo "No wifi device is found." + fi +} + +function updateConnectionToLTE() { + if [ $(existIF $IF_LTE) = 0 ]; then + echo "Connection type switched to lte" + CURRENT_CONNECTION=$IF_LTE + updateRouteToLTE + else + echo "No lte device is found" + fi + + echo "No network device found." +} + +function updateConnection() { + echo "Connection updating." + + # Check default route is current connection + default_route=$(get_default_route) + if [ $default_route = $CURRENT_CONNECTION ]; then + echo "Current connection $CURRENT_CONNECTION is default route. No need to update." + else + echo "Current connection $CURRENT_CONNECTION is not default route. Trying to update." + if [ $CURRENT_CONNECTION = $IF_ETH ]; then + updateConnectionToETH + elif [ $CURRENT_CONNECTION = $IF_WIFI ]; then + updateConnectionToWiFi + elif [ $CURRENT_CONNECTION = $IF_LTE ]; then + updateConnectionToLTE + else + echo "Unknown current connection: $CURRENT_CONNECTION" + fi + fi + + # Reconnect WIFI + if [ $(existIF $IF_WIFI) = 0 ]; then + ping -c 1 -W 1 1.1.1.1 -I $IF_WIFI > /dev/null 2>&1 + if [ $? = 0 ]; then + if [ $CURRENT_CONNECTION = $IF_WIFI ]; then + echo "wifi connected" + else + echo "wifi connected. But current connection $CURRENT_CONNECTION is not wifi $IF_WIFI. So update" + updateConnectionToWiFi + fi + else + echo "wifi not connected. trying to connect $WIFI_PROFILE" + restartWIFI + updateConnectionToWiFi + fi + else + echo "wifi device not found. skipped wifi reconnection." + fi + + # Check internet connection + ping -c 1 -W 1 1.1.1.1 > /dev/null 2>&1 + if [ $? = 0 ]; then + echo "Network connected" + return 0 + else + echo "Network disconnected. Trying to reconnect." + fi + + # First, try ethernet to connect with Ethernet if available + updateConnectionToETH + + # Second, try wifi to connect with Wi-Fi if available + updateConnectionToWiFi + + # connect with LTE if available + updateConnectionToLTE +} + +CURRENT_CONNECTION=$IF_ETH +while : +do + sleep 5 + updateConnection +done diff --git a/jsk_spot_robot/jsk_spot_startup/services/jsk-spot-utils-network.service b/jsk_spot_robot/jsk_spot_startup/services/jsk-spot-utils-network.service new file mode 100644 index 0000000000..8137664641 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/services/jsk-spot-utils-network.service @@ -0,0 +1,12 @@ +[Unit] +Description=JSK Network Service + +[Service] +Type=simple +ExecStart=/bin/bash -c ". /opt/ros/melodic/setup.bash && . /home/sktometometo/ros/ws_main_extend/devel/setup.bash && rosrun jsk_spot_startup update-network-connection.sh" +ExecStop=/bin/kill -WINCH ${MAINPID} +KillMode=control-group +KillSignal=SIGTERM + +[Install] +WantedBy=multi-user.target diff --git a/jsk_spot_robot/jsk_spot_startup/udev_rules/75-spot-m5stack.rules b/jsk_spot_robot/jsk_spot_startup/udev_rules/75-spot-m5stack.rules new file mode 100644 index 0000000000..1c5a8c97f6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/udev_rules/75-spot-m5stack.rules @@ -0,0 +1,3 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", SYMLINK+="elevator_device", GROUP="dialout" +SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", SYMLINK+="esp_now_ros_interface", GROUP="dialout" + diff --git a/jsk_spot_robot/jsk_spot_startup/udev_rules/89-spot-eye-devices.rules b/jsk_spot_robot/jsk_spot_startup/udev_rules/89-spot-eye-devices.rules new file mode 100644 index 0000000000..fd1615fc24 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/udev_rules/89-spot-eye-devices.rules @@ -0,0 +1,4 @@ +# M5Stamp +SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", ATTRS{serial}=="5319011411", SYMLINK+="eye_left", GROUP="dialout" +SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", ATTRS{serial}=="537A013618", SYMLINK+="eye_right", GROUP="dialout" + 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/spot_autowalk_data/CMakeLists.txt b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt new file mode 100644 index 0000000000..7dea41d2c2 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_autowalk_data) + +find_package(catkin REQUIRED COMPONENTS + catkin_virtualenv +) + +catkin_generate_virtualenv( + PYTHON_INTERPRETER python3 + USE_SYSTEM_PACKAGES FALSE + CHECK_VENV FALSE +) + +add_custom_target(${PROJECT_NAME}_download_autowalk_data ALL COMMAND python$ENV{ROS_PYTHON_VERSION} ${PROJECT_SOURCE_DIR}/scripts/download_autowalk_data.py) + +catkin_package() + +install(DIRECTORY autowalk + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS scripts/view_map.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/spot_autowalk_data/README.md b/jsk_spot_robot/spot_autowalk_data/README.md new file mode 100644 index 0000000000..31552a9930 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/README.md @@ -0,0 +1,116 @@ +# spot_autowalk_data + +This package includes autowalk data downloaded from public Googledrive folder with jsk_data. + +## scripts + +### download_autowalk_data.py + +this scripts download autowalk data from google drive. this script will be run when catkin build. + +### view_map.py + +this script visualize autowalk data and each waypoint id + +#### Usage + +when you want to visualize autowalk data without waypoint id, just run + +``` +rosrun spot_autowalk_data view_map.py +``` + +when you want to visualize autowalk data with waypoint id, run the script with `--draw-id` option + +``` +rosrun spot_autowalk_data view_map.py --draw-id +``` + +## autowalk data in this packages + +### eng2_2FElevator_to_2FEntrance.walk + +![eng2_2FElevator_to_2FEntrance](https://user-images.githubusercontent.com/9410362/124133634-fbe92700-dabc-11eb-90d5-84b434729233.png) + +### eng2_2FElevator_to_MainGate + +![eng2_2FElevator_to_MainGate](https://user-images.githubusercontent.com/9410362/124133994-55e9ec80-dabd-11eb-8ba1-1bd118146244.png) + +### eng2_2Felevator_to_eng8_2FElevator + +![eng2_2FElevator_to_eng8_2FElevator](https://user-images.githubusercontent.com/9410362/124134033-600beb00-dabd-11eb-823c-483e75318f8b.png) + +### eng2_73B2_to_2FElevator + +![eng2_73B2_to_7FElevator](https://user-images.githubusercontent.com/9410362/135750882-3d72e538-316c-40a4-b209-cffd06a774a4.png) + +### eng2_73B2_to_81C1 + +![eng2_73B2_to_81C1](https://user-images.githubusercontent.com/9410362/124134118-7b76f600-dabd-11eb-829b-c36f105d7051.png) + +### eng2_elevator_7FElevator_to_2FElevator + +![eng2_elevator_7FElevator_to_2FElevator](https://user-images.githubusercontent.com/9410362/124134162-86318b00-dabd-11eb-86e3-092fcc5e8719.png) + +### eng_TelephoneBox_to_HongoMainGate + +![eng_TelephoneBox_to_HongoMainGate](https://user-images.githubusercontent.com/9410362/124134207-90ec2000-dabd-11eb-988b-62ffbf98ca67.png) + +### eng2_elevator_3FElevator_to_2FElevator.walk + +![eng2_elevator_3FElevator_to_2FElevator walk](https://user-images.githubusercontent.com/9410362/124587250-6077fd80-de92-11eb-8014-2852dfa7f60a.png) + +### eng2_3FElevator_to_Mech_Office.walk + +![eng2_3FElevator_to_Mech_Office walk](https://user-images.githubusercontent.com/9410362/124587299-6f5eb000-de92-11eb-9427-0ed88e6ca414.png) + +### eng2_2FElevator_to_MainEntrance.walk + +![eng2_2FElevator_to_MainEntrance](https://user-images.githubusercontent.com/9410362/129509474-c3429db1-6a6f-4076-bbbb-c3fb27bc3167.png) + +### eng2_MainEntrance_to_HongoMainGate.walk + +![eng2_MainEntrance_to_HongoMainGate](https://user-images.githubusercontent.com/9410362/129509500-1bde2818-66c7-4770-a87d-88c4f9d65ae3.png) + +### eng2_MainEntrance_to_HongoMainGate_Daylight.walk + +![eng2_MainEntrance_to_HongoMainGate_Daylight](https://user-images.githubusercontent.com/9410362/129997430-620ee1dc-6659-4028-acd7-f80b7539364a.png) + +### eng2_2FElevator_to_subway.walk + +![eng2_2FElevator_to_subway](https://user-images.githubusercontent.com/9410362/132933793-63c627bc-e210-4922-9fa2-4c1538b6f45e.png) + +### eng2_subway_tablenavigation.walk + +![eng2_subway_tablenavigation](https://user-images.githubusercontent.com/9410362/132933800-6f68566e-8471-4ac4-a030-94414f4a1b83.png) + +### eng2_subway_tablenavigation_02.walk + +![eng2_subway_tablenavigation_02](https://user-images.githubusercontent.com/9410362/132933805-f3c10dc3-01da-429c-96b8-74225401d703.png) + +### eng2_73b2kitchen.walk + +![eng2_73b2kitchen.walk](https://user-images.githubusercontent.com/67531577/136342909-7979a058-538e-4f76-a705-0186439c478a.png) + +### eng2_73b2trashcans.walk + +![eng2_73b2trashcans.walk](https://user-images.githubusercontent.com/67531577/136343017-74229050-1b36-41e3-bba1-fcad53e92df7.png) + +## How to add autowalk data in this package + +1. record a new autowalk data +2. archive and compress an autowalk data directory (e.g. `autowalk.walk`) to `autowalk.walk.tar.gz` by `tar -zcvf autowalk.walk.tar.gz autowalk.walk` +3. upload `autowalk.walk.tar.gz` to [jsk_data public google drive folger](https://drive.google.com/drive/folders/1PPOZDoWbJnxM7XkPMPP3iKQqrji56Vgq?usp=sharing) +4. add new entry to download_autowalk_data.py like + +``` + download_data( + pkg_name=PKG, + path='autowalk/autowalk.walk.tar.gz', + url='https://drive.google.com/uc?id=', + md5='', + extract=True + ) +``` + +5. update README.md diff --git a/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore new file mode 100644 index 0000000000..72e8ffc0db --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore @@ -0,0 +1 @@ +* diff --git a/jsk_spot_robot/spot_autowalk_data/package.xml b/jsk_spot_robot/spot_autowalk_data/package.xml new file mode 100644 index 0000000000..a77a1f0987 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/package.xml @@ -0,0 +1,21 @@ + + + spot_autowalk_data + 1.1.0 + The spot_autowalk_data package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + jsk_data + catkin_virtualenv + python-gdown-pip + + + requirements.txt + + diff --git a/jsk_spot_robot/spot_autowalk_data/requirements.txt b/jsk_spot_robot/spot_autowalk_data/requirements.txt new file mode 100644 index 0000000000..934b434976 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/requirements.txt @@ -0,0 +1,4 @@ +bosdyn-client +bosdyn-mission +bosdyn-choreography-client +vtk diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py new file mode 100755 index 0000000000..78a5b07b06 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py @@ -0,0 +1,306 @@ +#!/usr/bin/env python + +from jsk_data import download_data + + +def main(): + PKG = 'spot_autowalk_data' + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_81C1.walk.tar.gz', + url='https://drive.google.com/uc?id=1IDTCP7n4LCowizW3mFQvTOQtE4qOH9tx', + md5='65f09629c0ac2aa21df6f179c9875bd0', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_7FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1O8o6voq2v8WenfaUYcmpSU-IwJSsXW5_', + md5='b75ff21caf475a044e7ea197ffe9e378', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=12MOg5okckmQlYiM6flkdeMYxqjn9-C9l', + md5='67ae3210cbfb55791fff6494f84abb3b', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1iyx0y1dPu4HUPMNepR_VaZd_WEaC5Lku', + md5='915916d084abd54c2c17f0738a726da3', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_2FEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1cYUn_qnRslWuH0ZMEBN6ovqmdcUB0MzY', + md5='78c6e1e8e5967b216c9f53e38893750e', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_MainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=1CIuStpjxIA188MLxUsfUjI-47Ev36Wiv', + md5='47d669bcb1394b97c95e5d77f78da3e5', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_eng8_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1oyU1ufqy9gryPXw8YZ45Ff7AR8K825dT', + md5='ac9a67567104df2ddd78b8c53fa61ba2', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng_TelephoneBox_to_HongoMainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=120WC6SE4C_9XIvy0j3HEraljIigjcJ5U', + md5='6f89cd74bea3934171e0ae720747da24', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_3FElevator_to_Mech_Office.walk.tar.gz', + url='https://drive.google.com/uc?id=10pWP1qnbr5TCfQVUeOHnDacdwQ3X3Awh', + md5='52564afd6471c9551c8f77ac031a366d', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_3FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1OFB38ISYjTtu9A87QtfYyVLhtcGnpu2a', + md5='e8ed341b22c5eb66373d29b580bede05', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_MainEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1e5hCLtfOlDYWhrQoefYGxz2AnjaSfs_f', + md5='e79f25769b702c203d4f9229c81a1f89', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MainEntrance_to_HongoMainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=13mG7iFMkFWN04S7BeR2rPwvpL-0cTgko', + md5='e0260203d5a3fa4958014b4149c3749a', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk.tar.gz', + url='https://drive.google.com/uc?id=1EHpKaMBa8rnRZRyK8KwTK3727cxNqph4', + md5='43cff60ed9dc444d3d7de0e4ef304d0f', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/73b2_inside.walk.tar.gz', + url='https://drive.google.com/uc?id=14BPDOJBNS2v5EoUyIZ9gDwm4VFXyj-FD', + md5='5b45fcd7271f42f32414602145b77bd8', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk.tar.gz', + url='https://drive.google.com/uc?id=1eZbBLwzl9nVpSAebtux5nbN1B4UfGEoh', + md5='01ef288775d6c21a634a17fa427e4900', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk.tar.gz', + url='https://drive.google.com/uc?id=1_zySu-e94wzHLaLYTbQuUd9A0Sb8x9NQ', + md5='fc9fb3161b745ff5f7b62be8977f7e56', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_7FElevator_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1kLSNCn2lhkcdOFBaYSVHUFzklhVz_jL8', + md5='fa5a26677d9136ef4671d7a32ca59097', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk.tar.gz', + url='https://drive.google.com/uc?id=1oSFjkwG0EcVXapMVgLlc_i1VtcWXwEwE', + md5='72ba498fe56700bee31c8ce166e77008', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_subway.walk.tar.gz', + url='https://drive.google.com/uc?id=1uRZA0JnFyenVcZDtXhQGzqGAIcO9lwop', + md5='ef40ee8677ef68b4beee4e516a11d1f1', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_subway_tablenavigation.walk.tar.gz', + url='https://drive.google.com/uc?id=1Lye5ZfFLgmU-G8TdevSiHG72kN04XnFt', + md5='e65a06fbb7b2037659bf657ebb4e5d36', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_subway_tablenavigation_02.walk.tar.gz', + url='https://drive.google.com/uc?id=1GgIra90yX21IMYYL8VgrY9YJpGevA1ek', + md5='08eca6ac0e1310236f32f229370aa161', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_GarbageCollectionArea_inside_clockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1Ur6pZZKxZQsP_3zsbikWFkOWOB1KsKlb', + md5='da542fc9b4376378dc2a644454175e06', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_inside_clockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1ipkw696vWe_DeO87ZcoQmTgFUE7adpL7', + md5='71d5555f4f6772a9022768fe5dabf813', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_inside_counterclockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1pIrpc_q9GVCtH4YO2-E2rXSEf1yTfP4U', + md5='d0f279bc94b194600bccf3a1c5d98c35', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1rUCVbhbSmUd4OEA6W35quKs-6CJIJW7b', + md5='2fdfc99d640bc9c796c36e3127078fcf', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73b2kitchen.walk.tar.gz', + url='https://drive.google.com/uc?id=1JrDRIQpGpB6sqofIyk9eR03KsE2EJ5Ev', + md5='8666d1c8b21ae7ffb88978ed0b1bfb33', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73b2trashcans.walk.tar.gz', + url='https://drive.google.com/uc?id=1w19wmLsQHMM9ErjK4mN-zstWkvrHITLO', + md5='264c2e561a0c128a26b490059e10e30c', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_inside.walk.tar.gz', + url='https://drive.google.com/uc?id=1dSbU0tADrxwGLEBywMt312NXUgFHtsCy', + md5='5460bc55c34824ce75722881a4e0b77d', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_to_MainEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1aepZnkxV8SLo9a8DdCEU9KnNoyjG7sZF', + md5='4aba2398e8eae87509820180b2e89cb0', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_to_3felevator_subwayside.walk.tar.gz', + url='https://drive.google.com/uc?id=1twc6Ui5z7nqqZHMHPeynNAyE19PKyihN', + md5='6d720c0c6f5692a205b386c9248d69f6', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1fentrance_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1Q7gX8Cg58dUtV0he7R8fxYciinYnAmd9', + md5='b82831bd317014367af5f28614368bd9', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2fentrance_to_1fentrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1eKdUYOyWEO2lz07uUlpayb6StFC_hCco', + md5='0b7c8e960825014a3d3b4637265e966c', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_83A2.walk.tar.gz', + url='https://drive.google.com/uc?id=1GwOD53C7I8dpi3YcrrMcHlHbN3k_Auj1', + md5='aa063402ab604b012ae7c7a4123fb5ac', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7FElevator_to_73B2_trash.walk.tar.gz', + url='https://drive.google.com/uc?id=1gmkQa5oTSIJAKST8SBLev_nui_XN40dE', + md5='d8b0711270fb2ae67159d28d50bb5231', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7f_around.walk.tar.gz', + url='https://drive.google.com/uc?id=1lKELjOiUU02RnrK2V7AWos8GkJqylEKu', + md5='daae9c4c7555780b148baad7906b6710', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_outside.walk.tar.gz', + url='https://drive.google.com/uc?id=1E9QJJYz7Puk72KL2lFRTKfPtRiCHZYYw', + md5='f44de8d70da1bd1a512b5e1002335c81', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_8F.walk.tar.gz', + url='https://drive.google.com/uc?id=1LPluVprB4FCZ8L7ePpeQM8qFzijHQ3Ic', + md5='1032a79f9ac5201caddcefcb867637a5', + extract=True + ) + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py new file mode 100644 index 0000000000..16891ea280 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 +# Copyright (c) 2021 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 + +from vtk.util import numpy_support +import google.protobuf.timestamp_pb2 +import math +import numpy as np +import numpy.linalg +import os +import sys +import time +import vtk + +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.client.frame_helpers import * +from bosdyn.client.math_helpers import * +""" +This example shows how to load and view a graph nav map. + +""" + + +def numpy_to_poly_data(pts): + """ + Converts numpy array data into vtk poly data. + :param pts: the numpy array to convert (3 x N). + :return: a vtkPolyData. + """ + pd = vtk.vtkPolyData() + pd.SetPoints(vtk.vtkPoints()) + # Makes a deep copy + pd.GetPoints().SetData(numpy_support.numpy_to_vtk(pts.copy())) + + f = vtk.vtkVertexGlyphFilter() + f.SetInputData(pd) + f.Update() + pd = f.GetOutput() + + return pd + + +def mat_to_vtk(mat): + """ + Converts a 4x4 homogenous transform into a vtk transform object. + :param mat: A 4x4 homogenous transform (numpy array). + :return: A VTK transform object representing the transform. + """ + t = vtk.vtkTransform() + t.SetMatrix(mat.flatten()) + return t + + +def vtk_to_mat(transform): + """ + Converts a VTK transform object to 4x4 homogenous numpy matrix. + :param transform: an object of type vtkTransform + : return: a numpy array with a 4x4 matrix representation of the transform. + """ + tf_matrix = transform.GetMatrix() + out = np.array(np.eye(4)) + for r in range(4): + for c in range(4): + out[r, c] = tf_matrix.GetElement(r, c) + return out + + +def api_to_vtk_se3_pose(se3_pose): + """ + Convert a bosdyn SDK SE3Pose into a VTK pose. + :param se3_pose: the bosdyn SDK SE3 Pose. + :return: A VTK pose representing the bosdyn SDK SE3 Pose. + """ + return mat_to_vtk(se3_pose.to_matrix()) + + +def create_fiducial_object(world_object, waypoint, renderer): + """ + Creates a VTK object representing a fiducial. + :param world_object: A WorldObject representing a fiducial. + :param waypoint: The waypoint the AprilTag is associated with. + :param renderer: The VTK renderer + :return: a tuple of (vtkActor, 4x4 homogenous transform) representing the vtk actor for the fiducial, and its + transform w.r.t the waypoint. + """ + fiducial_object = world_object.apriltag_properties + odom_tform_fiducial_filtered = get_a_tform_b( + world_object.transforms_snapshot, ODOM_FRAME_NAME, + world_object.apriltag_properties.frame_name_fiducial_filtered) + waypoint_tform_odom = SE3Pose.from_obj(waypoint.waypoint_tform_ko) + waypoint_tform_fiducial_filtered = api_to_vtk_se3_pose( + waypoint_tform_odom * odom_tform_fiducial_filtered) + plane_source = vtk.vtkPlaneSource() + plane_source.SetCenter(0.0, 0.0, 0.0) + plane_source.SetNormal(0.0, 0.0, 1.0) + plane_source.Update() + plane = plane_source.GetOutput() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(plane) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetColor(0.5, 0.7, 0.9) + actor.SetScale(fiducial_object.dimensions.x, fiducial_object.dimensions.y, 1.0) + renderer.AddActor(actor) + return actor, waypoint_tform_fiducial_filtered + + +def create_point_cloud_object(waypoints, snapshots, waypoint_id): + """ + Create a VTK object representing the point cloud in a snapshot. Note that in graph_nav, "point cloud" refers to the + feature cloud of a waypoint -- that is, a collection of visual features observed by all five cameras at a particular + point in time. The visual features are associated with points that are rigidly attached to a waypoint. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of waypoint snapshot ID to waypoint snapshot. + :param waypoint_id: the waypoint ID of the waypoint whose point cloud we want to render. + :return: a vtkActor containing the point cloud data. + """ + wp = waypoints[waypoint_id] + snapshot = snapshots[wp.snapshot_id] + cloud = snapshot.point_cloud + odom_tform_cloud = get_a_tform_b(cloud.source.transforms_snapshot, ODOM_FRAME_NAME, + cloud.source.frame_name_sensor) + waypoint_tform_odom = SE3Pose.from_obj(wp.waypoint_tform_ko) + waypoint_tform_cloud = api_to_vtk_se3_pose(waypoint_tform_odom * odom_tform_cloud) + + point_cloud_data = np.frombuffer(cloud.data, dtype=np.float32).reshape(int(cloud.num_points), 3) + poly_data = numpy_to_poly_data(point_cloud_data) + arr = vtk.vtkFloatArray() + for i in range(cloud.num_points): + arr.InsertNextValue(point_cloud_data[i, 2]) + arr.SetName("z_coord") + poly_data.GetPointData().AddArray(arr) + poly_data.GetPointData().SetActiveScalars("z_coord") + actor = vtk.vtkActor() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(poly_data) + mapper.ScalarVisibilityOn() + actor.SetMapper(mapper) + actor.GetProperty().SetPointSize(2) + actor.SetUserTransform(waypoint_tform_cloud) + return actor + + +def create_waypoint_object(renderer, waypoints, snapshots, waypoint_id): + """ + Creates a VTK object representing a waypoint and its point cloud. + :param renderer: The VTK renderer. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of snapshot ID to snapshot. + :param waypoint_id: the waypoint id of the waypoint object we wish to create. + :return: A vtkAssembly representing the waypoint (an axis) and its point cloud. + """ + assembly = vtk.vtkAssembly() + actor = vtk.vtkAxesActor() + actor.SetXAxisLabelText("") + actor.SetYAxisLabelText("") + actor.SetZAxisLabelText("") + actor.SetTotalLength(0.2, 0.2, 0.2) + point_cloud_actor = create_point_cloud_object(waypoints, snapshots, waypoint_id) + assembly.AddPart(actor) + assembly.AddPart(point_cloud_actor) + renderer.AddActor(assembly) + return assembly + + +def make_line(pt_A, pt_B, renderer): + """ + Creates a VTK object which is a white line between two points. + :param pt_A: starting point of the line. + :param pt_B: ending point of the line. + :param renderer: the VTK renderer. + :return: A VTK object that is a while line between pt_A and pt_B. + """ + line_source = vtk.vtkLineSource() + line_source.SetPoint1(pt_A[0], pt_A[1], pt_A[2]) + line_source.SetPoint2(pt_B[0], pt_B[1], pt_B[2]) + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputConnection(line_source.GetOutputPort()) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetLineWidth(2) + actor.GetProperty().SetColor(0.7, 0.7, 0.7) + renderer.AddActor(actor) + return actor + + +def make_text(name, pt, renderer): + """ + Creates white text on a black background at a particular point. + :param name: The text to display. + :param pt: The point in the world where the text will be displayed. + :param renderer: The VTK renderer + :return: the vtkActor representing the text. + """ + actor = vtk.vtkTextActor() + actor.SetInput(name) + prop = actor.GetTextProperty() + prop.SetBackgroundColor(0.0, 0.0, 0.0) + prop.SetBackgroundOpacity(0.5) + prop.SetFontSize(16) + coord = actor.GetPositionCoordinate() + coord.SetCoordinateSystemToWorld() + coord.SetValue((pt[0], pt[1], pt[2])) + + renderer.AddActor(actor) + return actor + + +def create_edge_object(curr_wp_tform_to_wp, world_tform_curr_wp, renderer): + # Concatenate the edge transform. + world_tform_to_wp = np.dot(world_tform_curr_wp, curr_wp_tform_to_wp) + # Make a line between the current waypoint and the neighbor. + make_line(world_tform_curr_wp[:3, 3], world_tform_to_wp[:3, 3], renderer) + return world_tform_to_wp + + +def load_map(path): + """ + Load a map from the given file path. + :param path: Path to the root directory of the map. + :return: the graph, waypoints, waypoint snapshots and edge snapshots. + """ + with open(os.path.join(path, "graph"), "rb") as graph_file: + # Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the + # edges between them. + data = graph_file.read() + current_graph = map_pb2.Graph() + current_graph.ParseFromString(data) + + # Set up maps from waypoint ID to waypoints, edges, snapshots, etc. + current_waypoints = {} + current_waypoint_snapshots = {} + current_edge_snapshots = {} + + # For each waypoint, load any snapshot associated with it. + for waypoint in current_graph.waypoints: + current_waypoints[waypoint.id] = waypoint + + # Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large. + file_name = os.path.join(path, "waypoint_snapshots", waypoint.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + # Similarly, edges have snapshot data. + for edge in current_graph.edges: + + if not edge.snapshot_id: + continue # Rare case that snapshot id is empty + + file_name = os.path.join(path, "edge_snapshots", edge.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + current_edge_snapshots[edge_snapshot.id] = edge_snapshot + print("Loaded graph with {} waypoints and {} edges".format( + len(current_graph.waypoints), len(current_graph.edges))) + return (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) + + +def create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, renderer, draw_id=True): + """ + Creates all the VTK objects associated with the graph. + :param current_graph: the graph to use. + :param current_waypoint_snapshots: dict from snapshot id to snapshot. + :param current_waypoints: dict from waypoint id to waypoint. + :param renderer: The VTK renderer + :return: the average position in world space of all the waypoints. + """ + waypoint_objects = {} + # Create VTK objects associated with each waypoint. + for waypoint in current_graph.waypoints: + waypoint_objects[waypoint.id] = create_waypoint_object(renderer, current_waypoints, + current_waypoint_snapshots, + waypoint.id) + # Now, perform a breadth first search of the graph starting from an arbitrary waypoint. Graph nav graphs + # have no global reference frame. The only thing we can say about waypoints is that they have relative + # transformations to their neighbors via edges. So the goal is to get the whole graph into a global reference + # frame centered on some waypoint as the origin. + queue = [] + queue.append((current_graph.waypoints[0], np.eye(4))) + visited = {} + # Get the camera in the ballpark of the right position by centering it on the average position of a waypoint. + avg_pos = np.array([0.0, 0.0, 0.0]) + + # Breadth first search. + while len(queue) > 0: + # Visit a waypoint. + curr_element = queue[0] + queue.pop(0) + curr_waypoint = curr_element[0] + visited[curr_waypoint.id] = True + + # We now know the global pose of this waypoint, so set the pose. + waypoint_objects[curr_waypoint.id].SetUserTransform(mat_to_vtk(curr_element[1])) + world_tform_current_waypoint = curr_element[1] + # Add text to the waypoint. + print('{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id)) + if draw_id: + make_text( + '{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id), + world_tform_current_waypoint[:3, 3], + renderer) + + # For each fiducial in the waypoint's snapshot, add an object at the world pose of that fiducial. + if (curr_waypoint.snapshot_id in current_waypoint_snapshots): + snapshot = current_waypoint_snapshots[curr_waypoint.snapshot_id] + for fiducial in snapshot.objects: + if fiducial.HasField("apriltag_properties"): + (fiducial_object, + curr_wp_tform_fiducial) = create_fiducial_object(fiducial, curr_waypoint, + renderer) + world_tform_fiducial = np.dot(world_tform_current_waypoint, + vtk_to_mat(curr_wp_tform_fiducial)) + fiducial_object.SetUserTransform(mat_to_vtk(world_tform_fiducial)) + make_text( + 'fid: {}'.format(fiducial.apriltag_properties.tag_id), world_tform_fiducial[:3, 3], + renderer) + + # Now, for each edge, walk along the edge and concatenate the transform to the neighbor. + for edge in current_graph.edges: + # If the edge is directed away from us... + if edge.id.from_waypoint == curr_waypoint.id and edge.id.to_waypoint not in visited: + current_waypoint_tform_to_waypoint = SE3Pose.from_obj( + edge.from_tform_to).to_matrix() + world_tform_to_wp = create_edge_object(current_waypoint_tform_to_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.to_waypoint], world_tform_to_wp)) + avg_pos += world_tform_to_wp[:3, 3] + # If the edge is directed toward us... + elif edge.id.to_waypoint == curr_waypoint.id and edge.id.from_waypoint not in visited: + current_waypoint_tform_from_waypoint = (SE3Pose.from_obj( + edge.from_tform_to).inverse()).to_matrix() + world_tform_from_wp = create_edge_object(current_waypoint_tform_from_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.from_waypoint], world_tform_from_wp)) + avg_pos += world_tform_from_wp[:3, 3] + return avg_pos + + +def main( path, draw_id ): + # Load the map from the given file. + (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) = load_map(path) + + # Create the renderer. + renderer = vtk.vtkRenderer() + renderer.SetBackground(0.05, 0.1, 0.15) + + avg_pos = create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, + renderer, draw_id) + + # Compute the average waypoint position to place the camera appropriately. + avg_pos /= len(current_waypoints) + camera_pos = avg_pos + np.array([-1, 0, 5]) + + camera = renderer.GetActiveCamera() + camera.SetViewUp(0, 0, 1) + camera.SetPosition(camera_pos[0], camera_pos[1], camera_pos[2]) + + # Create the VTK renderer and interactor. + renderWindow = vtk.vtkRenderWindow() + renderWindow.SetWindowName(path) + renderWindow.AddRenderer(renderer) + renderWindowInteractor = vtk.vtkRenderWindowInteractor() + renderWindowInteractor.SetRenderWindow(renderWindow) + renderWindow.SetSize(1280, 720) + style = vtk.vtkInteractorStyleTerrain() + renderWindowInteractor.SetInteractorStyle(style) + renderer.ResetCamera() + + # Start rendering. + renderWindow.Render() + renderWindow.Start() + renderWindowInteractor.Start() + + +if __name__ == '__main__': + + parser = argparse.ArgumentParser() + + parser.add_argument('map_directory') + parser.add_argument('--draw-id', help='Draw ID of each waypoints', action='store_true') + + args = parser.parse_args() + + if args.draw_id: + main( args.map_directory, True ) + else: + main( args.map_directory, False ) diff --git a/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json b/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..2efcb04f38 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json @@ -0,0 +1,30 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/jsk_robot_startup/include/**", + "/opt/ros/noetic/share/julius/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_pr2_robot/pr2_base_trajectory_action/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/speak_and_wait_recovery/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_gps/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_msg_filters/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_msgs/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_serialization/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/update_move_base_parameter_recovery/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/jsk_spot_robot/spot_ros_client/.vscode/settings.json b/jsk_spot_robot/spot_ros_client/.vscode/settings.json new file mode 100644 index 0000000000..97568205e8 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ] +} diff --git a/jsk_spot_robot/spot_ros_client/CMakeLists.txt b/jsk_spot_robot/spot_ros_client/CMakeLists.txt new file mode 100644 index 0000000000..4c7961bfb2 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_ros_client) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package() + +catkin_install_python(PROGRAMS scripts/demo.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/spot_ros_client/README.md b/jsk_spot_robot/spot_ros_client/README.md new file mode 100644 index 0000000000..f6918bd87c --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/README.md @@ -0,0 +1,9 @@ +# spot_ros_client + +This package provide a python client library for spot_ros. + +## Demo + +```python +rosrun spot_ros_client demo.py +``` diff --git a/jsk_spot_robot/spot_ros_client/package.xml b/jsk_spot_robot/spot_ros_client/package.xml new file mode 100644 index 0000000000..a0e71c116f --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/package.xml @@ -0,0 +1,31 @@ + + + spot_ros_client + 1.1.0 + The spot_ros_client package + + Koki Shinjo + + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + spot_msgs + std_srvs + geometry_msgs + jsk_spot_behavior_msgs + + actionlib + spot_msgs + std_srvs + geometry_msgs + jsk_spot_behavior_msgs + + + + diff --git a/jsk_spot_robot/spot_ros_client/scripts/demo.py b/jsk_spot_robot/spot_ros_client/scripts/demo.py new file mode 100755 index 0000000000..955588af34 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/scripts/demo.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python + +import rospy +import math +from spot_ros_client.libspotros import SpotRosClient +from geometry_msgs.msg import Quaternion # for body pose + +rospy.init_node('spot_ros_client_demo') +client = SpotRosClient() + +# claim +rospy.loginfo('claim') +client.claim() +rospy.sleep(2) + +# power on +rospy.loginfo('power on') +client.power_on() +rospy.sleep(2) + +# stand +rospy.loginfo('stand') +client.stand() +rospy.sleep(2) + +# sit +rospy.loginfo('sit') +client.sit() +rospy.sleep(2) + +# stand +rospy.loginfo('stand') +client.stand() +rospy.sleep(2) + +# send velocity commands +rospy.loginfo('sending cmd_vel') +rate = rospy.Rate(10) +start_time = rospy.Time.now() +while not rospy.is_shutdown() and rospy.Time.now() < start_time + rospy.Duration(10): + rate.sleep() + client.pub_cmd_vel(0, 0, 0.5) + +# change body pose +quaternion_rotated = Quaternion(x=0,y=0,z=math.sin(0.2),w=math.cos(0.2)) +quaternion_equal = Quaternion(x=0,y=0,z=0,w=1) + +rospy.loginfo('rotating body pose') +client.pub_body_pose(0,quaternion_rotated) +client.stand() +rospy.sleep(5) + +client.pub_body_pose(0,quaternion_equal) +client.stand() +rospy.sleep(5) + +rospy.loginfo('changing body height') +client.pub_body_pose(0.2,quaternion_equal) +client.stand() +rospy.sleep(5) + +client.pub_body_pose(0,quaternion_equal) +client.stand() +rospy.sleep(5) + +# switch stair mode +rospy.loginfo('switching stair mode') +client.stair_mode(True) +rospy.sleep(5) +client.stair_mode(False) +rospy.sleep(5) + +# send a trajectory command +rospy.loginfo('sending trajectory command') +client.trajectory(1.0,1.0,1.57,5,blocking=True) +rospy.sleep(5) + +# use graphnav navigation +## assuming spot is at initial place of example.walk +rospy.loginfo('sending navigate to command') +import rospkg +upload_filepath = rospkg.RosPack().get_path('spot_autowalk_data')+'/autowalk/eng2_73b2_to_81c1_night.walk' +rospy.loginfo('upload map') +client.upload_graph(upload_filepath) +rospy.sleep(1) +rospy.loginfo('localization') +client.set_localization_fiducial() +rospy.sleep(1) +rospy.loginfo('list graph') +waypoint_ids = client.list_graph() +rospy.sleep(1) +rospy.loginfo('send command') +client.navigate_to(waypoint_ids[-1],blocking=False) +rospy.sleep(1) +rospy.loginfo('wait for result') +client.wait_navigate_to_result(rospy.Duration(30)) +rospy.sleep(1) +rospy.loginfo('get a result') +result = client.get_navigate_to_result() diff --git a/jsk_spot_robot/spot_ros_client/setup.py b/jsk_spot_robot/spot_ros_client/setup.py new file mode 100644 index 0000000000..fa9d4a812b --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_ros_client'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py new file mode 100644 index 0000000000..a45c5f4930 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py @@ -0,0 +1 @@ +import spot_ros_client.libspotros as libspotros diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py new file mode 100644 index 0000000000..86279a1661 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -0,0 +1,582 @@ +import PyKDL +import rospy +import actionlib +import tf2_ros +import tf2_geometry_msgs + +import math + +# msg +from std_msgs.msg import Bool +from std_msgs.msg import Float32 +from std_msgs.msg import String +from spot_msgs.msg import Feedback +from spot_msgs.msg import PowerState +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +# services +from std_srvs.srv import Trigger +from std_srvs.srv import TriggerRequest +from std_srvs.srv import SetBool +from std_srvs.srv import SetBoolRequest +from spot_msgs.srv import ListGraph +from spot_msgs.srv import ListGraphRequest +from spot_msgs.srv import SetLocalizationFiducial +from spot_msgs.srv import SetLocalizationFiducialRequest +from spot_msgs.srv import SetLocalizationWaypoint +from spot_msgs.srv import SetLocalizationWaypointRequest +from spot_msgs.srv import SetLocomotion +from spot_msgs.srv import SetLocomotionRequest +from spot_msgs.srv import UploadGraph +from spot_msgs.srv import UploadGraphRequest +from spot_msgs.srv import Dock +from spot_msgs.srv import DockRequest +from jsk_spot_behavior_msgs.srv import ResetCurrentNode, ResetCurrentNodeRequest +# actions +from jsk_spot_behavior_msgs.msg import NavigationAction +from jsk_spot_behavior_msgs.msg import NavigationGoal +from spot_msgs.msg import NavigateToAction +from spot_msgs.msg import NavigateToGoal +from spot_msgs.msg import TrajectoryAction +from spot_msgs.msg import TrajectoryGoal + + +def calc_distance_to_pose(pose): + if isinstance(pose, Pose): + return math.sqrt(pose.position.x ** 2 + pose.position.y ** 2 + pose.position.z ** 2) + elif isinstance(pose, PoseStamped): + pose = pose.pose + return math.sqrt(pose.position.x ** 2 + pose.position.y ** 2 + pose.position.z ** 2) + else: + raise TypeError( + 'pose must be geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped') + + +def convert_msg_point_to_kdl_vector(point): + return PyKDL.Vector(point.x, point.y, point.z) + + +def get_nearest_person_pose(topicname='/spot_recognition_person_tracker/people_pose_array'): + try: + msg = rospy.wait_for_message( + topicname, + PoseArray, + timeout=rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logwarn('Timeout exceede: {}'.format(e)) + return None + + if len(msg.poses) == 0: + rospy.logwarn('No person visible') + return None + + distance = calc_distance_to_pose(msg.poses[0]) + target_pose = msg.poses[0] + for pose in msg.poses: + if calc_distance_to_pose(pose) < distance: + distance = calc_distance_to_pose(pose) + target_pose = pose + + pose_stamped = PoseStamped() + pose_stamped.header = msg.header + pose_stamped.pose = target_pose + + return pose_stamped + + +def get_diff_for_person(pose): + + if isinstance(pose, Pose): + x = pose.position.x + y = pose.position.y + z = pose.position.z + elif isinstance(pose, PoseStamped): + x = pose.pose.position.x + y = pose.pose.position.y + z = pose.pose.position.z + else: + raise TypeError( + 'pose must be geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped') + + yaw = math.atan2(y, x) + try: + pitch = math.acos(z / math.sqrt(x**2 + y**2)) + except ValueError: + pitch = 0 + return pitch, yaw + + +class SpotRosClient: + + def __init__(self, + topicname_cmd_vel='/spot/cmd_vel', + topicname_body_pose='/spot/body_pose', + topicname_cable_connected='/spot/status/cable_connected', + topicname_current_node_id='/spot_behavior_manager_server/current_node_id', + topicname_laptop_percentage='/spot/status/laptop_battery_percentage', + topicname_battery_percentage='/spot/status/battery_percentage', + topicname_status_feedback='/spot/status/feedback', + topicname_status_power_state='/spot/status/power_state', + servicename_claim='/spot/claim', + servicename_release='/spot/release', + servicename_stop='/spot/stop', + servicename_self_right='/spot/self_right', + servicename_sit='/spot/sit', + servicename_stand='/spot/stand', + servicename_power_on='/spot/power_on', + servicename_power_off='/spot/power_off', + servicename_estop_hard='/spot/estop/hard', + servicename_estop_gentle='/spot/estop/gentle', + servicename_stair_mode='/spot/stair_mode', + servicename_locomotion_mode='/spot/locomotion_mode', + servicename_upload_graph='/spot/upload_graph', + servicename_list_graph='/spot/list_graph', + servicename_set_localization_fiducial='/spot/set_localization_fiducial', + servicename_set_localization_waypoint='/spot/set_localization_waypoint', + servicename_dock='/spot/dock', + servicename_undock='/spot/undock', + servicename_reset_current_node='/spot_behavior_manager_server/reset_current_node_id', + actionname_navigate_to='/spot/navigate_to', + actionname_trajectory='/spot/trajectory', + actionname_execute_behaviors='/spot_behavior_manager_server/execute_behaviors', + duration_timeout=0.05): + + self.topicname_cable_connected = topicname_cable_connected + self.topicname_current_node_id = topicname_current_node_id + self.topicname_laptop_percentage = topicname_laptop_percentage + self.topicname_battery_percentage = topicname_battery_percentage + self.topicname_status_feedback = topicname_status_feedback + self.topicname_status_power_state = topicname_status_power_state + + # Publishers + self._pub_cmd_vel = rospy.Publisher( + topicname_cmd_vel, + Twist, + queue_size=1 + ) + self._pub_body_pose = rospy.Publisher( + topicname_body_pose, + Pose, + queue_size=1 + ) + rospy.loginfo("Publisher initialization done.") + + # wait for services + rospy.loginfo("Waiting all service available...") + try: + rospy.wait_for_service(servicename_claim, rospy.Duration(5)) + rospy.wait_for_service(servicename_release, rospy.Duration(5)) + rospy.wait_for_service(servicename_stop, rospy.Duration(5)) + rospy.wait_for_service(servicename_self_right, rospy.Duration(5)) + rospy.wait_for_service(servicename_sit, rospy.Duration(5)) + rospy.wait_for_service(servicename_stand, rospy.Duration(5)) + rospy.wait_for_service(servicename_power_on, rospy.Duration(5)) + rospy.wait_for_service(servicename_power_off, rospy.Duration(5)) + rospy.wait_for_service(servicename_estop_hard, rospy.Duration(5)) + rospy.wait_for_service(servicename_estop_gentle, rospy.Duration(5)) + rospy.wait_for_service(servicename_stair_mode, rospy.Duration(5)) + rospy.wait_for_service( + servicename_locomotion_mode, rospy.Duration(5)) + rospy.wait_for_service(servicename_upload_graph, rospy.Duration(5)) + rospy.wait_for_service(servicename_list_graph, rospy.Duration(5)) + rospy.wait_for_service( + servicename_set_localization_fiducial, rospy.Duration(5)) + rospy.wait_for_service( + servicename_set_localization_waypoint, rospy.Duration(5)) + rospy.wait_for_service(servicename_dock, rospy.Duration(5)) + rospy.wait_for_service(servicename_undock, rospy.Duration(5)) + rospy.wait_for_service(servicename_reset_current_node, rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logerr('Service unavaliable: {}'.format(e)) + + # Service Clients + self._srv_client_claim = rospy.ServiceProxy( + servicename_claim, + Trigger + ) + self._srv_client_release = rospy.ServiceProxy( + servicename_release, + Trigger + ) + self._srv_client_stop = rospy.ServiceProxy( + servicename_stop, + Trigger + ) + self._srv_client_self_right = rospy.ServiceProxy( + servicename_self_right, + Trigger + ) + self._srv_client_sit = rospy.ServiceProxy( + servicename_sit, + Trigger + ) + self._srv_client_stand = rospy.ServiceProxy( + servicename_stand, + Trigger + ) + self._srv_client_power_on = rospy.ServiceProxy( + servicename_power_on, + Trigger + ) + self._srv_client_power_off = rospy.ServiceProxy( + servicename_power_off, + Trigger + ) + self._srv_client_estop_hard = rospy.ServiceProxy( + servicename_estop_hard, + Trigger + ) + self._srv_client_estop_gentle = rospy.ServiceProxy( + servicename_estop_gentle, + Trigger + ) + self._srv_client_stair_mode = rospy.ServiceProxy( + servicename_stair_mode, + SetBool + ) + self._srv_client_locomotion_mode = rospy.ServiceProxy( + servicename_locomotion_mode, + SetLocomotion + ) + self._srv_client_upload_graph = rospy.ServiceProxy( + servicename_upload_graph, + UploadGraph + ) + self._srv_client_list_graph = rospy.ServiceProxy( + servicename_list_graph, + ListGraph + ) + self._srv_client_set_localization_fiducial = rospy.ServiceProxy( + servicename_set_localization_fiducial, + SetLocalizationFiducial + ) + self._srv_client_set_localization_waypoint = rospy.ServiceProxy( + servicename_set_localization_waypoint, + SetLocalizationWaypoint + ) + self._srv_client_dock = rospy.ServiceProxy( + servicename_dock, + Dock + ) + self._srv_client_undock = rospy.ServiceProxy( + servicename_undock, + Trigger + ) + self._srv_client_reset_current_node = rospy.ServiceProxy( + servicename_reset_current_node, + ResetCurrentNode + ) + + rospy.loginfo("Service initialization done.") + + # Action Clients + self._actionclient_navigate_to = actionlib.SimpleActionClient( + actionname_navigate_to, + NavigateToAction + ) + self._actionclient_trajectory = actionlib.SimpleActionClient( + actionname_trajectory, + TrajectoryAction + ) + self._actionclient_execute_behaviors = actionlib.SimpleActionClient( + actionname_execute_behaviors, + NavigationAction + ) + + rospy.loginfo("Waiting actions available") + # wait for action + try: + self._actionclient_navigate_to.wait_for_server(rospy.Duration(5)) + self._actionclient_trajectory.wait_for_server(rospy.Duration(5)) + self._actionclient_execute_behaviors.wait_for_server( + rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logerr('Action unavaliable: {}'.format(e)) + + # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + rospy.loginfo("Action initialization done.") + + rospy.loginfo("Initialization done") + + def get_laptop_percepntage(self): + try: + msg = rospy.wait_for_message( + self.topicname_laptop_percentage, Float32, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def get_battery_percepntage(self): + try: + msg = rospy.wait_for_message( + self.topicname_battery_percentage, Float32, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def get_robot_pose(self): + try: + frame_odom_to_base = tf2_geometry_msgs.transform_to_kdl( + self.tf_buffer.lookup_transform( + 'odom', + 'base_link', + rospy.Time() + ) + ) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + return None + return frame_odom_to_base + + def go_pose(self, target_frame, timeout = None): + self.trajectory( + target_frame.p[0], + target_frame.p[1], + target_frame.M.GetRPY()[2], + duration=timeout, + blocking=True + ) + + def is_connected(self): + try: + msg = rospy.wait_for_message( + self.topicname_cable_connected, Bool, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_sitting(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_feedback, Feedback, timeout=rospy.Duration(5)) + return msg.sitting + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_standing(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_feedback, Feedback, timeout=rospy.Duration(5)) + return msg.standing + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_powered_on(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_power_state, PowerState, timeout=rospy.Duration(5)) + if msg.motor_power_state == PowerState.STATE_ON: + return True + else: + return False + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def pub_cmd_vel(self, vx, vy, vtheta): + msg = Twist() + msg.linear.x = vx + msg.linear.y = vy + msg.angular.z = vtheta + self._pub_cmd_vel.publish(msg) + + def pub_body_pose(self, height, orientation): + msg = Pose() + msg.position.z = height + msg.orientation = orientation + self._pub_body_pose.publish(msg) + + def claim(self): + res = self._srv_client_claim(TriggerRequest()) + return res.success, res.message + + def release(self): + res = self._srv_client_release(TriggerRequest()) + return res.success, res.message + + def stop(self): + res = self._srv_client_stop(TriggerRequest()) + return res.success, res.message + + def self_right(self): + res = self._srv_client_self_right(TriggerRequest()) + return res.success, res.message + + def sit(self): + res = self._srv_client_sit(TriggerRequest()) + return res.success, res.message + + def stand(self): + res = self._srv_client_stand(TriggerRequest()) + return res.success, res.message + + def power_on(self): + res = self._srv_client_power_on(TriggerRequest()) + return res.success, res.message + + def power_off(self): + res = self._srv_client_power_off(TriggerRequest()) + return res.success, res.message + + def estop_hard(self): + res = self._srv_client_estop_hard(TriggerRequest()) + return res.success, res.message + + def estop_gentle(self): + res = self._srv_client_estop_gentle(TriggerRequest()) + return res.success, res.message + + def stair_mode(self, stair_mode): + res = self._srv_client_stair_mode(SetBoolRequest(data=stair_mode)) + return res.success, res.message + + def locomotion_mode(self, locomotion_mode): + res = self._srv_client_locomotion_mode( + SetLocomotionRequest(locomotion_mode=locomotion_mode)) + return res.success, res.message + + def upload_graph(self, upload_filepath): + res = self._srv_client_upload_graph( + UploadGraphRequest(upload_filepath=upload_filepath)) + return res.success, res.message + + def list_graph(self): + res = self._srv_client_list_graph(ListGraphRequest()) + return res.waypoint_ids + + def set_localization_fiducial(self): + res = self._srv_client_set_localization_fiducial( + SetLocalizationFiducialRequest()) + return res.success, res.message + + def set_localization_waypoint(self, waypoint_id): + res = self._srv_client_set_localization_waypoint( + SetLocalizationWaypointRequest(waypoint_id=waypoint_id)) + return res.success, res.message + + def dock(self, dock_id): + req = DockRequest() + req.dock_id = dock_id + self.pub_body_pose(0, Quaternion(0, 0, 0, 1.0)) + self.sit() + self.stand() + res = self._srv_client_dock(req) + return res.success, res.message + + def undock(self): + self.power_on() + res = self._srv_client_undock(TriggerRequest()) + if not self.is_powered_on(): + self.power_on() + if not self.is_standing(): + self.stand() + return res.success, res.message + + def auto_dock(self, dock_id, home_node_id='eng2_73B2_dock'): + res = self.execute_behaviors(home_node_id) + if not res.success: + return res.success, res.message + success, message = self.dock(dock_id) + return success, message + + def auto_undock(self): + self.sit() + self.power_off() + self.release() + self.claim() + self.power_on() + if self.is_connected(): + self.undock() + else: + self.stand() + + def navigate_to(self, + id_navigate_to, + velocity_limit=(1.0, 1.0, 1.0), + blocking=False): + goal = NavigateToGoal() + goal.id_navigate_to = id_navigate_to + goal.velocity_limit.linear.x = float(velocity_limit[0]) + goal.velocity_limit.linear.y = float(velocity_limit[1]) + goal.velocity_limit.angular.z = float(velocity_limit[2]) + self._actionclient_navigate_to.send_goal(goal) + if blocking: + self._actionclient_navigate_to.wait_for_result() + return self._actionclient_navigate_to.get_result() + + def wait_for_navigate_to_result(self, duration=rospy.Duration(0)): + return self._actionclient_navigate_to.wait_for_result(duration) + + def get_navigate_to_result(self): + return self._actionclient_navigate_to.get_result() + + def cancel_navigate_to(self): + self._actionclient_navigate_to.cancel_all_goals() + + def get_current_node(self): + try: + msg = rospy.wait_for_message( + self.topicname_current_node_id, String, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def execute_behaviors(self, target_node_id, blocking=True): + goal = NavigationGoal() + goal.target_node_id = target_node_id + self._actionclient_execute_behaviors.send_goal(goal) + if blocking: + self._actionclient_execute_behaviors.wait_for_result() + return self._actionclient_execute_behaviors.get_result() + + def wait_execute_behaviors_result(self, duration=None): + if duration is None: + return self._actionclient_execute_behaviors.wait_for_result() + else: + return self._actionclient_execute_behaviors.wait_for_result(duration=duration) + + def get_execute_behaviors_result(self): + return self._actionclient_execute_behaviors.get_result() + + def reset_current_node(self, node): + req = ResetCurrentNodeRequest() + req.current_node_id = node + res = self._srv_client_reset_current_node(req) + return res.success + + def list_nodes(self): + return list(rospy.get_param("/spot_behavior_manager_server/map/nodes", {}).keys()) + + # \brief call trajectory service + # \param x x value of the target position [m] + # \param x y value of the target position [m] + # \param theta theta value of the target position [rad] + # \param duration duration of trajectory command [secs] + def trajectory(self, x, y, theta, duration=None, blocking=False): + rotation = PyKDL.Rotation.RotZ(theta) + goal = TrajectoryGoal() + goal.target_pose.header.frame_id = 'body' + goal.target_pose.pose.position.x = x + goal.target_pose.pose.position.y = y + goal.target_pose.pose.orientation.x = rotation.GetQuaternion()[0] + goal.target_pose.pose.orientation.y = rotation.GetQuaternion()[1] + goal.target_pose.pose.orientation.z = rotation.GetQuaternion()[2] + goal.target_pose.pose.orientation.w = rotation.GetQuaternion()[3] + if duration is None: + goal.duration.data = rospy.Duration(math.sqrt(x**2 + y**2)/0.5) + else: + goal.duration.data = rospy.Duration(duration) + self._actionclient_trajectory.send_goal(goal) + if blocking: + self._actionclient_trajectory.wait_for_result() 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..2d49a15d63 --- /dev/null +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -0,0 +1,58 @@ +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 + collada_urdf + 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(_urdf_to_collada ${collada_urdf_PREFIX}/lib/collada_urdf/urdf_to_collada) +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 euslisp test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +install(FILES spot.l spot-interface.l spot-util.l 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..f57df8cb74 --- /dev/null +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -0,0 +1,578 @@ +(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)) + (:set-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)) + (:set-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)) + (:start-recording + () + "Start AutoWalk recording" + (call-trigger-service "/spot/start-recording")) + (:stop-recording + () + "Stop AutoWalk recording" + (call-trigger-service "/spot/stop-recording")) + (:upload-graph + (path &key (initial-localization-fiducial t) (wait nil)) + "upload graph for autowalk" + (let (r + (srvname "/spot/upload_graph")) + (if wait (ros::wait-for-service srvname)) + (setq r (ros::service-call srvname + (instance spot_msgs::UploadGraphRequest :init :upload_filepath path))) + (ros::ros-info "Call \"~A\" returns .. \"~A\"" srvname (send r :message)) + (if initial-localization-fiducial (send self :initial-localization-fiducial)) + (send r :success))) + (:download-graph + (path &key (wait nil)) + "Download AutoWalk graph from robot" + (let (r) + (if wait (ros::wait-for-service srvname)) + (setq r (ros::service-call srvname + (instance spot_msgs::DownloadGraphRequest :init :download_filepath path))) + (ros::ros-info "Call \"~A\" returns... \"~A\"" srvname (send r :message)) + (send r :success) + )) + (: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))) + (:clear-graph + () + "Clear the AutoWalk graph in the robot." + (let () + (call-trigger-service "/spot/clear_graph") + )) + (: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 @@ + + +