[DO NOT MERGE] Develop/spot arm

.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

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

jsk_robot_common/jsk_robot_startup/package.xml

@@ -19,6 +19,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>urdf</build_depend>
+
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>app_manager</exec_depend>
   <exec_depend>dynamic_reconfigure</exec_depend>

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,13 +74,26 @@ 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)
         self.enable_button = int(rospy.get_param('~enable_button', 0))
         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()
 

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,17 +350,17 @@ 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;
                 }
           } else if (msg->axes[axe_dock_]==1) {
             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;
             }

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://<robot_ip>:9001
+
+#### rwt_app_chooser
+
+URI: http://<robot_ip>: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 <robot ip address or robot_name.local>
+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://<robot address:443/`. So it is `https://10.0.0.3:443` by default.
+
+For BelKa, this configuration is changed to DHCP. And this ethernet port is also connected to ethernet port of docking station and belka's docking station is connected to 133 network. And robot FQDN is assigned like `spot-BD-<robot SN>-p.jsk.imi.i.u-tokyo.ac.jp` . You can connect `https://spot-BD-<robot SN>-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-<SN>` 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 [email protected]`, you can forward access of robot to your local machine by `ssh [email protected] -L 443:192.168.50.3:443` and you can open webconsole by accesing `https://localhost:443`.