jsk_spot_robot package for spot arm

.travis.rosinstall.melodic

@@ -64,3 +64,9 @@
     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/common_logger.launch

@@ -5,6 +5,9 @@
   <arg name="save_tf" default="true" />
   <arg name="save_joint_states" default="true" />
   <arg name="save_speech" default="true" />
+  <arg name="save_faces" default="true" />
+  <arg name="save_dialogflow" default="true" />
+  <arg name="save_google_chat" default="true" />
   <arg name="save_smach" default="true" />
   <arg name="save_base_trajectory" default="false" />
   <arg name="save_object_detection" default="false" />
@@ -236,6 +239,48 @@
       </rosparam>
     </node>
 
+    <!-- face save aws face data -->
+    <node if="$(arg save_faces)"
+          name="face_logger"
+          pkg="jsk_robot_startup" type="mongo_record.py"
+          machine="$(arg machine)"
+          respawn="$(arg respawn)">
+      <rosparam subst_value="true">
+        topics:
+        - /aws_auto_checkin_app/output/class
+        - /aws_detect_faces/attributes
+      </rosparam>
+    </node>
+
+    <!-- face dialogflow I/O -->
+    <node if="$(arg save_dialogflow)"
+          name="dialogflow_logger"
+          pkg="jsk_robot_startup" type="mongo_record.py"
+          machine="$(arg machine)"
+          respawn="$(arg respawn)">
+      <rosparam subst_value="true">
+        topics:
+        - /dialogflow_client/text_action/goal
+        - /dialogflow_client/text_action/result
+      </rosparam>
+    </node>
+
+    <!-- google chat -->
+    <node if="$(arg save_google_chat)"
+          name="google_chat_logger"
+          pkg="jsk_robot_startup" type="mongo_record.py"
+          machine="$(arg machine)"
+          respawn="$(arg respawn)">
+      <rosparam subst_value="true">
+        topics:
+        - /google_chat_ros/send/goal
+        - /google_chat_ros/send/result
+        - /google_chat_ros/message_activity
+        - /google_chat_ros
+        - /google_chat_ros/card_activity
+      </rosparam>
+    </node>
+
     <!-- base trajecotry logger -->
     <node if="$(arg save_base_trajectory)"
           name="base_trajectory_logger"

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,122 @@
+jsk_spot_robot
+==============
+
+## Manuals
+
+- [Supported Documents of Boston Dynamics](https://www.bostondynamics.com/spot/training/documentation)
+- [Spot ROS User Documentation](http://www.clearpathrobotics.com/assets/guides/melodic/spot-ros/ros_usage.html#taking-control-of-the-robot)
+
+## Installation
+
+To setup a new internal PC and spot user, Please see [this page](./SetupInternalPCAndSpotUser.md).
+
+### How to set up a catkin workspace for a remote PC
+
+Create a workspace
+
+```bash
+source /opt/ros/melodic/setup.bash
+mkdir ~/spot_ws/src -p
+cd ~/spot_ws/src
+wstool init .
+wstool set jsk-ros-pkg/jsk_robot https://github.com/k-okada/jsk_robot.git --git -v spot_arm
+wstool update
+wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_user.rosinstall
+wstool update
+rosdep update
+rosdep install -y -r --from-paths . --ignore-src
+cd $HOME/spot_ws
+catkin init
+catkin build -j4 jsk_spot_startup spoteus
+```
+
+## How to run
+
+### Bringup spot
+
+First, please turn on spot and turn on motors according to [the OPERATION section of spot user guide](https://www.bostondynamics.com/sites/default/files/inline-files/spot-user-guide.pdf) and power on the internal PC.
+
+Basically, ros systemd services will start automatically. So you can use spot now.
+
+
+#### superviosur
+
+URI: http://<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