From f2c9b3ae44e1e366a262860283f0c114479d155d Mon Sep 17 00:00:00 2001
From: Koki Shinjo <sktometometo@gmail.com>
Date: Tue, 28 Nov 2023 20:39:46 +0900
Subject: [PATCH] [spot_ros_client] add print

---
 .../.vscode/c_cpp_properties.json             | 30 +++++++++++++++++++
 .../spot_ros_client/.vscode/settings.json     | 10 +++++++
 .../src/spot_ros_client/libspotros.py         | 17 ++++++++---
 3 files changed, 53 insertions(+), 4 deletions(-)
 create mode 100644 jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json
 create mode 100644 jsk_spot_robot/spot_ros_client/.vscode/settings.json

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/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py
index 3daaf9e6dc..86279a1661 100644
--- 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
@@ -162,8 +162,10 @@ def __init__(self,
             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))
@@ -268,6 +270,8 @@ def __init__(self,
             ResetCurrentNode
         )
 
+        rospy.loginfo("Service initialization done.")
+
         # Action Clients
         self._actionclient_navigate_to = actionlib.SimpleActionClient(
             actionname_navigate_to,
@@ -282,10 +286,7 @@ def __init__(self,
             NavigationAction
         )
 
-        #
-        self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
-
+        rospy.loginfo("Waiting actions available")
         # wait for action
         try:
             self._actionclient_navigate_to.wait_for_server(rospy.Duration(5))
@@ -295,6 +296,14 @@ def __init__(self,
         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(