From a8ef919651031ae791dbd199e2baef5c6b2a50ab Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 28 Oct 2020 16:08:45 +0900 Subject: [PATCH 001/261] [jsk_spot_robot][spoteus] add jsk_spot_robot/spoteus --- jsk_spot_robot/README.md | 3 ++ jsk_spot_robot/spoteus/CMakeLists.txt | 57 ++++++++++++++++++++++ jsk_spot_robot/spoteus/package.xml | 27 ++++++++++ jsk_spot_robot/spoteus/spot-utils.l | 23 +++++++++ jsk_spot_robot/spoteus/spot.yaml | 40 +++++++++++++++ jsk_spot_robot/spoteus/test/test-spot.l | 16 ++++++ jsk_spot_robot/spoteus/test/test-spot.test | 3 ++ 7 files changed, 169 insertions(+) create mode 100644 jsk_spot_robot/README.md create mode 100644 jsk_spot_robot/spoteus/CMakeLists.txt create mode 100644 jsk_spot_robot/spoteus/package.xml create mode 100644 jsk_spot_robot/spoteus/spot-utils.l create mode 100644 jsk_spot_robot/spoteus/spot.yaml create mode 100755 jsk_spot_robot/spoteus/test/test-spot.l create mode 100644 jsk_spot_robot/spoteus/test/test-spot.test diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md new file mode 100644 index 0000000000..b4233dc97f --- /dev/null +++ b/jsk_spot_robot/README.md @@ -0,0 +1,3 @@ +jsk_spot_robot +============== + diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt new file mode 100644 index 0000000000..bdd6811a40 --- /dev/null +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -0,0 +1,57 @@ +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 + spot_description +) + +catkin_package() + +########### +## 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) + +if(NOT EXISTS ${PROJECT_SOURCE_DIR}/spot.l) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.dae + COMMAND echo "${_urdf_to_collada} ${PROJECT_SOURCE_DIR}/spot.urdf spot.dae" + COMMAND ${_urdf_to_collada} ${PROJECT_SOURCE_DIR}/spot.urdf ${PROJECT_SOURCE_DIR}/spot.dae + DEPENDS ${PROJECT_SOURCE_DIR}/spot.urdf) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.l + COMMAND echo "${_collada2eus} spot.dae spot.l" + COMMAND ${_collada2eus} ${PROJECT_SOURCE_DIR}/spot.dae ${PROJECT_SOURCE_DIR}/spot.yaml ${PROJECT_SOURCE_DIR}/spot.l + DEPENDS ${PROJECT_SOURCE_DIR}/spot.dae ${PROJECT_SOURCE_DIR}/spot.yaml ${_collada2eus}) + add_custom_target(compile_spot ALL DEPENDS ${PROJECT_SOURCE_DIR}/spot.l) +endif() + + +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/package.xml b/jsk_spot_robot/spoteus/package.xml new file mode 100644 index 0000000000..48f12fda60 --- /dev/null +++ b/jsk_spot_robot/spoteus/package.xml @@ -0,0 +1,27 @@ + + + spoteus + 1.1.0 + The spots package + + Kei Okada + Yoshiki Obinata + BSD + Kei Okada + Yoshiki Obinata + + catkin + + collada_urdf + euscollada + rostest + roseus + pr2eus + spot_description + + roseus + pr2eus + + + + diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l new file mode 100644 index 0000000000..2fa492afc9 --- /dev/null +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -0,0 +1,23 @@ +(require :spot "package://spoteus/spot.l") + +(defmethod spot-robot + (: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))))) + ) + diff --git a/jsk_spot_robot/spoteus/spot.yaml b/jsk_spot_robot/spoteus/spot.yaml new file mode 100644 index 0000000000..4a5472962f --- /dev/null +++ b/jsk_spot_robot/spoteus/spot.yaml @@ -0,0 +1,40 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +larm: + - front_left_hip_x : larm-shoulder-r + - front_left_hip_y : larm-shoulder-p + - front_left_knee : larm-elbow-p +rarm: + - front_right_hip_x : rarm-shoulder-r + - front_right_hip_y : rarm-shoulder-p + - front_right_knee : rarm-elbow-p +lleg: + - rear_left_hip_x : lleg-crotch-r + - rear_left_hip_y : lleg-crotch-p + - rear_left_knee : lleg-knee-p +rleg: + - rear_right_hip_x : rleg-crotch-r + - rear_right_hip_y : rleg-crotch-p + - rear_right_knee : rleg-knee-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] + +larm-end-coords: + parent : front_left_lower_leg + translate : [0, 0, -0.38] + rotate : [0, 1, 0, 0] +rarm-end-coords: + parent : front_right_lower_leg + translate : [0, 0, -0.38] + rotate : [0, 1, 0, 0] +lleg-end-coords: + parent : rear_left_lower_leg + translate : [0, 0, -0.38] + rotate : [0, 1, 0, 0] +rleg-end-coords: + parent : rear_right_lower_leg + translate : [0, 0, -0.38] + rotate : [0, 1, 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..ead03cb148 --- /dev/null +++ b/jsk_spot_robot/spoteus/test/test-spot.l @@ -0,0 +1,16 @@ +#!/usr/bin/env roseus +(require :unittest "lib/llib/unittest.l") +(require "package://spoteus/spot-util.l") + +(init-unit-test) + +(deftest test-pose + (let (robot) + (setq robot (instance spot-robot :init)) + )) + +(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 @@ + + + From 026a1d2241d9b55abce9a856078dc975187e97a1 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Oct 2020 09:59:44 +0900 Subject: [PATCH 002/261] [spoteus] avoid generate spot.l if not found --- jsk_spot_robot/spoteus/CMakeLists.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt index bdd6811a40..0b7e955eec 100644 --- a/jsk_spot_robot/spoteus/CMakeLists.txt +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -8,11 +8,18 @@ find_package(catkin REQUIRED COMPONENTS collada_urdf roseus euscollada - spot_description ) +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 ## ########### From 99465a904acc87fe559383fe47d78dcd38867d6f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Oct 2020 12:36:12 +0900 Subject: [PATCH 003/261] [spoteus] update body colors --- jsk_spot_robot/spoteus/spot-utils.l | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l index 2fa492afc9..3dfffefba3 100644 --- a/jsk_spot_robot/spoteus/spot-utils.l +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -1,6 +1,35 @@ (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.0 1.0 1.0 0.0)) + (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.0 1.0 1.0 0.0)) + (assoc b (send (class self) :methods))))) + ) + (send* self :init-orig args)) (:legs ;; support legs for all limbs (&rest args) (case (car args) From 234b14813133b245553390b6daab20052b9fea97 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 30 Oct 2020 21:55:27 +0900 Subject: [PATCH 004/261] [spoteus] add first commit of spot-interface.l sample.l --- jsk_spot_robot/spoteus/sample.l | 16 +++++ jsk_spot_robot/spoteus/spot-interface.l | 94 +++++++++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 jsk_spot_robot/spoteus/sample.l create mode 100644 jsk_spot_robot/spoteus/spot-interface.l diff --git a/jsk_spot_robot/spoteus/sample.l b/jsk_spot_robot/spoteus/sample.l new file mode 100644 index 0000000000..1d6d1422d5 --- /dev/null +++ b/jsk_spot_robot/spoteus/sample.l @@ -0,0 +1,16 @@ +(load "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/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l new file mode 100644 index 0000000000..717d268692 --- /dev/null +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -0,0 +1,94 @@ +(ros::roseus "spot") + +(require "package://spoteus/spot-utils.l") +(require "package://pr2eus/robot-interface.l") + +(ros::roseus-add-srvs "std_srvs") + +(defun call-trigger-service (srvname &key (wait nil)) + "Call std_srv/Trigger service" + (let (r) + (if wait (ros::wait-for-service srvname)) + (setq r (ros::service-call srvname (instance std_srvs::TriggerRequest :init))) + (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message)) + (send r :success))) + +(defclass spot-interface + :super robot-move-base-interface + :slots () + ) + +(defmethod spot-interface + (:init + (&rest args) + (send-super* :init :robot spot-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args) + ) + (:default-controller () ) ;; spot does not provide any JTA controllers + (:state + (&rest args) + (prog1 + (send-message self robot-interface :state args) + (case (car args) + (: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" (call-trigger-service "/spot/claim")) + (:release () "Relase the robot control" (call-trigger-service "/spot/release")) + (:power-on () "Power on the robot" (call-trigger-service "/spot/power_on")) + (:power-off () "Power off the robot" (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")) + ;; + (: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)) + "contorl 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) + + ) + +(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) + + (when create-viewer (objects (list *spot*))) + ) From 4fb45aa5b7eadf8b5f23337141de9362cbb40f64 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 5 Nov 2020 15:26:28 +0900 Subject: [PATCH 005/261] [spoteus] add /spot/status --- jsk_spot_robot/spoteus/spot-interface.l | 144 +++++++++++++++++++++++- 1 file changed, 140 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 717d268692..e26491b873 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -4,6 +4,7 @@ (require "package://pr2eus/robot-interface.l") (ros::roseus-add-srvs "std_srvs") +(ros::roseus-add-msgs "spot_msgs") (defun call-trigger-service (srvname &key (wait nil)) "Call std_srv/Trigger service" @@ -21,20 +22,155 @@ (defmethod spot-interface (:init (&rest args) - (send-super* :init :robot spot-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args) - ) + (prog1 + (send-super* :init :robot spot-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args) + ;; 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/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) + )) (:default-controller () ) ;; spot does not provide any JTA controllers + (: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-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 ((s (string-upcase (substitute #\- #\_ (send state :identifier))))) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-IDENTIFIER" s) *keyword-package*) + (send state :identifier)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-CHARGE-PERCENTAGE" s) *keyword-package*) + (send state :charge_percentage)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-ESTIMATED-RUNTIME" s) *keyword-package*) + (send (send state :estimated_runtime) :to-sec)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-CURRENT" s) *keyword-package*) + (send state :current)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-VOLTAGE" s) *keyword-package*) + (send state :voltage)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-TEMPERATURES" s) *keyword-package*) + (send state :temperatures)) + (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-STATUS" s) *keyword-package*) + (case (send state :status) (0 'unknown) (1 'missing) (2 'charging) (3 'discharging) (4 'booting)))))) + (: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 :sevirity) (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) + (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))) + (: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))))))) + (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")) From dd5b1ee8699222f01db1c22d69392ed3a1cc9f7b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 20 Nov 2020 22:40:49 +0900 Subject: [PATCH 006/261] [spoteus] add move-to.l example --- jsk_spot_robot/spoteus/move-to.l | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 jsk_spot_robot/spoteus/move-to.l diff --git a/jsk_spot_robot/spoteus/move-to.l b/jsk_spot_robot/spoteus/move-to.l new file mode 100644 index 0000000000..6d5120fbba --- /dev/null +++ b/jsk_spot_robot/spoteus/move-to.l @@ -0,0 +1,20 @@ +(load "spot-interface.l") + +(spot-init nil) ;; do not create-viewer + +(setq path "/home/lokada/work/2020-11-18 203458 73B1.walk" ) +;; use fedical start to goal, -1 means goal +(send *ri* :navigate-to path -1 :initial-localization-fiducial t :initial-localization-waypoint 0) +;; do not use fedical but start 73b2 to 73a2 +(send *ri* :navigate-to path 7 :initial-localization-fiducial t :initial-localization-waypoint 0) +;; back to 73b2 +(send *ri* :navigate-to path 0 :initial-localization-fiducial nil :initial-localization-waypoint 7) +;; go to 73a2 again, you do not need fedical +(send *ri* :navigate-to path 7 :initial-localization-fiducial nil :initial-localization-waypoint 0) +;; and got to follow original route +(send *ri* :navigate-to path -1 :initial-localization-fiducial nil :initial-localization-waypoint 7) +(ros::ros-info "done") + +(send *ri* :sit)(unix::sleep 3) ;; need to fix... +(send *ri* :power-off)(unix::sleep 3) ;; need to fix +(send *ri* :release) From ae4dd83e3cb5051ee3f1be4b520c58f386c3e136 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 20 Nov 2020 22:41:09 +0900 Subject: [PATCH 007/261] [spoteus] add roseus interface to :navigate-to --- jsk_spot_robot/spoteus/spot-interface.l | 77 ++++++++++++++++++++++++- 1 file changed, 75 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index e26491b873..3d5ef96fd6 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -5,6 +5,7 @@ (ros::roseus-add-srvs "std_srvs") (ros::roseus-add-msgs "spot_msgs") +(ros::roseus-add-srvs "spot_msgs") (defun call-trigger-service (srvname &key (wait nil)) "Call std_srv/Trigger service" @@ -24,6 +25,9 @@ (&rest args) (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 (ros::wait-for-service "/spot/claim" 5) + (ros::ros-error "could not communicate with robot, may be forget to roslaunch spot_driver driver.launh, 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) @@ -174,10 +178,18 @@ ;; (:estop-gentle () (call-trigger-service "/spot/estop/gentle")) (:estop-hard () (call-trigger-service "/spot/estop/hard")) - (:claim () "Claim the robot control" (call-trigger-service "/spot/claim")) + (: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" (call-trigger-service "/spot/power_on")) - (:power-off () "Power off the robot" (call-trigger-service "/spot/power_off")) + (:power-off + () + "Power off the robot" + (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")) @@ -215,6 +227,60 @@ (ros::rate 10) t) + (:list-graph + (upload-filepath) + (let (r) + (setq r (ros::service-call "/spot/list_graph" + (instance spot_msgs::ListGraphRequest :init :upload_filepath upload-filepath))) + (ros::ros-info "Call \"/spot/list_graph\" returns ..") + (dolist (id (send r :waypoint_ids)) + (ros::ros-info " \"~A\"" id)) + (send r :waypoint_ids))) + + (: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)) + (:navigate-to + (upload-path navigate-to &key (initial-localization-fiducial t) (initial-localization-waypoint nil)) + (let (ids c goal ret) + (setq ids (send self :list-graph upload-path)) + (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 "run navigation from ~A" upload-path) + (ros::ros-info "use fiducal localization ~A" initial-localization-fiducial) + (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 :upload_path upload-path) + (send goal :goal :navigate_to navigate-to) + (send goal :goal :initial_localization_fiducial initial-localization-fiducial) + (send goal :goal :initial_localization_waypoint initial-localization-waypoint) + (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)) ) (defun spot-init (&optional (create-viewer)) @@ -225,6 +291,13 @@ (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*))) ) From e00cce40d7daf0b035f53deffce7f916bac9749a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 1 Nov 2020 20:40:25 +0900 Subject: [PATCH 008/261] [jsk_spot_robot] add jsk_spot.rosintall --- jsk_spot_robot/jsk_spot.rosinstall | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot.rosinstall diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall new file mode 100644 index 0000000000..094cf3a3bf --- /dev/null +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -0,0 +1,8 @@ +- git: + local-name: jsk_robot + uri: https://github.com/k-okada/jsk_robot.git + version: spot +- git: + local-name: spot-ros + uri: https://github.com/clearpathrobotics/spot_ros.git + version: master From 451d2a4612c39bce67123fde0999be719fdf626c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 1 Nov 2020 20:40:39 +0900 Subject: [PATCH 009/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 41 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index b4233dc97f..038460709c 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -1,3 +1,44 @@ 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) + +## How to run + +### Setting up a catkin workspace for a new user in the internal pc + +``` +$ mkdir $HOME/catkin_ws/src -p +$ cd $HOME/catkin_ws/src +$ wstool init . +$ wstool merge -t . https://raw.githubusercontent.com/k-okada/jsk_robot/spot/jsk_spot_robot/jsk_spot.rosinstall +$ source /opt/ros/$ROS_DISTRO/setup.bash +$ rosdep install -y -r --from-paths . --ignore-src +$ cd ../ +$ catkin build spoteus jsk_spot_startup +$ source $HOME/catkin_ws/devel/setup.bash +``` + +### 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) + +After that, please run the ros driver. You can now control spot from ROS! + +``` +$ source /opt/ros/$ROS_DISTRO/setup.bash +$ source $HOME/catkin_ws/deve/setup.bash +$ roslaunch jsk_spot_startup driver.launch +``` + +You can run RViz already configured for spot. + +``` +# in another terminal +$ source /opt/ros/$ROS_DISTRO/setup.bash +$ source $HOME/catkin_ws/deve/setup.bash +$ roslaunch jsk_spot_startup rviz.launch +``` From 3c59eef1148f34ab2e1cf75df74dbe31928cb294 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 1 Nov 2020 20:56:12 +0900 Subject: [PATCH 010/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 038460709c..f7adae000b 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -31,7 +31,7 @@ After that, please run the ros driver. You can now control spot from ROS! ``` $ source /opt/ros/$ROS_DISTRO/setup.bash $ source $HOME/catkin_ws/deve/setup.bash -$ roslaunch jsk_spot_startup driver.launch +$ roslaunch jsk_spot_startup driver.launch username:= password:= ``` You can run RViz already configured for spot. From fd0de6fea7868d697c01294d631fd3324ca26d39 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 1 Nov 2020 20:58:53 +0900 Subject: [PATCH 011/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index f7adae000b..30e67c2df1 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -30,15 +30,23 @@ After that, please run the ros driver. You can now control spot from ROS! ``` $ source /opt/ros/$ROS_DISTRO/setup.bash -$ source $HOME/catkin_ws/deve/setup.bash +$ source $HOME/catkin_ws/devel/setup.bash $ roslaunch jsk_spot_startup driver.launch username:= password:= ``` +You can launch ros driver without specifying username and password by sourcing catkin_ws under the spot user. + +``` +$ source /opt/ros/$ROS_DISTRO/setup.bash +$ source /home/spot/catkin_ws/devel/setup.bash +$ roslaunch jsk_spot_startup driver.launch +``` + You can run RViz already configured for spot. ``` # in another terminal $ source /opt/ros/$ROS_DISTRO/setup.bash -$ source $HOME/catkin_ws/deve/setup.bash +$ source $HOME/catkin_ws/devel/setup.bash $ roslaunch jsk_spot_startup rviz.launch ``` From 552c4a33a08ce1d4c6b3cd17b9eabed7664f2096 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Feb 2021 20:40:55 +0900 Subject: [PATCH 012/261] [spoteus] support walking mode services --- jsk_spot_robot/spoteus/spot-interface.l | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 3d5ef96fd6..4ce3bb1f72 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -15,6 +15,22 @@ (ros::ros-info "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) + (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) + (if wait (ros::wait-for-service srvname)) + (setq r (ros::service-call srvname (instance spot_msgs::SetLocomotion :init :locomotion_mode locomotion_hint))) + (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message)) + (send r :success))) + (defclass spot-interface :super robot-move-base-interface :slots () @@ -194,6 +210,8 @@ (: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")) + (:set-stair-mode (is-stair-mode) "Set stair mode" (call-set-bool-service "/spot/stair_mode")) ;; (:send-cmd-vel-raw (x y d &key (topic-name "/spot/cmd_vel")) From c0a0d26d9004839450e7109eaae5a2761b8d8215 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 16 Mar 2021 15:31:15 +0900 Subject: [PATCH 013/261] [spoteus] missing args , typo --- jsk_spot_robot/spoteus/spot-interface.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 4ce3bb1f72..c0f9deb986 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -27,7 +27,7 @@ "Call spot_msgs/SetLocomotion service" (let (r) (if wait (ros::wait-for-service srvname)) - (setq r (ros::service-call srvname (instance spot_msgs::SetLocomotion :init :locomotion_mode locomotion_hint))) + (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))) @@ -155,7 +155,7 @@ (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 :sevirity) (0 'unknown) (1 'info) (2 'warn) (3 'critical)))))) + (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)) @@ -210,8 +210,8 @@ (: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")) - (:set-stair-mode (is-stair-mode) "Set stair mode" (call-set-bool-service "/spot/stair_mode")) + (: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)) ;; (:send-cmd-vel-raw (x y d &key (topic-name "/spot/cmd_vel")) From 2f2465bdc8fcc44912741803a84e4662f7d87fc7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Mar 2021 09:00:57 +0900 Subject: [PATCH 014/261] [spoteus] add go-pos-nowait --- jsk_spot_robot/spoteus/spot-interface.l | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index c0f9deb986..37a7e8462f 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -244,6 +244,23 @@ (send self :send-cmd-vel-raw 0 0 0)) (ros::rate 10) t) + (:go-pos-no-wait + (x y &optional (d 0) (srvname "") (timeout 10)) ;; [m] [m] [degree] + "move robot toward (x, y, d) (units are m, m and degrees respectively) and wait to reach the goal." + (setq req (instance spot_msgs::TrajectoryCommandRequest :init)) + (send req :target_pose :header :frame_id "body") + (send req :target_pose :pose :position :x x) + (send req :target_pose :pose :position :y y) + (send req :target_pose :pose :orientation :z (sin (/ (deg2rad d) 2))) + (send req :target_pose :pose :orientation :w (cos (/ (deg2rad d) 2))) + (setq r (ros::service-call srvname req)) + (if (send r :success) + (send r :success) + (progn + (ros::roseus-error (send r :message)) + (send r :success) + )) + ) (:list-graph (upload-filepath) From ae0eb5b4e185edd6ae8c93378584444554eee9f2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Mar 2021 16:38:45 +0900 Subject: [PATCH 015/261] [spoteus] support trajectory action and add go-pos, go-pos-wait, go-pos-no-wait --- jsk_spot_robot/spoteus/spot-interface.l | 48 ++++++++++++++++--------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 37a7e8462f..e4f0dc10e8 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -33,12 +33,12 @@ (defclass spot-interface :super robot-move-base-interface - :slots () + :slots (trajectory-cmd-action) ) (defmethod spot-interface (:init - (&rest args) + (&rest args &key (trajectory-cmd-action-name "/spot/trajectory")) (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 @@ -56,6 +56,9 @@ (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)) )) (:default-controller () ) ;; spot does not provide any JTA controllers (:spot-status-metrics-callback @@ -244,22 +247,33 @@ (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::TrajectoryActionGoal :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 :rotation :z (sin (/ (deg2rad d) 2))) + (send trajectory-cmd-goal-msg :target_pose :pose :rotation :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 + "move robot toward (x, y, d) (units are m, m and degrees respectively). and wait" + (x y &optional (d 0) (timeout 10)) ;; [m] [m] [degree] + (send self :go-pos x y d :timeout timeout :wait t) + ) (:go-pos-no-wait - (x y &optional (d 0) (srvname "") (timeout 10)) ;; [m] [m] [degree] - "move robot toward (x, y, d) (units are m, m and degrees respectively) and wait to reach the goal." - (setq req (instance spot_msgs::TrajectoryCommandRequest :init)) - (send req :target_pose :header :frame_id "body") - (send req :target_pose :pose :position :x x) - (send req :target_pose :pose :position :y y) - (send req :target_pose :pose :orientation :z (sin (/ (deg2rad d) 2))) - (send req :target_pose :pose :orientation :w (cos (/ (deg2rad d) 2))) - (setq r (ros::service-call srvname req)) - (if (send r :success) - (send r :success) - (progn - (ros::roseus-error (send r :message)) - (send r :success) - )) + "move robot toward (x, y, d) (units are m, m and degrees respectively)." + (x y &optional (d 0) (timeout 10)) ;; [m] [m] [degree] + (send self :go-pos x y d :timeout timeout :wait nil) ) (:list-graph From 12cc141827db340be4ab10d6a4d3277041856454 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Mar 2021 16:52:56 +0900 Subject: [PATCH 016/261] [spoteus] fix bugs in go-pos --- jsk_spot_robot/spoteus/spot-interface.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index e4f0dc10e8..8fd76b44e8 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -251,13 +251,13 @@ (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::TrajectoryActionGoal :init)) + (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 :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 :rotation :z (sin (/ (deg2rad d) 2))) - (send trajectory-cmd-goal-msg :target_pose :pose :rotation :w (cos (/ (deg2rad d) 2))) + (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) From 005dc7aef6d07a57fe83901f4051eb6b6533046a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 18 Mar 2021 18:28:58 +0900 Subject: [PATCH 017/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 47 ++++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 19 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 30e67c2df1..02a84bdec0 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -1,6 +1,12 @@ jsk_spot_robot ============== +Currently, this packages require + +- [spot_ros]() with [this patch](https://github.com/clearpathrobotics/spot_ros/pull/25) +- [common_msgs]() with [this patch](https://github.com/ros/common_msgs/pull/171) +- [jsk_recognition]() with [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579) and [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581) + ## Manuals - [Supported Documents of Boston Dynamics](https://www.bostondynamics.com/spot/training/documentation) @@ -14,11 +20,10 @@ jsk_spot_robot $ mkdir $HOME/catkin_ws/src -p $ cd $HOME/catkin_ws/src $ wstool init . -$ wstool merge -t . https://raw.githubusercontent.com/k-okada/jsk_robot/spot/jsk_spot_robot/jsk_spot.rosinstall +$ wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall $ source /opt/ros/$ROS_DISTRO/setup.bash $ rosdep install -y -r --from-paths . --ignore-src -$ cd ../ -$ catkin build spoteus jsk_spot_startup +$ catkin build $ source $HOME/catkin_ws/devel/setup.bash ``` @@ -26,27 +31,31 @@ $ source $HOME/catkin_ws/devel/setup.bash 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) -After that, please run the ros driver. You can now control spot from ROS! +After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! -``` -$ source /opt/ros/$ROS_DISTRO/setup.bash -$ source $HOME/catkin_ws/devel/setup.bash -$ roslaunch jsk_spot_startup driver.launch username:= password:= +```bash +$ roslaunch jsk_spot_startup jsk_spot_bringup.launch username:= password:= ``` -You can launch ros driver without specifying username and password by sourcing catkin_ws under the spot user. +This launch includes +- driver launch file for spot +- bringup launch for additional peripheral devices (Respeaker, insta 360 air and ublox GPS module) +- teleoperation launch +- interaction launch with Speech-To-Text and Text-To-Speech -``` -$ source /opt/ros/$ROS_DISTRO/setup.bash -$ source /home/spot/catkin_ws/devel/setup.bash -$ roslaunch jsk_spot_startup driver.launch +For visualization, you can run RViz with jsk configuration. + +```bash +$ roslaunch jsk_spot_startup rviz.launch ``` -You can run RViz already configured for spot. +You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. -``` -# in another terminal -$ source /opt/ros/$ROS_DISTRO/setup.bash -$ source $HOME/catkin_ws/devel/setup.bash -$ roslaunch jsk_spot_startup rviz.launch +For development, `record.launch` and `play.launch` are useful for rosbag recording and playing. + +```bash +# Record a rosbag file +$ roslaunch jsk_spot_startup record.launch rosbag:= +# Play a rosbag file +$ roslaunch jsk_spot_startup play.launch rosbag:= ``` From 5fa44b54acb0c638f3fc2b47c45f36ee5249014c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 18 Mar 2021 18:29:07 +0900 Subject: [PATCH 018/261] [jsk_spot_robot] update rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 094cf3a3bf..1bc1f1d226 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,8 +1,16 @@ - git: - local-name: jsk_robot - uri: https://github.com/k-okada/jsk_robot.git - version: spot + local-name: jsk-ros-pkg/jsk_robot + uri: https://github.com/sktometometo/jsk_robot.git + version: develop/spot +- git: + local-name: jsk-ros-pkg/jsk_recognition + uri: https://github.com/sktometometo/jsk_recognition.git + version: develop/spot +- git: + local-name: common_msgs + uri: https://github.com/sktometometo/common_msgs.git + version: PR/add-panorama-info - git: local-name: spot-ros - uri: https://github.com/clearpathrobotics/spot_ros.git - version: master + uri: https://github.com/sktometometo/spot_ros.git + version: feature/add-go-pos From 796755a0b49c8b23ca70af46a79161db8acbf5d7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 20 Mar 2021 09:51:30 +0900 Subject: [PATCH 019/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 02a84bdec0..b97f8938dd 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -16,15 +16,18 @@ Currently, this packages require ### Setting up a catkin workspace for a new user in the internal pc -``` -$ mkdir $HOME/catkin_ws/src -p -$ cd $HOME/catkin_ws/src -$ wstool init . -$ wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall -$ source /opt/ros/$ROS_DISTRO/setup.bash -$ rosdep install -y -r --from-paths . --ignore-src -$ catkin build -$ source $HOME/catkin_ws/devel/setup.bash +```bash +source /opt/ros/$ROS_DISTRO/setup.bash +mkdir $HOME/catkin_ws/src -p +cd $HOME/catkin_ws +catkin init +cd src +wstool init . +wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall +wstool update +rosdep install -y -r --from-paths . --ignore-src +catkin build +source $HOME/catkin_ws/devel/setup.bash ``` ### Bringup spot @@ -34,7 +37,7 @@ First, please turn on spot and turn on motors according to [the OPERATION sectio After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! ```bash -$ roslaunch jsk_spot_startup jsk_spot_bringup.launch username:= password:= +roslaunch jsk_spot_startup jsk_spot_bringup.launch username:= password:= ``` This launch includes @@ -46,7 +49,7 @@ This launch includes For visualization, you can run RViz with jsk configuration. ```bash -$ roslaunch jsk_spot_startup rviz.launch +roslaunch jsk_spot_startup rviz.launch ``` You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. @@ -55,7 +58,7 @@ For development, `record.launch` and `play.launch` are useful for rosbag recordi ```bash # Record a rosbag file -$ roslaunch jsk_spot_startup record.launch rosbag:= +roslaunch jsk_spot_startup record.launch rosbag:= # Play a rosbag file -$ roslaunch jsk_spot_startup play.launch rosbag:= +roslaunch jsk_spot_startup play.launch rosbag:= ``` From 7f85b1e330cc0687f380ff6c71725e7653a7c723 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 15 Apr 2021 09:58:43 +0900 Subject: [PATCH 020/261] [jsk_spot_robot] update .rosinstall file (#9) --- jsk_spot_robot/jsk_spot.rosinstall | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 1bc1f1d226..3dc12cbad1 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,16 +1,32 @@ +# PanoramaInfo.msg is used. +# So we need to use this branch until it is merged to master +- git: + local-name: common_msgs + uri: https://github.com/sktometometo/common_msgs.git + version: PR/add-panorama-info +# this is a develop branch for spot +# We need to use this develop branch until it is merged to master - git: local-name: jsk-ros-pkg/jsk_robot uri: https://github.com/sktometometo/jsk_robot.git version: develop/spot +# currently, patches below are required for object detection with insta 360 air +# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579 +# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581 +# this branch is applied version of these patches. +# We need to use it until these patches are merged. - git: local-name: jsk-ros-pkg/jsk_recognition uri: https://github.com/sktometometo/jsk_recognition.git version: develop/spot +# Some TTS and STT nodes and 3rdparty drivers are required - git: - local-name: common_msgs - uri: https://github.com/sktometometo/common_msgs.git - version: PR/add-panorama-info + local-name: jsk-ros-pkg/jsk_3rdparty + uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git + version: master +# 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: feature/add-go-pos + version: develop/spot From ab5f109a73a4e1f58c7ce331ad294b5eb2ebafd7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 19 Apr 2021 13:12:54 +0900 Subject: [PATCH 021/261] [spoteus] add .gitignore (#14) --- jsk_spot_robot/spoteus/.gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 jsk_spot_robot/spoteus/.gitignore 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 From b57a930b89408a34111ecc48401c9143790e643c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Wed, 21 Apr 2021 19:33:42 +0900 Subject: [PATCH 022/261] [jsk_spot_robot] Add requirements.txt to explictly install bosdyn SDK (#17) --- jsk_spot_robot/README.md | 1 + jsk_spot_robot/requirements.txt | 4 ++++ 2 files changed, 5 insertions(+) create mode 100644 jsk_spot_robot/requirements.txt diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index b97f8938dd..711b0946b5 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -26,6 +26,7 @@ wstool init . wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall wstool update rosdep install -y -r --from-paths . --ignore-src +pip3 install -r requirements.txt catkin build source $HOME/catkin_ws/devel/setup.bash ``` diff --git a/jsk_spot_robot/requirements.txt b/jsk_spot_robot/requirements.txt new file mode 100644 index 0000000000..91a8d9c71d --- /dev/null +++ b/jsk_spot_robot/requirements.txt @@ -0,0 +1,4 @@ +bosdyn-client == 2.3.4 +bosdyn-mission == 2.3.4 +bosdyn-api == 2.3.4 +bosdyn-core == 2.3.4 From 5e8398a97121466bde760126441b01e529cc0074 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata <27789460+mqcmd196@users.noreply.github.com> Date: Sat, 24 Apr 2021 10:14:25 +0900 Subject: [PATCH 023/261] [spoteus] fix comment bug (#19) --- jsk_spot_robot/spoteus/spot-interface.l | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 8fd76b44e8..0b7c69999a 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -43,7 +43,7 @@ (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 (ros::wait-for-service "/spot/claim" 5) - (ros::ros-error "could not communicate with robot, may be forget to roslaunch spot_driver driver.launh, or did not power on the robot")) + (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) @@ -232,7 +232,7 @@ (x y d ;; [m/sec] [m/sec] [rad/sec] &optional (msec 1000) ;; msec is total animation time [msec] &key (stop t) (wait t)) - "contorl the robot velocity x([m/sec]) y([m/sec]) d([rad/sec]) msec([msec]). msec is the time to travel." + "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)) @@ -266,13 +266,13 @@ ) ) (:go-pos-wait - "move robot toward (x, y, d) (units are m, m and degrees respectively). and 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 - "move robot toward (x, y, d) (units are m, m and degrees respectively)." (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) ) From 57270c775adff38d1ff723b653f6160a3bd66316 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sat, 10 Apr 2021 01:05:20 +0900 Subject: [PATCH 024/261] [jsk_spot_startup] Refactor the function related the autowalk navigation in eus spot-interface --- jsk_spot_robot/spoteus/spot-interface.l | 62 +++++++++++++++++-------- 1 file changed, 43 insertions(+), 19 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 0b7c69999a..3e21b14841 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -275,17 +275,6 @@ "move robot toward (x, y, d) (units are m, m and degrees respectively)." (send self :go-pos x y d :timeout timeout :wait nil) ) - - (:list-graph - (upload-filepath) - (let (r) - (setq r (ros::service-call "/spot/list_graph" - (instance spot_msgs::ListGraphRequest :init :upload_filepath upload-filepath))) - (ros::ros-info "Call \"/spot/list_graph\" returns ..") - (dolist (id (send r :waypoint_ids)) - (ros::ros-info " \"~A\"" id)) - (send r :waypoint_ids))) - (:find-waypoint-position-from-id (id ids) (let (ret) @@ -300,10 +289,50 @@ (if (< index (length ids)) (setq ret (elt ids index))) ret)) + (:initial-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)) + (:initial-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)) + (:upload-path + (filepath &key (initial-localization-fiducial t) (wait nil)) + "upload graph for autowalk" + (let (r) + (if wait (ros::wait-for-service srvname)) + (setq r (ros::service-call "/spot/upload_graph" + (instance spot_msgs::UploadGraphRequest :init :upload_filepath filepath))) + (ros::ros-info "Call \"/spot/upload_graph\" returns .. \"~A\"" (send r :message)) + (if initial-localization-fiducial (send self :initial-localization-fiducial)) + (send r :message))) + (: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))) (:navigate-to - (upload-path navigate-to &key (initial-localization-fiducial t) (initial-localization-waypoint nil)) + (navigate-to &key (initial-localization-waypoint nil)) (let (ids c goal ret) - (setq ids (send self :list-graph upload-path)) + (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) @@ -311,8 +340,6 @@ (setq c (instance ros::simple-action-client :init "/spot/navigate_to" spot_msgs::NavigateToAction)) (send c :wait-for-server) - (ros::ros-info "run navigation from ~A" upload-path) - (ros::ros-info "use fiducal localization ~A" initial-localization-fiducial) (ros::ros-info "initial waypoit ~3D/~3D ~A" (if initial-localization-waypoint (position initial-localization-waypoint ids :test #'string=) @@ -322,10 +349,7 @@ (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 :upload_path upload-path) - (send goal :goal :navigate_to navigate-to) - (send goal :goal :initial_localization_fiducial initial-localization-fiducial) - (send goal :goal :initial_localization_waypoint initial-localization-waypoint) + (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)) From 5ce069facce650e5e000e3e2950cea4e880825a2 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 16 Apr 2021 20:38:26 +0900 Subject: [PATCH 025/261] [jsk_spot_startup] Refactor the move-to sample using eus --- jsk_spot_robot/spoteus/move-to.l | 48 +++++++++++++++++++++----------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/jsk_spot_robot/spoteus/move-to.l b/jsk_spot_robot/spoteus/move-to.l index 6d5120fbba..c7950de3f8 100644 --- a/jsk_spot_robot/spoteus/move-to.l +++ b/jsk_spot_robot/spoteus/move-to.l @@ -1,20 +1,34 @@ -(load "spot-interface.l") +(load "package://spoteus/spot-interface.l") (spot-init nil) ;; do not create-viewer -(setq path "/home/lokada/work/2020-11-18 203458 73B1.walk" ) -;; use fedical start to goal, -1 means goal -(send *ri* :navigate-to path -1 :initial-localization-fiducial t :initial-localization-waypoint 0) -;; do not use fedical but start 73b2 to 73a2 -(send *ri* :navigate-to path 7 :initial-localization-fiducial t :initial-localization-waypoint 0) -;; back to 73b2 -(send *ri* :navigate-to path 0 :initial-localization-fiducial nil :initial-localization-waypoint 7) -;; go to 73a2 again, you do not need fedical -(send *ri* :navigate-to path 7 :initial-localization-fiducial nil :initial-localization-waypoint 0) -;; and got to follow original route -(send *ri* :navigate-to path -1 :initial-localization-fiducial nil :initial-localization-waypoint 7) -(ros::ros-info "done") - -(send *ri* :sit)(unix::sleep 3) ;; need to fix... -(send *ri* :power-off)(unix::sleep 3) ;; need to fix -(send *ri* :release) +(setq *path* (ros::get-param "~path" "/home/spot/autowalk_files/2021-04-12_eng2_73b2_to_81c1_night.walk")) +(setq *init-waypoint* (floor (ros::get-param "~init_waypoint" 0))) +(setq *upload* (ros::get-param "~upload" t)) + +(if *upload* (send *ri* :upload-path *path*)) + +(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 "start barometer measurement?") +(if (y-or-n-p) t (exit)) + +(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) From aac8205d8288e6b3b5ad2ad99bbd373d0859db9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Sun, 18 Apr 2021 08:17:01 +0900 Subject: [PATCH 026/261] [jsk_spot_startup] Fix the bug in spot eus move-to sample (#13) --- jsk_spot_robot/spoteus/move-to.l | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/jsk_spot_robot/spoteus/move-to.l b/jsk_spot_robot/spoteus/move-to.l index c7950de3f8..907e84e5bd 100644 --- a/jsk_spot_robot/spoteus/move-to.l +++ b/jsk_spot_robot/spoteus/move-to.l @@ -2,7 +2,7 @@ (spot-init nil) ;; do not create-viewer -(setq *path* (ros::get-param "~path" "/home/spot/autowalk_files/2021-04-12_eng2_73b2_to_81c1_night.walk")) +(setq *path* (ros::get-param "~path" "/home/spot/autowalk_files/2021-04-16_eng2_73b2_to_81c1_night.walk")) (setq *init-waypoint* (floor (ros::get-param "~init_waypoint" 0))) (setq *upload* (ros::get-param "~upload" t)) @@ -21,13 +21,9 @@ ;; go to 81C1 (send *ri* :navigate-to -1) -(ros::ros-info "start barometer measurement?") -(if (y-or-n-p) t (exit)) - (ros::ros-info "ready go back to 73B2?") (if (y-or-n-p) t (exit)) - ;; go back to 73B2 (send *ri* :navigate-to 0) From a4c842c993df327402a1ffdfd258e592439571bf Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata <27789460+mqcmd196@users.noreply.github.com> Date: Sat, 24 Apr 2021 10:31:19 +0900 Subject: [PATCH 027/261] [spoteus] add body-pose method (#18) * add body-pose method * add calculate rpy -> quaternion methods * adapt create-quaternion func to the body-pose method * add args --- jsk_spot_robot/spoteus/spot-interface.l | 32 +++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 3e21b14841..3d01f701d4 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -275,6 +275,19 @@ "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 + (r p y + &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)) + (let ((pose-msg (instance geometry_msgs::Pose :init)) + (quaternion-msg (send self :create-quaternion-msg-from-rpy r p y))) + (send pose-msg :orientation quaternion-msg) + (ros::publish topic-name pose-msg)) + (send self :stand)) (:find-waypoint-position-from-id (id ids) (let (ret) @@ -354,6 +367,25 @@ (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)))) + (print (list sin-roll cos-roll + sin-pitch cos-pitch + sin-yaw cos-yaw)) + (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)) + (- (* sin-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)) From 5b57f49fb9af1e574aea06278c13d0eb5d5f7164 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 25 Apr 2021 18:14:29 +0900 Subject: [PATCH 028/261] [spoteus][spoteus_demo] add spoteus_demo package (#15) * [spoteus][spoteus_demo] create spoteus_demo package * [spoteus_demo] add shebang and change permission * [spoteus_demo] add gitignore to autowalk directory * [jsk_data] add auto download script for autowalk data * [spoteus_demo] rename sample scripts, add comments and update filepath to relative path from this package * [jsk_spot_robot] update jsk_spot.rosinstall to add jsk_common * [spoteus_demo] udpate package.xml * [spoteus_demo] fix format --- jsk_spot_robot/jsk_spot.rosinstall | 6 +++++ jsk_spot_robot/spoteus_demo/CMakeLists.txt | 11 +++++++++ .../spoteus_demo/autowalk/.gitignore | 1 + .../euslisp/sample_navigate_to.l} | 12 ++++++++-- .../euslisp/sample_visualization.l} | 9 ++++++- jsk_spot_robot/spoteus_demo/package.xml | 24 +++++++++++++++++++ .../scripts/download_autowalk_data.py | 19 +++++++++++++++ 7 files changed, 79 insertions(+), 3 deletions(-) create mode 100644 jsk_spot_robot/spoteus_demo/CMakeLists.txt create mode 100644 jsk_spot_robot/spoteus_demo/autowalk/.gitignore rename jsk_spot_robot/{spoteus/move-to.l => spoteus_demo/euslisp/sample_navigate_to.l} (68%) mode change 100644 => 100755 rename jsk_spot_robot/{spoteus/sample.l => spoteus_demo/euslisp/sample_visualization.l} (68%) mode change 100644 => 100755 create mode 100644 jsk_spot_robot/spoteus_demo/package.xml create mode 100755 jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 3dc12cbad1..55d4258041 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -24,6 +24,12 @@ local-name: jsk-ros-pkg/jsk_3rdparty uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git version: master +# Until https://github.com/jsk-ros-pkg/jsk_common/pull/1685 is merged, this patch is +# required to build spoteus_demo +- git: + local-name: jsk-ros-pkg/jsk_common + uri: https://github.com/sktometometo/jsk_common.git + version: PR/update-download-data # This is a develop branch for jsk version. # We need to use it until it is merged to master - git: diff --git a/jsk_spot_robot/spoteus_demo/CMakeLists.txt b/jsk_spot_robot/spoteus_demo/CMakeLists.txt new file mode 100644 index 0000000000..0c2b884f6b --- /dev/null +++ b/jsk_spot_robot/spoteus_demo/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spoteus_demo) + +find_package(catkin REQUIRED COMPONENTS + roseus + spoteus +) + +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() diff --git a/jsk_spot_robot/spoteus_demo/autowalk/.gitignore b/jsk_spot_robot/spoteus_demo/autowalk/.gitignore new file mode 100644 index 0000000000..72e8ffc0db --- /dev/null +++ b/jsk_spot_robot/spoteus_demo/autowalk/.gitignore @@ -0,0 +1 @@ +* diff --git a/jsk_spot_robot/spoteus/move-to.l b/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l old mode 100644 new mode 100755 similarity index 68% rename from jsk_spot_robot/spoteus/move-to.l rename to jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l index 907e84e5bd..02e58a1df7 --- a/jsk_spot_robot/spoteus/move-to.l +++ b/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l @@ -1,15 +1,23 @@ +#!/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" "/home/spot/autowalk_files/2021-04-16_eng2_73b2_to_81c1_night.walk")) +(setq *path* (ros::get-param "~path" (format nil "~A/autowalk/eng2_73b2_to_81c1_night.walk" (ros::rospack-find "spoteus_demo")))) (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) diff --git a/jsk_spot_robot/spoteus/sample.l b/jsk_spot_robot/spoteus_demo/euslisp/sample_visualization.l old mode 100644 new mode 100755 similarity index 68% rename from jsk_spot_robot/spoteus/sample.l rename to jsk_spot_robot/spoteus_demo/euslisp/sample_visualization.l index 1d6d1422d5..1d5e539adc --- a/jsk_spot_robot/spoteus/sample.l +++ b/jsk_spot_robot/spoteus_demo/euslisp/sample_visualization.l @@ -1,4 +1,11 @@ -(load "spot-interface.l") +#!/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) diff --git a/jsk_spot_robot/spoteus_demo/package.xml b/jsk_spot_robot/spoteus_demo/package.xml new file mode 100644 index 0000000000..280784a731 --- /dev/null +++ b/jsk_spot_robot/spoteus_demo/package.xml @@ -0,0 +1,24 @@ + + + spoteus_demo + 1.1.0 + The spoteus_demo package + + Kei Okada + Yoshiki Obinata + Koki Shinjo + BSD + Kei Okada + Yoshiki Obinata + Koki Shinjo + + catkin + jsk_data + + roseus + spoteus + + + + + diff --git a/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py b/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py new file mode 100755 index 0000000000..1ccf549fa5 --- /dev/null +++ b/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python + +from jsk_data import download_data + + +def main(): + PKG = 'spoteus_demo' + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73b2_to_81c1_night.walk.tar.gz', + url='https://drive.google.com/uc?id=1PXgMsmN3uKPG3YAap9vdfrBduEiLgAl8', + md5='a79dd142013b12c0babb4400be583739', + extract=True + ) + + +if __name__ == '__main__': + main() From 68093347d7e99aca845c2f465ed41cac587f6c2d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 11:05:29 +0900 Subject: [PATCH 029/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 711b0946b5..6e76d03ac0 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -31,6 +31,14 @@ catkin build source $HOME/catkin_ws/devel/setup.bash ``` +After this, please modify the credential file and remove it from git tracking. + +```bash +roscd jsk_spot_startup +# modify auth/credential_config.yaml +git update-index --skip-worktree auth/spot_credential.yaml +``` + ### 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) @@ -38,7 +46,7 @@ First, please turn on spot and turn on motors according to [the OPERATION sectio After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! ```bash -roslaunch jsk_spot_startup jsk_spot_bringup.launch username:= password:= +roslaunch jsk_spot_startup jsk_spot_bringup.launch ``` This launch includes From 1f6860e0ffae8b941c8aacebdba702260505469f Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Tue, 27 Apr 2021 19:12:59 +0900 Subject: [PATCH 030/261] [jsk_spot_robot] some changes in README.md (#22) * Update README.md * Update README.md add $rosdep update, fix file path in pip3 install and fix launch file name --- jsk_spot_robot/README.md | 61 ++++++++++++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 6e76d03ac0..3965b95516 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -16,19 +16,23 @@ Currently, this packages require ### Setting up a catkin workspace for a new user in the internal pc +#### setup a catkin workspace for spot driver + +Create a workspace for spot driver + ```bash source /opt/ros/$ROS_DISTRO/setup.bash mkdir $HOME/catkin_ws/src -p -cd $HOME/catkin_ws -catkin init -cd src +cd $HOME/catkin_ws/src wstool init . wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall wstool update +rosdep update rosdep install -y -r --from-paths . --ignore-src -pip3 install -r requirements.txt +pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt +cd $HOME/catkin_ws +catkin init catkin build -source $HOME/catkin_ws/devel/setup.bash ``` After this, please modify the credential file and remove it from git tracking. @@ -39,6 +43,37 @@ roscd jsk_spot_startup git update-index --skip-worktree auth/spot_credential.yaml ``` +#### setup a catkin workspace for coral usb + +Please see [this page](https://github.com/knorth55/coral_usb_ros) for details. + +First, install requirements. + +```bash +sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy +sudo apt-get install python3-opencv +sudo apt-get install ros-melodic-catkin +``` + +And create a workspace for coral_usb_ros + +```bash +source /opt/ros/$ROS_DISTRO/setup.bash +mkdir $HOME/coral_ws/src -p +cd $HOME/coral_ws/src +git clone https://github.com/knorth55/coral_usb_ros.git +wstool init . +wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot_coral.rosinstall +wstool merge -t . coral_usb_ros/fc.rosinstall +wstool merge -t . coral_usb_ros/fc.rosinstall.melodic +wstool update +rosdep install -y -r --from-paths . --ignore-src +cd $HOME/coral_ws +catkin init +catkin config -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 jsk_spot_startup coral_usb +``` + ### 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) @@ -46,6 +81,7 @@ First, please turn on spot and turn on motors according to [the OPERATION sectio After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! ```bash +source $HOME/catkin_ws/devel/setup.bash roslaunch jsk_spot_startup jsk_spot_bringup.launch ``` @@ -55,19 +91,28 @@ This launch includes - teleoperation launch - interaction launch with Speech-To-Text and Text-To-Speech +And you can run object detection with + +```bash +source $HOME/coral_ws/devel/setup.bash +roslaunch jsk_spot_startup object_detection_and_tracking.launch +``` + For visualization, you can run RViz with jsk configuration. ```bash +source $HOME/catkin_ws/devel/setup.bash roslaunch jsk_spot_startup rviz.launch ``` You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. -For development, `record.launch` and `play.launch` are useful for rosbag recording and playing. +For development, `rosbag_record.launch` and `rosbag_play.launch` are useful for rosbag recording and playing. ```bash +source $HOME/catkin_ws/devel/setup.bash # Record a rosbag file -roslaunch jsk_spot_startup record.launch rosbag:= +roslaunch jsk_spot_startup rosbag_record.launch rosbag:= # Play a rosbag file -roslaunch jsk_spot_startup play.launch rosbag:= +roslaunch jsk_spot_startup rosbag_play.launch rosbag:= ``` From 35a4acbb6dcad3de01c00ab3166b9f47dbff35fe Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 25 Apr 2021 21:15:29 +0900 Subject: [PATCH 031/261] [spoteus] Fix bug about conversion from RPY Euler angle to Qauternion --- jsk_spot_robot/spoteus/spot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 3d01f701d4..a2156d88d8 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -380,7 +380,7 @@ (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)) - (- (* sin-roll cos-pitch sin-yaw) (* sin-roll sin-pitch cos-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)) From 4d7d184dece75c4bc836ecb750a7e24d09aec87c Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 25 Apr 2021 22:39:16 +0900 Subject: [PATCH 032/261] [spoteus] Make logging quiet when continuously execute :body-pose --- jsk_spot_robot/spoteus/spot-interface.l | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index a2156d88d8..552794189d 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -12,7 +12,7 @@ (let (r) (if wait (ros::wait-for-service srvname)) (setq r (ros::service-call srvname (instance std_srvs::TriggerRequest :init))) - (ros::ros-info "Call \"~A\" returns \"~A\"" srvname (send r :message)) + (ros::ros-debug "Call \"~A\" returns \"~A\"" srvname (send r :message)) (send r :success))) (defun call-set-bool-service (srvname data &key (wait nil)) @@ -374,9 +374,6 @@ (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)))) - (print (list sin-roll cos-roll - sin-pitch cos-pitch - sin-yaw cos-yaw)) (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)) From e44b1ee6d583c254ba95fc22c29dfe2867c684b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Fri, 25 Jun 2021 09:50:45 +0900 Subject: [PATCH 033/261] [spoteus] Fix the dead lock in naming the slots about battery status (#23) --- jsk_spot_robot/spoteus/spot-interface.l | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 552794189d..9e77e02b8d 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -118,21 +118,23 @@ (:spot-status-battery-states-callback (msg) (dolist (state (send msg :battery_states)) - (let ((s (string-upcase (substitute #\- #\_ (send state :identifier))))) - (send self :set-robot-state1 (intern (format nil "BATTERY-STATES-~A-IDENTIFIER" s) *keyword-package*) + (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" s) *keyword-package*) + (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" s) *keyword-package*) + (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" s) *keyword-package*) + (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" s) *keyword-package*) + (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" s) *keyword-package*) + (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" s) *keyword-package*) - (case (send state :status) (0 'unknown) (1 'missing) (2 'charging) (3 'discharging) (4 'booting)))))) + (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 From 9bd5059f71d831d4cd310e373cf199f70d1d8541 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 10:40:28 +0900 Subject: [PATCH 034/261] [jsk_spot_robot] update rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 55d4258041..25c0e11aa0 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -24,12 +24,12 @@ local-name: jsk-ros-pkg/jsk_3rdparty uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git version: master -# Until https://github.com/jsk-ros-pkg/jsk_common/pull/1685 is merged, this patch is -# required to build spoteus_demo +# Until sktometometo/feature/add-tf-relay-package is merged, this patch is +# required for spot_basic_behaviors - git: local-name: jsk-ros-pkg/jsk_common uri: https://github.com/sktometometo/jsk_common.git - version: PR/update-download-data + version: feature/add-tf-relay-package # This is a develop branch for jsk version. # We need to use it until it is merged to master - git: From 08b7115ea5a93dae9dfe9f79fdfc75baddf23083 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 11:12:50 +0900 Subject: [PATCH 035/261] [jsk_spot_robot] add app_manager dependencies --- jsk_spot_robot/jsk_spot.rosinstall | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 25c0e11aa0..bc07b44020 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -36,3 +36,12 @@ local-name: spot-ros uri: https://github.com/sktometometo/spot_ros.git version: develop/spot +- git: + local-name: PR2/app_manager + uri: https://github.com/PR2/app_manager.git + version: kinetic-devel +## fetch15 version of roswww is necessary for basic authentication +- git: + local-name: tork-a/roswww + uri: https://github.com/knorth55/roswww.git + version: fetch15 From bc0658cd70a985b42ab07b14f1ced46e3b91bb93 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 3 Jul 2021 13:26:45 +0900 Subject: [PATCH 036/261] [jsk_spot_robot] add visualization_rwt to rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index bc07b44020..9231cf8552 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -45,3 +45,8 @@ local-name: tork-a/roswww uri: https://github.com/knorth55/roswww.git version: fetch15 +# visualization_rwt is not released for melodic +- git: + local-name: tork-a/visualization_rwt + uri: https://github.com/tork-a/visualization_rwt.git + version: kinetic-devel From 818449b083ecd1f6327d5abe271b1c15ab5e7ba5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 4 Jul 2021 11:03:47 +0900 Subject: [PATCH 037/261] [jsk_spot_robot] update rosinstall for jsk_3rdparty --- jsk_spot_robot/jsk_spot.rosinstall | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 9231cf8552..5a721af7f9 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -20,10 +20,11 @@ uri: https://github.com/sktometometo/jsk_recognition.git version: develop/spot # Some TTS and STT nodes and 3rdparty drivers are required +# And dialogflow-executive, switchbot_ros, webrtcvad\ros is required - git: local-name: jsk-ros-pkg/jsk_3rdparty - uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git - version: master + uri: https://github.com/sktometometo/jsk_3rdparty.git + version: develop/spot # Until sktometometo/feature/add-tf-relay-package is merged, this patch is # required for spot_basic_behaviors - git: From fb1ac73cd5eb7d8b4615d08da3047a670423d81d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 4 Jul 2021 11:06:38 +0900 Subject: [PATCH 038/261] [jsk_spot_robot] update rosinstall for jsk_demos --- jsk_spot_robot/jsk_spot.rosinstall | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 5a721af7f9..a3e49a4ada 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -31,6 +31,11 @@ local-name: jsk-ros-pkg/jsk_common uri: https://github.com/sktometometo/jsk_common.git version: feature/add-tf-relay-package +# jsk_spot_watch_dog is requried for jsk_spot_apps +- git: + local-name: jsk-ros-pkg/jsk_demos + uri: https://github.com/sktometometo/jsk_demos.git + version: develop/spot # This is a develop branch for jsk version. # We need to use it until it is merged to master - git: From 421c6c38040aec7ae6246946e9b0cc12b95482b4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 11:18:26 +0900 Subject: [PATCH 039/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 3965b95516..c136abfd12 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -32,7 +32,7 @@ rosdep install -y -r --from-paths . --ignore-src pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt cd $HOME/catkin_ws catkin init -catkin build +catkin build -j4 -c ``` After this, please modify the credential file and remove it from git tracking. @@ -71,7 +71,7 @@ rosdep install -y -r --from-paths . --ignore-src cd $HOME/coral_ws catkin init catkin config -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 jsk_spot_startup coral_usb +catkin build jsk_spot_startup coral_usb -j4 -c ``` ### Bringup spot From 62c3d594837ceb6da7cb718be6bdb6aa8bc9c8c0 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 11 Jul 2021 17:52:06 +0900 Subject: [PATCH 040/261] [jsk_spot_robot] add app_manager_utils --- jsk_spot_robot/jsk_spot.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index a3e49a4ada..91be046d67 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -46,6 +46,10 @@ local-name: PR2/app_manager uri: https://github.com/PR2/app_manager.git version: kinetic-devel +- git: + local-name: knorth55/app_manager_utils + uri: https://github.com/knorth55/app_manager_utils.git + version: master ## fetch15 version of roswww is necessary for basic authentication - git: local-name: tork-a/roswww From 3f82fe1856e447bccb7d94c71fdacb0d21f6b3f4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 11:21:25 +0900 Subject: [PATCH 041/261] [jsk_spot_robot] add recorder plugin to rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 91be046d67..7c64c0b5bf 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -50,6 +50,10 @@ local-name: knorth55/app_manager_utils uri: https://github.com/knorth55/app_manager_utils.git version: master +- git: + local-name: knorth55/audio_video_recorder + uri: https://github.com/knorth55/audio_video_recorder.git + version: main ## fetch15 version of roswww is necessary for basic authentication - git: local-name: tork-a/roswww From d38c4838731b4e067ae3e21fb7938fec20502de6 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 15 Jul 2021 09:31:22 +0900 Subject: [PATCH 042/261] [jsk_spot_robot] udpate rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 7c64c0b5bf..b594bca3d6 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,20 +1,6 @@ -# PanoramaInfo.msg is used. -# So we need to use this branch until it is merged to master -- git: - local-name: common_msgs - uri: https://github.com/sktometometo/common_msgs.git - version: PR/add-panorama-info -# this is a develop branch for spot -# We need to use this develop branch until it is merged to master -- git: - local-name: jsk-ros-pkg/jsk_robot - uri: https://github.com/sktometometo/jsk_robot.git - version: develop/spot -# currently, patches below are required for object detection with insta 360 air -# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579 +# Until PRs below are merged, we need to use develop/spot branch in sktometometo/jsk_recognition.git # - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581 -# this branch is applied version of these patches. -# We need to use it until these patches are merged. +# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2596 - git: local-name: jsk-ros-pkg/jsk_recognition uri: https://github.com/sktometometo/jsk_recognition.git From 5f04da2727eddfc20bb2c657ee82d22b2c4efb22 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 22 Jul 2021 17:24:43 +0900 Subject: [PATCH 043/261] [spoteus_demo] add sample_basics.l --- .../spoteus_demo/euslisp/sample_basics.l | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100755 jsk_spot_robot/spoteus_demo/euslisp/sample_basics.l diff --git a/jsk_spot_robot/spoteus_demo/euslisp/sample_basics.l b/jsk_spot_robot/spoteus_demo/euslisp/sample_basics.l new file mode 100755 index 0000000000..9f726e7680 --- /dev/null +++ b/jsk_spot_robot/spoteus_demo/euslisp/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) From 8ace4992fbe659a951e97e5fb08304668f917b88 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 11 Aug 2021 17:52:53 +0900 Subject: [PATCH 044/261] [spoteus_demo] remove autowalk data downloing --- jsk_spot_robot/spoteus_demo/CMakeLists.txt | 2 -- .../spoteus_demo/autowalk/.gitignore | 1 - .../spoteus_demo/euslisp/sample_navigate_to.l | 2 +- jsk_spot_robot/spoteus_demo/package.xml | 2 +- .../scripts/download_autowalk_data.py | 19 ------------------- 5 files changed, 2 insertions(+), 24 deletions(-) delete mode 100644 jsk_spot_robot/spoteus_demo/autowalk/.gitignore delete mode 100755 jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py diff --git a/jsk_spot_robot/spoteus_demo/CMakeLists.txt b/jsk_spot_robot/spoteus_demo/CMakeLists.txt index 0c2b884f6b..48d5a77308 100644 --- a/jsk_spot_robot/spoteus_demo/CMakeLists.txt +++ b/jsk_spot_robot/spoteus_demo/CMakeLists.txt @@ -6,6 +6,4 @@ find_package(catkin REQUIRED COMPONENTS spoteus ) -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() diff --git a/jsk_spot_robot/spoteus_demo/autowalk/.gitignore b/jsk_spot_robot/spoteus_demo/autowalk/.gitignore deleted file mode 100644 index 72e8ffc0db..0000000000 --- a/jsk_spot_robot/spoteus_demo/autowalk/.gitignore +++ /dev/null @@ -1 +0,0 @@ -* diff --git a/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l b/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l index 02e58a1df7..81a9b58e1e 100755 --- a/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l +++ b/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l @@ -9,7 +9,7 @@ (spot-init nil) ;; do not create-viewer -(setq *path* (ros::get-param "~path" (format nil "~A/autowalk/eng2_73b2_to_81c1_night.walk" (ros::rospack-find "spoteus_demo")))) +(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)) diff --git a/jsk_spot_robot/spoteus_demo/package.xml b/jsk_spot_robot/spoteus_demo/package.xml index 280784a731..985130de82 100644 --- a/jsk_spot_robot/spoteus_demo/package.xml +++ b/jsk_spot_robot/spoteus_demo/package.xml @@ -13,8 +13,8 @@ Koki Shinjo catkin - jsk_data + spot_autowalk_data roseus spoteus diff --git a/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py b/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py deleted file mode 100755 index 1ccf549fa5..0000000000 --- a/jsk_spot_robot/spoteus_demo/scripts/download_autowalk_data.py +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env python - -from jsk_data import download_data - - -def main(): - PKG = 'spoteus_demo' - - download_data( - pkg_name=PKG, - path='autowalk/eng2_73b2_to_81c1_night.walk.tar.gz', - url='https://drive.google.com/uc?id=1PXgMsmN3uKPG3YAap9vdfrBduEiLgAl8', - md5='a79dd142013b12c0babb4400be583739', - extract=True - ) - - -if __name__ == '__main__': - main() From f113b58ef024129a5463848b5fd073557276a0e6 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 25 Aug 2021 19:30:31 +0900 Subject: [PATCH 045/261] [jsk_spot_robot] update jsk_spot.rosinstall for aerial_robot --- jsk_spot_robot/jsk_spot.rosinstall | 42 ++++++++++++++++++++++++------ 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index b594bca3d6..d7d6907fd1 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,33 +1,59 @@ # Until PRs below are merged, we need to use develop/spot branch in sktometometo/jsk_recognition.git -# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581 # - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2596 - git: local-name: jsk-ros-pkg/jsk_recognition uri: https://github.com/sktometometo/jsk_recognition.git version: develop/spot -# Some TTS and STT nodes and 3rdparty drivers are required -# And dialogflow-executive, switchbot_ros, webrtcvad\ros is required +# spot-jsk will use packages below. +# - dialogflow-executive +# - webrtcvad_ros +# - switchbot_ros +# - respeaker_ros +# - ros_speech_recognition +# - aques_talk +# Until PRs below are merged, we need to use develop/spot branch +# - webrtcvad_ros +# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/282 +# - switchbot_ros update +# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/278 +# - dialogflow_ros update +# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/242 - git: local-name: jsk-ros-pkg/jsk_3rdparty uri: https://github.com/sktometometo/jsk_3rdparty.git version: develop/spot -# Until sktometometo/feature/add-tf-relay-package is merged, this patch is -# required for spot_basic_behaviors +# Until PR below is merged, we need to use develop/spot branch +# - https://github.com/jsk-ros-pkg/jsk_common/pull/1698 - git: local-name: jsk-ros-pkg/jsk_common uri: https://github.com/sktometometo/jsk_common.git - version: feature/add-tf-relay-package -# jsk_spot_watch_dog is requried for jsk_spot_apps + version: develop/spot +# Until PRs below are merged, we need to use develop/spot branch +# - person following demo +# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1343 +# - mech office demo +# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1341 +# - sing and dance demo +# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1340 +# - person leading demo +# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1339 +# - watchdog demo +# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1332 +# + patches +# - https://github.com/tongtybj/jsk_demos/pull/2 +# - https://github.com/tongtybj/jsk_demos/pull/1 - git: local-name: jsk-ros-pkg/jsk_demos uri: https://github.com/sktometometo/jsk_demos.git version: develop/spot +# 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 +# app_manager and utilities are requied - git: local-name: PR2/app_manager uri: https://github.com/PR2/app_manager.git @@ -40,7 +66,7 @@ local-name: knorth55/audio_video_recorder uri: https://github.com/knorth55/audio_video_recorder.git version: main -## fetch15 version of roswww is necessary for basic authentication +# fetch15 version of roswww is necessary for basic authentication - git: local-name: tork-a/roswww uri: https://github.com/knorth55/roswww.git From d7c1ddfc8c0142708ea56632f2a4ad92981f51b3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Sep 2021 12:21:28 +0900 Subject: [PATCH 046/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 83 ++++++++++++++++++++++++++++++++-------- 1 file changed, 68 insertions(+), 15 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index c136abfd12..52b027e056 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -1,12 +1,6 @@ jsk_spot_robot ============== -Currently, this packages require - -- [spot_ros]() with [this patch](https://github.com/clearpathrobotics/spot_ros/pull/25) -- [common_msgs]() with [this patch](https://github.com/ros/common_msgs/pull/171) -- [jsk_recognition]() with [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579) and [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581) - ## Manuals - [Supported Documents of Boston Dynamics](https://www.bostondynamics.com/spot/training/documentation) @@ -14,7 +8,28 @@ Currently, this packages require ## How to run -### Setting up a catkin workspace for a new user in the internal pc +### How to set up catkin workspace in a development pc. + +Create a workspace + +```bash +source /opt/ros/$ROS_DISTRO/setup.bash +mkdir $HOME/spot_ws/src -p +cd $HOME/spot_ws/src +wstool init . +wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +wstool update +wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot.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 $HOME/spot_ws +catkin init +catkin build -j4 -c +``` + +### How to set up a new user in the internal pc. #### setup a catkin workspace for spot driver @@ -22,20 +37,20 @@ Create a workspace for spot driver ```bash source /opt/ros/$ROS_DISTRO/setup.bash -mkdir $HOME/catkin_ws/src -p -cd $HOME/catkin_ws/src +mkdir $HOME/spot_ws/src -p +cd $HOME/spot_ws/src wstool init . wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.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 $HOME/catkin_ws +cd $HOME/spot_ws catkin init catkin build -j4 -c ``` -After this, please modify the credential file and remove it from git tracking. +After this, please modify the credential files and remove it from git tracking. ```bash roscd jsk_spot_startup @@ -74,14 +89,47 @@ catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/inc catkin build jsk_spot_startup coral_usb -j4 -c ``` -### Bringup spot +#### Set environmental variables + +Set your ROS_IP to WiFi AP address. + +``` +echo "# ROS_IP +rossetip 10.42.0.1 # change IP address if your wifi AP address is not this." >> ~/.bashrc +``` + +Add environmental variables for [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/dialogflow_task_executive) and [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) +Please see them for more details. + +``` +echo "# Credentials +export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file +export DIALOGFLOW_PROJECT_ID= +export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml" >> ~/.bashrc +``` + +#### Setting up udev file and add the user to groups + +Create udev rule for spot spinal if your pc doesn't have it. + +```bash +sudo sh -c 'echo "SUBSYSTEM==\"tty\", ATTRS{idVendor}==\"0403\", ATTRS{idProduct}==\"6001\", ATTRS{serial}==\"A7044PJ7\", SYMLINK+=\"spot-spinal\", GROUP=\"dialout\"" > /etc/udev/rules.d/99-spot-spinal.rules' +``` + +Add your user to dialout and audio group + +```bash +sudo groupadd dialout audio +``` + +### How to bring up 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) After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! ```bash -source $HOME/catkin_ws/devel/setup.bash +source $HOME/spot_ws/devel/setup.bash roslaunch jsk_spot_startup jsk_spot_bringup.launch ``` @@ -90,6 +138,7 @@ This launch includes - bringup launch for additional peripheral devices (Respeaker, insta 360 air and ublox GPS module) - teleoperation launch - interaction launch with Speech-To-Text and Text-To-Speech +- so on. And you can run object detection with @@ -101,7 +150,7 @@ roslaunch jsk_spot_startup object_detection_and_tracking.launch For visualization, you can run RViz with jsk configuration. ```bash -source $HOME/catkin_ws/devel/setup.bash +source $HOME/spot_ws/devel/setup.bash roslaunch jsk_spot_startup rviz.launch ``` @@ -110,9 +159,13 @@ You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./ For development, `rosbag_record.launch` and `rosbag_play.launch` are useful for rosbag recording and playing. ```bash -source $HOME/catkin_ws/devel/setup.bash +source $HOME/spot_ws/devel/setup.bash # Record a rosbag file roslaunch jsk_spot_startup rosbag_record.launch rosbag:= # Play a rosbag file roslaunch jsk_spot_startup rosbag_play.launch rosbag:= ``` + +### How to control spot from a remove development pc. + +TODO From 615d14c43bff5bffa1f603072a74bd4082571ce4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Sep 2021 12:56:21 +0900 Subject: [PATCH 047/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 52b027e056..27e2df4b5b 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -47,6 +47,7 @@ rosdep install -y -r --from-paths . --ignore-src pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt cd $HOME/spot_ws catkin init +catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release catkin build -j4 -c ``` @@ -56,6 +57,10 @@ After this, please modify the credential files and remove it from git tracking. roscd jsk_spot_startup # modify auth/credential_config.yaml git update-index --skip-worktree auth/spot_credential.yaml + +roscd spot_basic_behaviors +# modify config/switchbot_ros/token.yaml +git update-index --skip-worktree config/switchbot_ros/token.yaml ``` #### setup a catkin workspace for coral usb @@ -89,6 +94,14 @@ catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/inc catkin build jsk_spot_startup coral_usb -j4 -c ``` +After that, please download models for coral_usb_ros. + +``` +source /opt/ros/$ROS_DISTRO/setup.bash +source $HOME/coral_ws/devel/setup.bash +rosrun coral_usb download_models.py +``` + #### Set environmental variables Set your ROS_IP to WiFi AP address. @@ -122,7 +135,7 @@ Add your user to dialout and audio group sudo groupadd dialout audio ``` -### How to bring up spot +### 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) From 450add98d63e8d5d9d2e1f21426dacf0fb229b3e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 2 Sep 2021 14:39:59 +0900 Subject: [PATCH 048/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 50 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 27e2df4b5b..16d38c3cd2 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -181,4 +181,52 @@ roslaunch jsk_spot_startup rosbag_play.launch rosbag:= Date: Fri, 3 Sep 2021 09:08:04 +0900 Subject: [PATCH 049/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 16d38c3cd2..e81746145e 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -40,7 +40,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash mkdir $HOME/spot_ws/src -p cd $HOME/spot_ws/src wstool init . -wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot.rosinstall +wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +wstool update +wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot.rosinstall wstool update rosdep update rosdep install -y -r --from-paths . --ignore-src @@ -81,10 +83,11 @@ And create a workspace for coral_usb_ros source /opt/ros/$ROS_DISTRO/setup.bash mkdir $HOME/coral_ws/src -p cd $HOME/coral_ws/src -git clone https://github.com/knorth55/coral_usb_ros.git wstool init . -wstool merge -t . https://github.com/sktometometo/jsk_robot/raw/develop/spot/jsk_spot_robot/jsk_spot_coral.rosinstall -wstool merge -t . coral_usb_ros/fc.rosinstall +wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git +wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +wstool update +wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_coral.rosinstall wstool merge -t . coral_usb_ros/fc.rosinstall.melodic wstool update rosdep install -y -r --from-paths . --ignore-src From 8838bef8d49b1107f97a85a20c5bbd7bbac8ea06 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 18 Sep 2021 20:10:27 +0900 Subject: [PATCH 050/261] [jsk_spot_robot] update README and rosinstall --- jsk_spot_robot/README.md | 2 ++ jsk_spot_robot/jsk_spot.rosinstall | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index e81746145e..c5bc79cfc7 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -13,6 +13,8 @@ jsk_spot_robot Create a workspace ```bash +sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy +sudo apt-get install ros-melodic-catkin source /opt/ros/$ROS_DISTRO/setup.bash mkdir $HOME/spot_ws/src -p cd $HOME/spot_ws/src diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index d7d6907fd1..bb3bc7866e 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -52,7 +52,7 @@ - git: local-name: spot-ros uri: https://github.com/sktometometo/spot_ros.git - version: develop/spot + version: develop/spot-rebased # app_manager and utilities are requied - git: local-name: PR2/app_manager From f8557b992fdf8662b0977ae265e340f73b0e08c5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 23 Sep 2021 09:56:31 +0900 Subject: [PATCH 051/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 175 +++++++++++++++++++++++++-------------- 1 file changed, 115 insertions(+), 60 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index c5bc79cfc7..3517cd617c 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -8,7 +8,7 @@ jsk_spot_robot ## How to run -### How to set up catkin workspace in a development pc. +### How to set up catkin workspace (for a user) Create a workspace @@ -21,7 +21,7 @@ cd $HOME/spot_ws/src wstool init . wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot.rosinstall +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 @@ -31,82 +31,118 @@ catkin init catkin build -j4 -c ``` -### How to set up a new user in the internal pc. +If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. -#### setup a catkin workspace for spot driver +``` +roscd spot_basic_behaviors +# modify config/switchbot_ros/token.yaml +git update-index --skip-worktree config/switchbot_ros/token.yaml +``` + +### How to set up spot user in the internal pc. -Create a workspace for spot driver +Spot user have 3 workspaces + +- `spot_driver_ws`: a workspace to run driver.launch. which requires python3 build version of geometry3 +- `spot_coral_ws`: a workspace to run object_detection.launch ( which includes coral_usb_ros node ) which requires python3 build version of opencv_brindge +- `spot_ws`: a workspace to run other launch ( python2 ) + +#### setting up `spot_driver_ws` + +Create a workspace for spot_driver. ```bash +sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy +sudo apt-get install ros-melodic-catkin source /opt/ros/$ROS_DISTRO/setup.bash -mkdir $HOME/spot_ws/src -p -cd $HOME/spot_ws/src +mkdir $HOME/spot_driver_ws/src -p +cd $HOME/spot_driver_ws/src wstool init . wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot.rosinstall +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 $HOME/spot_ws +cd $HOME/spot_driver_ws catkin init -catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release +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 -c ``` -After this, please modify the credential files and remove it from git tracking. +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 - -roscd spot_basic_behaviors -# modify config/switchbot_ros/token.yaml -git update-index --skip-worktree config/switchbot_ros/token.yaml ``` -#### setup a catkin workspace for coral usb +#### setting up `spot_coral_ws` -Please see [this page](https://github.com/knorth55/coral_usb_ros) for details. +First, follow [Edge TPU dependencies installation section of coral_usb_ros](https://github.com/knorth55/coral_usb_ros#edge-tpu-dependencies-installation) -First, install requirements. +Create a workspace for coral_usb. ```bash sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy -sudo apt-get install python3-opencv sudo apt-get install ros-melodic-catkin -``` - -And create a workspace for coral_usb_ros - -```bash source /opt/ros/$ROS_DISTRO/setup.bash -mkdir $HOME/coral_ws/src -p -cd $HOME/coral_ws/src +mkdir $HOME/spot_coral_ws/src -p +cd $HOME/spot_coral_ws/src wstool init . -wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git wstool update wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_coral.rosinstall wstool merge -t . coral_usb_ros/fc.rosinstall.melodic wstool update +rosdep update rosdep install -y -r --from-paths . --ignore-src -cd $HOME/coral_ws +cd $HOME/spot_coral_ws catkin init -catkin config -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 jsk_spot_startup coral_usb -j4 -c +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 -c ``` After that, please download models for coral_usb_ros. ``` source /opt/ros/$ROS_DISTRO/setup.bash -source $HOME/coral_ws/devel/setup.bash +source $HOME/spot_coral_ws/devel/setup.bash rosrun coral_usb download_models.py ``` +#### setting up `spot_ws` + +Create `spot_ws` + +```bash +source /opt/ros/$ROS_DISTRO/setup.bash +mkdir $HOME/spot_ws/src -p +cd $HOME/spot_ws/src +wstool init . +wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +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 +pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt +cd $HOME/spot_ws +catkin init +catkin build -j4 -c +``` + +If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. + +``` +roscd spot_basic_behaviors +# modify config/switchbot_ros/token.yaml +git update-index --skip-worktree config/switchbot_ros/token.yaml +``` + #### Set environmental variables Set your ROS_IP to WiFi AP address. @@ -134,37 +170,68 @@ Create udev rule for spot spinal if your pc doesn't have it. sudo sh -c 'echo "SUBSYSTEM==\"tty\", ATTRS{idVendor}==\"0403\", ATTRS{idProduct}==\"6001\", ATTRS{serial}==\"A7044PJ7\", SYMLINK+=\"spot-spinal\", GROUP=\"dialout\"" > /etc/udev/rules.d/99-spot-spinal.rules' ``` -Add your user to dialout and audio group +Create udev rule for joy. + +TODO + +Add spot user to groups ```bash -sudo groupadd dialout audio +sudo gpasswd -a dialout +sudo gpasswd -a audio +sudo gpasswd -a plugdev +sudo gpasswd -a video ``` ### 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) -After that, please run the ros driver and other basic programs with `jsk_spot_bringup.launch`. You can now control spot from ROS! +After that, login with spot user and run the driver, object_detection and other basic programs. + +#### for spot_driver ```bash -source $HOME/spot_ws/devel/setup.bash -roslaunch jsk_spot_startup jsk_spot_bringup.launch +source $HOME/spot_driver_ws/devel/setup.bash +roslaunch jsk_spot_startup driver.launch +``` + +#### for object detection + +```bash +source $HOME/spot_coral_ws/devel/setup.bash +roslaunch jsk_spot_startup object_detection.launch +``` + +#### for other programs + +``` +source $HOME/spot_ws/deve/setup.bash +roslaunch jsk_spot_startup jsk_spot_bringup.launch use_driver:=false use_object_detection:=false ``` This launch includes -- driver launch file for spot - bringup launch for additional peripheral devices (Respeaker, insta 360 air and ublox GPS module) - teleoperation launch - interaction launch with Speech-To-Text and Text-To-Speech - so on. -And you can run object detection with +### Development with remote PC -```bash -source $HOME/coral_ws/devel/setup.bash -roslaunch jsk_spot_startup object_detection_and_tracking.launch +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. + +``` +rossetip +rossetmaster spot-jsk.local +source ~/spot_ws/devel/setup.bash ``` +#### Visualization + For visualization, you can run RViz with jsk configuration. ```bash @@ -172,35 +239,23 @@ source $HOME/spot_ws/devel/setup.bash roslaunch jsk_spot_startup rviz.launch ``` +#### Teleoperation + You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. +#### rosbag record and play + For development, `rosbag_record.launch` and `rosbag_play.launch` are useful for rosbag recording and playing. ```bash source $HOME/spot_ws/devel/setup.bash # Record a rosbag file roslaunch jsk_spot_startup rosbag_record.launch rosbag:= -# Play a rosbag file +# Play a rosbag file ( don't run this with setting ros_master_uri to the robot ) roslaunch jsk_spot_startup rosbag_play.launch rosbag:= ``` -### How to control spot from a remove development pc. - -First, connect your development PC's wifi adapter to 'spot-jsk' Access point. - -For every terminals in this section, Set your ROS_IP and ROS_MASTER_URI and source spot_ws. - -``` -rossetip -rossetmaster spot-jsk.local -source ~/spot_ws/devel/setup.bash -``` - -If spot is already brough up, you can see spot information from RViz. - -``` -roslaunch jsk_spot_startup rviz.launch -``` +#### Control API If you want to control Spot, there are basically 3 options. @@ -208,7 +263,7 @@ If you want to control Spot, there are basically 3 options. - Use python client ( spot_ros_client, experimental ) - Use raw ROS interface -#### spoteus +##### spoteus spoteus is roseus client for ROS Interface of spot_ros. From 320a9a653d44b5ec2b953afe9100c3a6bace1225 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 23 Sep 2021 09:58:08 +0900 Subject: [PATCH 052/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 3517cd617c..005a930ef6 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -6,7 +6,7 @@ jsk_spot_robot - [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) -## How to run +## Installation ### How to set up catkin workspace (for a user) @@ -183,6 +183,8 @@ sudo gpasswd -a plugdev sudo gpasswd -a video ``` +## 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) From 3d4bb8dd01a68ff24a3b6def6810ce7aa3a192b6 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 23 Sep 2021 22:41:25 +0900 Subject: [PATCH 053/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 005a930ef6..0d70ad9b60 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -15,7 +15,7 @@ Create a workspace ```bash sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy sudo apt-get install ros-melodic-catkin -source /opt/ros/$ROS_DISTRO/setup.bash +source /opt/ros/melodic/setup.bash mkdir $HOME/spot_ws/src -p cd $HOME/spot_ws/src wstool init . @@ -54,7 +54,7 @@ Create a workspace for spot_driver. ```bash sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy sudo apt-get install ros-melodic-catkin -source /opt/ros/$ROS_DISTRO/setup.bash +source /opt/ros/melodic/setup.bash mkdir $HOME/spot_driver_ws/src -p cd $HOME/spot_driver_ws/src wstool init . @@ -88,7 +88,7 @@ Create a workspace for coral_usb. ```bash sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy sudo apt-get install ros-melodic-catkin -source /opt/ros/$ROS_DISTRO/setup.bash +source /opt/ros/melodic/setup.bash mkdir $HOME/spot_coral_ws/src -p cd $HOME/spot_coral_ws/src wstool init . @@ -109,7 +109,7 @@ catkin build -j4 -c After that, please download models for coral_usb_ros. ``` -source /opt/ros/$ROS_DISTRO/setup.bash +source /opt/ros/melodic/setup.bash source $HOME/spot_coral_ws/devel/setup.bash rosrun coral_usb download_models.py ``` @@ -119,7 +119,7 @@ rosrun coral_usb download_models.py Create `spot_ws` ```bash -source /opt/ros/$ROS_DISTRO/setup.bash +source /opt/ros/melodic/setup.bash mkdir $HOME/spot_ws/src -p cd $HOME/spot_ws/src wstool init . @@ -143,6 +143,16 @@ roscd spot_basic_behaviors git update-index --skip-worktree config/switchbot_ros/token.yaml ``` +#### Install scripts and systemd services + +Install scripts and systemd services + +``` +source /opt/ros/melodic/setup.bash +source ~/spot_ws/devel/setup.bash +rosrun jsk_spot_startup deploy-scripts-and-services.sh +``` + #### Set environmental variables Set your ROS_IP to WiFi AP address. @@ -162,17 +172,20 @@ export DIALOGFLOW_PROJECT_ID= export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml" >> ~/.bashrc ``` +And please add the same settings to /var/lib/robot/config.bash + #### Setting up udev file and add the user to groups -Create udev rule for spot spinal if your pc doesn't have it. +Create udev rule for insta360air, spot spinal and joys. ```bash -sudo sh -c 'echo "SUBSYSTEM==\"tty\", ATTRS{idVendor}==\"0403\", ATTRS{idProduct}==\"6001\", ATTRS{serial}==\"A7044PJ7\", SYMLINK+=\"spot-spinal\", GROUP=\"dialout\"" > /etc/udev/rules.d/99-spot-spinal.rules' +roscd jsk_spot_startup +sudo cp udev_rules/* /etc/udev/rules.d/ ``` -Create udev rule for joy. +Please modify each udev file according to your configuration. -TODO +#### Add the user to groups Add spot user to groups From f24c54b7a9c6620b28a84cc62a3c680a3bde2a6c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 10:42:54 +0900 Subject: [PATCH 054/261] [jsk_spot_robot] Update udev rules installation instruction --- jsk_spot_robot/README.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 0d70ad9b60..cf86079824 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -176,13 +176,26 @@ And please add the same settings to /var/lib/robot/config.bash #### Setting up udev file and add the user to groups -Create udev rule for insta360air, spot spinal and joys. +Create udev rule for insta360air, spot spinal. ```bash roscd jsk_spot_startup sudo cp udev_rules/* /etc/udev/rules.d/ ``` +Create udev rules for joy pads + +```bash +roscd jsk_spot_teleop +sudo cp config/udev/* /etc/udev/rules.d/ +``` + +And reload + +```bash +sudo udevadm control --reload-rules +``` + Please modify each udev file according to your configuration. #### Add the user to groups From 756c7a2551e0af9e07acce709f6d101a6b022b7c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 25 Sep 2021 00:03:38 +0900 Subject: [PATCH 055/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index cf86079824..5a8330cce6 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -167,9 +167,11 @@ Please see them for more details. ``` echo "# Credentials -export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file -export DIALOGFLOW_PROJECT_ID= -export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml" >> ~/.bashrc +export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file # for dialogflow +export DIALOGFLOW_PROJECT_ID= # for dialogflow +export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml # for pydrive +" >> ~/.bashrc + ``` And please add the same settings to /var/lib/robot/config.bash From 6f205701b756508e6ba83d81433e52c5411648ee Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 12:02:02 +0900 Subject: [PATCH 056/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 198 ++-------------- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 226 +++++++++++++++++++ 2 files changed, 241 insertions(+), 183 deletions(-) create mode 100644 jsk_spot_robot/SetupInternalPCAndSpotUser.md diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 5a8330cce6..5060764047 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -8,116 +8,12 @@ jsk_spot_robot ## Installation +To setup a new internal PC and spot user, Please see [this page](./SetupInternalPCAndSpotUser.md). + ### How to set up catkin workspace (for a user) Create a workspace -```bash -sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy -sudo apt-get install ros-melodic-catkin -source /opt/ros/melodic/setup.bash -mkdir $HOME/spot_ws/src -p -cd $HOME/spot_ws/src -wstool init . -wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot -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 -pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt -cd $HOME/spot_ws -catkin init -catkin build -j4 -c -``` - -If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. - -``` -roscd spot_basic_behaviors -# modify config/switchbot_ros/token.yaml -git update-index --skip-worktree config/switchbot_ros/token.yaml -``` - -### How to set up spot user in the internal pc. - -Spot user have 3 workspaces - -- `spot_driver_ws`: a workspace to run driver.launch. which requires python3 build version of geometry3 -- `spot_coral_ws`: a workspace to run object_detection.launch ( which includes coral_usb_ros node ) which requires python3 build version of opencv_brindge -- `spot_ws`: a workspace to run other launch ( python2 ) - -#### setting up `spot_driver_ws` - -Create a workspace for spot_driver. - -```bash -sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy -sudo apt-get install ros-melodic-catkin -source /opt/ros/melodic/setup.bash -mkdir $HOME/spot_driver_ws/src -p -cd $HOME/spot_driver_ws/src -wstool init . -wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot -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 $HOME/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 -c -``` - -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 -``` - -#### setting up `spot_coral_ws` - -First, follow [Edge TPU dependencies installation section of coral_usb_ros](https://github.com/knorth55/coral_usb_ros#edge-tpu-dependencies-installation) - -Create a workspace for coral_usb. - -```bash -sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy -sudo apt-get install ros-melodic-catkin -source /opt/ros/melodic/setup.bash -mkdir $HOME/spot_coral_ws/src -p -cd $HOME/spot_coral_ws/src -wstool init . -wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot -wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git -wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_coral.rosinstall -wstool merge -t . coral_usb_ros/fc.rosinstall.melodic -wstool update -rosdep update -rosdep install -y -r --from-paths . --ignore-src -cd $HOME/spot_coral_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 -c -``` - -After that, please download models for coral_usb_ros. - -``` -source /opt/ros/melodic/setup.bash -source $HOME/spot_coral_ws/devel/setup.bash -rosrun coral_usb download_models.py -``` - -#### setting up `spot_ws` - -Create `spot_ws` - ```bash source /opt/ros/melodic/setup.bash mkdir $HOME/spot_ws/src -p @@ -143,97 +39,37 @@ roscd spot_basic_behaviors git update-index --skip-worktree config/switchbot_ros/token.yaml ``` -#### Install scripts and systemd services - -Install scripts and systemd services - -``` -source /opt/ros/melodic/setup.bash -source ~/spot_ws/devel/setup.bash -rosrun jsk_spot_startup deploy-scripts-and-services.sh -``` - -#### Set environmental variables - -Set your ROS_IP to WiFi AP address. - -``` -echo "# ROS_IP -rossetip 10.42.0.1 # change IP address if your wifi AP address is not this." >> ~/.bashrc -``` - -Add environmental variables for [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/dialogflow_task_executive) and [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) -Please see them for more details. - -``` -echo "# Credentials -export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file # for dialogflow -export DIALOGFLOW_PROJECT_ID= # for dialogflow -export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml # for pydrive -" >> ~/.bashrc - -``` - -And please add the same settings to /var/lib/robot/config.bash - -#### Setting up udev file and add the user to groups - -Create udev rule for insta360air, spot spinal. - -```bash -roscd jsk_spot_startup -sudo cp udev_rules/* /etc/udev/rules.d/ -``` - -Create udev rules for joy pads - -```bash -roscd jsk_spot_teleop -sudo cp config/udev/* /etc/udev/rules.d/ -``` - -And reload - -```bash -sudo udevadm control --reload-rules -``` - -Please modify each udev file according to your configuration. +## How to run -#### Add the user to groups +### Bringup spot -Add spot user to groups +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. -```bash -sudo gpasswd -a dialout -sudo gpasswd -a audio -sudo gpasswd -a plugdev -sudo gpasswd -a video -``` +Basically, ros systemd services will start automatically. So you can use spot now. -## How to run +#### Teleoperation -### Bringup spot +You can control spot with DualSense controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. -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) +#### Start basic roslaunch manually -After that, login with spot user and run the driver, object_detection and other basic programs. +If you want to launch basic ROS launches manually for some reason, please login `spot` user and follow this instruction. -#### for spot_driver +##### for spot_driver ```bash source $HOME/spot_driver_ws/devel/setup.bash roslaunch jsk_spot_startup driver.launch ``` -#### for object detection +##### for object detection ```bash source $HOME/spot_coral_ws/devel/setup.bash roslaunch jsk_spot_startup object_detection.launch ``` -#### for other programs +##### for other programs ``` source $HOME/spot_ws/deve/setup.bash @@ -246,7 +82,7 @@ This launch includes - interaction launch with Speech-To-Text and Text-To-Speech - so on. -### Development with remote PC +### Development with a remote PC Please create `spot_ws` to your PC. @@ -256,7 +92,7 @@ And for every terminals in this section, Set your ROS_IP and ROS_MASTER_URI and ``` rossetip -rossetmaster spot-jsk.local +rossetmaster source ~/spot_ws/devel/setup.bash ``` @@ -269,10 +105,6 @@ source $HOME/spot_ws/devel/setup.bash roslaunch jsk_spot_startup rviz.launch ``` -#### Teleoperation - -You can control spot with DualShock3 controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. - #### rosbag record and play For development, `rosbag_record.launch` and `rosbag_play.launch` are useful for rosbag recording and playing. diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md new file mode 100644 index 0000000000..1a24feb971 --- /dev/null +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -0,0 +1,226 @@ +# 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 PC + +### Install Ubuntu and ROS + +TODO + +### Setup wifi interfaces + +TODO + + +## How to set up the spot user + +First, create a `spot` user to your PC if it does not exist. + +``` +TODO +``` + +And add `spot` user to sudo group. + +``` +sudo gpasswd -a spot sudo +``` + +systemd services of JSK Spot system use workspaces in `spot` user. +`spot` user should have 3 workspaces for this use. + +- `spot_driver_ws`: a workspace to run driver.launch. which requires python3 build version of geometry3 +- `spot_coral_ws`: a workspace to run object_detection.launch ( which includes coral_usb_ros node ) which requires python3 build version of opencv_brindge +- `spot_ws`: a workspace to run other launch ( python2 ) + +### Prerequities + +Install necessary packages for workspace building + +``` +sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy +sudo apt-get install ros-melodic-catkin +``` + + +### 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/sktometometo/jsk_robot.git --git -v develop/spot +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 -c +``` + +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 +``` + + +### setting up `spot_coral_ws` + +First, follow [Edge TPU dependencies installation section of coral_usb_ros](https://github.com/knorth55/coral_usb_ros#edge-tpu-dependencies-installation) +Then, create a workspace for coral_usb. + +```bash +source /opt/ros/melodic/setup.bash +mkdir ~/spot_coral_ws/src -p +cd ~/spot_coral_ws/src +wstool init . +wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot +wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git +wstool update +wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_coral.rosinstall +wstool merge -t . coral_usb_ros/fc.rosinstall.melodic +wstool update +rosdep update +rosdep install -y -r --from-paths . --ignore-src +cd ~/spot_coral_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 -c +``` + +After that, please download models for coral_usb_ros. + +``` +source /opt/ros/melodic/setup.bash +source ~/spot_coral_ws/devel/setup.bash +rosrun coral_usb download_models.py +``` + + +### setting up `spot_ws` + +Create `spot_ws` + +```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/sktometometo/jsk_robot.git --git -v develop/spot +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 +pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt +cd ~/spot_ws +catkin init +catkin build -j4 -c +``` + +If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. + +``` +roscd spot_basic_behaviors +# modify config/switchbot_ros/token.yaml +git update-index --skip-worktree config/switchbot_ros/token.yaml +``` + + +### Setup Google Service Clients + +In order to use `dialogflow_task_executive` and `gdrive_ros`, please prepair authentication files. + +- [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/blob/master/dialogflow_task_executive/README.md) + + Please download a service account key JSON file of your dialogflow project and put it under `/var/lib/robot/`. +- [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) + + Please download client config file, create `settings.yaml` and put them under `/var/lib/robot/`. + + +### Install scripts and systemd services + +Install scripts and systemd services + +``` +source /opt/ros/melodic/setup.bash +source ~/spot_ws/devel/setup.bash +rosrun jsk_spot_startup deploy-scripts-and-services.sh +``` + + +### Set environmental variables + +Modify `/var/lib/robot/config.bash` to set environmental variables. + +Set `IF_ETH`, `IF_WIFI`, `IF_LTE` to your network interface names to use `jsk-spot-utils-network.service` + +``` +export IF_ETH="noethernetdevice" +export IF_WIFI="nowifidevice" +export IF_LTE="noltedevice" +``` + +Set your `ROS_IP` to WiFi AP address to ip address of your wifi AP interface. + +``` +WIFI_AP_IP=10.42.0.1 +rossetmaster $WIFI_AP_IP +rossetip $WIFI_AP_IP +``` + +Add environmental variables for [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/dialogflow_task_executive) and [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) +Please see them for more details. + +``` +export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file # for dialogflow +export DIALOGFLOW_PROJECT_ID= # for dialogflow +export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml # for pydrive +``` + + +### Setting up udev files + +Create udev rule for insta360air, spot spinal. + +```bash +roscd jsk_spot_startup +sudo cp udev_rules/* /etc/udev/rules.d/ +``` + +Create udev rules for joy pads + +```bash +roscd jsk_spot_teleop +sudo cp config/udev/* /etc/udev/rules.d/ +``` + +And reload them + +```bash +sudo udevadm control --reload-rules +``` + +Please modify each udev rule according to your configuration. + + +### Add the user to groups + +Add spot user to groups + +```bash +sudo gpasswd -a dialout +sudo gpasswd -a audio +sudo gpasswd -a plugdev +sudo gpasswd -a video +``` From 0daa458dc5f6d83ef884239bb7c0828919016a02 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 12:16:55 +0900 Subject: [PATCH 057/261] [jsk_spot_robot] update documentation --- jsk_spot_robot/README.md | 28 +++++++++++++++----- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 17 ++++++++++++ 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 5060764047..66dc6e89d0 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -16,8 +16,8 @@ Create a workspace ```bash source /opt/ros/melodic/setup.bash -mkdir $HOME/spot_ws/src -p -cd $HOME/spot_ws/src +mkdir ~/spot_ws/src -p +cd ~/spot_ws/src wstool init . wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot wstool update @@ -25,7 +25,6 @@ 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 -pip3 install -r jsk-ros-pkg/jsk_robot/jsk_spot_robot/requirements.txt cd $HOME/spot_ws catkin init catkin build -j4 -c @@ -47,10 +46,6 @@ First, please turn on spot and turn on motors according to [the OPERATION sectio Basically, ros systemd services will start automatically. So you can use spot now. -#### Teleoperation - -You can control spot with DualSense controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. - #### Start basic roslaunch manually If you want to launch basic ROS launches manually for some reason, please login `spot` user and follow this instruction. @@ -82,6 +77,25 @@ This launch includes - interaction launch with Speech-To-Text and Text-To-Speech - so on. +### Teleoperation + +You can control spot with DualSense controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. + + +### Web UI + +Spot have some web UI. + +#### cockpit + +URI: https://:21443 + +TODO + +#### rwt_app_chooser + +TODO + ### Development with a remote PC Please create `spot_ws` to your PC. diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 1a24feb971..188ba08200 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -224,3 +224,20 @@ sudo gpasswd -a audio sudo gpasswd -a plugdev sudo gpasswd -a video ``` + +#### Setup cockpit + +Cockpit is server management tool. If you use Spot CORE cockpit is already setup. + +``` +sudo apt install cockpit +``` + +``` +roscd jsk_spot_startup +sudo cp -r config/cockpit.socket.d /etc/systemd/system/cockpit.socket.d +sudo systemctl daemon-reload +sudo systemctl start cockpit.socket +``` + +Then you can access cockpit by `https://:21443` From edeac18dae173fd530988f1c6944d2b5c0489d66 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 27 Sep 2021 16:05:06 +0900 Subject: [PATCH 058/261] [jsk_spot_robot] add postfix setup --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 188ba08200..41477ce98c 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -241,3 +241,7 @@ sudo systemctl start cockpit.socket ``` Then you can access cockpit by `https://:21443` + +#### Setup postfix with gmail + +TODO From a8756c5e78d9fd6f5864f15674b569d6c57c5cf8 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 10:48:16 +0900 Subject: [PATCH 059/261] [spoteus][spoteus_demo] add spoteus_demo package and move demo scripts from spoteus --- .../euslisp => spoteus/demo}/sample_basics.l | 0 .../demo}/sample_navigate_to.l | 0 .../demo}/sample_visualization.l | 0 jsk_spot_robot/spoteus_demo/CMakeLists.txt | 9 ------- jsk_spot_robot/spoteus_demo/package.xml | 24 ------------------- 5 files changed, 33 deletions(-) rename jsk_spot_robot/{spoteus_demo/euslisp => spoteus/demo}/sample_basics.l (100%) rename jsk_spot_robot/{spoteus_demo/euslisp => spoteus/demo}/sample_navigate_to.l (100%) rename jsk_spot_robot/{spoteus_demo/euslisp => spoteus/demo}/sample_visualization.l (100%) delete mode 100644 jsk_spot_robot/spoteus_demo/CMakeLists.txt delete mode 100644 jsk_spot_robot/spoteus_demo/package.xml diff --git a/jsk_spot_robot/spoteus_demo/euslisp/sample_basics.l b/jsk_spot_robot/spoteus/demo/sample_basics.l similarity index 100% rename from jsk_spot_robot/spoteus_demo/euslisp/sample_basics.l rename to jsk_spot_robot/spoteus/demo/sample_basics.l diff --git a/jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l b/jsk_spot_robot/spoteus/demo/sample_navigate_to.l similarity index 100% rename from jsk_spot_robot/spoteus_demo/euslisp/sample_navigate_to.l rename to jsk_spot_robot/spoteus/demo/sample_navigate_to.l diff --git a/jsk_spot_robot/spoteus_demo/euslisp/sample_visualization.l b/jsk_spot_robot/spoteus/demo/sample_visualization.l similarity index 100% rename from jsk_spot_robot/spoteus_demo/euslisp/sample_visualization.l rename to jsk_spot_robot/spoteus/demo/sample_visualization.l diff --git a/jsk_spot_robot/spoteus_demo/CMakeLists.txt b/jsk_spot_robot/spoteus_demo/CMakeLists.txt deleted file mode 100644 index 48d5a77308..0000000000 --- a/jsk_spot_robot/spoteus_demo/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(spoteus_demo) - -find_package(catkin REQUIRED COMPONENTS - roseus - spoteus -) - -catkin_package() diff --git a/jsk_spot_robot/spoteus_demo/package.xml b/jsk_spot_robot/spoteus_demo/package.xml deleted file mode 100644 index 985130de82..0000000000 --- a/jsk_spot_robot/spoteus_demo/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - spoteus_demo - 1.1.0 - The spoteus_demo package - - Kei Okada - Yoshiki Obinata - Koki Shinjo - BSD - Kei Okada - Yoshiki Obinata - Koki Shinjo - - catkin - - spot_autowalk_data - roseus - spoteus - - - - - From dfc482bd0b573d8c763eb5909fbaf2e77dc64687 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 10:48:58 +0900 Subject: [PATCH 060/261] [jsk_spot_robot] update README.md --- jsk_spot_robot/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 66dc6e89d0..06e4775270 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -159,7 +159,7 @@ for example, when you want to move spot 1m forward, type. send *ri* :go-pose 1 0 ``` -And there are some examples in spoteus_demo. +And there are some examples in `spoteus/demo`. - `sample_basics.l` + example of basic usage of Spot From 7c97c2399ab9b4bfcaffd140ed84517c81744466 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 2 Oct 2021 11:34:12 +0900 Subject: [PATCH 061/261] [jsk_spot_robot] update jsk_recognition branch --- jsk_spot_robot/jsk_spot.rosinstall | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index bb3bc7866e..e70a32ce35 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,9 +1,7 @@ -# Until PRs below are merged, we need to use develop/spot branch in sktometometo/jsk_recognition.git -# - https://github.com/jsk-ros-pkg/jsk_recognition/pull/2596 - git: local-name: jsk-ros-pkg/jsk_recognition uri: https://github.com/sktometometo/jsk_recognition.git - version: develop/spot + version: PR/add-frame-id-arg-to-camera-launch # spot-jsk will use packages below. # - dialogflow-executive # - webrtcvad_ros @@ -12,10 +10,6 @@ # - ros_speech_recognition # - aques_talk # Until PRs below are merged, we need to use develop/spot branch -# - webrtcvad_ros -# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/282 -# - switchbot_ros update -# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/278 # - dialogflow_ros update # + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/242 - git: From d7fee5260dee20b2c97d847f064f3ae3dc7f1e50 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 9 Nov 2021 16:28:22 +0900 Subject: [PATCH 062/261] [jsk_spot_robot] remove audio_video_recorder from jsk_spot_user.rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 4 ---- 1 file changed, 4 deletions(-) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index e70a32ce35..beeb604191 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -56,10 +56,6 @@ local-name: knorth55/app_manager_utils uri: https://github.com/knorth55/app_manager_utils.git version: master -- git: - local-name: knorth55/audio_video_recorder - uri: https://github.com/knorth55/audio_video_recorder.git - version: main # fetch15 version of roswww is necessary for basic authentication - git: local-name: tork-a/roswww From 1fd6ddc9b067d1e27710be0f67b02214a55a8321 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 10 Nov 2021 23:47:40 +0900 Subject: [PATCH 063/261] [spoteus] Add body inverse kinematics to control spot pose in euslisp --- jsk_spot_robot/spoteus/spot-utils.l | 45 ++++++++++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l index 3dfffefba3..a1f3fda4be 100644 --- a/jsk_spot_robot/spoteus/spot-utils.l +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -48,5 +48,48 @@ (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) + &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 + args) + (send body_lk :dissoc body-coords)))) ) - From 348009d2d02fce1b2b3006f9e66921ff2c3a448a Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Fri, 12 Nov 2021 20:12:08 +0900 Subject: [PATCH 064/261] [spoteus] support body pose z by sending coords --- jsk_spot_robot/spoteus/spot-interface.l | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 9e77e02b8d..dfe9a2b975 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -278,16 +278,14 @@ (send self :go-pos x y d :timeout timeout :wait nil) ) (:body-pose - (r p y + (coords &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)) - (let ((pose-msg (instance geometry_msgs::Pose :init)) - (quaternion-msg (send self :create-quaternion-msg-from-rpy r p y))) - (send pose-msg :orientation quaternion-msg) + (let ((pose-msg (ros::coords->tf-pose coords))) (ros::publish topic-name pose-msg)) (send self :stand)) (:find-waypoint-position-from-id From 354d8b69945d36199f26de355bdb80697b405324 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Mon, 15 Nov 2021 13:25:11 +0900 Subject: [PATCH 065/261] [spoteus] support both r p y and coords, still need test --- jsk_spot_robot/spoteus/spot-interface.l | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index dfe9a2b975..00387a7243 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -278,15 +278,23 @@ (send self :go-pos x y d :timeout timeout :wait nil) ) (:body-pose - (coords - &key (topic-name "/spot/body_pose")) + (&optional (r p y coords) + &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)) - (let ((pose-msg (ros::coords->tf-pose coords))) - (ros::publish topic-name pose-msg)) + (if (boundp 'coords) + (progn + (ros::ros-info "Got coords variable as optional args. r p y args would be ignored.") + (let ((pose-msg (ros::coords->tf-pose coords))) + (ros::publish topic-name pose-msg))) + (progn + (let ((pose-msg (instance geometry_msgs::Pose :init)) + (quaternion-msg (send self :create-quaternion-msg-from-rpy r p y))) + (send pose-msg :orientation quaternion-msg) + (ros::publish topic-name pose-msg)))) (send self :stand)) (:find-waypoint-position-from-id (id ids) From 81c18ed203213d7a281c1edb09450d838b9ab041 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 16 Nov 2021 14:20:32 +0900 Subject: [PATCH 066/261] [spoteus] fix bugs and support both vec and coords --- jsk_spot_robot/spoteus/spot-interface.l | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 00387a7243..9c38e2bde5 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -278,22 +278,23 @@ (send self :go-pos x y d :timeout timeout :wait nil) ) (:body-pose - (&optional (r p y coords) - &key (topic-name "/spot/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 (boundp 'coords) - (progn - (ros::ros-info "Got coords variable as optional args. r p y args would be ignored.") - (let ((pose-msg (ros::coords->tf-pose coords))) - (ros::publish topic-name pose-msg))) + (if (vectorp pose) (progn + (ros::ros-info "Got r p y float-vector as args.") (let ((pose-msg (instance geometry_msgs::Pose :init)) - (quaternion-msg (send self :create-quaternion-msg-from-rpy r p y))) + (quaternion-msg (send self :create-quaternion-msg-from-rpy (aref pose 0) (aref pose 1) (aref pose 2)))) (send pose-msg :orientation quaternion-msg) + (ros::publish topic-name pose-msg))) + (progn + (ros::ros-info "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 From c53c47e35c466c3562a8371f3977fe17c3afe389 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 16 Nov 2021 14:42:40 +0900 Subject: [PATCH 067/261] [spoteus] fix log levec --- jsk_spot_robot/spoteus/spot-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 9c38e2bde5..6737f08fe7 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -287,13 +287,13 @@ (unix:sleep 1)) (if (vectorp pose) (progn - (ros::ros-info "Got r p y float-vector as args.") + (ros::ros-debug "Got r p y float-vector as args.") (let ((pose-msg (instance geometry_msgs::Pose :init)) (quaternion-msg (send self :create-quaternion-msg-from-rpy (aref pose 0) (aref pose 1) (aref pose 2)))) (send pose-msg :orientation quaternion-msg) (ros::publish topic-name pose-msg))) (progn - (ros::ros-info "Got coords variable as args.") + (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)) From bda3852dec1e47620db77dcbea1d4bd070b81d4b Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 16 Nov 2021 15:03:10 +0900 Subject: [PATCH 068/261] [spoteus] support both vector and list --- jsk_spot_robot/spoteus/spot-interface.l | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 6737f08fe7..5c1a36ce2a 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -285,11 +285,11 @@ (unless (ros::get-topic-publisher topic-name) (ros::advertise topic-name geometry_msgs::Pose 1) (unix:sleep 1)) - (if (vectorp pose) + (if (or (vectorp pose) (listp pose)) (progn - (ros::ros-debug "Got r p y float-vector as args.") + (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 (aref pose 0) (aref pose 1) (aref pose 2)))) + (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 From bed052dff7c329a6f2c5ea8515cf15a285eb1a9e Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 16 Nov 2021 16:13:34 +0900 Subject: [PATCH 069/261] [spoteus] support new body pose in sample --- jsk_spot_robot/spoteus/demo/sample_basics.l | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/jsk_spot_robot/spoteus/demo/sample_basics.l b/jsk_spot_robot/spoteus/demo/sample_basics.l index 9f726e7680..fcd8910854 100755 --- a/jsk_spot_robot/spoteus/demo/sample_basics.l +++ b/jsk_spot_robot/spoteus/demo/sample_basics.l @@ -55,19 +55,19 @@ ;; body-pose (format t "Testing body-pose~%") -(send *ri* :body-pose 0 0.2 0) +(send *ri* :body-pose '(0 0.2 0)) (ros::duration-sleep 1) -(send *ri* :body-pose 0 -0.2 0) +(send *ri* :body-pose '(0 -0.2 0)) (ros::duration-sleep 1) -(send *ri* :body-pose 0 0 0.2) +(send *ri* :body-pose '(0 0 0.2)) (ros::duration-sleep 1) -(send *ri* :body-pose 0 0 -0.2) +(send *ri* :body-pose '(0 0 -0.2)) (ros::duration-sleep 1) -(send *ri* :body-pose 0.1 0 0) +(send *ri* :body-pose '(0.1 0 0)) (ros::duration-sleep 1) -(send *ri* :body-pose -0.1 0 0) +(send *ri* :body-pose '(-0.1 0 0)) (ros::duration-sleep 1) -(send *ri* :body-pose 0 0 0) +(send *ri* :body-pose '(0 0 0)) (ros::duration-sleep 1) ;; go pos From 48e2a63d1a057d3700dc33c40986b28ab32eda39 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 23 Nov 2021 11:58:37 +0900 Subject: [PATCH 070/261] [spoteus] Restrict XY movement in spot :fullbody-inverse-kinematics --- jsk_spot_robot/spoteus/spot-utils.l | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l index a1f3fda4be..f35efae143 100644 --- a/jsk_spot_robot/spoteus/spot-utils.l +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -56,6 +56,7 @@ (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: @@ -90,6 +91,7 @@ Example: :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)))) ) From 59221efff8f4b21af9d8d88c9e1ba49be54e118e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 12:13:17 +0900 Subject: [PATCH 071/261] [jsk_spot_robot] add gdrive_ros to rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index beeb604191..ba05f726b3 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -12,6 +12,8 @@ # Until PRs below are merged, we need to use develop/spot branch # - dialogflow_ros update # + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/242 +# - gdrive_ros update +# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/287 - git: local-name: jsk-ros-pkg/jsk_3rdparty uri: https://github.com/sktometometo/jsk_3rdparty.git From 9c05f93f911cf86f72765b1820fb5eede52fce56 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 12:38:26 +0900 Subject: [PATCH 072/261] [jsk_spot_robot] move jsk_spot_user.rosinstall to jsk_spot_dev and add new one --- jsk_spot_robot/README.md | 10 +-- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 2 +- jsk_spot_robot/jsk_spot.rosinstall | 67 +------------------- 3 files changed, 4 insertions(+), 75 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 06e4775270..3ade42a481 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -10,7 +10,7 @@ jsk_spot_robot To setup a new internal PC and spot user, Please see [this page](./SetupInternalPCAndSpotUser.md). -### How to set up catkin workspace (for a user) +### How to set up a catkin workspace for a remove PC Create a workspace @@ -30,14 +30,6 @@ catkin init catkin build -j4 -c ``` -If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. - -``` -roscd spot_basic_behaviors -# modify config/switchbot_ros/token.yaml -git update-index --skip-worktree config/switchbot_ros/token.yaml -``` - ## How to run ### Bringup spot diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 41477ce98c..83459c462f 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -119,7 +119,7 @@ cd ~/spot_ws/src wstool init . wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_user.rosinstall +wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_dev.rosinstall wstool update rosdep update rosdep install -y -r --from-paths . --ignore-src diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index ba05f726b3..8e86614f5d 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -1,70 +1,7 @@ -- git: - local-name: jsk-ros-pkg/jsk_recognition - uri: https://github.com/sktometometo/jsk_recognition.git - version: PR/add-frame-id-arg-to-camera-launch -# spot-jsk will use packages below. -# - dialogflow-executive -# - webrtcvad_ros -# - switchbot_ros -# - respeaker_ros -# - ros_speech_recognition -# - aques_talk -# Until PRs below are merged, we need to use develop/spot branch -# - dialogflow_ros update -# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/242 -# - gdrive_ros update -# + https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/287 -- git: - local-name: jsk-ros-pkg/jsk_3rdparty - uri: https://github.com/sktometometo/jsk_3rdparty.git - version: develop/spot -# Until PR below is merged, we need to use develop/spot branch -# - https://github.com/jsk-ros-pkg/jsk_common/pull/1698 -- git: - local-name: jsk-ros-pkg/jsk_common - uri: https://github.com/sktometometo/jsk_common.git - version: develop/spot -# Until PRs below are merged, we need to use develop/spot branch -# - person following demo -# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1343 -# - mech office demo -# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1341 -# - sing and dance demo -# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1340 -# - person leading demo -# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1339 -# - watchdog demo -# + https://github.com/jsk-ros-pkg/jsk_demos/pull/1332 -# + patches -# - https://github.com/tongtybj/jsk_demos/pull/2 -# - https://github.com/tongtybj/jsk_demos/pull/1 -- git: - local-name: jsk-ros-pkg/jsk_demos - uri: https://github.com/sktometometo/jsk_demos.git - version: develop/spot -# spot-ros is required for spot +# spot-ros is required for spot_msgs # 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-rebased -# app_manager and utilities are requied -- git: - local-name: PR2/app_manager - uri: https://github.com/PR2/app_manager.git - version: kinetic-devel -- git: - local-name: knorth55/app_manager_utils - uri: https://github.com/knorth55/app_manager_utils.git - version: master -# fetch15 version of roswww is necessary for basic authentication -- git: - local-name: tork-a/roswww - uri: https://github.com/knorth55/roswww.git - version: fetch15 -# visualization_rwt is not released for melodic -- git: - local-name: tork-a/visualization_rwt - uri: https://github.com/tork-a/visualization_rwt.git - version: kinetic-devel + version: develop/spot From e2465a726b544414faca24e4a9494a0638c6bd88 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 14:58:15 +0900 Subject: [PATCH 073/261] [spoteus] update package.xml for dependencies --- jsk_spot_robot/spoteus/package.xml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/package.xml b/jsk_spot_robot/spoteus/package.xml index 48f12fda60..258253ca59 100644 --- a/jsk_spot_robot/spoteus/package.xml +++ b/jsk_spot_robot/spoteus/package.xml @@ -2,7 +2,7 @@ spoteus 1.1.0 - The spots package + The spoteus package Kei Okada Yoshiki Obinata @@ -19,8 +19,10 @@ pr2eus spot_description - roseus pr2eus + roseus + spot_msgs + std_srvs From 37a57b4a55d97b87e12a399e39632c12333e5eb1 Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Thu, 25 Nov 2021 15:09:40 +0900 Subject: [PATCH 074/261] [spoteus] add method for dock and undock --- jsk_spot_robot/spoteus/spot-interface.l | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 5c1a36ce2a..4e35b294a0 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -31,6 +31,14 @@ (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) + (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) @@ -218,6 +226,13 @@ (: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) From 6a6177b911a8f303130481a7d11c6f9abf50f2a4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 1 Nov 2020 20:40:05 +0900 Subject: [PATCH 075/261] [jsk_spot_startup] add jsk_spot_startup package --- .../jsk_spot_startup/CMakeLists.txt | 10 + .../jsk_spot_startup/config/spot.rviz | 518 ++++++++++++++++++ .../jsk_spot_startup/config/twist_mux.yaml | 14 + .../jsk_spot_startup/launch/driver.launch | 23 + .../jsk_spot_startup/launch/rviz.launch | 3 + jsk_spot_robot/jsk_spot_startup/package.xml | 19 + 6 files changed, 587 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_startup/config/spot.rviz create mode 100644 jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/driver.launch create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/rviz.launch create mode 100644 jsk_spot_robot/jsk_spot_startup/package.xml 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..fd54e64544 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.0.2) +project(jsk_spot_startup) + +find_package(catkin REQUIRED) + +catkin_package( +) + +include_directories( +) 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..fb163aef43 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -0,0 +1,518 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 853 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: FrontLeftDepthCloud +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - 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 + 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_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 + 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_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: true + Frame Timeout: 15 + Frames: + All Enabled: true + back: + Value: true + back_fisheye: + Value: true + body: + Value: true + front_left_hip: + Value: true + front_left_lower_leg: + Value: true + front_left_upper_leg: + Value: true + front_right_hip: + Value: true + front_right_lower_leg: + Value: true + front_right_upper_leg: + Value: true + frontleft: + Value: true + frontleft_fisheye: + Value: true + frontright: + Value: true + frontright_fisheye: + Value: true + gpe: + Value: true + head: + Value: true + left: + Value: true + left_fisheye: + Value: true + odom: + Value: true + rear_left_hip: + Value: true + rear_left_lower_leg: + Value: true + rear_left_upper_leg: + Value: true + rear_right_hip: + Value: true + rear_right_lower_leg: + Value: true + rear_right_upper_leg: + Value: true + right: + Value: true + right_fisheye: + Value: true + vision: + Value: true + Marker Scale: 0.25 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + body: + front_left_hip: + front_left_upper_leg: + front_left_lower_leg: + {} + front_right_hip: + front_right_upper_leg: + front_right_lower_leg: + {} + head: + back: + back_fisheye: + {} + frontleft: + frontleft_fisheye: + {} + frontright: + frontright_fisheye: + {} + left: + left_fisheye: + {} + right: + right_fisheye: + {} + odom: + gpe: + {} + rear_left_hip: + rear_left_upper_leg: + rear_left_lower_leg: + {} + rear_right_hip: + rear_right_upper_leg: + rear_right_lower_leg: + {} + vision: + {} + Update Interval: 0 + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.45363283 + Min Value: -0.155580759 + 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: 2.612113 + Min Value: -0.112739921 + 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.41352618 + Min Value: -0.128979474 + 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: 1.45274436 + Min Value: -0.124786079 + 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: 0.655641437 + Min Value: -0.0852906406 + 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: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /twist_marker_server/update + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: 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 + Topic: /initialpose + - 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: 4.59715652 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.143477932 + Y: 0.615365922 + Z: 0.880245864 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.530398846 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.32234859 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1855 + X: 65 + Y: 24 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..12d99e7374 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml @@ -0,0 +1,14 @@ +topics: +- name : bt_joy + topic : bluetooth_teleop/cmd_vel + timeout : 0.5 + priority: 9 +- name : interactive_marker + topic : twist_marker_server/cmd_vel + timeout : 0.5 + priority: 8 +- name : head_joy + topic : joy_head/cmd_vel + timeout : 0.5 + priority: 7 +locks: [] diff --git a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch new file mode 100644 index 0000000000..8a4c2cb3ed --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + 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..d9facb1ed3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -0,0 +1,3 @@ + + + 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..ae5ef2b373 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -0,0 +1,19 @@ + + + jsk_spot_startup + 0.0.0 + The jsk_spot_startup package + + Koki Shinjo + + BSD + + catkin + + spot_driver + spot_description + interactive_marker_twist_server + + + + From 5d8c065516589aff9aa8a14976ea1ffeddc4e369 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 3 Nov 2020 01:19:52 +0900 Subject: [PATCH 076/261] [jsk_spot_startup] fix the filepath of a rviz config in rviz.launch --- jsk_spot_robot/jsk_spot_startup/launch/rviz.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch index d9facb1ed3..c4943b7b9b 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -1,3 +1,3 @@ - + From c44a4101604dfdb1ae63807d11960678c75240d9 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 3 Nov 2020 01:22:12 +0900 Subject: [PATCH 077/261] [jsk_spot_startup] update the package.xml to fix the version, a maintainer tag, the author tag, and dependencies --- jsk_spot_robot/jsk_spot_startup/package.xml | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index ae5ef2b373..fcd51c7ab9 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -1,18 +1,25 @@ jsk_spot_startup - 0.0.0 + 1.1.0 The jsk_spot_startup package - Koki Shinjo - + Kei Okada + Koki Shinjo BSD catkin - spot_driver - spot_description + roslaunch + roslint + interactive_marker_twist_server + roslaunch + rviz + spot_description + spot_driver + + rostest From a73d4115f949950c29a0e18a04f50651579966f8 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 3 Nov 2020 01:22:51 +0900 Subject: [PATCH 078/261] [jsk_spot_startup] add Install section, Testing Section to CMakeLists.txt --- .../jsk_spot_startup/CMakeLists.txt | 31 +++++++++++++++++-- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index fd54e64544..1d8558f3b5 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -3,8 +3,33 @@ project(jsk_spot_startup) find_package(catkin REQUIRED) -catkin_package( -) -include_directories( +################################### +## catkin specific configuration ## +################################### +catkin_package() + + +############# +## Install ## +############# +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS ) + + +############# +## 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() From eb8d2de344025b6ca7f5c01b150e52c716ab8821 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 18 Nov 2020 23:26:07 +0900 Subject: [PATCH 079/261] [jsk_spot_startup] add play.launch and record.launch --- jsk_spot_robot/jsk_spot_startup/launch/play.launch | 8 ++++++++ jsk_spot_robot/jsk_spot_startup/launch/record.launch | 0 2 files changed, 8 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/play.launch create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/record.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/play.launch b/jsk_spot_robot/jsk_spot_startup/launch/play.launch new file mode 100644 index 0000000000..42b16ba3ce --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/play.launch @@ -0,0 +1,8 @@ + + + + + + diff --git a/jsk_spot_robot/jsk_spot_startup/launch/record.launch b/jsk_spot_robot/jsk_spot_startup/launch/record.launch new file mode 100644 index 0000000000..e69de29bb2 From fe450915b6961074060824d111bf343fe53bd1f3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 11 Feb 2021 08:18:08 +0900 Subject: [PATCH 080/261] [jsk_spot_startup] update record.launch --- .../jsk_spot_startup/launch/record.launch | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/record.launch b/jsk_spot_robot/jsk_spot_startup/launch/record.launch index e69de29bb2..9ede0290a0 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/record.launch @@ -0,0 +1,52 @@ + + + + + From 3c2ba4d3a2148f3822ca663f3812a23a264a1dd1 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Feb 2021 23:04:58 +0900 Subject: [PATCH 081/261] [jsk_spot_startup] add peripheral.launch to bringup peripheral devices --- jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch b/jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch new file mode 100644 index 0000000000..6446752c6b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch @@ -0,0 +1,2 @@ + + From 8e5821d3ae6e37f9b5d0dfee3fa23e60b3a47fab Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Feb 2021 23:08:33 +0900 Subject: [PATCH 082/261] [jsk_spot_startup] update driver.launch to add required property to spot_ros.py --- jsk_spot_robot/jsk_spot_startup/launch/driver.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch index 8a4c2cb3ed..2a3a6516ef 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch @@ -6,7 +6,7 @@ - + From e59a5edadc0a04159e562dca911fe1fe8b8a4c27 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 16 Feb 2021 23:10:20 +0900 Subject: [PATCH 083/261] [jsk_spot_startup] add panoramma image topic to record.launch and add use_compress arg --- .../jsk_spot_startup/launch/record.launch | 135 +++++++++++++++++- 1 file changed, 134 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/record.launch b/jsk_spot_robot/jsk_spot_startup/launch/record.launch index 9ede0290a0..b2ed325def 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/record.launch @@ -1,13 +1,146 @@ + - + + + + + + + + + + + + + + + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + 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..3e11531bb3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/static_tf_republisher.py @@ -0,0 +1,39 @@ +#!/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() From d8c7dabadc1fe8cf1002e333b6a4ae3c0a79d22a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 17 Feb 2021 18:05:39 +0900 Subject: [PATCH 087/261] [jsk_spot_startup] update rviz config to visualize battery percentage --- .../jsk_spot_startup/config/spot.rviz | 48 +++++++++++++------ .../jsk_spot_startup/launch/rviz.launch | 13 ++++- .../node_scripts/battery_visualizer.py | 28 +++++++++++ 3 files changed, 74 insertions(+), 15 deletions(-) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index cee805ec9a..57dd0dbbc1 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.6832844614982605 - Tree Height: 806 + Tree Height: 833 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -273,8 +273,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -5.317804336547852 - Min Value: -6.9601287841796875 + Max Value: -1.434785008430481 + Min Value: -3.1749267578125 Value: true Axis: Z Channel Name: intensity @@ -311,8 +311,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -5.340297698974609 - Min Value: -7.058243274688721 + Max Value: 1.4876224994659424 + Min Value: -3.2850937843322754 Value: true Axis: Z Channel Name: intensity @@ -349,8 +349,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -4.671159744262695 - Min Value: -7.215898513793945 + Max Value: -0.7464075088500977 + Min Value: -3.199531078338623 Value: true Axis: Z Channel Name: intensity @@ -387,8 +387,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.353130578994751 - Min Value: -6.98671293258667 + Max Value: 0.1477804183959961 + Min Value: -3.1852667331695557 Value: true Axis: Z Channel Name: intensity @@ -425,8 +425,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -5.087186336517334 - Min Value: -7.822200298309326 + Max Value: -0.31662750244140625 + Min Value: -3.2191684246063232 Value: true Axis: Z Channel Name: intensity @@ -489,6 +489,26 @@ Visualization Manager: duration: 1000 frame: base_link line_width: 0.009999999776482582 + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: BatteryPercentage + 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: 14 + top: 50 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -527,7 +547,7 @@ Visualization Manager: X: 0 Y: 0 Z: 0 - Focal Shape Fixed Size: true + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View @@ -535,7 +555,7 @@ Visualization Manager: Pitch: 1 Target Frame: base_link Value: OrbitOriented (rtabmap_ros) - Yaw: 3.1399998664855957 + Yaw: 3.140000104904175 Saved: ~ Window Geometry: Displays: @@ -543,7 +563,7 @@ Window Geometry: Height: 1052 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015700000363fc0200000011fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002300000018e0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d006100670065000000013f000000660000000000000000fb0000000a0049006d0061006700650000000164000000940000000000000000fb0000000a0049006d00610067006500000001a3000000e80000000000000000fc00000227000001970000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000002ad0000000000000000000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch index c4943b7b9b..6d0cd8bc93 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -1,3 +1,14 @@ - + + + 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..673e45fd9e --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py @@ -0,0 +1,28 @@ +#!/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() From 0b93284f182572891c44aace86abc914c9ea5ddf Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 17 Feb 2021 18:06:06 +0900 Subject: [PATCH 088/261] [jsk_spot_startup] update play.launch --- jsk_spot_robot/jsk_spot_startup/launch/play.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/play.launch b/jsk_spot_robot/jsk_spot_startup/launch/play.launch index 92d0026631..b67cf16cdb 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/play.launch @@ -25,6 +25,7 @@ type="play" name="$(anon rosbag)" args="$(arg rosbag) --clock" + required="true" /> Date: Thu, 18 Feb 2021 15:13:01 +0900 Subject: [PATCH 089/261] [jsk_spot_startup] add jsk_spot_bringup.launch --- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch 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..343c5a2e7c --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -0,0 +1,11 @@ + + + + + + + From bd601f85aade29b608196f7349bd276cbbf8c0a3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 18 Feb 2021 18:34:11 +0900 Subject: [PATCH 090/261] [jsk_spot_startup] update rviz config --- .../jsk_spot_startup/config/spot.rviz | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index 57dd0dbbc1..0cd1193a02 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.6832844614982605 - Tree Height: 833 + Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -273,8 +273,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -1.434785008430481 - Min Value: -3.1749267578125 + Max Value: -0.5423845052719116 + Min Value: -11.18303394317627 Value: true Axis: Z Channel Name: intensity @@ -311,8 +311,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.4876224994659424 - Min Value: -3.2850937843322754 + Max Value: 2.0937347412109375 + Min Value: -10.284269332885742 Value: true Axis: Z Channel Name: intensity @@ -349,8 +349,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.7464075088500977 - Min Value: -3.199531078338623 + Max Value: -0.01675701141357422 + Min Value: -5.776913642883301 Value: true Axis: Z Channel Name: intensity @@ -387,8 +387,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.1477804183959961 - Min Value: -3.1852667331695557 + Max Value: 0.5832391381263733 + Min Value: -3.825223207473755 Value: true Axis: Z Channel Name: intensity @@ -425,8 +425,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.31662750244140625 - Min Value: -3.2191684246063232 + Max Value: 0.8464637994766235 + Min Value: -8.379054069519043 Value: true Axis: Z Channel Name: intensity @@ -536,7 +536,7 @@ Visualization Manager: Value: true Views: Current: - Class: rtabmap_ros/OrbitOriented + Class: rviz/Orbit Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -547,23 +547,23 @@ Visualization Manager: X: 0 Y: 0 Z: 0 - Focal Shape Fixed Size: false + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1 Target Frame: base_link - Value: OrbitOriented (rtabmap_ros) - Yaw: 3.140000104904175 + Value: Orbit (rviz) + Yaw: 0 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1052 + Height: 1025 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -572,6 +572,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 1920 - Y: 0 + Width: 1853 + X: 67 + Y: 27 From b7d46ac2c364429c9a520ca0c111156960e059b7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 25 Feb 2021 21:43:34 +0900 Subject: [PATCH 091/261] [jsk_spot_startup] add resize node for panorama image --- jsk_spot_robot/jsk_spot_startup/launch/play.launch | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/play.launch b/jsk_spot_robot/jsk_spot_startup/launch/play.launch index b67cf16cdb..0280bd2c8c 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/play.launch @@ -34,6 +34,18 @@ args="$(arg rosbag)" /> + + + + + + + + Date: Thu, 25 Feb 2021 22:12:03 +0900 Subject: [PATCH 092/261] [jsk_spot_startup] update rviz.launch and add rviz config --- .../config/spot_with_mot.rviz | 606 ++++++++++++++++++ .../jsk_spot_startup/launch/rviz.launch | 4 +- 2 files changed, 609 insertions(+), 1 deletion(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz new file mode 100644 index 0000000000..e326b222d0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz @@ -0,0 +1,606 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /TrackerVisualization1 + Splitter Ratio: 0.6832844614982605 + Tree Height: 833 + - 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: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - 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 + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + jsk_head_mount_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: true + Frame Timeout: 15 + Frames: + All Enabled: true + back: + Value: true + back_fisheye: + Value: true + base_link: + Value: true + body: + Value: true + camera: + Value: true + flat_body: + Value: true + front_left_hip: + Value: true + front_left_lower_leg: + Value: true + front_left_upper_leg: + Value: true + front_rail: + Value: true + front_right_hip: + Value: true + front_right_lower_leg: + Value: true + front_right_upper_leg: + Value: true + frontleft: + Value: true + frontleft_fisheye: + Value: true + frontright: + Value: true + frontright_fisheye: + Value: true + gpe: + Value: true + head: + Value: true + jsk_head_mount_frame: + Value: true + left: + Value: true + left_fisheye: + Value: true + odom: + Value: true + rear_left_hip: + Value: true + rear_left_lower_leg: + Value: true + rear_left_upper_leg: + Value: true + rear_rail: + Value: true + rear_right_hip: + Value: true + rear_right_lower_leg: + Value: true + rear_right_upper_leg: + Value: true + right: + Value: true + right_fisheye: + Value: true + vision: + Value: true + Marker Scale: 0.25 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + body: + base_link: + {} + flat_body: + {} + front_left_hip: + front_left_upper_leg: + front_left_lower_leg: + {} + front_rail: + {} + front_right_hip: + front_right_upper_leg: + front_right_lower_leg: + {} + head: + back: + back_fisheye: + {} + frontleft: + frontleft_fisheye: + {} + frontright: + frontright_fisheye: + {} + left: + left_fisheye: + {} + right: + right_fisheye: + {} + jsk_head_mount_frame: + camera: + {} + rear_left_hip: + rear_left_upper_leg: + rear_left_lower_leg: + {} + rear_rail: + {} + rear_right_hip: + rear_right_upper_leg: + rear_right_lower_leg: + {} + vision: + {} + gpe: + {} + Update Interval: 0 + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: -7.258291244506836 + Min Value: -13.677467346191406 + 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.287733554840088 + Min Value: -14.104598999023438 + 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: -4.950493335723877 + Min Value: -12.155380249023438 + 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: -5.917332649230957 + Min Value: -9.742229461669922 + 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: -5.278988838195801 + Min Value: -12.110198020935059 + 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: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /twist_marker_server/update + Value: true + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: OverlayPanoramaImage + 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/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: Battery + 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: 14 + top: 50 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: TrackerVisualization + Topic: /deep_sort_tracker/output/viz + Value: true + alpha: 0.800000011920929 + height: 128 + keep aspect ratio: true + left: 1100 + overwrite alpha value: false + top: 0 + transport hint: raw + width: 700 + 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: 10 + 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: 1 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 0 + 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: 1920 + Y: 27 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch index 6d0cd8bc93..7f79c13b99 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -1,9 +1,11 @@ + + Date: Mon, 1 Mar 2021 13:00:05 +0900 Subject: [PATCH 093/261] [jsk_spot_startup] update spot_with_mot rviz config --- .../config/spot_with_mot.rviz | 44 ++++++++++++------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz index e326b222d0..d4786dfd33 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz @@ -4,9 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /TrackerVisualization1 + - /BoundingBoxArray1 Splitter Ratio: 0.6832844614982605 - Tree Height: 833 + Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -289,8 +289,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -7.258291244506836 - Min Value: -13.677467346191406 + Max Value: 3.874974012374878 + Min Value: -6.676506519317627 Value: true Axis: Z Channel Name: intensity @@ -327,8 +327,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.287733554840088 - Min Value: -14.104598999023438 + Max Value: 2.8503782749176025 + Min Value: -9.096238136291504 Value: true Axis: Z Channel Name: intensity @@ -365,8 +365,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -4.950493335723877 - Min Value: -12.155380249023438 + Max Value: 1.0685012340545654 + Min Value: -2.668184757232666 Value: true Axis: Z Channel Name: intensity @@ -403,8 +403,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -5.917332649230957 - Min Value: -9.742229461669922 + Max Value: 0.8432009220123291 + Min Value: -2.781191110610962 Value: true Axis: Z Channel Name: intensity @@ -441,8 +441,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -5.278988838195801 - Min Value: -12.110198020935059 + Max Value: 0.7082797884941101 + Min Value: -6.111994743347168 Value: true Axis: Z Channel Name: intensity @@ -538,6 +538,18 @@ Visualization Manager: top: 0 transport hint: raw width: 700 + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /panorama_rect_array_to_bounding_box_array/output + Unreliable: false + Value: true + alpha: 0.800000011920929 + color: 25; 255; 0 + coloring: Auto + line width: 0.004999999888241291 + only edge: false + show coords: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -581,10 +593,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1 + Pitch: 0.6449992656707764 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0 + Yaw: 5.533181190490723 Saved: ~ Window Geometry: Displays: @@ -592,7 +604,7 @@ Window Geometry: Height: 1025 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001570000037efc0200000013fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002300000018e0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d006100670065000000013f000000660000000000000000fb0000000a0049006d0061006700650000000164000000940000000000000000fb0000000a0049006d00610067006500000001a3000000e80000000000000000fc00000227000001970000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000002ad0000000000000000fb0000000a0049006d0061006700650100000177000000ed0000000000000000fb0000000a0049006d00610067006501000001eb000001d00000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -602,5 +614,5 @@ Window Geometry: Views: collapsed: true Width: 1920 - X: 1920 + X: 0 Y: 27 From 0c15d87b37400ec927515c31cd21ef3effb946bc Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 2 Mar 2021 18:37:24 +0900 Subject: [PATCH 094/261] [jsk_spot_startup] add respeaker_node to peripheral.launch --- .../launch/image_projection_test.launch | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch b/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch new file mode 100644 index 0000000000..9b75c772cd --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch @@ -0,0 +1,42 @@ + + + + + base_frame: camera + projection_type: image_projection_plugins::MercatorProjection + projection_parameters: + image_width: 2000 + image_height: 1000 + cylinder_radius: 1.0 + vertical_fov: 180 + cameras: + - '/spot/depth/frontleft' + - '/spot/depth/frontright' + - '/spot/depth/left' + - '/spot/depth/right' + - '/spot/depth/back' + spot: + depth: + frontleft: + image_topic: image + camera_info_topic: camera_info + frontright: + image_topic: image + camera_info_topic: camera_info + left: + image_topic: image + camera_info_topic: camera_info + right: + image_topic: image + camera_info_topic: camera_info + back: + image_topic: image + camera_info_topic: camera_info + + + From 85c99a9219c8fecbe4a8ddd7854723cf07dfd1d1 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 2 Mar 2021 18:38:56 +0900 Subject: [PATCH 095/261] [jsk_spot_startup] removed unused launch --- .../launch/image_projection_test.launch | 42 ------------------- 1 file changed, 42 deletions(-) delete mode 100644 jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch b/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch deleted file mode 100644 index 9b75c772cd..0000000000 --- a/jsk_spot_robot/jsk_spot_startup/launch/image_projection_test.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - base_frame: camera - projection_type: image_projection_plugins::MercatorProjection - projection_parameters: - image_width: 2000 - image_height: 1000 - cylinder_radius: 1.0 - vertical_fov: 180 - cameras: - - '/spot/depth/frontleft' - - '/spot/depth/frontright' - - '/spot/depth/left' - - '/spot/depth/right' - - '/spot/depth/back' - spot: - depth: - frontleft: - image_topic: image - camera_info_topic: camera_info - frontright: - image_topic: image - camera_info_topic: camera_info - left: - image_topic: image - camera_info_topic: camera_info - right: - image_topic: image - camera_info_topic: camera_info - back: - image_topic: image - camera_info_topic: camera_info - - - From f309cc15eef2fbe4a3fcd18f547cc01da99b2879 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 16:12:11 +0900 Subject: [PATCH 096/261] [jsk_spot_startup] add interaction launch --- .../launch/interaction.launch | 66 +++++++++++++++++++ .../launch/jsk_spot_bringup.launch | 10 ++- 2 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/interaction.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch new file mode 100644 index 0000000000..b939bb7bf9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + audio_topic: $(arg audio_topic) + n_channel: 1 + depth: 16 + sample_rate: 16000 + engine: "Google" + language: "en-US" + continuous: True + + + + + + + audio_topic: $(arg audio_topic) + n_channel: 1 + depth: 16 + sample_rate: 16000 + engine: "Sphinx" + language: "en-US" + continuous: True + + + + + + + + + + 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 index 343c5a2e7c..a2927904ac 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -1,11 +1,17 @@ + + + + From adec05dddabffdab624f6476bc953225d098c91f Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 16:46:18 +0900 Subject: [PATCH 097/261] [jsk_spot_startup] fix bugs in interaction.launch --- .../jsk_spot_startup/launch/interaction.launch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index b939bb7bf9..a79affdf4b 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -10,8 +10,8 @@ - - + + @@ -21,7 +21,7 @@ type="speech_recognition_node.py" respawn="true" output="screen" - if="$(eval $(arg speech_recognition_language) == 'english' and $(arg speech_recognition_network) == 'online')" + if="$(eval arg('speech_recognition_language') == 'english' and arg('speech_recognition_network') == 'online')" > @@ -40,7 +40,7 @@ type="speech_recognition_node.py" respawn="true" output="screen" - if="$(eval $(arg speech_recognition_language) == 'english' and $(arg speech_recognition_network) == 'offline')" + if="$(eval arg('speech_recognition_language') == 'english' and arg('speech_recognition_network') == 'offline')" > @@ -56,7 +56,7 @@ From 51fdb0c6e2cb04721b100e4e709885d96910db55 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 16:46:45 +0900 Subject: [PATCH 098/261] [jsk_spot_startup] add rviz config --- .../config/spot_with_mot_person_follow.rviz | 616 ++++++++++++++++++ 1 file changed, 616 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz new file mode 100644 index 0000000000..f931fbe973 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz @@ -0,0 +1,616 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.6832844614982605 + Tree Height: 806 + - 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: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - 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 + camera: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + jsk_head_mount_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 + respeaker_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back: + Value: true + back_fisheye: + Value: true + base_link: + Value: true + body: + Value: true + camera: + Value: true + flat_body: + Value: true + front_left_hip: + Value: true + front_left_lower_leg: + Value: true + front_left_upper_leg: + Value: true + front_rail: + Value: true + front_right_hip: + Value: true + front_right_lower_leg: + Value: true + front_right_upper_leg: + Value: true + frontleft: + Value: true + frontleft_fisheye: + Value: true + frontright: + Value: true + frontright_fisheye: + Value: true + gpe: + Value: true + head: + Value: true + jsk_head_mount_frame: + Value: true + left: + Value: true + left_fisheye: + Value: true + odom: + Value: true + rear_left_hip: + Value: true + rear_left_lower_leg: + Value: true + rear_left_upper_leg: + Value: true + rear_rail: + Value: true + rear_right_hip: + Value: true + rear_right_lower_leg: + Value: true + rear_right_upper_leg: + Value: true + respeaker_base: + Value: true + right: + Value: true + right_fisheye: + Value: true + vision: + Value: true + Marker Scale: 0.25 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + body: + base_link: + {} + flat_body: + {} + front_left_hip: + front_left_upper_leg: + front_left_lower_leg: + {} + front_rail: + {} + front_right_hip: + front_right_upper_leg: + front_right_lower_leg: + {} + head: + back: + back_fisheye: + {} + frontleft: + frontleft_fisheye: + {} + frontright: + frontright_fisheye: + {} + left: + left_fisheye: + {} + right: + right_fisheye: + {} + jsk_head_mount_frame: + camera: + {} + respeaker_base: + {} + rear_left_hip: + rear_left_upper_leg: + rear_left_lower_leg: + {} + rear_rail: + {} + rear_right_hip: + rear_right_upper_leg: + rear_right_lower_leg: + {} + vision: + {} + gpe: + {} + Update Interval: 0 + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 4.692697048187256 + Min Value: -6.975428104400635 + 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.843818426132202 + Min Value: -13.485079765319824 + 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: 2.006343364715576 + Min Value: -5.905572891235352 + 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: 1.3017923831939697 + Min Value: -3.124415874481201 + 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: 1.6489735841751099 + Min Value: -4.094615459442139 + 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: OverlayPanoramaImage + 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/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: Battery + 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: 14 + top: 50 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: TrackerVisualization + Topic: /deep_sort_tracker/output/viz + Value: true + alpha: 0.800000011920929 + height: 128 + keep aspect ratio: true + left: 1100 + overwrite alpha value: false + top: 0 + transport hint: raw + width: 700 + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /spot_person_follower/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: 10 + 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.9649990797042847 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 4.268178939819336 + 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: 1853 + X: 67 + Y: 27 From c0f2adf005f6976f25d137f9d0f7ed9dffe9aae4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 17:14:17 +0900 Subject: [PATCH 099/261] [jsk_spot_startup] fix typo --- jsk_spot_robot/jsk_spot_startup/launch/interaction.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index a79affdf4b..9205145d8a 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -1,5 +1,5 @@ - + From dfcc81659015604e0cf21f5e7925835d1cfcbfdb Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 20:50:50 +0900 Subject: [PATCH 100/261] [jsk_spot_startup] update rviz config --- .../config/spot_with_mot_person_follow.rviz | 41 +++++++++++++------ 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz index f931fbe973..d6b8465870 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz @@ -296,8 +296,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 4.692697048187256 - Min Value: -6.975428104400635 + Max Value: 3.769904613494873 + Min Value: -8.357810974121094 Value: true Axis: Z Channel Name: intensity @@ -334,8 +334,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 3.843818426132202 - Min Value: -13.485079765319824 + Max Value: 3.1473419666290283 + Min Value: -6.22025203704834 Value: true Axis: Z Channel Name: intensity @@ -372,8 +372,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.006343364715576 - Min Value: -5.905572891235352 + Max Value: 0.42404574155807495 + Min Value: -2.6966326236724854 Value: true Axis: Z Channel Name: intensity @@ -410,8 +410,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.3017923831939697 - Min Value: -3.124415874481201 + Max Value: 0.3233707547187805 + Min Value: -5.461534023284912 Value: true Axis: Z Channel Name: intensity @@ -448,8 +448,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.6489735841751099 - Min Value: -4.094615459442139 + Max Value: 0.6551579833030701 + Min Value: -10.876521110534668 Value: true Axis: Z Channel Name: intensity @@ -548,6 +548,21 @@ Visualization Manager: line width: 0.05000000074505806 only edge: true show coords: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /respeaker/sound_localization + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -598,11 +613,11 @@ Visualization Manager: Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1025 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: From de63e3560c4677fd83fe4b8cce73009dd3b8793d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 20:51:23 +0900 Subject: [PATCH 101/261] [jsk_spot_startup] use english by default to speech_recognition --- .../launch/interaction.launch | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index 9205145d8a..7a324ec969 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -1,6 +1,6 @@ - - + + @@ -15,13 +15,14 @@ - - + @@ -29,28 +30,26 @@ n_channel: 1 depth: 16 sample_rate: 16000 - engine: "Google" + engine: "Sphinx" language: "en-US" continuous: True - - + + - audio_topic: $(arg audio_topic) - n_channel: 1 - depth: 16 - sample_rate: 16000 - engine: "Sphinx" language: "en-US" - continuous: True + self_cancellation: true + tts_tolerance: 0.5 From 7e0340250764b7cad089a5ce2339f4d280e6c387 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 4 Mar 2021 22:24:14 +0900 Subject: [PATCH 102/261] [jsk_spot_startup] add japanese online speech recgonition --- .../launch/interaction.launch | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index 7a324ec969..c17864ae1c 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -1,5 +1,5 @@ - + @@ -49,10 +49,13 @@ language: "en-US" self_cancellation: true + tts_action_names: + - robotsound + - robotsound_jp tts_tolerance: 0.5 - + + + + + + + language: "ja-JP" + self_cancellation: true + tts_action_names: + - robotsound + - robotsound_jp + tts_tolerance: 0.5 + + From 9fd75fb3c1e02029f47bf03a5a3dde74ba7e9d16 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 5 Mar 2021 16:04:42 +0900 Subject: [PATCH 103/261] [jsk_spot_startup] fix topic name --- jsk_spot_robot/jsk_spot_startup/launch/interaction.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index c17864ae1c..76899b65da 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -74,7 +74,7 @@ if="$(eval arg('speech_recognition_language') == 'japanese' and arg('speech_recognition_network') == 'online')" > - + language: "ja-JP" self_cancellation: true From 1f48aa4f3cc81186a5313373a17f0d4c791a76c8 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 5 Mar 2021 21:30:23 +0900 Subject: [PATCH 104/261] [jsk_spot_startup] update twist_mux to add develop cmd_vel input --- jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml index 12d99e7374..5595597ef3 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml +++ b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml @@ -11,4 +11,8 @@ topics: topic : joy_head/cmd_vel timeout : 0.5 priority: 7 +- name : develop_input + topic : develop_input/cmd_vel + timeout : 0.5 + priority: 6 locks: [] From 5391d62a1f60d7bb9c5a68064122c793942b37aa Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 5 Mar 2021 21:31:13 +0900 Subject: [PATCH 105/261] [jsk_spot_startup] add respawn attribute to spot_ros.py node in driver.launch --- .../jsk_spot_startup/launch/driver.launch | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch index 2a3a6516ef..839d55d678 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/driver.launch @@ -6,7 +6,14 @@ - + @@ -15,7 +22,11 @@ - + From b7ac7de264e17826b05024d5f3ff33e76c43769d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 5 Mar 2021 21:33:12 +0900 Subject: [PATCH 106/261] [jsk_spot_startup] fix audio topic name in interaction.launchg --- jsk_spot_robot/jsk_spot_startup/launch/interaction.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch index 76899b65da..68910b10aa 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch @@ -2,7 +2,7 @@ - + @@ -24,7 +24,7 @@ output="screen" if="$(eval arg('speech_recognition_language') == 'english' and arg('speech_recognition_network') == 'offline')" > - + audio_topic: $(arg audio_topic) n_channel: 1 @@ -62,7 +62,7 @@ > - + Date: Fri, 19 Mar 2021 09:58:12 +0900 Subject: [PATCH 107/261] [jsk_spot_startup] mv basic launch to launch/include --- .../jsk_spot_startup/launch/{ => include}/driver.launch | 0 .../jsk_spot_startup/launch/{ => include}/interaction.launch | 0 .../jsk_spot_startup/launch/{ => include}/peripheral.launch | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename jsk_spot_robot/jsk_spot_startup/launch/{ => include}/driver.launch (100%) rename jsk_spot_robot/jsk_spot_startup/launch/{ => include}/interaction.launch (100%) rename jsk_spot_robot/jsk_spot_startup/launch/{ => include}/peripheral.launch (100%) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch similarity index 100% rename from jsk_spot_robot/jsk_spot_startup/launch/driver.launch rename to jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch similarity index 100% rename from jsk_spot_robot/jsk_spot_startup/launch/interaction.launch rename to jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch similarity index 100% rename from jsk_spot_robot/jsk_spot_startup/launch/peripheral.launch rename to jsk_spot_robot/jsk_spot_startup/launch/include/peripheral.launch From ed316d484714cd2813600e832c9f3bfa3e911fe5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 19 Mar 2021 16:20:56 +0900 Subject: [PATCH 108/261] [jsk_spot_startup] fix jsk_spot_bringup.launch --- .../launch/jsk_spot_bringup.launch | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) 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 index a2927904ac..bb539e25c1 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -1,17 +1,25 @@ - - + + + - - + + + + + + - - + + + + + From e4bd2082257571c19ac62d7a8c151436923468cf Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 19 Mar 2021 16:35:43 +0900 Subject: [PATCH 109/261] [jsk_spot_startup] add additional topics to record.launch --- .../jsk_spot_startup/launch/record.launch | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/record.launch b/jsk_spot_robot/jsk_spot_startup/launch/record.launch index b2ed325def..1958b10bac 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/record.launch @@ -129,6 +129,35 @@ /spot/status/power_state /spot/status/system_faults /spot/status/wifi + /spot/trajectory/cancel + /spot/trajectory/feedback + /spot/trajectory/goal + /spot/trajectory/result + /spot/trajectory/status + /audio + /bluetooth_teleop/cmd_vel + /develop_input/cmd_vel + /is_speeching + /joint_states + /joy_head/cmd_vel + /robotsound + /robotsound/cancel + /robotsound/feedback + /robotsound/goal + /robotsound/result + /robotsound/status + /robotsound_jp + /robotsound_jp/cancel + /robotsound_jp/feedback + /robotsound_jp/goal + /robotsound_jp/result + /robotsound_jp/status + /sound_direction + /sound_localization + /speech_audio + /speech_to_text_jp + /speech_to_text + /spot_teleop/joy " output="screen" /> @@ -180,6 +209,35 @@ /spot/status/power_state /spot/status/system_faults /spot/status/wifi + /spot/trajectory/cancel + /spot/trajectory/feedback + /spot/trajectory/goal + /spot/trajectory/result + /spot/trajectory/status + /audio + /bluetooth_teleop/cmd_vel + /develop_input/cmd_vel + /is_speeching + /joint_states + /joy_head/cmd_vel + /robotsound + /robotsound/cancel + /robotsound/feedback + /robotsound/goal + /robotsound/result + /robotsound/status + /robotsound_jp + /robotsound_jp/cancel + /robotsound_jp/feedback + /robotsound_jp/goal + /robotsound_jp/result + /robotsound_jp/status + /sound_direction + /sound_localization + /speech_audio + /speech_to_text_jp + /speech_to_text + /spot_teleop/joy " output="screen" /> From ec9d9c8fa09b28b19fc9b3a3e883dcdc4f6afc36 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 20 Mar 2021 13:10:42 +0900 Subject: [PATCH 110/261] [jsk_spot_startup] update rviz config --- .../jsk_spot_startup/config/spot.rviz | 130 ++++++++++-------- .../jsk_spot_startup/launch/rviz.launch | 1 + 2 files changed, 77 insertions(+), 54 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index 0cd1193a02..a258573add 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -3,9 +3,10 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /PanoramaDetection1 Splitter Ratio: 0.6832844614982605 - Tree Height: 441 + Tree Height: 806 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -33,24 +34,6 @@ Toolbars: Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -70,6 +53,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + camera: + Alpha: 1 + Show Axes: false + Show Trail: false front_left_hip: Alpha: 1 Show Axes: false @@ -104,6 +91,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + jsk_head_mount_frame: + Alpha: 1 + Show Axes: false + Show Trail: false rear_left_hip: Alpha: 1 Show Axes: false @@ -138,6 +129,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + respeaker_base: + Alpha: 1 + Show Axes: false + Show Trail: false Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -157,6 +152,8 @@ Visualization Manager: Value: true body: Value: true + camera: + Value: true flat_body: Value: true front_left_hip: @@ -185,6 +182,8 @@ Visualization Manager: Value: true head: Value: true + jsk_head_mount_frame: + Value: true left: Value: true left_fisheye: @@ -205,6 +204,8 @@ Visualization Manager: Value: true rear_right_upper_leg: Value: true + respeaker_base: + Value: true right: Value: true right_fisheye: @@ -249,6 +250,11 @@ Visualization Manager: right: right_fisheye: {} + jsk_head_mount_frame: + camera: + {} + respeaker_base: + {} rear_left_hip: rear_left_upper_leg: rear_left_lower_leg: @@ -273,8 +279,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.5423845052719116 - Min Value: -11.18303394317627 + Max Value: 0.9668689966201782 + Min Value: -7.657993316650391 Value: true Axis: Z Channel Name: intensity @@ -285,7 +291,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/frontleft/image - Depth Map Transport Hint: raw + Depth Map Transport Hint: compressedDepth Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -311,8 +317,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.0937347412109375 - Min Value: -10.284269332885742 + Max Value: 3.6703248023986816 + Min Value: -5.959461688995361 Value: true Axis: Z Channel Name: intensity @@ -323,7 +329,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/frontright/image - Depth Map Transport Hint: raw + Depth Map Transport Hint: compressedDepth Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -349,8 +355,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.01675701141357422 - Min Value: -5.776913642883301 + Max Value: -0.07669979333877563 + Min Value: -5.655144691467285 Value: true Axis: Z Channel Name: intensity @@ -361,7 +367,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/left/image - Depth Map Transport Hint: raw + Depth Map Transport Hint: compressedDepth Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -387,8 +393,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.5832391381263733 - Min Value: -3.825223207473755 + Max Value: -0.6268391609191895 + Min Value: -4.479696273803711 Value: true Axis: Z Channel Name: intensity @@ -399,7 +405,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/right/image - Depth Map Transport Hint: raw + Depth Map Transport Hint: compressedDepth Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -425,8 +431,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.8464637994766235 - Min Value: -8.379054069519043 + Max Value: 0.06454968452453613 + Min Value: -6.0565595626831055 Value: true Axis: Z Channel Name: intensity @@ -437,7 +443,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/back/image - Depth Map Transport Hint: raw + Depth Map Transport Hint: compressedDepth Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -459,19 +465,23 @@ Visualization Manager: Value: true Enabled: true Name: DepthClouds - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /twist_marker_server/update - Value: true + - Class: jsk_rviz_plugin/OverlayImage + Enabled: false + Name: PanoramaImage + Topic: /dual_fisheye_to_panorama/output/quater + Value: false + alpha: 0.800000011920929 + height: 128 + keep aspect ratio: true + left: 0 + overwrite alpha value: false + top: 0 + transport hint: compressed + width: 700 - Class: jsk_rviz_plugin/OverlayImage Enabled: true - Name: OverlayPanoramaImage - Topic: /dual_fisheye_to_panorama/output + Name: PanoramaDetection + Topic: /spot_recognition/object_detection_image Value: true alpha: 0.800000011920929 height: 128 @@ -479,7 +489,7 @@ Visualization Manager: left: 0 overwrite alpha value: false top: 0 - transport hint: raw + transport hint: compressed width: 700 - Class: jsk_rviz_plugin/TFTrajectory Enabled: true @@ -491,7 +501,7 @@ Visualization Manager: line_width: 0.009999999776482582 - Class: jsk_rviz_plugin/PieChart Enabled: true - Name: BatteryPercentage + Name: Battery Topic: /spot/status/battery_percentage Value: true auto color change: false @@ -509,6 +519,18 @@ Visualization Manager: size: 128 text size: 14 top: 50 + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: DetectedObjects + Topic: /rect_array_in_panorama_to_bounding_box_array/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 @@ -537,7 +559,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10 + Distance: 8.336650848388672 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -552,10 +574,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1 + Pitch: 0.8149988055229187 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0 + Yaw: 0.4968072175979614 Saved: ~ Window Geometry: Displays: @@ -563,7 +585,7 @@ Window Geometry: Height: 1025 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -572,6 +594,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1920 + X: 0 Y: 27 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch index 7f79c13b99..a0ee9d9e45 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -5,6 +5,7 @@ pkg="rviz" type="rviz" name="rviz" + required="True" args="-d $(find jsk_spot_startup)/config/$(arg config_type).rviz" /> From e56f0db6de5ef8c039884002eea5752151bfe8c1 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 20 Mar 2021 17:01:37 +0900 Subject: [PATCH 111/261] [jsk_spot_startup] updte rviz config --- .../jsk_spot_startup/config/spot.rviz | 33 +- .../config/spot_with_mot.rviz | 618 ----------------- .../config/spot_with_mot_person_follow.rviz | 631 ------------------ 3 files changed, 16 insertions(+), 1266 deletions(-) delete mode 100644 jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz delete mode 100644 jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index a258573add..943a303e0c 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -3,10 +3,9 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: - - /PanoramaDetection1 + Expanded: ~ Splitter Ratio: 0.6832844614982605 - Tree Height: 806 + Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -279,8 +278,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.9668689966201782 - Min Value: -7.657993316650391 + Max Value: -2.340333938598633 + Min Value: -10.503416061401367 Value: true Axis: Z Channel Name: intensity @@ -317,8 +316,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 3.6703248023986816 - Min Value: -5.959461688995361 + Max Value: -1.2692270278930664 + Min Value: -8.975149154663086 Value: true Axis: Z Channel Name: intensity @@ -355,8 +354,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.07669979333877563 - Min Value: -5.655144691467285 + Max Value: -3.3314270973205566 + Min Value: -6.635621547698975 Value: true Axis: Z Channel Name: intensity @@ -393,8 +392,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.6268391609191895 - Min Value: -4.479696273803711 + Max Value: -3.897368907928467 + Min Value: -7.379003047943115 Value: true Axis: Z Channel Name: intensity @@ -431,8 +430,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.06454968452453613 - Min Value: -6.0565595626831055 + Max Value: -3.3236377239227295 + Min Value: -9.60813045501709 Value: true Axis: Z Channel Name: intensity @@ -522,7 +521,7 @@ Visualization Manager: - Class: jsk_rviz_plugin/BoundingBoxArray Enabled: true Name: DetectedObjects - Topic: /rect_array_in_panorama_to_bounding_box_array/bbox_array + Topic: /spot_recognition/bbox_array Unreliable: false Value: true alpha: 0.800000011920929 @@ -559,7 +558,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 8.336650848388672 + Distance: 9.355134963989258 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -574,10 +573,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8149988055229187 + Pitch: 0.6199987530708313 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.4968072175979614 + Yaw: 4.3468146324157715 Saved: ~ Window Geometry: Displays: diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz deleted file mode 100644 index d4786dfd33..0000000000 --- a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot.rviz +++ /dev/null @@ -1,618 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /BoundingBoxArray1 - 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: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - 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 - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - 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 - jsk_head_mount_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: true - Frame Timeout: 15 - Frames: - All Enabled: true - back: - Value: true - back_fisheye: - Value: true - base_link: - Value: true - body: - Value: true - camera: - Value: true - flat_body: - Value: true - front_left_hip: - Value: true - front_left_lower_leg: - Value: true - front_left_upper_leg: - Value: true - front_rail: - Value: true - front_right_hip: - Value: true - front_right_lower_leg: - Value: true - front_right_upper_leg: - Value: true - frontleft: - Value: true - frontleft_fisheye: - Value: true - frontright: - Value: true - frontright_fisheye: - Value: true - gpe: - Value: true - head: - Value: true - jsk_head_mount_frame: - Value: true - left: - Value: true - left_fisheye: - Value: true - odom: - Value: true - rear_left_hip: - Value: true - rear_left_lower_leg: - Value: true - rear_left_upper_leg: - Value: true - rear_rail: - Value: true - rear_right_hip: - Value: true - rear_right_lower_leg: - Value: true - rear_right_upper_leg: - Value: true - right: - Value: true - right_fisheye: - Value: true - vision: - Value: true - Marker Scale: 0.25 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - odom: - body: - base_link: - {} - flat_body: - {} - front_left_hip: - front_left_upper_leg: - front_left_lower_leg: - {} - front_rail: - {} - front_right_hip: - front_right_upper_leg: - front_right_lower_leg: - {} - head: - back: - back_fisheye: - {} - frontleft: - frontleft_fisheye: - {} - frontright: - frontright_fisheye: - {} - left: - left_fisheye: - {} - right: - right_fisheye: - {} - jsk_head_mount_frame: - camera: - {} - rear_left_hip: - rear_left_upper_leg: - rear_left_lower_leg: - {} - rear_rail: - {} - rear_right_hip: - rear_right_upper_leg: - rear_right_lower_leg: - {} - vision: - {} - gpe: - {} - Update Interval: 0 - Value: true - - Class: rviz/Group - Displays: - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: true - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 3.874974012374878 - Min Value: -6.676506519317627 - 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: 2.8503782749176025 - Min Value: -9.096238136291504 - 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.0685012340545654 - Min Value: -2.668184757232666 - 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: 0.8432009220123291 - Min Value: -2.781191110610962 - 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: 0.7082797884941101 - Min Value: -6.111994743347168 - 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: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /twist_marker_server/update - Value: true - - Class: jsk_rviz_plugin/OverlayImage - Enabled: true - Name: OverlayPanoramaImage - 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/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: Battery - 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: 14 - top: 50 - - Class: jsk_rviz_plugin/OverlayImage - Enabled: true - Name: TrackerVisualization - Topic: /deep_sort_tracker/output/viz - Value: true - alpha: 0.800000011920929 - height: 128 - keep aspect ratio: true - left: 1100 - overwrite alpha value: false - top: 0 - transport hint: raw - width: 700 - - Class: jsk_rviz_plugin/BoundingBoxArray - Enabled: true - Name: BoundingBoxArray - Topic: /panorama_rect_array_to_bounding_box_array/output - Unreliable: false - Value: true - alpha: 0.800000011920929 - color: 25; 255; 0 - coloring: Auto - line width: 0.004999999888241291 - only edge: false - 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: 10 - 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.6449992656707764 - Target Frame: base_link - Value: Orbit (rviz) - Yaw: 5.533181190490723 - 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/spot_with_mot_person_follow.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz deleted file mode 100644 index d6b8465870..0000000000 --- a/jsk_spot_robot/jsk_spot_startup/config/spot_with_mot_person_follow.rviz +++ /dev/null @@ -1,631 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.6832844614982605 - Tree Height: 806 - - 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: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - 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 - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - 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 - jsk_head_mount_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 - respeaker_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - back: - Value: true - back_fisheye: - Value: true - base_link: - Value: true - body: - Value: true - camera: - Value: true - flat_body: - Value: true - front_left_hip: - Value: true - front_left_lower_leg: - Value: true - front_left_upper_leg: - Value: true - front_rail: - Value: true - front_right_hip: - Value: true - front_right_lower_leg: - Value: true - front_right_upper_leg: - Value: true - frontleft: - Value: true - frontleft_fisheye: - Value: true - frontright: - Value: true - frontright_fisheye: - Value: true - gpe: - Value: true - head: - Value: true - jsk_head_mount_frame: - Value: true - left: - Value: true - left_fisheye: - Value: true - odom: - Value: true - rear_left_hip: - Value: true - rear_left_lower_leg: - Value: true - rear_left_upper_leg: - Value: true - rear_rail: - Value: true - rear_right_hip: - Value: true - rear_right_lower_leg: - Value: true - rear_right_upper_leg: - Value: true - respeaker_base: - Value: true - right: - Value: true - right_fisheye: - Value: true - vision: - Value: true - Marker Scale: 0.25 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - odom: - body: - base_link: - {} - flat_body: - {} - front_left_hip: - front_left_upper_leg: - front_left_lower_leg: - {} - front_rail: - {} - front_right_hip: - front_right_upper_leg: - front_right_lower_leg: - {} - head: - back: - back_fisheye: - {} - frontleft: - frontleft_fisheye: - {} - frontright: - frontright_fisheye: - {} - left: - left_fisheye: - {} - right: - right_fisheye: - {} - jsk_head_mount_frame: - camera: - {} - respeaker_base: - {} - rear_left_hip: - rear_left_upper_leg: - rear_left_lower_leg: - {} - rear_rail: - {} - rear_right_hip: - rear_right_upper_leg: - rear_right_lower_leg: - {} - vision: - {} - gpe: - {} - Update Interval: 0 - Value: true - - Class: rviz/Group - Displays: - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: true - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 3.769904613494873 - Min Value: -8.357810974121094 - 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.1473419666290283 - Min Value: -6.22025203704834 - 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: 0.42404574155807495 - Min Value: -2.6966326236724854 - 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: 0.3233707547187805 - Min Value: -5.461534023284912 - 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: 0.6551579833030701 - Min Value: -10.876521110534668 - 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: OverlayPanoramaImage - 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/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: Battery - 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: 14 - top: 50 - - Class: jsk_rviz_plugin/OverlayImage - Enabled: true - Name: TrackerVisualization - Topic: /deep_sort_tracker/output/viz - Value: true - alpha: 0.800000011920929 - height: 128 - keep aspect ratio: true - left: 1100 - overwrite alpha value: false - top: 0 - transport hint: raw - width: 700 - - Class: jsk_rviz_plugin/BoundingBoxArray - Enabled: true - Name: BoundingBoxArray - Topic: /spot_person_follower/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 - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /respeaker/sound_localization - Unreliable: false - Value: true - 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: 10 - 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.9649990797042847 - Target Frame: base_link - Value: Orbit (rviz) - Yaw: 4.268178939819336 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1025 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015700000363fc0200000013fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002300000018e0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d006100670065000000013f000000660000000000000000fb0000000a0049006d0061006700650000000164000000940000000000000000fb0000000a0049006d00610067006500000001a3000000e80000000000000000fc00000227000001970000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000002ad0000000000000000fb0000000a0049006d0061006700650100000177000000ed0000000000000000fb0000000a0049006d00610067006501000001eb000001d00000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e00000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1853 - X: 67 - Y: 27 From d2e11f368867623588af0f60a4bf0f897c9bc93a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 22 Mar 2021 18:46:08 +0900 Subject: [PATCH 112/261] [jsk_spot_startup] add cmd_vel_smoother --- .../jsk_spot_startup/launch/include/driver.launch | 14 +++++++++++++- jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 839d55d678..0252c937f7 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -22,13 +22,25 @@ + + + + + + + + + + + + - + diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index fcd51c7ab9..f7cbd012ce 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -18,6 +18,7 @@ rviz spot_description spot_driver + cmd_vel_smoother rostest From 9d570de46678955dccada041a871f0eebff44ce8 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 22 Mar 2021 18:51:30 +0900 Subject: [PATCH 113/261] [jsk_spot_startup] add spot_recognition topics to record.launch --- jsk_spot_robot/jsk_spot_startup/launch/record.launch | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/record.launch b/jsk_spot_robot/jsk_spot_startup/launch/record.launch index 1958b10bac..79a55afa90 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/record.launch @@ -158,6 +158,11 @@ /speech_to_text_jp /speech_to_text /spot_teleop/joy + /spot_recognition/bbox_array + /spot_recognition/class + /spot_recognition/rects + /spot_recognition/object_detection_image/compressed + /spot_recognition/tracking_labels " output="screen" /> @@ -238,6 +243,11 @@ /speech_to_text_jp /speech_to_text /spot_teleop/joy + /spot_recognition/bbox_array + /spot_recognition/class + /spot_recognition/rects + /spot_recognition/object_detection_image/compressed + /spot_recognition/tracking_labels " output="screen" /> From 4dbfc7a40ef33a14df16ae36e0d9b07922d94653 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Thu, 15 Apr 2021 10:00:17 +0900 Subject: [PATCH 114/261] [jsk_spot_startup] (#10) - make rosbag play node as an independent terminal to enable the keyboard input, suahs as pause - add fisheye panorama image in rviz --- .../jsk_spot_startup/config/spot.rviz | 47 ++++++++++++------- .../jsk_spot_startup/launch/play.launch | 2 +- 2 files changed, 32 insertions(+), 17 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index 943a303e0c..8f63bb7d29 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -3,9 +3,10 @@ Panels: Help Height: 0 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Image1 Splitter Ratio: 0.6832844614982605 - Tree Height: 441 + Tree Height: 806 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -278,8 +279,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -2.340333938598633 - Min Value: -10.503416061401367 + Max Value: 0.7146440148353577 + Min Value: -1.1043601036071777 Value: true Axis: Z Channel Name: intensity @@ -316,8 +317,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -1.2692270278930664 - Min Value: -8.975149154663086 + Max Value: 0.8748507499694824 + Min Value: -1.1722642183303833 Value: true Axis: Z Channel Name: intensity @@ -354,8 +355,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.3314270973205566 - Min Value: -6.635621547698975 + Max Value: -0.13511446118354797 + Min Value: -1.2677093744277954 Value: true Axis: Z Channel Name: intensity @@ -392,8 +393,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.897368907928467 - Min Value: -7.379003047943115 + Max Value: -0.3557555675506592 + Min Value: -1.1095689535140991 Value: true Axis: Z Channel Name: intensity @@ -430,8 +431,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -3.3236377239227295 - Min Value: -9.60813045501709 + Max Value: 0.9053994417190552 + Min Value: -2.7137227058410645 Value: true Axis: Z Channel Name: intensity @@ -530,6 +531,18 @@ Visualization Manager: line width: 0.05000000074505806 only edge: true show coords: false + - Class: rviz/Image + Enabled: true + Image Topic: /dual_fisheye_to_panorama/output/quater + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -573,7 +586,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6199987530708313 + Pitch: 0.6249987483024597 Target Frame: base_link Value: Orbit (rviz) Yaw: 4.3468146324157715 @@ -584,7 +597,9 @@ Window Geometry: Height: 1025 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 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 + Image: + collapsed: false + QMainWindow State: 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 Selection: collapsed: false Time: @@ -593,6 +608,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 + Width: 1853 + X: 67 Y: 27 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/play.launch b/jsk_spot_robot/jsk_spot_startup/launch/play.launch index 0280bd2c8c..258198d250 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/play.launch @@ -25,7 +25,7 @@ type="play" name="$(anon rosbag)" args="$(arg rosbag) --clock" - required="true" + launch-prefix="xterm -e" /> Date: Wed, 24 Nov 2021 11:05:16 +0900 Subject: [PATCH 115/261] [jsk_spot_startup] split credential to yaml file --- jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml | 3 +++ .../jsk_spot_startup/launch/include/driver.launch | 8 ++------ .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 4 +--- 3 files changed, 6 insertions(+), 9 deletions(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml 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..415533c3f1 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml @@ -0,0 +1,3 @@ +username: "dummyusername" +password: "dummypassword" +hostname: "192.168.50.3" diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 0252c937f7..808e619b14 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -1,7 +1,5 @@ - - - + @@ -15,9 +13,7 @@ respawn="true" > - - - + 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 index bb539e25c1..9b1ce7237b 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -7,9 +7,7 @@ - - - + From 5074a6562ee65d8d38bb4cdb65758f6f9bcacb2d Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata <27789460+mqcmd196@users.noreply.github.com> Date: Tue, 20 Apr 2021 17:26:17 +0900 Subject: [PATCH 116/261] [jsk_spot_startup] Add options to the files for rosbag and add dependency (#16) * rename rosbag launch files * add duration, start etc... * add xterm to exec_depend for using rosbag play --- .../{play.launch => rosbag_play.launch} | 25 +++--- .../{record.launch => rosbag_record.launch} | 79 ------------------- jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 3 files changed, 11 insertions(+), 94 deletions(-) rename jsk_spot_robot/jsk_spot_startup/launch/{play.launch => rosbag_play.launch} (85%) rename jsk_spot_robot/jsk_spot_startup/launch/{record.launch => rosbag_record.launch} (69%) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/play.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch similarity index 85% rename from jsk_spot_robot/jsk_spot_startup/launch/play.launch rename to jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch index 258198d250..a09877df00 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch @@ -1,11 +1,18 @@ - + + + + + + + + @@ -24,7 +31,7 @@ pkg="rosbag" type="play" name="$(anon rosbag)" - args="$(arg rosbag) --clock" + args="$(arg rosbag) $(arg loop_flag) --clock --start $(arg start_time) $(arg duration)" launch-prefix="xterm -e" /> - - - - - - - - - - - - - - - - - - - - - + + + diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index 545ebb1007..15e7d9fedb 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -13,6 +13,7 @@ roslaunch roslint + laptop_battery_monitor interactive_marker_twist_server roslaunch rviz From 2555a0fb49a2d3c51d63f5d8d9332b3a1e7f42e2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 16 Jun 2021 00:37:15 +0900 Subject: [PATCH 118/261] [jsk_spot_startup] add laptop battery visualizer to rviz --- .../jsk_spot_startup/config/spot.rviz | 76 ++++++++++++------- .../launch/rosbag_record.launch | 1 + .../jsk_spot_startup/launch/rviz.launch | 6 ++ .../node_scripts/laptop_battery_visualizer.py | 28 +++++++ 4 files changed, 85 insertions(+), 26 deletions(-) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/laptop_battery_visualizer.py diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index 8f63bb7d29..9cd0fc5075 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -4,9 +4,13 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Image1 + - /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: 806 + Tree Height: 370 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -279,9 +283,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.7146440148353577 - Min Value: -1.1043601036071777 - Value: true + Max Value: 1.5 + Min Value: -1.5 + Value: false Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -317,9 +321,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.8748507499694824 - Min Value: -1.1722642183303833 - Value: true + Max Value: 1.5 + Min Value: -1.5 + Value: false Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -355,9 +359,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.13511446118354797 - Min Value: -1.2677093744277954 - Value: true + Max Value: 1.5 + Min Value: -1.5 + Value: false Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -393,9 +397,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: -0.3557555675506592 - Min Value: -1.1095689535140991 - Value: true + Max Value: 1.5 + Min Value: -1.5 + Value: false Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -431,9 +435,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.9053994417190552 - Min Value: -2.7137227058410645 - Value: true + Max Value: 1.5 + Min Value: -1.5 + Value: false Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -501,7 +505,7 @@ Visualization Manager: line_width: 0.009999999776482582 - Class: jsk_rviz_plugin/PieChart Enabled: true - Name: Battery + Name: SpotBattery Topic: /spot/status/battery_percentage Value: true auto color change: false @@ -517,7 +521,27 @@ Visualization Manager: min value: 0 show caption: true size: 128 - text size: 14 + 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 @@ -586,20 +610,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6249987483024597 + Pitch: 0.6799988746643066 Target Frame: base_link Value: Orbit (rviz) - Yaw: 4.3468146324157715 + Yaw: 5.106814861297607 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1025 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -608,6 +632,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1920 + X: 0 Y: 27 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch index 46ba6d8e92..4fc6c0a804 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch @@ -61,6 +61,7 @@ /is_speeching /joint_states /joy_head/cmd_vel + /laptop_charge /robotsound /robotsound/cancel /robotsound/feedback diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch index a0ee9d9e45..928ed46871 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rviz.launch @@ -14,4 +14,10 @@ type="battery_visualizer.py" name="battery_visualizer" /> + + 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..d2f6527411 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/laptop_battery_visualizer.py @@ -0,0 +1,28 @@ +#!/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() From 226041884a9d67ce37e7e833dfbef838124d6c69 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 17 Jun 2021 12:02:51 +0900 Subject: [PATCH 119/261] [jsk_spot_startup] add object detection visualization to rosbag_play --- .../jsk_spot_startup/launch/rosbag_play.launch | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch index a09877df00..c1e514865c 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch @@ -119,4 +119,22 @@ args="compressedDepth in:=/spot/depth/right/image raw out:=/spot/depth/right/image" if="$(arg use_decompress)" /> + + + + + + + + + From 9feadc58098efcd5b3dafd9d39cbc6e99eb826d3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 4 Jul 2021 10:44:21 +0900 Subject: [PATCH 120/261] [jsk_spot_startup] add front microphone (#27) --- .../jsk_spot_startup/launch/include/interaction.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index 68910b10aa..41cb01114b 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -2,7 +2,7 @@ - + From b0fc828556d4450da0b26c91b5c9a067d5d80f1b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 24 Nov 2021 10:40:12 +0900 Subject: [PATCH 121/261] [jsk_spot_startup] add shinjo to maintainers --- jsk_spot_robot/jsk_spot_startup/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index 15e7d9fedb..56998bef07 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -5,7 +5,9 @@ The jsk_spot_startup package Kei Okada + Koki Shinjo Koki Shinjo + BSD catkin From 3318a2e609e55aaa58b54bd5746a6ebf3809c429 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 3 Jul 2021 11:35:54 +0900 Subject: [PATCH 122/261] [jsk_spot_startup] add cable_connection\detector.py --- .../launch/include/driver.launch | 7 +++++ .../node_scripts/cable_connection_detector.py | 29 +++++++++++++++++++ 2 files changed, 36 insertions(+) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/cable_connection_detector.py diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index c2da0040c7..771429d3b4 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -46,4 +46,11 @@ > + + + 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..bb03ab30af --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/cable_connection_detector.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +import rospy +from spot_msgs.msg import BatteryStateArray, BatteryState +from std_msgs.msg import Bool + +class CalbeConnectionDetector: + + def __init__(self): + + rospy.pub = rospy.Publisher('/spot/status/cable_connected',Bool,queue_size=1) + rospy.sub_battery_state = rospy.Subscriber('/spot/status/battery_states',BatteryStateArray,self.callback_battery_state) + + def callback_battery_state(self,msg): + + if msg.battery_states[0].status == BatteryState.STATUS_DISCHARGING: + rospy.pub.publish(Bool(False)) + else: + rospy.pub.publish(Bool(True)) + +def main(): + + rospy.init_node('cable_connection_detector') + node = CalbeConnectionDetector() + rospy.spin() + + +if __name__ == '__main__': + main() From 326361af109c0d9eabd2bb383c9709d3077510f2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 15 Jul 2021 11:49:03 +0900 Subject: [PATCH 123/261] [jsk_spot_startup] update rviz config to use raw image --- .../jsk_spot_startup/config/spot.rviz | 445 ++++++++++++------ 1 file changed, 300 insertions(+), 145 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index 9cd0fc5075..fee9ea6df2 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -99,182 +99,131 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - rear_left_hip: + kinova_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - rear_left_lower_leg: + kinova_base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - rear_left_upper_leg: + kinova_dummy_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - rear_rail: + kinova_end_effector_link: Alpha: 1 Show Axes: false Show Trail: false - rear_right_hip: + kinova_forearm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - rear_right_lower_leg: + kinova_gripper_base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - rear_right_upper_leg: + kinova_left_finger_dist_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - respeaker_base: + kinova_left_finger_prox_link: Alpha: 1 Show Axes: false Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - back: - Value: true - back_fisheye: - Value: true - base_link: - Value: true - body: Value: true - camera: - Value: true - flat_body: - Value: true - front_left_hip: - Value: true - front_left_lower_leg: - Value: true - front_left_upper_leg: - Value: true - front_rail: - Value: true - front_right_hip: - Value: true - front_right_lower_leg: - Value: true - front_right_upper_leg: - Value: true - frontleft: - Value: true - frontleft_fisheye: - Value: true - frontright: - Value: true - frontright_fisheye: - Value: true - gpe: - Value: true - head: + kinova_lower_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - jsk_head_mount_frame: + kinova_right_finger_dist_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - left: + kinova_right_finger_prox_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - left_fisheye: + kinova_shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true - odom: + kinova_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + kinova_upper_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false Value: true 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: - Value: true + 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 respeaker_base: - Value: true - right: - Value: true - right_fisheye: - Value: true - vision: - Value: true + Alpha: 1 + Show Axes: false + Show Trail: false + 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: - odom: - body: - base_link: - {} - flat_body: - {} - front_left_hip: - front_left_upper_leg: - front_left_lower_leg: - {} - front_rail: - {} - front_right_hip: - front_right_upper_leg: - front_right_lower_leg: - {} - head: - back: - back_fisheye: - {} - frontleft: - frontleft_fisheye: - {} - frontright: - frontright_fisheye: - {} - left: - left_fisheye: - {} - right: - right_fisheye: - {} - jsk_head_mount_frame: - camera: - {} - respeaker_base: - {} - rear_left_hip: - rear_left_upper_leg: - rear_left_lower_leg: - {} - rear_rail: - {} - rear_right_hip: - rear_right_upper_leg: - rear_right_lower_leg: - {} - vision: - {} - gpe: - {} + {} Update Interval: 0 - Value: true + Value: false - Class: rviz/Group Displays: - Alpha: 1 @@ -283,9 +232,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5 - Min Value: -1.5 - Value: false + Max Value: 3.2305479049682617 + Min Value: -0.3259410858154297 + Value: true Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -295,7 +244,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/frontleft/image - Depth Map Transport Hint: compressedDepth + Depth Map Transport Hint: raw Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -321,9 +270,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5 - Min Value: -1.5 - Value: false + Max Value: 2.065394163131714 + Min Value: -0.46760129928588867 + Value: true Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -333,7 +282,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/frontright/image - Depth Map Transport Hint: compressedDepth + Depth Map Transport Hint: raw Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -359,9 +308,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5 - Min Value: -1.5 - Value: false + Max Value: 1.4258588552474976 + Min Value: -0.40428322553634644 + Value: true Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -371,7 +320,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/left/image - Depth Map Transport Hint: compressedDepth + Depth Map Transport Hint: raw Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -397,9 +346,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5 - Min Value: -1.5 - Value: false + Max Value: 1.0295515060424805 + Min Value: -0.33090418577194214 + Value: true Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -409,7 +358,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/right/image - Depth Map Transport Hint: compressedDepth + Depth Map Transport Hint: raw Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -435,9 +384,9 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.5 - Min Value: -1.5 - Value: false + Max Value: 2.5774011611938477 + Min Value: -0.597380518913269 + Value: true Axis: Z Channel Name: intensity Class: rviz/DepthCloud @@ -447,7 +396,7 @@ Visualization Manager: Color Transport Hint: raw Decay Time: 0 Depth Map Topic: /spot/depth/back/image - Depth Map Transport Hint: compressedDepth + Depth Map Transport Hint: raw Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -480,7 +429,7 @@ Visualization Manager: left: 0 overwrite alpha value: false top: 0 - transport hint: compressed + transport hint: raw width: 700 - Class: jsk_rviz_plugin/OverlayImage Enabled: true @@ -493,7 +442,7 @@ Visualization Manager: left: 0 overwrite alpha value: false top: 0 - transport hint: compressed + transport hint: raw width: 700 - Class: jsk_rviz_plugin/TFTrajectory Enabled: true @@ -567,6 +516,212 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ElevatorDoorPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /spot_recognition/elevator_door_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + back: + Value: false + back_fisheye: + Value: false + base_link: + Value: false + body: + Value: false + camera: + Value: false + flat_body: + Value: false + front_left_hip: + Value: false + front_left_lower_leg: + Value: false + front_left_upper_leg: + Value: false + front_rail: + Value: false + front_right_hip: + Value: false + front_right_lower_leg: + Value: false + front_right_upper_leg: + Value: false + frontleft: + Value: false + frontleft_fisheye: + Value: false + frontright: + Value: false + frontright_fisheye: + Value: false + gpe: + Value: false + head: + Value: false + jsk_head_mount_frame: + Value: false + kinova_arm_link: + Value: false + kinova_base_link: + Value: false + kinova_dummy_link: + Value: false + kinova_end_effector_link: + Value: false + kinova_forearm_link: + Value: false + kinova_gripper_base_link: + Value: false + kinova_left_finger_dist_link: + Value: false + kinova_left_finger_prox_link: + Value: false + kinova_lower_wrist_link: + Value: false + kinova_right_finger_dist_link: + Value: false + kinova_right_finger_prox_link: + Value: false + kinova_shoulder_link: + Value: false + kinova_tool_frame: + Value: false + kinova_upper_wrist_link: + Value: false + left: + Value: false + left_fisheye: + Value: false + odom: + Value: false + rear_left_hip: + Value: false + rear_left_lower_leg: + Value: false + rear_left_upper_leg: + Value: false + rear_rail: + Value: false + rear_right_hip: + Value: false + rear_right_lower_leg: + Value: false + rear_right_upper_leg: + Value: false + respeaker_base: + Value: false + right: + Value: false + right_fisheye: + Value: false + vision: + Value: false + Marker Scale: 2 + Name: ElevatorFrame + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + body: + base_link: + {} + flat_body: + {} + front_left_hip: + front_left_upper_leg: + front_left_lower_leg: + {} + front_rail: + kinova_base_link: + kinova_shoulder_link: + kinova_arm_link: + kinova_forearm_link: + kinova_lower_wrist_link: + kinova_upper_wrist_link: + kinova_end_effector_link: + kinova_dummy_link: + kinova_gripper_base_link: + kinova_left_finger_prox_link: + kinova_left_finger_dist_link: + {} + kinova_right_finger_prox_link: + kinova_right_finger_dist_link: + {} + kinova_tool_frame: + {} + front_right_hip: + front_right_upper_leg: + front_right_lower_leg: + {} + head: + back: + back_fisheye: + {} + frontleft: + frontleft_fisheye: + {} + frontright: + frontright_fisheye: + {} + left: + left_fisheye: + {} + right: + right_fisheye: + {} + jsk_head_mount_frame: + camera: + {} + respeaker_base: + {} + rear_left_hip: + rear_left_upper_leg: + rear_left_lower_leg: + {} + rear_rail: + {} + rear_right_hip: + rear_right_upper_leg: + rear_right_lower_leg: + {} + vision: + {} + gpe: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -595,7 +750,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 9.355134963989258 + Distance: 8.482810974121094 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -610,10 +765,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6799988746643066 + Pitch: 0.3449980318546295 Target Frame: base_link Value: Orbit (rviz) - Yaw: 5.106814861297607 + Yaw: 4.94684362411499 Saved: ~ Window Geometry: Displays: From 4d0e520e5f155491550cf7fb092facdca01372f5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 13 Aug 2021 11:30:38 +0900 Subject: [PATCH 124/261] [jsk_spot_startup] update battery scripts and update launch --- .../launch/include/driver.launch | 7 ++ .../node_scripts/battery_notifier.py | 64 +++++++++++++++++++ .../node_scripts/cable_connection_detector.py | 48 +++++++++++--- 3 files changed, 111 insertions(+), 8 deletions(-) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 771429d3b4..6a666f5bdf 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -53,4 +53,11 @@ > + + + 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..8f1c0f2039 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python + +import rospy +from spot_msgs.msg import BatteryStateArray +from sensor_msgs.msg import BatteryState + +from spot_ros_client.libspotros import SpotRosClient +from sound_play.libsoundplay import SoundClient + + +class SpotBatteryNotifier(object): + + def __init__(self): + + self._battery_spot = 0 + self._battery_laptop = 0 + + self._sub_spot = rospy.Subscriber( + '/spot/status/battery_states', + BatteryStateArray, + self._cb_spot ) + self._sub_laptop = rospy.Subscriber( + '/laptop_charge', + BatteryState, + self._cb_laptop ) + + spot_client = SpotRosClient() + sound_client = SoundClient( + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) + + threshold_warning_spot = float(rospy.get_param('~threshold_warning_spot', 20)) + threshold_warning_laptop = float(rospy.get_param('~threshold_warning_laptop', 20)) + + threshold_estop_spot = float(rospy.get_param('~threshold_estop_spot', 5)) + threshold_estop_laptop = float(rospy.get_param('~threshold_estop_laptop', 5)) + + rate = rospy.Rate(0.1) + while not rospy.is_shutdown(): + + rate.sleep() + + if self._battery_spot < threshold_warning_spot or\ + self._battery_laptop < threshold_warning_laptop: + rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format(self._battery_spot,self._battery_laptop)) + sound_client.say('バッテリー残量が少ないです。') + + if self._battery_spot < threshold_estop_spot or\ + self._battery_laptop < threshold_estop_laptop: + rospy.logerr('Battery is low. Estop.') + sound_client.say('バッテリー残量が少ないため、動作を停止します') + spot_client.estop_gentle() + spot_client.estop_hard() + + + def _cb_spot(self, msg): + + self._battery_spot = msg.battery_states[0].charge_percentage + + def _cb_laptop(self, msg): + + self._battery_laptop = msg.percentage 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 index bb03ab30af..fe819db316 100755 --- 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 @@ -1,22 +1,54 @@ #!/usr/bin/env python import rospy -from spot_msgs.msg import BatteryStateArray, BatteryState +import spot_msgs.msg.BatteryStateArray, spot_msgs.msg.BatteryState +import sensor_msgs.msg.BatteryState from std_msgs.msg import Bool class CalbeConnectionDetector: def __init__(self): - rospy.pub = rospy.Publisher('/spot/status/cable_connected',Bool,queue_size=1) - rospy.sub_battery_state = rospy.Subscriber('/spot/status/battery_states',BatteryStateArray,self.callback_battery_state) + self.pub = rospy.Publisher('/spot/status/cable_connected',Bool,queue_size=1) - def callback_battery_state(self,msg): + self.msg_battery_spot = None + self.msg_battery_laptop = None + + self.sub_battery_spot = rospy.Subscriber( + '/spot/status/battery_states', + spot_msgs.msg.BatteryStateArray, + self.callback_battery_spot) + self.sub_battery_laptop = rospy.Subscriber( + '/laptop_charge', + sensor_msgs.msg.BatteryState, + self.callback_battery_laptop) + + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + if self.msg_battery_spot == None or self.msg_battery_laptop == None: + continue + self.pub.publish(Bool(self.check_connection())) + + def check_connection(self): + + connected = False + + if self.msg_battery_spot.battery_states[0].status == spot_msgs.msg.BatteryState.STATUS_CHARGING or\ + self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or\ + self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: #? + return True + + return connected + + def callback_battery_spot(self,msg): + + self.msg_battery_spot = msg + + def callback_battery_laptop(self,msg): + + self.msg_battery_laptop = msg - if msg.battery_states[0].status == BatteryState.STATUS_DISCHARGING: - rospy.pub.publish(Bool(False)) - else: - rospy.pub.publish(Bool(True)) def main(): From 454bf6378fb5edb438cdf903d453e5bba33e82b3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 13 Aug 2021 11:38:53 +0900 Subject: [PATCH 125/261] [jsk_spot_startup] fix battery scripts --- .../jsk_spot_startup/node_scripts/battery_notifier.py | 1 + .../node_scripts/cable_connection_detector.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) 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 index 8f1c0f2039..60f6b8901b 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# -*- encoding: utf-8 -*- import rospy from spot_msgs.msg import BatteryStateArray 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 index fe819db316..0110f6ea26 100755 --- 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 @@ -1,8 +1,8 @@ #!/usr/bin/env python import rospy -import spot_msgs.msg.BatteryStateArray, spot_msgs.msg.BatteryState -import sensor_msgs.msg.BatteryState +import spot_msgs.msg +import sensor_msgs.msg from std_msgs.msg import Bool class CalbeConnectionDetector: From 68fbed9a6544046d55189aa83475c986a9ec0421 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 29 Jun 2021 18:47:00 +0900 Subject: [PATCH 126/261] [jsk_spot_startup] Add exec_depend packages to jsk_spot_startup --- jsk_spot_robot/jsk_spot_startup/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index 56998bef07..bd909ad0cd 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -23,6 +23,10 @@ spot_driver cmd_vel_smoother xterm + sound_play + jsk_perception + aques_talk + spoteus rostest From 16ad09d44ac97e067dc6c123e1b1619946d099b9 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 26 Aug 2021 15:12:30 +0900 Subject: [PATCH 127/261] [jsk_spot_startup] add launch_robot_state_publisher arg --- .../jsk_spot_startup/launch/include/driver.launch | 6 +++++- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 6a666f5bdf..98d772f6b4 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -1,7 +1,11 @@ + + + + + - + From 551a20c9c4627da33a46939c103a9a6da3ece2ce Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 29 Aug 2021 13:15:10 +0900 Subject: [PATCH 128/261] [jsk_spot_startup] move battery visualization script to driver.launch --- .../jsk_spot_startup/launch/include/driver.launch | 12 ++++++++++++ jsk_spot_robot/jsk_spot_startup/launch/rviz.launch | 12 ------------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 98d772f6b4..b58822bfc5 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -50,6 +50,18 @@ > + + + + - - - - From 172809e4953486cf63bbd271f329754c77380a24 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 8 Sep 2021 12:53:59 +0900 Subject: [PATCH 129/261] [jsk_spot_startup] update package.xml --- jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index bd909ad0cd..f66206f447 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -21,6 +21,7 @@ rviz spot_description spot_driver + twist_mux cmd_vel_smoother xterm sound_play From 393cd093ff1619dec8a3b6cbbcaf06a64ae1ea8b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Sep 2021 02:20:24 +0900 Subject: [PATCH 130/261] [jsk_spot_startup] add use_ args --- jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch | 2 ++ 1 file changed, 2 insertions(+) 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 index 94dfb30e68..8d627deef6 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -3,6 +3,8 @@ + + Date: Fri, 10 Sep 2021 04:46:38 +0900 Subject: [PATCH 131/261] [jsk_spot_startup] update spot rviz --- .../jsk_spot_startup/config/spot.rviz | 325 +----------------- 1 file changed, 19 insertions(+), 306 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index fee9ea6df2..b98d5ee8fa 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -10,7 +10,7 @@ Panels: - /DepthClouds1/RightDepthCloud1/Autocompute Value Bounds1 - /DepthClouds1/RearDepthCloud1/Autocompute Value Bounds1 Splitter Ratio: 0.6832844614982605 - Tree Height: 370 + Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -57,10 +57,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - camera: - Alpha: 1 - Show Axes: false - Show Trail: false front_left_hip: Alpha: 1 Show Axes: false @@ -95,77 +91,18 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - jsk_head_mount_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - kinova_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - kinova_end_effector_link: - Alpha: 1 - Show Axes: false - Show Trail: false - kinova_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_gripper_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_left_finger_dist_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_left_finger_prox_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_lower_wrist_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_right_finger_dist_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_right_finger_prox_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - kinova_shoulder_link: + insta360_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - kinova_tool_frame: + jsk_head_mount_frame: Alpha: 1 Show Axes: false Show Trail: false - kinova_upper_wrist_link: + jsk_payload_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true rear_left_hip: Alpha: 1 Show Axes: false @@ -200,10 +137,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - respeaker_base: - Alpha: 1 - Show Axes: false - Show Trail: false Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -232,8 +165,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 3.2305479049682617 - Min Value: -0.3259410858154297 + Max Value: 2.072983741760254 + Min Value: 0.4096323251724243 Value: true Axis: Z Channel Name: intensity @@ -270,8 +203,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.065394163131714 - Min Value: -0.46760129928588867 + Max Value: 3.060708999633789 + Min Value: 0.31034958362579346 Value: true Axis: Z Channel Name: intensity @@ -308,8 +241,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.4258588552474976 - Min Value: -0.40428322553634644 + Max Value: 1.9569889307022095 + Min Value: 0.40128064155578613 Value: true Axis: Z Channel Name: intensity @@ -346,8 +279,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.0295515060424805 - Min Value: -0.33090418577194214 + Max Value: 2.684413433074951 + Min Value: 0.4553413391113281 Value: true Axis: Z Channel Name: intensity @@ -384,8 +317,8 @@ Visualization Manager: Value: true Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 2.5774011611938477 - Min Value: -0.597380518913269 + Max Value: 4.3172607421875 + Min Value: 0.1980423927307129 Value: true Axis: Z Channel Name: intensity @@ -419,10 +352,10 @@ Visualization Manager: Enabled: true Name: DepthClouds - Class: jsk_rviz_plugin/OverlayImage - Enabled: false + Enabled: true Name: PanoramaImage Topic: /dual_fisheye_to_panorama/output/quater - Value: false + Value: true alpha: 0.800000011920929 height: 128 keep aspect ratio: true @@ -504,224 +437,6 @@ Visualization Manager: line width: 0.05000000074505806 only edge: true show coords: false - - Class: rviz/Image - Enabled: true - Image Topic: /dual_fisheye_to_panorama/output/quater - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ElevatorDoorPoints - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /spot_recognition/elevator_door_points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - back: - Value: false - back_fisheye: - Value: false - base_link: - Value: false - body: - Value: false - camera: - Value: false - flat_body: - Value: false - front_left_hip: - Value: false - front_left_lower_leg: - Value: false - front_left_upper_leg: - Value: false - front_rail: - Value: false - front_right_hip: - Value: false - front_right_lower_leg: - Value: false - front_right_upper_leg: - Value: false - frontleft: - Value: false - frontleft_fisheye: - Value: false - frontright: - Value: false - frontright_fisheye: - Value: false - gpe: - Value: false - head: - Value: false - jsk_head_mount_frame: - Value: false - kinova_arm_link: - Value: false - kinova_base_link: - Value: false - kinova_dummy_link: - Value: false - kinova_end_effector_link: - Value: false - kinova_forearm_link: - Value: false - kinova_gripper_base_link: - Value: false - kinova_left_finger_dist_link: - Value: false - kinova_left_finger_prox_link: - Value: false - kinova_lower_wrist_link: - Value: false - kinova_right_finger_dist_link: - Value: false - kinova_right_finger_prox_link: - Value: false - kinova_shoulder_link: - Value: false - kinova_tool_frame: - Value: false - kinova_upper_wrist_link: - Value: false - left: - Value: false - left_fisheye: - Value: false - odom: - Value: false - rear_left_hip: - Value: false - rear_left_lower_leg: - Value: false - rear_left_upper_leg: - Value: false - rear_rail: - Value: false - rear_right_hip: - Value: false - rear_right_lower_leg: - Value: false - rear_right_upper_leg: - Value: false - respeaker_base: - Value: false - right: - Value: false - right_fisheye: - Value: false - vision: - Value: false - Marker Scale: 2 - Name: ElevatorFrame - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - odom: - body: - base_link: - {} - flat_body: - {} - front_left_hip: - front_left_upper_leg: - front_left_lower_leg: - {} - front_rail: - kinova_base_link: - kinova_shoulder_link: - kinova_arm_link: - kinova_forearm_link: - kinova_lower_wrist_link: - kinova_upper_wrist_link: - kinova_end_effector_link: - kinova_dummy_link: - kinova_gripper_base_link: - kinova_left_finger_prox_link: - kinova_left_finger_dist_link: - {} - kinova_right_finger_prox_link: - kinova_right_finger_dist_link: - {} - kinova_tool_frame: - {} - front_right_hip: - front_right_upper_leg: - front_right_lower_leg: - {} - head: - back: - back_fisheye: - {} - frontleft: - frontleft_fisheye: - {} - frontright: - frontright_fisheye: - {} - left: - left_fisheye: - {} - right: - right_fisheye: - {} - jsk_head_mount_frame: - camera: - {} - respeaker_base: - {} - rear_left_hip: - rear_left_upper_leg: - rear_left_lower_leg: - {} - rear_rail: - {} - rear_right_hip: - rear_right_upper_leg: - rear_right_lower_leg: - {} - vision: - {} - gpe: - {} - Update Interval: 0 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -772,13 +487,11 @@ Visualization Manager: Saved: ~ Window Geometry: Displays: - collapsed: false + collapsed: true Height: 1025 - Hide Left Dock: false + Hide Left Dock: true Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000015700000363fc0200000012fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002300000018e0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000001f2000001ae0000001600fffffffb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d006100670065000000013f000000660000000000000000fb0000000a0049006d0061006700650000000164000000940000000000000000fb0000000a0049006d00610067006500000001a3000000e80000000000000000fc00000227000001970000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000002ad0000000000000000fb0000000a0049006d00610067006503000004f5000002820000028800000157000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006230000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: From 5b60500815a7f8a7bcd8813e66194c220c63a839 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 20 Sep 2021 18:16:56 +0900 Subject: [PATCH 132/261] [.travis] add spot_ros to .travis.rosinstall.melodic --- .travis.rosinstall.melodic | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.travis.rosinstall.melodic b/.travis.rosinstall.melodic index 08bade2c35..dba5c4c361 100644 --- a/.travis.rosinstall.melodic +++ b/.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 From fc00435b6de23a0eb678e49d653da40133490e47 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 17 Sep 2021 07:53:56 -0400 Subject: [PATCH 133/261] [jsk_spot_startup] fix driver.launch for spot_ros --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index b58822bfc5..822c211617 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -6,11 +6,9 @@ - - Date: Fri, 24 Sep 2021 18:45:33 +0900 Subject: [PATCH 134/261] [jsk_spot_startup] fix bugs --- jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch | 1 + 1 file changed, 1 insertion(+) 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 index 8d627deef6..67d525c0d4 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -8,6 +8,7 @@ From 2b43bba3d7f3cf35b9cb8f3eec1a93e88d8dbcaa Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 1 Oct 2021 21:27:57 +0900 Subject: [PATCH 135/261] [jsk_spot_startup] update cable_connection_detector.py --- .../node_scripts/cable_connection_detector.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 index 0110f6ea26..3a57930605 100755 --- 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 @@ -15,8 +15,8 @@ def __init__(self): self.msg_battery_laptop = None self.sub_battery_spot = rospy.Subscriber( - '/spot/status/battery_states', - spot_msgs.msg.BatteryStateArray, + '/spot/status/power_state', + spot_msgs.msg.PowerState, self.callback_battery_spot) self.sub_battery_laptop = rospy.Subscriber( '/laptop_charge', @@ -26,7 +26,7 @@ def __init__(self): rate = rospy.Rate(1) while not rospy.is_shutdown(): rate.sleep() - if self.msg_battery_spot == None or self.msg_battery_laptop == None: + if self.msg_battery_spot == None and self.msg_battery_laptop == None: continue self.pub.publish(Bool(self.check_connection())) @@ -34,7 +34,7 @@ def check_connection(self): connected = False - if self.msg_battery_spot.battery_states[0].status == spot_msgs.msg.BatteryState.STATUS_CHARGING or\ + if self.msg_battery_spot.shore_power_state == spot_msgs.msg.PowerState.STATE_ON_SHORE_POWER or\ self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or\ self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: #? return True From 50dd8cf012cc398fa41d6e5e9c78b9bdaf9cd34a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 2 Oct 2021 18:54:49 +0900 Subject: [PATCH 136/261] [jsk_spot_startup] fix bugs of cable_connection_detector --- .../node_scripts/cable_connection_detector.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) 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 index 3a57930605..602d29926e 100755 --- 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 @@ -32,14 +32,15 @@ def __init__(self): def check_connection(self): - connected = False - - if self.msg_battery_spot.shore_power_state == spot_msgs.msg.PowerState.STATE_ON_SHORE_POWER or\ - self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or\ - self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: #? + 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 - - return connected + elif self.msg_battery_laptop is not None and\ + (self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or\ + self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING): + return True + else: + return False def callback_battery_spot(self,msg): From 31f074f406dc7d874b23584d2b6742538e55ff27 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 2 Oct 2021 19:16:58 +0900 Subject: [PATCH 137/261] [jsk_spot_startup] changed spot_credential place to be loaded to /var/lib/robot/credentials --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 2 +- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 822c211617..b7a1d0040e 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -1,5 +1,5 @@ - + 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 index 67d525c0d4..bb7f4eb8d2 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -1,7 +1,6 @@ - - - + + From 56d6de6a97ee6f32641ee01095ea5706e3d1c8c0 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 6 Oct 2021 22:51:38 +0900 Subject: [PATCH 138/261] [jsk_spot_startup] add ros_speech_recognition node to interaction.launch --- .../jsk_spot_startup/launch/include/interaction.launch | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index 41cb01114b..1eeb683850 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -15,6 +15,14 @@ + + + + + + + + Date: Mon, 25 Oct 2021 17:17:25 +0900 Subject: [PATCH 139/261] [jsk_spot_startup] add battery temperature warning --- .../node_scripts/battery_notifier.py | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) 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 index 60f6b8901b..45c053913a 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -14,7 +14,9 @@ class SpotBatteryNotifier(object): def __init__(self): self._battery_spot = 0 + self._battery_temperature = 0 self._battery_laptop = 0 + self.last_warn_bat_temp_time = rospy.get_time() self._sub_spot = rospy.Subscriber( '/spot/status/battery_states', @@ -33,6 +35,8 @@ def __init__(self): ) threshold_warning_spot = float(rospy.get_param('~threshold_warning_spot', 20)) + threshold_warning_battery_temperature =\ + float(rospy.get_param('~threshold_warning_battery_temperature', 45)) threshold_warning_laptop = float(rospy.get_param('~threshold_warning_laptop', 20)) threshold_estop_spot = float(rospy.get_param('~threshold_estop_spot', 5)) @@ -55,11 +59,27 @@ def __init__(self): spot_client.estop_gentle() spot_client.estop_hard() + 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:{}C'\ + .format(self._battery_temperature_spot)) + 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_laptop(self, msg): self._battery_laptop = msg.percentage + +def main(): + + rospy.init_node('battery_notifier') + battery_notifier = SpotBatteryNotifier() + rospy.spin() + +if __name__=='__main_': + main() From c561f0bc97bc18592d8c412da7cd44961f2f8ebd Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Mon, 25 Oct 2021 19:37:17 +0900 Subject: [PATCH 140/261] [jsk_spot_robot] fix typo __main_ -> __main__ and change error messages --- .../jsk_spot_startup/node_scripts/battery_notifier.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 index 45c053913a..2130d1407f 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -59,10 +59,10 @@ def __init__(self): spot_client.estop_gentle() spot_client.estop_hard() - if self._battery_temperature < threshold_warning_battery_temperature\ + 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:{}C'\ - .format(self._battery_temperature_spot)) + 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() @@ -81,5 +81,5 @@ def main(): battery_notifier = SpotBatteryNotifier() rospy.spin() -if __name__=='__main_': +if __name__=='__main__': main() From e4578ee1ec4941ac0717c382791298cba889b81e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 28 Oct 2021 03:35:35 +0900 Subject: [PATCH 141/261] [jsk_spot_startup] suppress battery warning --- .../node_scripts/battery_notifier.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) 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 index 2130d1407f..c236c19bdf 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -13,9 +13,9 @@ class SpotBatteryNotifier(object): def __init__(self): - self._battery_spot = 0 + self._battery_spot = None + self._battery_laptop = None self._battery_temperature = 0 - self._battery_laptop = 0 self.last_warn_bat_temp_time = rospy.get_time() self._sub_spot = rospy.Subscriber( @@ -47,17 +47,16 @@ def __init__(self): rate.sleep() - if self._battery_spot < threshold_warning_spot or\ - self._battery_laptop < threshold_warning_laptop: - rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format(self._battery_spot,self._battery_laptop)) - sound_client.say('バッテリー残量が少ないです。') - - if self._battery_spot < threshold_estop_spot or\ - self._battery_laptop < threshold_estop_laptop: + if ((self._battery_spot is not None and self._battery_spot < threshold_estop_spot) + or (self._battery_laptop is not None and self._battery_laptop < threshold_estop_laptop)): rospy.logerr('Battery is low. Estop.') sound_client.say('バッテリー残量が少ないため、動作を停止します') spot_client.estop_gentle() spot_client.estop_hard() + elif ((self._battery_spot is not None and self._battery_spot < threshold_warning_spot) + or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): + rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format(self._battery_spot,self._battery_laptop)) + sound_client.say('バッテリー残量が少ないです。') if self._battery_temperature > threshold_warning_battery_temperature\ and (rospy.get_time() - self.last_warn_bat_temp_time) > 180: From 502459f3f4a361f15ad95fdb73e1f9bd46edeb6d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 29 Oct 2021 11:41:23 +0900 Subject: [PATCH 142/261] [jsk_spot_startup] add plug_server to driver.launch --- .../jsk_spot_startup/launch/include/driver.launch | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index b7a1d0040e..b0f1ec517d 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -74,4 +74,11 @@ > + + + From ef98fcfb02e58118e2027f11b98ba51d28bca8af Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Wed, 24 Nov 2021 21:57:56 +0900 Subject: [PATCH 143/261] [jsk_spot_startup] use voice_text as default --- .../jsk_spot_startup/launch/include/interaction.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index 1eeb683850..074bff6a67 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -10,7 +10,7 @@ - + From 8ece0e05070b3fdb5a62c8eb5465cbac2cc25e58 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 26 Nov 2021 12:38:46 +0900 Subject: [PATCH 144/261] [jsk_spot_startup] update package.xml and CMakeLists.txt --- .../jsk_spot_startup/CMakeLists.txt | 13 ++++++++-- jsk_spot_robot/jsk_spot_startup/package.xml | 25 ++++++++++++++----- 2 files changed, 30 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index 1d8558f3b5..9df4cbf3f3 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 2.8.3) project(jsk_spot_startup) find_package(catkin REQUIRED) @@ -13,11 +13,20 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY config launch +install(DIRECTORY auth config launch scripts services udev_rules template DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) +catkin_install_python(PROGRAMS + node_scripts/battery_notifier.py + node_scripts/battery_visualizer.py + node_scripts/cable_connection_detector.py + node_scripts/laptop_battery_visualizer.py + node_scripts/static_tf_republisher.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + ############# ## Testing ## diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index f66206f447..01f738273e 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -15,19 +15,32 @@ roslaunch roslint - laptop_battery_monitor + app_scheduler + aques_talk + audio_capture + cmd_vel_smoother + 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 roslaunch + rosserial_python rviz + sound_play spot_description spot_driver + spoteus + topic_tools twist_mux - cmd_vel_smoother + voice_text + xacro xterm - sound_play - jsk_perception - aques_talk - spoteus rostest From 231c4fa399cd12c6e2a8a8e6f40f94b5cab4aa1a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 1 Dec 2021 18:57:17 +0900 Subject: [PATCH 145/261] [jsk_spot_startup] add battery perceptage speech --- .../jsk_spot_startup/node_scripts/battery_notifier.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index c236c19bdf..063def04f0 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -56,7 +56,7 @@ def __init__(self): elif ((self._battery_spot is not None and self._battery_spot < threshold_warning_spot) or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format(self._battery_spot,self._battery_laptop)) - sound_client.say('バッテリー残量が少ないです。') + sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format(self._battery_spot,self._battery_laptop)) if self._battery_temperature > threshold_warning_battery_temperature\ and (rospy.get_time() - self.last_warn_bat_temp_time) > 180: From 7d21dc9d58e83e7c1e71aff8d2f3641265a29385 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 3 Dec 2021 19:29:07 +0900 Subject: [PATCH 146/261] [jsk_spot_startup] fix format for python --- .../node_scripts/battery_notifier.py | 46 +++++++++++-------- .../node_scripts/battery_visualizer.py | 13 ++++-- .../node_scripts/cable_connection_detector.py | 26 ++++++----- .../node_scripts/laptop_battery_visualizer.py | 12 +++-- .../node_scripts/static_tf_republisher.py | 15 +++--- 5 files changed, 67 insertions(+), 45 deletions(-) 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 index 063def04f0..976461a0ce 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -19,28 +19,32 @@ def __init__(self): self.last_warn_bat_temp_time = rospy.get_time() self._sub_spot = rospy.Subscriber( - '/spot/status/battery_states', - BatteryStateArray, - self._cb_spot ) + '/spot/status/battery_states', + BatteryStateArray, + self._cb_spot) self._sub_laptop = rospy.Subscriber( - '/laptop_charge', - BatteryState, - self._cb_laptop ) + '/laptop_charge', + BatteryState, + self._cb_laptop) spot_client = SpotRosClient() sound_client = SoundClient( - blocking=False, - sound_action='/robotsound_jp', - sound_topic='/robotsound_jp' - ) + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) - threshold_warning_spot = float(rospy.get_param('~threshold_warning_spot', 20)) + threshold_warning_spot = float( + rospy.get_param('~threshold_warning_spot', 20)) threshold_warning_battery_temperature =\ - float(rospy.get_param('~threshold_warning_battery_temperature', 45)) - threshold_warning_laptop = float(rospy.get_param('~threshold_warning_laptop', 20)) + float(rospy.get_param('~threshold_warning_battery_temperature', 45)) + threshold_warning_laptop = float( + rospy.get_param('~threshold_warning_laptop', 20)) - threshold_estop_spot = float(rospy.get_param('~threshold_estop_spot', 5)) - threshold_estop_laptop = float(rospy.get_param('~threshold_estop_laptop', 5)) + threshold_estop_spot = float( + rospy.get_param('~threshold_estop_spot', 5)) + threshold_estop_laptop = float( + rospy.get_param('~threshold_estop_laptop', 5)) rate = rospy.Rate(0.1) while not rospy.is_shutdown(): @@ -55,12 +59,14 @@ def __init__(self): spot_client.estop_hard() elif ((self._battery_spot is not None and self._battery_spot < threshold_warning_spot) or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): - rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format(self._battery_spot,self._battery_laptop)) - sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format(self._battery_spot,self._battery_laptop)) + rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format( + self._battery_spot, self._battery_laptop)) + sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format( + self._battery_spot, self._battery_laptop)) 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:{}'\ + 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() @@ -74,11 +80,13 @@ def _cb_laptop(self, msg): self._battery_laptop = msg.percentage + def main(): rospy.init_node('battery_notifier') battery_notifier = SpotBatteryNotifier() rospy.spin() -if __name__=='__main__': + +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 index 673e45fd9e..95be4670c5 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_visualizer.py @@ -4,14 +4,17 @@ from spot_msgs.msg import BatteryStateArray from std_msgs.msg import Float32 + class SpotBatteryVisualizer(object): def __init__(self): - rospy.init_node( 'spot_battery_visualizer' ) + 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 ) + 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): @@ -19,10 +22,12 @@ def _cb(self, msg): pub_msg.data = msg.battery_states[0].charge_percentage self._pub.publish(pub_msg) + def main(): battery_visualizer = SpotBatteryVisualizer() rospy.spin() -if __name__=='__main__': + +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 index 602d29926e..12ffd7dd06 100755 --- 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 @@ -5,23 +5,25 @@ 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.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) + '/spot/status/power_state', + spot_msgs.msg.PowerState, + self.callback_battery_spot) self.sub_battery_laptop = rospy.Subscriber( - '/laptop_charge', - sensor_msgs.msg.BatteryState, - self.callback_battery_laptop) + '/laptop_charge', + sensor_msgs.msg.BatteryState, + self.callback_battery_laptop) rate = rospy.Rate(1) while not rospy.is_shutdown(): @@ -33,20 +35,20 @@ def __init__(self): 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: + self.msg_battery_spot.shore_power_state == spot_msgs.msg.PowerState.STATE_ON_SHORE_POWER: return True elif self.msg_battery_laptop is not None and\ - (self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or\ - self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING): + (self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or + self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING): return True else: return False - def callback_battery_spot(self,msg): + def callback_battery_spot(self, msg): self.msg_battery_spot = msg - def callback_battery_laptop(self,msg): + def callback_battery_laptop(self, msg): self.msg_battery_laptop = msg 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 index d2f6527411..2969614532 100755 --- 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 @@ -4,14 +4,16 @@ from sensor_msgs.msg import BatteryState from std_msgs.msg import Float32 + class LaptopBatteryVisualizer(object): def __init__(self): - rospy.init_node( 'spot_battery_visualizer' ) + 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 ) + 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): @@ -19,10 +21,12 @@ def _cb(self, msg): pub_msg.data = msg.percentage self._pub.publish(pub_msg) + def main(): battery_visualizer = LaptopBatteryVisualizer() rospy.spin() -if __name__=='__main__': + +if __name__ == '__main__': main() 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 index 3e11531bb3..ff5363d6c0 100755 --- 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 @@ -10,9 +10,10 @@ from geometry_msgs.msg import TransformStamped from tf2_msgs.msg import TFMessage + def main(): - rospy.init_node( 'static_tf_republisher' ) + rospy.init_node('static_tf_republisher') topicname = '/tf_static' @@ -26,14 +27,16 @@ def main(): list_messages = [] transforms = [] - with rosbag.Bag( bagfilename, 'r' ) as inputbag: - for topic, msg, t in inputbag.read_messages( '/tf_static' ): - transforms.extend( msg.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))) + rospy.loginfo( + 'republish /tf_static with {} TransformStamped messages'.format(len(transforms))) broadcaster.sendTransform(transforms) rospy.spin() -if __name__=='__main__': + +if __name__ == '__main__': main() From 648150dc81b7b85b325525cdc5d4cddd8eaf0b74 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 3 Dec 2021 19:40:04 +0900 Subject: [PATCH 147/261] [jsk_spot_startup] go back to dock when battery low --- .../node_scripts/battery_notifier.py | 40 ++++++++++++++++++- 1 file changed, 38 insertions(+), 2 deletions(-) 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 index 976461a0ce..3e9c0fa444 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -4,6 +4,8 @@ import rospy from spot_msgs.msg import BatteryStateArray from sensor_msgs.msg import BatteryState +from app_manager.srv import StartApp +from app_manager.srv import StartAppRequest from spot_ros_client.libspotros import SpotRosClient from sound_play.libsoundplay import SoundClient @@ -18,6 +20,8 @@ def __init__(self): self._battery_temperature = 0 self.last_warn_bat_temp_time = rospy.get_time() + self._srvclient_start_app = rospy.ServiceProxy('~start_app', StartApp) + self._sub_spot = rospy.Subscriber( '/spot/status/battery_states', BatteryStateArray, @@ -34,13 +38,19 @@ def __init__(self): sound_topic='/robotsound_jp' ) - threshold_warning_spot = float( - rospy.get_param('~threshold_warning_spot', 20)) 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_warning_laptop = float( rospy.get_param('~threshold_warning_laptop', 20)) + threshold_return_spot = float( + rospy.get_param('~threshold_return_spot', 15)) + threshold_return_laptop = float( + rospy.get_param('~threshold_return_laptop', 15)) + threshold_estop_spot = float( rospy.get_param('~threshold_estop_spot', 5)) threshold_estop_laptop = float( @@ -57,6 +67,13 @@ def __init__(self): sound_client.say('バッテリー残量が少ないため、動作を停止します') spot_client.estop_gentle() spot_client.estop_hard() + + elif ((self._battery_spot is not None and self._battery_spot < threshold_return_spot) + or (self._battery_laptop is not None and self._battery_laptop < threshold_return_laptop)): + 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) or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format( @@ -80,6 +97,25 @@ def _cb_laptop(self, msg): self._battery_laptop = msg.percentage + def _call_go_back_home(self): + + try: + rospy.wait_for_service('~start_app', timeout=rospy.Duration(1)) + except rospy.exceptions.ROSException as e: + rospy.logerr( + 'Could not call \'go_back_home\' demo. : {}'.format(e)) + return False + + req = StartAppRequest() + req.name = 'jsk_spot_apps/go_back_home' + res = self._srvclient_start_app(req) + if not res.started: + rospy.logerr('Could not start \'go_back_home\' demo.') + return False + else: + rospy.loginfo('Successfully start \'go_back_home\' demo.') + return True + def main(): From 29ee7f3c041d3d14b890ebd02058ba00f3cca487 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Dec 2021 10:58:18 +0900 Subject: [PATCH 148/261] [jsk_spot_startup] update battery_notifier --- .../node_scripts/battery_notifier.py | 50 ++++++++++++------- 1 file changed, 31 insertions(+), 19 deletions(-) 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 index 3e9c0fa444..723cb2ae32 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -2,6 +2,7 @@ # -*- encoding: utf-8 -*- import rospy +from std_msgs.msg import Bool from spot_msgs.msg import BatteryStateArray from sensor_msgs.msg import BatteryState from app_manager.srv import StartApp @@ -18,6 +19,7 @@ def __init__(self): self._battery_spot = None self._battery_laptop = None self._battery_temperature = 0 + self._is_connected = False self.last_warn_bat_temp_time = rospy.get_time() self._srvclient_start_app = rospy.ServiceProxy('~start_app', StartApp) @@ -30,6 +32,10 @@ def __init__(self): '/laptop_charge', BatteryState, self._cb_laptop) + self._sub_connected = rospy.Subscriber( + '/spot/status/cable_connected', + Bool, + self._cb_connected) spot_client = SpotRosClient() sound_client = SoundClient( @@ -61,25 +67,27 @@ def __init__(self): rate.sleep() - if ((self._battery_spot is not None and self._battery_spot < threshold_estop_spot) - or (self._battery_laptop is not None and self._battery_laptop < threshold_estop_laptop)): - rospy.logerr('Battery is low. Estop.') - sound_client.say('バッテリー残量が少ないため、動作を停止します') - spot_client.estop_gentle() - spot_client.estop_hard() - - elif ((self._battery_spot is not None and self._battery_spot < threshold_return_spot) - or (self._battery_laptop is not None and self._battery_laptop < threshold_return_laptop)): - 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) - or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): - rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format( - self._battery_spot, self._battery_laptop)) - sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format( - self._battery_spot, self._battery_laptop)) + if not self._is_connected: + + if ((self._battery_spot is not None and self._battery_spot < threshold_estop_spot) + or (self._battery_laptop is not None and self._battery_laptop < threshold_estop_laptop)): + rospy.logerr('Battery is low. Estop.') + sound_client.say('バッテリー残量が少ないため、動作を停止します') + spot_client.estop_gentle() + spot_client.estop_hard() + + elif ((self._battery_spot is not None and self._battery_spot < threshold_return_spot) + or (self._battery_laptop is not None and self._battery_laptop < threshold_return_laptop)): + 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) + or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): + rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format( + self._battery_spot, self._battery_laptop)) + sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format( + self._battery_spot, self._battery_laptop)) if self._battery_temperature > threshold_warning_battery_temperature\ and (rospy.get_time() - self.last_warn_bat_temp_time) > 180: @@ -97,6 +105,10 @@ def _cb_laptop(self, msg): self._battery_laptop = msg.percentage + def _cb_connected(self, msg): + + self._is_connected = msg.data + def _call_go_back_home(self): try: From ea8aa1a849dece2fd5aa9180c414ed6a3786a63b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Dec 2021 11:05:30 +0900 Subject: [PATCH 149/261] [jsk_spot_startup] remove quatered panorama image --- .../jsk_spot_startup/config/spot.rviz | 2 +- .../launch/rosbag_play.launch | 2 +- .../launch/rosbag_record.launch | 78 +++++++++++++++++++ 3 files changed, 80 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz index b98d5ee8fa..c732bf2766 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/spot.rviz +++ b/jsk_spot_robot/jsk_spot_startup/config/spot.rviz @@ -354,7 +354,7 @@ Visualization Manager: - Class: jsk_rviz_plugin/OverlayImage Enabled: true Name: PanoramaImage - Topic: /dual_fisheye_to_panorama/output/quater + Topic: /dual_fisheye_to_panorama/output Value: true alpha: 0.800000011920929 height: 128 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch index c1e514865c..d3e8eee15a 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_play.launch @@ -126,7 +126,7 @@ type="draw_rects" name="object_detection_visualizer" > - + diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch index 4fc6c0a804..efcced70c5 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch @@ -88,6 +88,84 @@ " output="screen" /> + + + + + + + + + + + + + + + + + + + + device: $(optenv DEVICE_SPEAKER default) + + + + device: $(optenv DEVICE_SPEAKER default) + + + + + + - + From 457906cabaaa2498da67df424477a838079007ed Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Tue, 7 Dec 2021 20:54:16 +0900 Subject: [PATCH 151/261] [spoteus] add sample script of :go-to-spot --- .../spoteus/demo/sample_go_to_spot.l | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100755 jsk_spot_robot/spoteus/demo/sample_go_to_spot.l 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) From 169a8b17ba6502c4b0bf21a30b1e9467b599f984 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 17 Dec 2021 13:39:14 +0900 Subject: [PATCH 152/261] [jsk_spot_startup] fix battery notifier remapping --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index b0f1ec517d..bd11246375 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -72,6 +72,7 @@ type="battery_notifier.py" name="battery_notifier" > + Date: Tue, 27 Sep 2022 01:42:36 -0400 Subject: [PATCH 153/261] spoteus/CMakeLists.txt: use collada2eus to generate lisp from urdf, not dae for Noetic, see https://github.com/ros/collada_urdf/issues/43 --- jsk_spot_robot/spoteus/CMakeLists.txt | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt index 0b7e955eec..2d49a15d63 100644 --- a/jsk_spot_robot/spoteus/CMakeLists.txt +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -39,17 +39,11 @@ 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) -if(NOT EXISTS ${PROJECT_SOURCE_DIR}/spot.l) - add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.dae - COMMAND echo "${_urdf_to_collada} ${PROJECT_SOURCE_DIR}/spot.urdf spot.dae" - COMMAND ${_urdf_to_collada} ${PROJECT_SOURCE_DIR}/spot.urdf ${PROJECT_SOURCE_DIR}/spot.dae - DEPENDS ${PROJECT_SOURCE_DIR}/spot.urdf) - add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.l - COMMAND echo "${_collada2eus} spot.dae spot.l" - COMMAND ${_collada2eus} ${PROJECT_SOURCE_DIR}/spot.dae ${PROJECT_SOURCE_DIR}/spot.yaml ${PROJECT_SOURCE_DIR}/spot.l - DEPENDS ${PROJECT_SOURCE_DIR}/spot.dae ${PROJECT_SOURCE_DIR}/spot.yaml ${_collada2eus}) - add_custom_target(compile_spot ALL DEPENDS ${PROJECT_SOURCE_DIR}/spot.l) -endif() +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 From dbbed9765921842ef91c06ebfa6908d5ee67e796 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 27 Sep 2022 01:43:37 -0400 Subject: [PATCH 154/261] jsk_spot_robot/requirements.txt: update bosdyn to 3.2.0 --- jsk_spot_robot/requirements.txt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/requirements.txt b/jsk_spot_robot/requirements.txt index 91a8d9c71d..6306e82a90 100644 --- a/jsk_spot_robot/requirements.txt +++ b/jsk_spot_robot/requirements.txt @@ -1,4 +1,6 @@ -bosdyn-client == 2.3.4 -bosdyn-mission == 2.3.4 -bosdyn-api == 2.3.4 -bosdyn-core == 2.3.4 +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 From 33f4595c602dbb7ff5096ef1cbe440eadf18b301 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 27 Sep 2022 01:49:18 -0400 Subject: [PATCH 155/261] spoteus: use https://github.com/cst0/spot_ros for arm-spot, update spot.yml, add interface to spot-interface.l --- jsk_spot_robot/spoteus/spot-interface.l | 44 ++++++++++++++++++- jsk_spot_robot/spoteus/spot-utils.l | 18 +++++++- jsk_spot_robot/spoteus/spot.yaml | 56 +++++++++++++++---------- 3 files changed, 94 insertions(+), 24 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 4e35b294a0..3c76c76b28 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -6,6 +6,7 @@ (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" @@ -68,7 +69,48 @@ trajectory-cmd-action-name spot_msgs::TrajectoryAction :groupname groupname)) )) - (:default-controller () ) ;; spot does not provide any JTA controllers + ;; (: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")) + ;; (: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)) diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l index f35efae143..fab2f6e6e7 100644 --- a/jsk_spot_robot/spoteus/spot-utils.l +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -17,7 +17,21 @@ :_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.0 1.0 1.0 0.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 @@ -26,7 +40,7 @@ :_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.0 1.0 1.0 0.0)) + '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000)) (assoc b (send (class self) :methods))))) ) (send* self :init-orig args)) diff --git a/jsk_spot_robot/spoteus/spot.yaml b/jsk_spot_robot/spoteus/spot.yaml index 4a5472962f..63634f430f 100644 --- a/jsk_spot_robot/spoteus/spot.yaml +++ b/jsk_spot_robot/spoteus/spot.yaml @@ -2,39 +2,53 @@ ## - collada_joint_name : euslisp_joint_name (start with :) ## -larm: - - front_left_hip_x : larm-shoulder-r - - front_left_hip_y : larm-shoulder-p - - front_left_knee : larm-elbow-p -rarm: - - front_right_hip_x : rarm-shoulder-r - - front_right_hip_y : rarm-shoulder-p - - front_right_knee : rarm-elbow-p -lleg: - - rear_left_hip_x : lleg-crotch-r - - rear_left_hip_y : lleg-crotch-p - - rear_left_knee : lleg-knee-p -rleg: - - rear_right_hip_x : rleg-crotch-r - - rear_right_hip_y : rleg-crotch-p - - rear_right_knee : rleg-knee-p +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] + 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] -larm-end-coords: +lfleg-end-coords: parent : front_left_lower_leg translate : [0, 0, -0.38] rotate : [0, 1, 0, 0] -rarm-end-coords: +rfleg-end-coords: parent : front_right_lower_leg translate : [0, 0, -0.38] rotate : [0, 1, 0, 0] -lleg-end-coords: +lrleg-end-coords: parent : rear_left_lower_leg translate : [0, 0, -0.38] rotate : [0, 1, 0, 0] -rleg-end-coords: +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, 90] From f73131dc6537f6f01057acd7cb039b0c8937a1a6 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 23 Sep 2021 10:05:07 +0900 Subject: [PATCH 156/261] [jsk_spot_robot] update rosinstall --- jsk_spot_robot/jsk_spot.rosinstall | 7 ------- jsk_spot_robot/jsk_spot_driver.rosinstall | 12 ++++++++++++ 2 files changed, 12 insertions(+), 7 deletions(-) delete mode 100644 jsk_spot_robot/jsk_spot.rosinstall create mode 100644 jsk_spot_robot/jsk_spot_driver.rosinstall diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall deleted file mode 100644 index 8e86614f5d..0000000000 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ /dev/null @@ -1,7 +0,0 @@ -# spot-ros is required for spot_msgs -# 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 diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall new file mode 100644 index 0000000000..9ab2163cc0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -0,0 +1,12 @@ +# 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-rebased +# geometry2 have to be built for python3 +- git: + local-name: geometry2 + uri: https://github.com/ros/geometry2.git + version: 0.6.5 From 5cea46737b130961f82748b16298cd52d8c2fd8e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 27 Sep 2022 15:06:44 +0900 Subject: [PATCH 157/261] update to use https://github.com/cst0/spot_ros, which has better arm interfaces --- jsk_spot_robot/jsk_spot_driver.rosinstall | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 9ab2163cc0..23b1c6b91f 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -1,12 +1,28 @@ # 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/sktometometo/spot_ros.git - version: develop/spot-rebased + uri: https://github.com/k-okada/spot_ros.git + version: effea91835cf11411854f472c0ed5f00bf6d87b4 +# # 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 From fbf4000ebf6b5a1f1797b4421e05316f18e3366a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Sep 2022 21:28:56 -0400 Subject: [PATCH 158/261] [spoteus/spot-interface.l] support /spot/status/manipulator_state --- jsk_spot_robot/spoteus/spot-interface.l | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 3c76c76b28..218e960ea2 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -60,6 +60,7 @@ (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) @@ -155,6 +156,26 @@ (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 @@ -235,6 +256,7 @@ ;; (: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))) From f1af28ec156b88a04845f8cb38b3cf11a24ba5e8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Sep 2022 21:50:30 -0400 Subject: [PATCH 159/261] jsk_spot_startup/launch/jsk_spot_bringup.launch: add launch_interaction and launch_peripheral args --- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 index bb7f4eb8d2..63cb755609 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -1,6 +1,8 @@ + + @@ -14,13 +16,11 @@ - + - + From 2b81e2e1930671b9e93393c802d55a7323eb7dd7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Sep 2022 21:53:03 -0400 Subject: [PATCH 160/261] update jsk_spot_startup/launch/include/driver.launch to support teleop add teleop node to jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch update config/twist_mux.yaml for mux head_teleop and joy_teleop add image compressed node add teleop node to jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch update package.xml for image compresed node --- .../jsk_spot_startup/config/twist_mux.yaml | 11 ++--- .../launch/include/driver.launch | 41 +++++++++++++++++++ jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml index 5595597ef3..f5df2a4c8c 100644 --- a/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml +++ b/jsk_spot_robot/jsk_spot_startup/config/twist_mux.yaml @@ -1,18 +1,13 @@ topics: +# +# priority 9: high <--> 1: low +# - name : bt_joy topic : bluetooth_teleop/cmd_vel timeout : 0.5 priority: 9 -- name : interactive_marker - topic : twist_marker_server/cmd_vel - timeout : 0.5 - priority: 8 - name : head_joy topic : joy_head/cmd_vel timeout : 0.5 priority: 7 -- name : develop_input - topic : develop_input/cmd_vel - timeout : 0.5 - priority: 6 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 index bd11246375..eb3a36963f 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -41,6 +41,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + aques_talk audio_capture cmd_vel_smoother + compressed_image_transport image_transport interactive_marker_twist_server jsk_perception From 78e33b837f7fb3d57b6d3faa4f5c7e19124cd24a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Sep 2022 21:56:15 -0400 Subject: [PATCH 161/261] jsk_spot_robot/jsk_spot_driver.rosinstall: add jsk_mode_tools with custom limb support --- jsk_spot_robot/jsk_spot_driver.rosinstall | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 23b1c6b91f..c5934b6ea9 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -15,6 +15,11 @@ local-name: spot-ros uri: https://github.com/k-okada/spot_ros.git version: effea91835cf11411854f472c0ed5f00bf6d87b4 +## 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: From 142f2911c650c83637a271ecb15ae921fdd5f68d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 14 Oct 2022 23:13:00 -0400 Subject: [PATCH 162/261] jsk_robot_common/jsk_robot_startup/config/dualsense_joystick_teleop.yaml: # tuck/untuck need 'Unlock button' --- .../jsk_robot_startup/config/dualsense_joystick_teleop.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/config/dualsense_joystick_teleop.yaml b/jsk_robot_common/jsk_robot_startup/config/dualsense_joystick_teleop.yaml index 53057ec19d..b9a1916568 100644 --- a/jsk_robot_common/jsk_robot_startup/config/dualsense_joystick_teleop.yaml +++ b/jsk_robot_common/jsk_robot_startup/config/dualsense_joystick_teleop.yaml @@ -11,4 +11,7 @@ button_stair_mode: 3 button_locomotion_mode: -1 num_buttons: 14 axe_dock: 7 +# tuck/untuck need "Unlock button" +button_tuck: 4 +axe_tuck: 6 button_enable: 4 \ No newline at end of file From 4e8b24c7ee1f1700c153390158ef9a51c8ad5960 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 19 Oct 2022 23:59:12 -0400 Subject: [PATCH 163/261] [jsk_spot_startup] add apps --- .../jsk_spot_startup/apps/app.installed | 6 ++ .../apps/head_lead_demo/head-lead-teleop.l | 73 +++++++++++++++++ .../apps/head_lead_demo/head_lead_demo.app | 5 ++ .../head_lead_demo/head_lead_demo.interface | 3 + .../apps/head_lead_demo/head_lead_demo.xml | 22 ++++++ .../apps/hello_world/hello-world.l | 78 +++++++++++++++++++ .../apps/hello_world/hello_world.app | 5 ++ .../apps/hello_world/hello_world.interface | 3 + .../apps/hello_world/hello_world.xml | 6 ++ jsk_spot_robot/jsk_spot_startup/package.xml | 5 ++ 10 files changed, 206 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/app.installed create mode 100755 jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.app create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.interface create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml create mode 100755 jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello-world.l create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.app create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.interface create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/hello_world/hello_world.xml 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..2bf50ef955 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/app.installed @@ -0,0 +1,6 @@ +apps: +- app: jsk_spot_startup/hello_world + dispay: Hello World +- app: jsk_spot_startup/head_lead_demo + dispay: Head Lead Demo + \ No newline at end of file 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..6d51a49827 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l @@ -0,0 +1,73 @@ +#!/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)) +;; (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)) + +(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") + (return-from :start-func :finished)) + (send *ri* :speak "Hello, let's start walking") + (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) 2000 :default-controller) + (send *ri* :wait-interpolation) + (send *ri* :set-impedance-param :linear-stiffness #f(250 50 50) :rotational-stiffness #f(10 10 30) :linear-damping #f(1.5 1.5 1.5) :rotational-damping #f(0.5 0.5 0.5)) + :started) + +(defun end-func (args) + (send *ri* :speak "Thank You") + (send *ri* :stow-arm) + :finished) + +(defun walk-func (args) + (ros::rate 0.1) + (do-until-key + (if (null *continue*) (return-from walk-func :finished)) + (ros::sleep)) + :finished) +;; +(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) + (:end :finished :goal) + ) + '((:start 'start-func) ;; node-to-function maps + (:end 'end-func) + (:walk 'walk-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 (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..b9e600afdd --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + 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/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index 0153308949..fefef06d3d 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -15,6 +15,7 @@ roslaunch roslint + app_manager app_scheduler aques_talk audio_capture @@ -30,9 +31,12 @@ robot_state_publisher ros_speech_recognition rosbag + roseus + roseus_smach roslaunch rosserial_python rviz + rwt_app_chooser sound_play spot_description spot_driver @@ -46,5 +50,6 @@ rostest + From 1db780d913501c0fcc2ee799adf9549d08bb391e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:01:51 -0400 Subject: [PATCH 164/261] [quadruped_joystick_teleop.cpp] show response messages too --- .../src/quadruped_joystick_teleop.cpp | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) 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; } From 60cec34f363424fc238f93295d7fff614a9f8499 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:02:35 -0400 Subject: [PATCH 165/261] [jsk_spot_driver.rosinstall] add robot_upstart, aques_talk, app_manager --- jsk_spot_robot/jsk_spot_driver.rosinstall | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index c5934b6ea9..3b5041dd4a 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -31,3 +31,22 @@ local-name: vision_opencv uri: https://github.com/ros-perception/vision_opencv.git version: 1.13.0 +# 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_atlk + 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 From d30eb9ab39e8f696c7c25bdd1132abd1d7e262ef Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:04:41 -0400 Subject: [PATCH 166/261] jsk_spot_startup/{battery_notifier.py, /cable_connection_detector.py] remove laptop battery status --- .../node_scripts/battery_notifier.py | 76 ++++++++----------- .../node_scripts/cable_connection_detector.py | 11 --- 2 files changed, 30 insertions(+), 57 deletions(-) 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 index 723cb2ae32..e349832879 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -3,12 +3,10 @@ import rospy from std_msgs.msg import Bool +from std_srvs.srv import Trigger from spot_msgs.msg import BatteryStateArray from sensor_msgs.msg import BatteryState -from app_manager.srv import StartApp -from app_manager.srv import StartAppRequest -from spot_ros_client.libspotros import SpotRosClient from sound_play.libsoundplay import SoundClient @@ -17,27 +15,19 @@ class SpotBatteryNotifier(object): def __init__(self): self._battery_spot = None - self._battery_laptop = None self._battery_temperature = 0 self._is_connected = False self.last_warn_bat_temp_time = rospy.get_time() - self._srvclient_start_app = rospy.ServiceProxy('~start_app', StartApp) - self._sub_spot = rospy.Subscriber( '/spot/status/battery_states', BatteryStateArray, self._cb_spot) - self._sub_laptop = rospy.Subscriber( - '/laptop_charge', - BatteryState, - self._cb_laptop) self._sub_connected = rospy.Subscriber( '/spot/status/cable_connected', Bool, self._cb_connected) - spot_client = SpotRosClient() sound_client = SoundClient( blocking=False, sound_action='/robotsound_jp', @@ -49,18 +39,12 @@ def __init__(self): threshold_warning_spot = float( rospy.get_param('~threshold_warning_spot', 20)) - threshold_warning_laptop = float( - rospy.get_param('~threshold_warning_laptop', 20)) threshold_return_spot = float( rospy.get_param('~threshold_return_spot', 15)) - threshold_return_laptop = float( - rospy.get_param('~threshold_return_laptop', 15)) threshold_estop_spot = float( rospy.get_param('~threshold_estop_spot', 5)) - threshold_estop_laptop = float( - rospy.get_param('~threshold_estop_laptop', 5)) rate = rospy.Rate(0.1) while not rospy.is_shutdown(): @@ -69,25 +53,21 @@ def __init__(self): if not self._is_connected: - if ((self._battery_spot is not None and self._battery_spot < threshold_estop_spot) - or (self._battery_laptop is not None and self._battery_laptop < threshold_estop_laptop)): + if (self._battery_spot is not None and self._battery_spot < threshold_estop_spot): rospy.logerr('Battery is low. Estop.') sound_client.say('バッテリー残量が少ないため、動作を停止します') spot_client.estop_gentle() spot_client.estop_hard() - elif ((self._battery_spot is not None and self._battery_spot < threshold_return_spot) - or (self._battery_laptop is not None and self._battery_laptop < threshold_return_laptop)): + 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) - or (self._battery_laptop is not None and self._battery_laptop < threshold_warning_laptop)): - rospy.logwarn('Battery is low. Spot: {}, Laptop: {}'.format( - self._battery_spot, self._battery_laptop)) - sound_client.say('バッテリー残量が少ないです。本体が{}パーセント、ラップトップが{}パーセントです。'.format( - self._battery_spot, self._battery_laptop)) + 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: @@ -101,32 +81,36 @@ 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_laptop(self, msg): - - self._battery_laptop = msg.percentage - 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: - rospy.wait_for_service('~start_app', timeout=rospy.Duration(1)) - except rospy.exceptions.ROSException as e: - rospy.logerr( - 'Could not call \'go_back_home\' demo. : {}'.format(e)) - return False - - req = StartAppRequest() - req.name = 'jsk_spot_apps/go_back_home' - res = self._srvclient_start_app(req) - if not res.started: - rospy.logerr('Could not start \'go_back_home\' demo.') - return False - else: - rospy.loginfo('Successfully start \'go_back_home\' demo.') - return True + 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(): 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 index 12ffd7dd06..606c5abf36 100755 --- 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 @@ -20,10 +20,6 @@ def __init__(self): '/spot/status/power_state', spot_msgs.msg.PowerState, self.callback_battery_spot) - self.sub_battery_laptop = rospy.Subscriber( - '/laptop_charge', - sensor_msgs.msg.BatteryState, - self.callback_battery_laptop) rate = rospy.Rate(1) while not rospy.is_shutdown(): @@ -37,10 +33,6 @@ 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 - elif self.msg_battery_laptop is not None and\ - (self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_CHARGING or - self.msg_battery_laptop.power_supply_status == sensor_msgs.msg.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING): - return True else: return False @@ -48,9 +40,6 @@ def callback_battery_spot(self, msg): self.msg_battery_spot = msg - def callback_battery_laptop(self, msg): - - self.msg_battery_laptop = msg def main(): From 29ddbde3fc4780771f61949d6779f09a70915913 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:05:24 -0400 Subject: [PATCH 167/261] jsk_spot_startup/launch/rosbag_record.launch: we only needs compress, if you need raw topics, we do not want to write same thing twice --- .../launch/rosbag_record.launch | 91 ++----------------- 1 file changed, 6 insertions(+), 85 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch index efcced70c5..43df6e9098 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch @@ -4,7 +4,7 @@ - - From 1fbad10cf320e0c63696c4e2b82b51a80272293f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:07:32 -0400 Subject: [PATCH 168/261] launch/jsk_spot_bringup.launch add robot/type, robot/name, see https://github.com/ros-infrastructure/rep/pull/104 --- .../launch/jsk_spot_bringup.launch | 4 ++++ .../node_scripts/start-spot.l | 23 +++++++++++++++++++ 2 files changed, 27 insertions(+) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/start-spot.l 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 index 63cb755609..a397a494c1 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -6,6 +6,10 @@ + + + + Date: Thu, 20 Oct 2022 00:08:18 -0400 Subject: [PATCH 169/261] [jsk_spot_startup/launch/include/driver.launch] update minimum driver startup file --- .../launch/include/driver.launch | 96 ++++++++++++------- 1 file changed, 63 insertions(+), 33 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index eb3a36963f..5ad34b422f 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -1,6 +1,11 @@ + + + + + @@ -41,21 +46,9 @@ - - - - - - - - - + - + @@ -76,30 +69,58 @@ + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + - + + + + + + + + - + + + From 592aac73c4928c36eceab543f8cb80811a84903f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:09:52 -0400 Subject: [PATCH 170/261] use jsk_spot_startup/config/head_teleop_twist_joy.yaml --- .../apps/head_lead_demo/head_lead_demo.xml | 2 +- .../config/head_teleop_twist_joy.yaml | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/config/head_teleop_twist_joy.yaml 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 index b9e600afdd..2b6871649f 100644 --- 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 @@ -13,7 +13,7 @@ - + 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..e6e5f9025d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/config/head_teleop_twist_joy.yaml @@ -0,0 +1,12 @@ +axis_linear: + x: 0 + y: 1 +scale_linear: + x: 0.5 + y: 0.5 +axis_angular: + yaw: 2 +scale_angular: + yaw: 0.5 +enable_button: 0 +enable_turbo_button: -1 From 0e61b90be646c6094a7f724c734262d5fc22a2e6 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 00:11:37 -0400 Subject: [PATCH 171/261] add node_scripts/call_spot_dock_with_id.py, which support /spot/dock_with_id with Trigger service message --- .../auth/spot_credential.yaml | 1 + .../node_scripts/call_spot_dock_with_id.py | 31 +++++++++++++++++++ 2 files changed, 32 insertions(+) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/call_spot_dock_with_id.py diff --git a/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml b/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml index 415533c3f1..a9e90c9bf1 100644 --- a/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml +++ b/jsk_spot_robot/jsk_spot_startup/auth/spot_credential.yaml @@ -1,3 +1,4 @@ username: "dummyusername" password: "dummypassword" hostname: "192.168.50.3" +dock_id: 521 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() From f67dfcaa0a3a5e9c0820a858fd0417e36c3f94db Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 04:02:29 -0400 Subject: [PATCH 172/261] jsk_spot_startup/node_scripts/battery_notifier.py: notify battery status every 5 min --- .../jsk_spot_startup/node_scripts/battery_notifier.py | 9 +++++++++ 1 file changed, 9 insertions(+) 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 index e349832879..41bca22a03 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -46,13 +46,22 @@ def __init__(self): 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('バッテリー残量が少ないため、動作を停止します') From 8cace0c74a99637c15f70eb19da6cf3ab64c7afc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 04:06:25 -0400 Subject: [PATCH 173/261] update jsk_spot_robot/SetupInternalPCAndSpotUser.md, some important information is also removed in this commit, so pleaes check when you improve next time --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 226 +++++-------------- 1 file changed, 59 insertions(+), 167 deletions(-) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 83459c462f..62862bc340 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -10,29 +10,69 @@ TODO ### Setup wifi interfaces -TODO +Use [AKEIE](https://www.amazon.co.jp/gp/product/B08NB64TMH/ref=ppx_yo_dt_b_asin_title_o03_s01?ie=UTF8&psc=1) and (Calm USB L Cable (Up side))[https://www.amazon.co.jp/gp/product/B094DGRC9Q/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&th=1] +``` +$ lsusb +Bus 001 Device 003: ID 0bda:b812 Realtek Semiconductor Corp. +``` +``` +$ git clone https://github.com/cilynx/rtl88x2bu.git +cd rtl88x2bu +./deply.sh +sudo nmtui # to configure network +``` + +### 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 +``` ## How to set up the spot user -First, create a `spot` user to your PC if it does not exist. +First, create your user account to internal PC. ``` -TODO +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 + ``` -And add `spot` user to sudo group. +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 +``` + +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` user should have 3 workspaces for this use. - `spot_driver_ws`: a workspace to run driver.launch. which requires python3 build version of geometry3 -- `spot_coral_ws`: a workspace to run object_detection.launch ( which includes coral_usb_ros node ) which requires python3 build version of opencv_brindge -- `spot_ws`: a workspace to run other launch ( python2 ) ### Prerequities @@ -53,7 +93,7 @@ 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/sktometometo/jsk_robot.git --git -v develop/spot +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 @@ -75,173 +115,25 @@ git update-index --skip-worktree auth/spot_credential.yaml ``` -### setting up `spot_coral_ws` - -First, follow [Edge TPU dependencies installation section of coral_usb_ros](https://github.com/knorth55/coral_usb_ros#edge-tpu-dependencies-installation) -Then, create a workspace for coral_usb. - -```bash -source /opt/ros/melodic/setup.bash -mkdir ~/spot_coral_ws/src -p -cd ~/spot_coral_ws/src -wstool init . -wstool set jsk-ros-pkg/jsk_robot https://github.com/sktometometo/jsk_robot.git --git -v develop/spot -wstool set coral_usb_ros https://github.com/knorth55/coral_usb_ros.git --git -wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_coral.rosinstall -wstool merge -t . coral_usb_ros/fc.rosinstall.melodic -wstool update -rosdep update -rosdep install -y -r --from-paths . --ignore-src -cd ~/spot_coral_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 -c -``` - -After that, please download models for coral_usb_ros. - -``` -source /opt/ros/melodic/setup.bash -source ~/spot_coral_ws/devel/setup.bash -rosrun coral_usb download_models.py -``` - - -### setting up `spot_ws` - -Create `spot_ws` - -```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/sktometometo/jsk_robot.git --git -v develop/spot -wstool update -wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_spot_robot/jsk_spot_dev.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_ws -catkin init -catkin build -j4 -c -``` - -If you want to use switchbot_ros with spot_basic_behaviors, please add switch_bot token. - -``` -roscd spot_basic_behaviors -# modify config/switchbot_ros/token.yaml -git update-index --skip-worktree config/switchbot_ros/token.yaml -``` - - -### Setup Google Service Clients - -In order to use `dialogflow_task_executive` and `gdrive_ros`, please prepair authentication files. - -- [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/blob/master/dialogflow_task_executive/README.md) - + Please download a service account key JSON file of your dialogflow project and put it under `/var/lib/robot/`. -- [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) - + Please download client config file, create `settings.yaml` and put them under `/var/lib/robot/`. - - -### Install scripts and systemd services - -Install scripts and systemd services - -``` -source /opt/ros/melodic/setup.bash -source ~/spot_ws/devel/setup.bash -rosrun jsk_spot_startup deploy-scripts-and-services.sh -``` - - -### Set environmental variables - -Modify `/var/lib/robot/config.bash` to set environmental variables. - -Set `IF_ETH`, `IF_WIFI`, `IF_LTE` to your network interface names to use `jsk-spot-utils-network.service` - -``` -export IF_ETH="noethernetdevice" -export IF_WIFI="nowifidevice" -export IF_LTE="noltedevice" -``` - -Set your `ROS_IP` to WiFi AP address to ip address of your wifi AP interface. - -``` -WIFI_AP_IP=10.42.0.1 -rossetmaster $WIFI_AP_IP -rossetip $WIFI_AP_IP -``` - -Add environmental variables for [dialogflow_task_executive](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/dialogflow_task_executive) and [gdrive_ros](https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/gdrive_ros) -Please see them for more details. - -``` -export GOOGLE_APPLICATION_CREDENTIALS=/path/to/service_account_json_file # for dialogflow -export DIALOGFLOW_PROJECT_ID= # for dialogflow -export GOOGLE_DRIVE_SETTINGS_YAML=/path/to/pyDrive_setting_yaml # for pydrive -``` +## Troubleshootig +### Joystick did not respond -### Setting up udev files +Check `Setup Joystick` section and reconnet bluetooth -Create udev rule for insta360air, spot spinal. +### Wifi did not connec -```bash -roscd jsk_spot_startup -sudo cp udev_rules/* /etc/udev/rules.d/ +Check [roaming aggressiveness configuration](https://github.com/jsk-ros-pkg/jsk_robot/issues/1598) ``` - -Create udev rules for joy pads - -```bash -roscd jsk_spot_teleop -sudo cp config/udev/* /etc/udev/rules.d/ -``` - -And reload them - -```bash -sudo udevadm control --reload-rules -``` - -Please modify each udev rule according to your configuration. - - -### Add the user to groups - -Add spot user to groups - -```bash -sudo gpasswd -a dialout -sudo gpasswd -a audio -sudo gpasswd -a plugdev -sudo gpasswd -a video +$ wpa_cli get_network 0 bgscan ``` -#### Setup cockpit - -Cockpit is server management tool. If you use Spot CORE cockpit is already setup. - +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. ``` -sudo apt install cockpit +$ wpa_cli set_network 0 bgscan "\"simple:5:-50:3000\"" ``` +You can also check the output of wpa_supplicant. ``` -roscd jsk_spot_startup -sudo cp -r config/cockpit.socket.d /etc/systemd/system/cockpit.socket.d -sudo systemctl daemon-reload -sudo systemctl start cockpit.socket -``` - -Then you can access cockpit by `https://:21443` - -#### Setup postfix with gmail - -TODO +$ journalctl -u wpa_supplicant -f +``` \ No newline at end of file From ac2e25d01285f6c6b59a322e5fa99fe083ba41e6 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 18:40:31 +0900 Subject: [PATCH 174/261] update URL of spot_ros --- jsk_spot_robot/jsk_spot_driver.rosinstall | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 3b5041dd4a..5a5710a0e4 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -13,8 +13,8 @@ ## 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.git - version: effea91835cf11411854f472c0ed5f00bf6d87b4 + 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 From 511321f0dfebd2478897796fa562861dbf6cd2b2 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 20 Oct 2022 07:43:59 -0400 Subject: [PATCH 175/261] add end_effector_to_joy.py --- .../node_scripts/end_effector_to_joy.py | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/end_effector_to_joy.py 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..d398e97519 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/end_effector_to_joy.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +# -*- encoding: utf-8 -*- + +import rospy +from spot_msgs.msg import ManipulatorState +from sensor_msgs.msg import Joy + +class EndEffectorToJoy(object): + + def __init__(self): + self._sub = rospy.Subscriber( + '/spot/status/manipulator_state', + ManipulatorState, + self._cb) + self._pub = rospy.Publisher( + '/joy_head/joy_raw', + Joy) + self._deadzone = float(rospy.get_param('~deadzone', 4.0)) + self._maxrange = int(rospy.get_param('~maxrange', 10.0)) + + 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 + 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) + y = abs(y) + if x > self._maxrange: x = self._maxrange + if y > self._maxrange: y = self._maxrange + x = 0 if x < self._deadzone else x - self._deadzone + y = 0 if y < self._deadzone else y - self._deadzone + x = sign_x * x / (self._maxrange - self._deadzone) + y = sign_y * y / (self._maxrange - self._deadzone) + 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() From 85d0223d334a90bec42d435b6c38356d226e6d2b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 11:23:13 +0900 Subject: [PATCH 176/261] end_effector_to_joy.py: use float for maxrange, add offset_x, offset_y --- .../jsk_spot_startup/node_scripts/end_effector_to_joy.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 index d398e97519..8f6f277373 100755 --- 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 @@ -16,7 +16,9 @@ def __init__(self): '/joy_head/joy_raw', Joy) self._deadzone = float(rospy.get_param('~deadzone', 4.0)) - self._maxrange = int(rospy.get_param('~maxrange', 10.0)) + self._maxrange = float(rospy.get_param('~maxrange', 10.0)) + self._offset_x = float(rospy.get_param('~offset_x', 1.0)) + self._offset_y = float(rospy.get_param('~offset_y', 0.0)) def _cb(self, msg): joy = Joy() @@ -32,8 +34,8 @@ def _cb(self, msg): else: sign_x = cmp(x, 0) sign_y = cmp(y, 0) - x = abs(x) - y = abs(y) + x = abs(x) + self._offset_x + y = abs(y) + self._offset_y if x > self._maxrange: x = self._maxrange if y > self._maxrange: y = self._maxrange x = 0 if x < self._deadzone else x - self._deadzone From c0ba33c7e78295f9171522ebf5e67867716517be Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 11:28:40 +0900 Subject: [PATCH 177/261] call estop using raw rospy API --- .../jsk_spot_startup/node_scripts/battery_notifier.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 41bca22a03..e35b6ceae2 100755 --- a/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/battery_notifier.py @@ -3,7 +3,7 @@ import rospy from std_msgs.msg import Bool -from std_srvs.srv import Trigger +from std_srvs.srv import Trigger, TriggerRequest from spot_msgs.msg import BatteryStateArray from sensor_msgs.msg import BatteryState @@ -65,8 +65,8 @@ def __init__(self): if (self._battery_spot is not None and self._battery_spot < threshold_estop_spot): rospy.logerr('Battery is low. Estop.') sound_client.say('バッテリー残量が少ないため、動作を停止します') - spot_client.estop_gentle() - spot_client.estop_hard() + 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.') From 365c62810fb5b41678c85746f9b6377c672ece15 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 11:29:11 +0900 Subject: [PATCH 178/261] update head_teleop_joy param --- .../jsk_spot_startup/apps/head_lead_demo/head_lead_demo.xml | 5 ++++- .../jsk_spot_startup/config/head_teleop_twist_joy.yaml | 6 +++--- 2 files changed, 7 insertions(+), 4 deletions(-) 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 index 2b6871649f..ea88713b16 100644 --- 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 @@ -7,7 +7,10 @@ 2. head_teleop_joy_completion.py: subscribe joy_head/joy_raw and publish joy_head/joy_complemented, see Tukamoto RSJ 2022 paper 3. subscribe joy_head/joy_complemented and publish joy_head/cmd_vel --> - + + + + 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 index e6e5f9025d..7c7de945bb 100644 --- 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 @@ -2,11 +2,11 @@ axis_linear: x: 0 y: 1 scale_linear: - x: 0.5 - y: 0.5 + x: 1.0 + y: 1.0 axis_angular: yaw: 2 scale_angular: - yaw: 0.5 + yaw: 0.25 enable_button: 0 enable_turbo_button: -1 From 966c49ee498014e80b442a5c52212794a5767022 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 11:32:38 +0900 Subject: [PATCH 179/261] support min/max of x,y,yaw --- .../scripts/head_teleop_joy_completion.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) 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() From 698639d621ab91052ba83031d253e83b28d0be47 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 14:06:01 +0900 Subject: [PATCH 180/261] jsk_spot_startup/launch/jsk_spot_bringup.launch: enable to pass all args to driver.launch --- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index a397a494c1..484b726563 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -5,6 +5,7 @@ + @@ -14,9 +15,7 @@ - - + pass_all_args="true" > From 6a8f54be00d48b0e9ad9b2b0567bb974eec1bf78 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Oct 2022 14:15:26 +0900 Subject: [PATCH 181/261] jsk_spot_robot/jsk_spot_driver.rosinstall:fix typo sques_atlk -> aques_talk --- jsk_spot_robot/jsk_spot_driver.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 5a5710a0e4..8d7718155d 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -38,7 +38,7 @@ version: develop # aques_talk needs to be compiled from source - tar: - local-name: aques_atlk + 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: From a8ff6e193ae629d40e4f541d101e7a1e3a8dcd49 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 24 Oct 2022 10:13:44 +0900 Subject: [PATCH 182/261] update README --- jsk_spot_robot/README.md | 130 +++++++------------ jsk_spot_robot/SetupInternalPCAndSpotUser.md | 25 +++- 2 files changed, 68 insertions(+), 87 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 3ade42a481..4f32fbea15 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -10,7 +10,7 @@ jsk_spot_robot To setup a new internal PC and spot user, Please see [this page](./SetupInternalPCAndSpotUser.md). -### How to set up a catkin workspace for a remove PC +### How to set up a catkin workspace for a remote PC Create a workspace @@ -19,7 +19,7 @@ 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/sktometometo/jsk_robot.git --git -v develop/spot +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 @@ -27,7 +27,7 @@ rosdep update rosdep install -y -r --from-paths . --ignore-src cd $HOME/spot_ws catkin init -catkin build -j4 -c +catkin build -j4 jsk_spot_startup spoteus ``` ## How to run @@ -38,55 +38,14 @@ First, please turn on spot and turn on motors according to [the OPERATION sectio Basically, ros systemd services will start automatically. So you can use spot now. -#### Start basic roslaunch manually -If you want to launch basic ROS launches manually for some reason, please login `spot` user and follow this instruction. +#### superviosur -##### for spot_driver - -```bash -source $HOME/spot_driver_ws/devel/setup.bash -roslaunch jsk_spot_startup driver.launch -``` - -##### for object detection - -```bash -source $HOME/spot_coral_ws/devel/setup.bash -roslaunch jsk_spot_startup object_detection.launch -``` - -##### for other programs - -``` -source $HOME/spot_ws/deve/setup.bash -roslaunch jsk_spot_startup jsk_spot_bringup.launch use_driver:=false use_object_detection:=false -``` - -This launch includes -- bringup launch for additional peripheral devices (Respeaker, insta 360 air and ublox GPS module) -- teleoperation launch -- interaction launch with Speech-To-Text and Text-To-Speech -- so on. - -### Teleoperation - -You can control spot with DualSense controller. Please see [jsk_spot_teleop](./jsk_spot_teleop/README.md) for more details. - - -### Web UI - -Spot have some web UI. - -#### cockpit - -URI: https://:21443 - -TODO +URI: https://:9001 #### rwt_app_chooser -TODO +URI: https://:8000/rwt_app_chooser ### Development with a remote PC @@ -97,40 +56,11 @@ 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. ``` -rossetip rossetmaster +rossetip source ~/spot_ws/devel/setup.bash ``` -#### Visualization - -For visualization, you can run RViz with jsk configuration. - -```bash -source $HOME/spot_ws/devel/setup.bash -roslaunch jsk_spot_startup rviz.launch -``` - -#### rosbag record and play - -For development, `rosbag_record.launch` and `rosbag_play.launch` are useful for rosbag recording and playing. - -```bash -source $HOME/spot_ws/devel/setup.bash -# Record a rosbag file -roslaunch jsk_spot_startup rosbag_record.launch rosbag:= -# Play a rosbag file ( don't run this with setting ros_master_uri to the robot ) -roslaunch jsk_spot_startup rosbag_play.launch rosbag:= -``` - -#### Control API - -If you want to control Spot, there are basically 3 options. - -- Use spoteus ( roseus client, recommended ) -- Use python client ( spot_ros_client, experimental ) -- Use raw ROS interface - ##### spoteus spoteus is roseus client for ROS Interface of spot_ros. @@ -148,13 +78,45 @@ You can now control spot from roseus. for example, when you want to move spot 1m forward, type. ``` -send *ri* :go-pose 1 0 +(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 ``` -And there are some examples in `spoteus/demo`. +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 +``` -- `sample_basics.l` - + example of basic usage of Spot -- `sample_navigate_to.l` - + example of autowalk client -- `sample_visualization.l` +#### Install apps +If you would like to call your apps from rwt_app_chooser, you can diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 62862bc340..f6f0b00377 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -20,6 +20,9 @@ Bus 001 Device 003: ID 0bda:b812 Realtek Semiconductor Corp. $ 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 ``` @@ -39,6 +42,13 @@ $ sudo bluetoothctl [bluetooth]# connect D0:BC:C1:CB:48:37 ``` +### Setup timezone + +``` +sudo timedatectl set-timezone Asia/Tokyo +``` + + ## How to set up the spot user First, create your user account to internal PC. @@ -59,7 +69,8 @@ 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 +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 @@ -103,7 +114,7 @@ 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 -c +catkin build -j4 tf2_ros cv_bridge jsk_spot_startup spoteus robot_upstart ``` After this, please modify the credential files for spot_driver. @@ -136,4 +147,12 @@ $ wpa_cli set_network 0 bgscan "\"simple:5:-50:3000\"" You can also check the output of wpa_supplicant. ``` $ journalctl -u wpa_supplicant -f -``` \ No newline at end of file +``` + +### 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 +``` From 9de087357bc6f2a1cb761f0d0e1bd965c682daf8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 1 Nov 2022 23:11:00 +0900 Subject: [PATCH 183/261] add jsk_spot_user.rosinstall --- jsk_spot_robot/jsk_spot_user.rosinstall | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_user.rosinstall diff --git a/jsk_spot_robot/jsk_spot_user.rosinstall b/jsk_spot_robot/jsk_spot_user.rosinstall new file mode 100644 index 0000000000..d874d457d3 --- /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: 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 +# 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 From 5936a23ad6c50a09bcc83db9fd765f862c9f296c Mon Sep 17 00:00:00 2001 From: a-ichikura Date: Thu, 3 Nov 2022 16:14:41 +0900 Subject: [PATCH 184/261] fix supervisor and app chooser URLs --- jsk_spot_robot/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 4f32fbea15..9f642f4988 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -41,11 +41,11 @@ Basically, ros systemd services will start automatically. So you can use spot no #### superviosur -URI: https://:9001 +URI: http://:9001 #### rwt_app_chooser -URI: https://:8000/rwt_app_chooser +URI: http://:8000/rwt_app_chooser ### Development with a remote PC From a8f764585b113853fbff50135d97832692ed7728 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 9 Nov 2022 20:24:44 +0900 Subject: [PATCH 185/261] add start-grasp/stop-grasp method --- jsk_spot_robot/spoteus/spot-interface.l | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 218e960ea2..ee3855812e 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -104,6 +104,24 @@ (: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)) ;; (:dummy-controller () ;; (list ;; (list From 70e31970a793da799d2e5e75e6cca27f95a5d10b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 9 Nov 2022 20:39:16 +0900 Subject: [PATCH 186/261] add missing dependencse, fix style --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index f6f0b00377..0f2d05d2a3 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -77,7 +77,7 @@ 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/ @@ -90,8 +90,8 @@ systemd services of JSK Spot system use workspaces in `spot` user. Install necessary packages for workspace building ``` -sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy -sudo apt-get install ros-melodic-catkin +sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy python3-opencv +sudo apt-get install ros-melodic-catkin ros-melodic-vision-msgs ``` From 1f2ec8f40a4c796cc6905e5a48a86697bc0262c3 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 10 Nov 2022 20:02:17 +0900 Subject: [PATCH 187/261] jsk_robot_common/jsk_robot_startup/CMakeLists.txt: install python code using catkin_install_python --- jsk_robot_common/jsk_robot_startup/CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index a9ca7f482e..76ce6de029 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -140,6 +140,14 @@ endif() install(DIRECTORY lifelog util launch images config cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) +file(GLOB_RECURSE SCRIPT_PROGRAMS lifelog/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + endif() +endforeach() if(mongodb_store_FOUND) install(TARGETS jsk_robot_lifelog From bebaba6720ac433df27d5fe85a2be131291311d8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 10 Nov 2022 20:02:53 +0900 Subject: [PATCH 188/261] jsk_robot_startup/lifelog/common_logger.launch add save_faces and save_dialogflow --- .../lifelog/common_logger.launch | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch index 4d53b2e6e4..22e29277b8 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -5,6 +5,8 @@ + + @@ -236,6 +238,32 @@ + + + + topics: + - /aws_auto_checkin_app/class + - /aws_detect_faces/attributes + + + + + + + topics: + - /dialogflow_client/text_action/goal + - /dialogflow_client/text_action/result + + + Date: Thu, 10 Nov 2022 20:04:08 +0900 Subject: [PATCH 189/261] jsk_robot_startup/package.xml: update to version 3, add python3-* packages to exec_depend --- .../jsk_robot_startup/package.xml | 63 ++++++++++--------- 1 file changed, 35 insertions(+), 28 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/package.xml b/jsk_robot_common/jsk_robot_startup/package.xml index c86b879736..c008fbdf44 100644 --- a/jsk_robot_common/jsk_robot_startup/package.xml +++ b/jsk_robot_common/jsk_robot_startup/package.xml @@ -1,5 +1,5 @@ - + jsk_robot_startup 1.1.0 The jsk_robot_startup package @@ -19,33 +19,40 @@ sensor_msgs dynamic_reconfigure urdf - actionlib_msgs - app_manager - dynamic_reconfigure - geometry_msgs - gmapping - google_chat_ros - jsk_recognition_msgs - jsk_topic_tools - message_runtime - mongodb_store - nodelet - pointcloud_to_laserscan - posedetection_msgs - rosbridge_server - roscpp - roseus_mongo - rosgraph - rospy - rostwitter - roswww - sensor_msgs - tf - tf2_ros - tf2_py - tf2_geometry_msgs - urdf - jsk_tilt_laser + + actionlib_msgs + app_manager + dynamic_reconfigure + geometry_msgs + gmapping + google_chat_ros + jsk_recognition_msgs + jsk_topic_tools + message_runtime + mongodb_store + nodelet + pointcloud_to_laserscan + posedetection_msgs + python-bson + python3-bson + python-pymongo + python3-pymongo + python-tz + python3-tz + rosbridge_server + roscpp + roseus_mongo + rosgraph + rospy + rostwitter + roswww + sensor_msgs + tf + tf2_ros + tf2_py + tf2_geometry_msgs + urdf + jsk_tilt_laser rostest pr2_gazebo From 1ef195c03286425e83bed8849a353813d11787bc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 10 Nov 2022 20:05:29 +0900 Subject: [PATCH 190/261] install orocos_kdl_python, mongodb_store, catkin for compile python3 --- jsk_spot_robot/jsk_spot_driver.rosinstall | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 8d7718155d..631a16229b 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -31,6 +31,25 @@ 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 From 0f7977feed0fd01963e459f1adaef29792fb8196 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 10 Nov 2022 20:12:35 +0900 Subject: [PATCH 191/261] SetupInternalPCAndSpotUser.md: add information for more python3 installs --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 62 +++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 0f2d05d2a3..74e1de5a52 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -90,7 +90,7 @@ systemd services of JSK Spot system use workspaces in `spot` user. Install necessary packages for workspace building ``` -sudo apt-get install python3-catkin-pkg-modules python3-rospkg-modules python3-venv python3-empy python3-opencv +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 ``` @@ -125,6 +125,66 @@ roscd jsk_spot_startup 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 From f74dd986bcdb29f8401ca9313cb15964f0244bb2 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 10 Nov 2022 20:13:02 +0900 Subject: [PATCH 192/261] add launch/include/lifelog.launch --- .../launch/include/lifelog.launch | 90 +++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch 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..f56058f2b6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + topics: + - /sound_play/goal + + + From 9cc5bc86ca67f3525a09464fdcf68ab723241fa4 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 12:49:57 +0900 Subject: [PATCH 193/261] use jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml --- .../config/dualsense_teleop_twist_joy.yaml | 17 +++++++++++++++++ .../launch/include/driver.launch | 1 + 2 files changed, 18 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml 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..927590bf38 --- /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: 0.5 + y: 0.3 +scale_linear_turbo: + x: 1.2 + y: 0.3 +axis_angular: + yaw: 2 +scale_angular: + yaw: 0.5 +scale_angular_turbo: + yaw: 0.7 +enable_button: 4 +enable_turbo_button: 10 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 5ad34b422f..ed20a5f0dc 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -63,6 +63,7 @@ + From 9dc66f129e04deef7bc4aef83965765bd1090492 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 12:51:07 +0900 Subject: [PATCH 194/261] fix config/dualsense_teleop_twist_joy.yaml to speedup --- .../config/dualsense_teleop_twist_joy.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 index 927590bf38..4d8a889771 100644 --- 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 @@ -2,16 +2,16 @@ axis_linear: x: 1 y: 0 scale_linear: - x: 0.5 - y: 0.3 + x: 1.0 + y: 0.5 scale_linear_turbo: - x: 1.2 - y: 0.3 + x: 2.0 + y: 1.0 axis_angular: yaw: 2 scale_angular: - yaw: 0.5 + yaw: 1.0 scale_angular_turbo: - yaw: 0.7 + yaw: 2.0 enable_button: 4 enable_turbo_button: 10 From 00553221b86ad627f4f0be6e1186c101b3cbfe4f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 14:18:49 +0900 Subject: [PATCH 195/261] end_effector_to_joy.py: use position, not force for better performance. --- .../apps/head_lead_demo/head-lead-teleop.l | 20 ++++++- .../apps/head_lead_demo/head_lead_demo.xml | 6 +- .../node_scripts/end_effector_to_joy.py | 56 ++++++++++++++----- 3 files changed, 65 insertions(+), 17 deletions(-) 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 index 6d51a49827..09a124694b 100755 --- 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 @@ -11,16 +11,32 @@ (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") - (return-from :start-func :finished)) + (ros::ros-warn "Robot is on dock or powered, quit from apps") + (return-from start-func :finished)) (send *ri* :speak "Hello, let's start walking") (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) 2000 :default-controller) (send *ri* :wait-interpolation) - (send *ri* :set-impedance-param :linear-stiffness #f(250 50 50) :rotational-stiffness #f(10 10 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 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 start-func :finished)) + (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data t)) :started) (defun end-func (args) (send *ri* :speak "Thank You") + (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)) (send *ri* :stow-arm) :finished) 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 index ea88713b16..a6331837bd 100644 --- 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 @@ -8,8 +8,10 @@ 3. subscribe joy_head/joy_complemented and publish joy_head/cmd_vel --> - - + + + + 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 index 8f6f277373..430d67db32 100755 --- 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 @@ -2,24 +2,46 @@ # -*- 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._sub = rospy.Subscriber( - '/spot/status/manipulator_state', - ManipulatorState, - self._cb) + self._tf_buffer = tf2_ros.Buffer() + self._listener = tf2_ros.TransformListener(self._tf_buffer) self._pub = rospy.Publisher( '/joy_head/joy_raw', Joy) - self._deadzone = float(rospy.get_param('~deadzone', 4.0)) - self._maxrange = float(rospy.get_param('~maxrange', 10.0)) - self._offset_x = float(rospy.get_param('~offset_x', 1.0)) + 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.loginfo("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.loginfo("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() @@ -27,6 +49,14 @@ def _cb(self, msg): 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 + rospy.logerr([trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z, x, y]) + 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 @@ -36,12 +66,12 @@ def _cb(self, msg): sign_y = cmp(y, 0) x = abs(x) + self._offset_x y = abs(y) + self._offset_y - if x > self._maxrange: x = self._maxrange - if y > self._maxrange: y = self._maxrange - x = 0 if x < self._deadzone else x - self._deadzone - y = 0 if y < self._deadzone else y - self._deadzone - x = sign_x * x / (self._maxrange - self._deadzone) - y = sign_y * y / (self._maxrange - self._deadzone) + 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 From 036be2fa5a9dc162f58506236b98b09dfb660e71 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 14:19:46 +0900 Subject: [PATCH 196/261] fix jsk_spot_startup/config/head_teleop_twist_joy.yaml: head_teleop_joy_completion.py did not consider scale_linear, use x_min/x_max etc... --- .../config/head_teleop_twist_joy.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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 index 7c7de945bb..364aaf2941 100644 --- 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 @@ -1,12 +1,13 @@ axis_linear: x: 0 y: 1 -scale_linear: - x: 1.0 - y: 1.0 + x_min: -0.5 + x_max: 1.25 + y_min: -0.5 + y_max: 0.5 axis_angular: yaw: 2 -scale_angular: - yaw: 0.25 + yaw_min: -0.75 + yaw_max: 0.75 enable_button: 0 enable_turbo_button: -1 From 6fd0e052da0a905b93344356effb73e6774604bc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 14:20:50 +0900 Subject: [PATCH 197/261] use jsk_spot_robot/jsk_spot_startup/CMakeLists.txt: catkin_install_python to install scripts --- jsk_spot_robot/jsk_spot_startup/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index 9df4cbf3f3..478621e5bf 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -18,14 +18,14 @@ install(DIRECTORY auth config launch scripts services udev_rules template USE_SOURCE_PERMISSIONS ) -catkin_install_python(PROGRAMS - node_scripts/battery_notifier.py - node_scripts/battery_visualizer.py - node_scripts/cable_connection_detector.py - node_scripts/laptop_battery_visualizer.py - node_scripts/static_tf_republisher.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +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_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + endif() +endforeach() ############# From 17de7ac8327a8e4494361d347b69c0cfd28a5a5b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 15:44:29 +0900 Subject: [PATCH 198/261] head-lead-teleop.l: run start-func(set-impedance) when R Joy Stick is pressed --- .../apps/head_lead_demo/head-lead-teleop.l | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 index 09a124694b..32bead101a 100755 --- 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 @@ -41,9 +41,10 @@ :finished) (defun walk-func (args) - (ros::rate 0.1) + (ros::rate 2) (do-until-key (if (null *continue*) (return-from walk-func :finished)) + (ros::spin-once) (ros::sleep)) :finished) ;; @@ -84,6 +85,14 @@ (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) + ;; state machine (exec-state-machine (walk-sm) '((description . "お散歩しました！")(image . ""))) (exit) From 83d4a0b73e3fc62190099dec61ac6dad2f6be57d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 17 Nov 2022 13:47:14 +0900 Subject: [PATCH 199/261] use jsk_topic_tools/SynchronizedThrottle to generate compressed/thorttle images for mongodb --- .../launch/include/driver.launch | 19 ++++-- .../launch/include/lifelog.launch | 65 +++++++++++++++++-- .../launch/include/lifelog_rgb_image.launch | 19 ++++++ 3 files changed, 91 insertions(+), 12 deletions(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/include/lifelog_rgb_image.launch diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index ed20a5f0dc..292c132841 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -73,15 +73,22 @@ - - + + + + + + diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch index f56058f2b6..b72bff6d74 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch @@ -22,6 +22,7 @@ + @@ -33,12 +34,7 @@ - - - - - - + @@ -54,10 +50,62 @@ + + + + + approximate_sync: true + topics: + - /spot/camera/hand_color/image/output/compressed + - /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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + periodic: false threshold: 0.01 @@ -85,6 +133,11 @@ topics: - /sound_play/goal + - /robotsound/goal + - /robotsound_jp/goal + - /sound_play + - /robotsound + - /robotsound_jp 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 + + + From 37b49001e12752cdb8384c7dc54d5aebd6857700 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 17 Nov 2022 19:40:34 +0900 Subject: [PATCH 200/261] add sample_pick_object_in_image.launch ../node_scripts/sample_pick_object_in_image.l --- .../launch/sample_pick_object_in_image.launch | 12 ++++ .../sample_pick_object_in_image.l | 58 +++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/sample_pick_object_in_image.launch create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/sample_pick_object_in_image.l 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/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") + ) + + + + + From f31842e2cfe73fd32f0b5b362d801e23856de722 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 26 Nov 2022 16:53:44 +0900 Subject: [PATCH 201/261] fix head-end-coords : set z-axis to up, to follow jsk style, suppot (send robot :head :look-at), add tests --- jsk_spot_robot/spoteus/spot-utils.l | 34 ++++++++++++++++++++++++- jsk_spot_robot/spoteus/spot.yaml | 2 +- jsk_spot_robot/spoteus/test/test-spot.l | 27 +++++++++++++++++--- 3 files changed, 58 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l index fab2f6e6e7..86e7b92b1b 100644 --- a/jsk_spot_robot/spoteus/spot-utils.l +++ b/jsk_spot_robot/spoteus/spot-utils.l @@ -43,7 +43,39 @@ '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000)) (assoc b (send (class self) :methods))))) ) - (send* self :init-orig args)) + (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) diff --git a/jsk_spot_robot/spoteus/spot.yaml b/jsk_spot_robot/spoteus/spot.yaml index 63634f430f..1157a69b53 100644 --- a/jsk_spot_robot/spoteus/spot.yaml +++ b/jsk_spot_robot/spoteus/spot.yaml @@ -51,4 +51,4 @@ rrleg-end-coords: arm-end-coords: parent: arm0.link_wr1 translate : [0.175, 0, -0.03] - rotate : [1, 0, 0, 90] + 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 index ead03cb148..c39aec74d0 100755 --- a/jsk_spot_robot/spoteus/test/test-spot.l +++ b/jsk_spot_robot/spoteus/test/test-spot.l @@ -1,16 +1,37 @@ #!/usr/bin/env roseus (require :unittest "lib/llib/unittest.l") -(require "package://spoteus/spot-util.l") +(require "package://spoteus/spot-utils.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)) )) -(deftest test-spot-init - (spot-init)) +(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))) + +;; spot-interface is not work on simulator +;; (deftest test-spot-init +;; (spot-init)) (run-all-tests) (exit) From cb48298949cfcdd041b39aec0b4095016d0efb4d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 26 Nov 2022 17:15:52 +0900 Subject: [PATCH 202/261] launch/include/driver.launch: fix /spot/status/battery_percentage type to Float32 --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 292c132841..1429922fb3 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -120,7 +120,7 @@ From 43d80b979172e90ba394e69b954973620900342b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 26 Nov 2022 17:16:29 +0900 Subject: [PATCH 203/261] use 'arm' branch of k-okada/spot_ros-arm reposity, instead of hash --- jsk_spot_robot/jsk_spot_user.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_user.rosinstall b/jsk_spot_robot/jsk_spot_user.rosinstall index d874d457d3..1fb7336910 100644 --- a/jsk_spot_robot/jsk_spot_user.rosinstall +++ b/jsk_spot_robot/jsk_spot_user.rosinstall @@ -3,7 +3,7 @@ - git: local-name: spot-ros uri: https://github.com/k-okada/spot_ros-arm.git - version: 0a8c4b29768ae946d7c84aec9a2ae2d185ed97a9 + 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 From d805954cd6bea9e238cc5eabe2c1e12e0799ccd4 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 16:03:07 +0900 Subject: [PATCH 204/261] add *ri* interface to :pick-object-in-image --- jsk_spot_robot/spoteus/spot-interface.l | 26 ++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index ee3855812e..7810656e6c 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -42,7 +42,7 @@ (defclass spot-interface :super robot-move-base-interface - :slots (trajectory-cmd-action) + :slots (trajectory-cmd-action pick-object-in-image-client pick-object-in-image-feedback-msg) ) (defmethod spot-interface @@ -69,6 +69,8 @@ (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 @@ -122,6 +124,28 @@ (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 From cee1106befca3d315269dc50111b9581bffc9d22 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:49:50 +0900 Subject: [PATCH 205/261] use logwarn for enable/distable center information --- .../jsk_spot_startup/node_scripts/end_effector_to_joy.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index 430d67db32..e62715641d 100755 --- 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 @@ -33,10 +33,10 @@ 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.loginfo("Enabled end_effector_to_joy with center ({}, {})".format(self._center_x, self._center_y)) + 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.loginfo("Disabled end_effector_to_joy with center") + rospy.logwarn("Disabled end_effector_to_joy with center") if self._sub: self._sub.unregister() self._sub = None @@ -54,7 +54,6 @@ def _cb(self, msg): except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr(e) return None - rospy.logerr([trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z, x, y]) 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)) From 424121a37f293e44afe75465d8f14271372fb9b8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:50:21 +0900 Subject: [PATCH 206/261] set respawn true for battery notifier --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 1429922fb3..064bff108c 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -141,6 +141,7 @@ pkg="jsk_spot_startup" type="battery_notifier.py" name="battery_notifier" + respawn="true" > From 5c489a2cc2bc1d251d0446be4a6ce44fa87cbe89 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:51:26 +0900 Subject: [PATCH 207/261] use google_chat ad dialogfow in interaction.launch --- .../launch/include/interaction.launch | 127 ++++-------------- 1 file changed, 24 insertions(+), 103 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index eea5c224b1..dff5912c47 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -1,108 +1,29 @@ - - + + + + + + - + + + + + + + + + + + - - - - device: $(optenv DEVICE_SPEAKER default) - - - - - - - device: $(optenv DEVICE_SPEAKER default) - - - - - - - - - - - - - - - - - - - - - - - - - audio_topic: $(arg audio_topic) - n_channel: 1 - depth: 16 - sample_rate: 16000 - engine: "Sphinx" - language: "en-US" - continuous: True - - - - - - - - language: "en-US" - self_cancellation: true - tts_action_names: - - robotsound - - robotsound_jp - tts_tolerance: 0.5 - - - - - - - - - - - - - - language: "ja-JP" - self_cancellation: true - tts_action_names: - - robotsound - - robotsound_jp - tts_tolerance: 0.5 - - + + + + + + + From 7cbacc2b7288137e28915bba5e6084fc94fb6bba Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:53:24 +0900 Subject: [PATCH 208/261] save data only when /publish_trigger_mongodb_event published --- .../launch/include/lifelog.launch | 80 ++++++++++++++++++- 1 file changed, 77 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch index b72bff6d74..a14f6147ad 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch @@ -22,7 +22,7 @@ - + @@ -55,14 +55,47 @@ - + + + + + + + + + + + + + + approximate_sync: true topics: - - /spot/camera/hand_color/image/output/compressed + - /publish_trigger_mongodb_event - /spot/camera/hand_color/image/compressed - /spot/camera/frontleft/image/compressed - /spot/camera/frontright/image/compressed @@ -106,6 +139,38 @@ + + + + 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 @@ -128,6 +193,8 @@ - pr2_controllers_msgs/Pr2GripperCommandActionResult - sound_play/SoundRequestActionResult - sound_play/SoundRequestActionGoal + - spot_msgs/PickObjectInImageActionGoal + - spot_msgs/PickObjectInImageActionResult @@ -140,4 +207,11 @@ - /robotsound_jp + + topics: + - /faces_name + - /aws_auto_checkin_app/output/class + - /aws_detect_faces/attributes + + From 2e68bf0796008568f26026e454527388a3abeaac Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:58:44 +0900 Subject: [PATCH 209/261] add ublox gps driver --- jsk_spot_robot/jsk_spot_driver.rosinstall | 17 +++++++++++++++++ .../launch/include/driver.launch | 5 +++++ 2 files changed, 22 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 631a16229b..a074b875e8 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -69,3 +69,20 @@ 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 diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 064bff108c..e4881f4d7f 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -162,4 +162,9 @@ + + + + + From bd3a72c4719339bdf6f8663f0abeee4490f9ce77 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:59:15 +0900 Subject: [PATCH 210/261] fix jsk_spot_startup/node_scripts instal location --- jsk_spot_robot/jsk_spot_startup/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index 478621e5bf..7552ce0555 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -21,9 +21,9 @@ install(DIRECTORY auth config launch scripts services udev_rules template 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_BIN_DESTINATION}) + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts) else() - install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts) endif() endforeach() From e193510abc1f8dfd6f4a248fd705e4fe2cac7b45 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 22:59:56 +0900 Subject: [PATCH 211/261] fix config/dualsense_teleop_twist_joy.yaml parameters --- .../jsk_spot_startup/config/dualsense_teleop_twist_joy.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 4d8a889771..bfd30e14b0 100644 --- 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 @@ -3,10 +3,10 @@ axis_linear: y: 0 scale_linear: x: 1.0 - y: 0.5 + y: 0.8 scale_linear_turbo: - x: 2.0 - y: 1.0 + x: 1.5 + y: 1.2 axis_angular: yaw: 2 scale_angular: From bc32d34aeef9a09d04693a9036d6ca6e5b19c0d5 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 23:01:20 +0900 Subject: [PATCH 212/261] update head-lead-teleop, add to play with dogtoy --- .../apps/head_lead_demo/head-lead-teleop.l | 183 ++++++++++++++++-- .../apps/head_lead_demo/head_lead_demo.xml | 13 +- 2 files changed, 182 insertions(+), 14 deletions(-) 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 index 32bead101a..09a5ea8be3 100755 --- 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 @@ -8,14 +8,53 @@ ;; (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)) -(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-warn "Robot is on dock or powered, quit from apps") - (return-from start-func :finished)) - (send *ri* :speak "Hello, let's start walking") +;; sample +(setq *person* nil) +(setq *ball* 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)) *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) 2000 :default-controller) + (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) @@ -27,11 +66,22 @@ (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 start-func :finished)) + (return-from init-walk-arm-setting :finished)) (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data t)) + ) + +(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") + (init-walk-arm-setting) :started) (defun end-func (args) + (ros::ros-info "end-func") (send *ri* :speak "Thank You") (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") @@ -40,13 +90,113 @@ (send *ri* :stow-arm) :finished) +;; *cmd-vel* is (cons (ros::time-now) msg) (defun walk-func (args) - (ros::rate 2) - (do-until-key - (if (null *continue*) (return-from walk-func :finished)) - (ros::spin-once) + (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) + :finished)) + +(setq *distance-thre* 2000) +(defun ball-play-func (args) + (ros::ros-info "ball-play") + (setq *default-av* #f(0.0 -130.0 120.0 0.0 10.0 0.0)) + (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*)) + (send *ri* :stow-arm) + (let ((image-source (first (car *ball*))) + (bbox (third (car *ball*))) + (rect (second (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)) + (send *ri* :pick-object-in-image image-source + (+ (send rect :x) (/ (send rect :width) 2)) + (+ (send rect :y) (/ (send rect :height) 2))) + (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???? + )) + (send *spot* :arm :angle-vector *default-av*) + (send *ri* :angle-vector (send *spot* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (when (send result :success) + (send *ri* :go-pos -0.5 0 0) + (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) + (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) + (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 + (> (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 () @@ -56,11 +206,15 @@ '((: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 @@ -92,6 +246,9 @@ (= (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) ;; state machine (exec-state-machine (walk-sm) '((description . "お散歩しました！")(image . ""))) 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 index a6331837bd..ee7b17ee13 100644 --- 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 @@ -2,6 +2,16 @@ + + + + + + + + + + - + From eb9209673749221c343763d399a4682982808255 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 7 Dec 2022 23:02:27 +0900 Subject: [PATCH 213/261] add google_chat_logger, fix /aws_auto_checkin_app/class location --- .../lifelog/common_logger.launch | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch index 22e29277b8..ae86cd3230 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -7,6 +7,7 @@ + @@ -246,7 +247,7 @@ respawn="$(arg respawn)"> topics: - - /aws_auto_checkin_app/class + - /aws_auto_checkin_app/output/class - /aws_detect_faces/attributes @@ -264,6 +265,22 @@ + + + + topics: + - /google_chat_ros/send/goal + - /google_chat_ros/send/result + - /google_chat_ros/message_activity + - /google_chat_ros + - /google_chat_ros/card_activity + + + Date: Wed, 7 Dec 2022 23:05:11 +0900 Subject: [PATCH 214/261] add perception.launch, ncb_provier, aws_auto_checkin --- .../launch/include/perception.launch | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/include/perception.launch 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 + + + + From f82437e235ffddc52b03c0f659970425c2a560f1 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 8 Dec 2022 14:42:18 +0900 Subject: [PATCH 215/261] add coreio extentions tools --- jsk_spot_robot/coreio_extension/Dockerfile | 42 +++ jsk_spot_robot/coreio_extension/Makefile | 29 ++ jsk_spot_robot/coreio_extension/README.md | 23 ++ .../coreio_extension/docker-compose.yml | 20 ++ .../coreio_extension/docker-requirements.txt | 40 +++ .../ncb_object_detection_coco.service | 18 + .../network_compute_server.py | 312 ++++++++++++++++++ 7 files changed, 484 insertions(+) create mode 100644 jsk_spot_robot/coreio_extension/Dockerfile create mode 100644 jsk_spot_robot/coreio_extension/Makefile create mode 100644 jsk_spot_robot/coreio_extension/README.md create mode 100644 jsk_spot_robot/coreio_extension/docker-compose.yml create mode 100644 jsk_spot_robot/coreio_extension/docker-requirements.txt create mode 100644 jsk_spot_robot/coreio_extension/ncb_object_detection_coco.service create mode 100644 jsk_spot_robot/coreio_extension/network_compute_server.py 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) From 0195454ddd898d0a625f15dae23236fa4d1f7e66 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 10 Feb 2023 19:26:35 +0900 Subject: [PATCH 216/261] add allow-other-keys for args --- jsk_spot_robot/spoteus/spot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 7810656e6c..8ae77e57c4 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -47,7 +47,7 @@ (defmethod spot-interface (:init - (&rest args &key (trajectory-cmd-action-name "/spot/trajectory")) + (&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 From 8391f0323aad356029f70fb8e8a07f7ebf1e8891 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 10 Feb 2023 22:07:40 +0900 Subject: [PATCH 217/261] fix typo on jsk_spot_driver.rosinstall/ubox --- jsk_spot_robot/jsk_spot_driver.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index a074b875e8..10f5a54ea8 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -85,4 +85,4 @@ # acc_lim: 3.0 # Survey-In Accuracy Limit [m] - git: local-name: ublox - uri: https://github.com:KumarRobotics/ublox.git + uri: https://github.com/KumarRobotics/ublox.git From 1ed14e3636c69f22682e0af8c5d088b808826439 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 30 Mar 2023 02:52:30 +0000 Subject: [PATCH 218/261] add use_gps --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 3 ++- jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index e4881f4d7f..6c0efe25a6 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -5,6 +5,7 @@ + @@ -163,7 +164,7 @@ - + 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 index 484b726563..91643dc584 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -6,6 +6,7 @@ + From d71c3a54f154231ca4670f426d13f9cb24b92939 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 30 Mar 2023 03:01:07 +0000 Subject: [PATCH 219/261] jsk_spot_robot/coreio/base: add development environment for CoreI/O, see https://dev.bostondynamics.com/docs/payload/coreio_documentation#developing-on-the-core-i-o, it is recommended to build on docker file.... --- jsk_spot_robot/coreio/base/Dockerfile | 78 +++++++++++++++++++++++++++ jsk_spot_robot/coreio/base/Makefile | 47 ++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 jsk_spot_robot/coreio/base/Dockerfile create mode 100644 jsk_spot_robot/coreio/base/Makefile diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile new file mode 100644 index 0000000000..0355e5859d --- /dev/null +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -0,0 +1,78 @@ +##### +## 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 + +ENTRYPOINT ["/home/spot/ros_entrypoint.sh"] +CMD ["bash"] \ No newline at end of file diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile new file mode 100644 index 0000000000..ca6f2734a5 --- /dev/null +++ b/jsk_spot_robot/coreio/base/Makefile @@ -0,0 +1,47 @@ +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 -w /home/spot/ws -ti dev_env ${1}; +endef +define docker_run +${1} +endef + +pre_build: + # 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) From 7aea0a3252238d67a3aef0fa0eb3e2ab6086177a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 30 Mar 2023 06:52:48 +0000 Subject: [PATCH 220/261] add script to buid network compute server docekr images, see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch3 --- .../coreio/network_compute_server/0_setup.sh | 5 + .../network_compute_server/1_capture.sh | 17 + .../coreio/network_compute_server/2_split.sh | 4 + .../network_compute_server/3_convert.sh | 7 + .../coreio/network_compute_server/4_train.sh | 8 + .../coreio/network_compute_server/5_run.sh | 6 + .../network_compute_server/6_build_ext.sh | 31 ++ .../network_compute_server/Dockerfile.l4t | 22 ++ .../network_compute_server/docker-compose.yml | 26 ++ .../docker-requirements.txt | 40 +++ .../network_compute_server/manifest.json | 7 + .../network_compute_server.py | 302 ++++++++++++++++++ 12 files changed, 475 insertions(+) create mode 100755 jsk_spot_robot/coreio/network_compute_server/0_setup.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/1_capture.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/2_split.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/3_convert.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/4_train.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/5_run.sh create mode 100755 jsk_spot_robot/coreio/network_compute_server/6_build_ext.sh create mode 100644 jsk_spot_robot/coreio/network_compute_server/Dockerfile.l4t create mode 100644 jsk_spot_robot/coreio/network_compute_server/docker-compose.yml create mode 100644 jsk_spot_robot/coreio/network_compute_server/docker-requirements.txt create mode 100644 jsk_spot_robot/coreio/network_compute_server/manifest.json create mode 100644 jsk_spot_robot/coreio/network_compute_server/network_compute_server.py 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..1df433380a --- /dev/null +++ b/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py @@ -0,0 +1,302 @@ +# 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) + 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 + 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_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) From df5c4010a3f335f6f24119464b2d041547eab518 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 30 Mar 2023 06:53:33 +0000 Subject: [PATCH 221/261] network_compute_server.py: fix network_compute_server.py for core i/o, see https://dev.bostondynamics.com/docs/python/fetch_tutorial/fetch6 --- .../network_compute_server.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) 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 index 1df433380a..d600d51577 100644 --- a/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py +++ b/jsk_spot_robot/coreio/network_compute_server/network_compute_server.py @@ -30,7 +30,7 @@ from google.protobuf import wrappers_pb2 from object_detection.utils import label_map_util -kServiceAuthority = "fetch-tutorial-worker.spot.robot" +kServiceAuthority = "object-detection-coco-worker.spot.robot" class TensorFlowObjectDetectionModel: @@ -67,6 +67,11 @@ def process_thread(args, request_queue, response_queue): 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: @@ -232,7 +237,11 @@ def register_with_robot(options): robot = sdk.create_robot(options.hostname) # Authenticate robot before being able to use it - bosdyn.client.util.authenticate(robot) + 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) @@ -261,7 +270,8 @@ def main(argv): 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') + 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) From f0ab1351cc50632c1779cea9925d763be2c44a91 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 31 Mar 2023 14:21:14 +0900 Subject: [PATCH 222/261] update Documents for CORE I/O --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 64 ++++++++++++++++---- 1 file changed, 52 insertions(+), 12 deletions(-) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 74e1de5a52..ab7f5a886a 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -2,29 +2,53 @@ This page describes how to setup a internal pc and spot user. -## How to setup a PC +## How to setup a [CORE I/O](https://dev.bostondynamics.com/docs/payload/coreio_documentation) -### Install Ubuntu and ROS +The CORE I/O is desgined to use filesystem is read only and user Docekr or Spot Extensions for user applications. -TODO + +### 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 [AKEIE](https://www.amazon.co.jp/gp/product/B08NB64TMH/ref=ppx_yo_dt_b_asin_title_o03_s01?ie=UTF8&psc=1) and (Calm USB L Cable (Up side))[https://www.amazon.co.jp/gp/product/B094DGRC9Q/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&th=1] +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 003: ID 0bda:b812 Realtek Semiconductor Corp. +Bus 001 Device 005: ID 2357:011e ``` +Make sure that the roming configuration works correctly (from core-io) ``` -$ 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 +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 @@ -40,6 +64,22 @@ $ 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 From 3b40a39d86bee9fb4a9f10e37bff8f67c25101ea Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 31 Mar 2023 14:21:37 +0900 Subject: [PATCH 223/261] coreio/base/Dockerfile: set TimeZone --- jsk_spot_robot/coreio/base/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile index 0355e5859d..0464b2e58d 100644 --- a/jsk_spot_robot/coreio/base/Dockerfile +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -74,5 +74,7 @@ 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"] \ No newline at end of file +CMD ["bash"] From 127b87a22fe5402f1c6348c0dc095525af1d407e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 1 Apr 2023 14:33:25 +0900 Subject: [PATCH 224/261] coreio/base/Makefile: create and share ~/.ros directory --- jsk_spot_robot/coreio/base/Makefile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index ca6f2734a5..ed00da7df5 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -9,13 +9,15 @@ $(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 -w /home/spot/ws -ti dev_env ${1}; +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 . From 01f1df70f78d6c7e89cee8bffc25ca0e79cb5da9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 10 Apr 2023 20:37:04 +0900 Subject: [PATCH 225/261] Update mongodb_replication_params.yaml `robot-database.jsk.imi.i.u-tokyo.ac.jp` is CNAME of `musca.jsk....` maybe this is the cause of following error ``` [/message_store /opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py:753] [ERROR] [1681119907.337053]: bad callback: > Traceback (most recent call last): File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback cb(msg) File "/home/spot/ws/src/mongodb_store/mongodb_store/scripts/message_store_node.py", line 103, in insert_ros_msg self.insert_ros_srv(msg) File "/home/spot/ws/src/mongodb_store/mongodb_store/scripts/message_store_node.py", line 152, in insert_ros_srv dc_util.store_message(extra_collection, obj, meta, obj_id) File "/home/spot/ws/src/mongodb_store/mongodb_store/src/mongodb_store/util.py", line 258, in store_message return collection.insert(doc) File "/usr/lib/python3/dist-packages/pymongo/collection.py", line 2941, in insert check_keys, manipulate, write_concern) File "/usr/lib/python3/dist-packages/pymongo/collection.py", line 599, in _insert bypass_doc_val, session) File "/usr/lib/python3/dist-packages/pymongo/collection.py", line 580, in _insert_one _check_write_command_response(result) File "/usr/lib/python3/dist-packages/pymongo/helpers.py", line 207, in _check_write_command_response _raise_last_write_error(write_errors) File "/usr/lib/python3/dist-packages/pymongo/helpers.py", line 188, in _raise_last_write_error raise DuplicateKeyError(error.get("errmsg"), 11000, error) pymongo.errors.DuplicateKeyError: E11000 duplicate key error collection: jsk_robot_lifelog.strelka index: _id_ dup key: { : ObjectId('6433daa31cf4582bb939adf7') } ``` --- .../jsk_robot_startup/lifelog/mongodb_replication_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 5caa58cf8f1d4c5726db834c6b75db462c1f8ef9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 10 Apr 2023 21:24:51 +0900 Subject: [PATCH 226/261] enable to query from extra server (replicate server), keep google_chat_ros data to mongodb --- .../jsk_spot_startup/launch/include/lifelog.launch | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch index a14f6147ad..3e6195e067 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/lifelog.launch @@ -15,6 +15,7 @@ + @@ -87,6 +88,12 @@ + + Date: Mon, 10 Apr 2023 21:26:14 +0900 Subject: [PATCH 227/261] add google cloud language and gdrive_ros, update credentials for strelka --- .../launch/include/interaction.launch | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index dff5912c47..1ffcb352ef 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -1,21 +1,23 @@ - + + - + - - - + + + + @@ -26,4 +28,16 @@ + + + + + + + + + + + + From 75017d5a4926dd829011575a5c712bccac695402 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 16 Dec 2022 17:48:07 +0900 Subject: [PATCH 228/261] add ublox in rosbag record --- jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch index 43df6e9098..b28009e057 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/rosbag_record.launch @@ -90,6 +90,8 @@ /spot_recognition/rects /spot_recognition/object_detection_image/compressed /spot_recognition/tracking_labels + /ublox/fix + /ublox/fix_velocity " output="screen" /> From f844aa15e02c62da0203e112ef524faae3ef9230 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 16 Dec 2022 17:51:29 +0900 Subject: [PATCH 229/261] update head-lead-teleop.l for ichikura demo --- .../apps/head_lead_demo/head-lead-teleop.l | 70 +++++++++++++++---- 1 file changed, 56 insertions(+), 14 deletions(-) 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 index 09a5ea8be3..6506a73a38 100755 --- 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 @@ -1,6 +1,11 @@ #!/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) @@ -8,9 +13,12 @@ ;; (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) @@ -33,8 +41,11 @@ (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)) *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)))) @@ -67,7 +78,7 @@ (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 t)) + (ros::service-call "end_effector_to_joy/set_enabled" (instance std_srvs::SetBoolRequest :init :data *head-lead*)) ) (defun start-func (args) @@ -77,7 +88,14 @@ ;(return-from start-func :finished) ) (send *ri* :speak "Hello, let's start walking") - (init-walk-arm-setting) + (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) @@ -118,7 +136,6 @@ (setq *distance-thre* 2000) (defun ball-play-func (args) (ros::ros-info "ball-play") - (setq *default-av* #f(0.0 -130.0 120.0 0.0 10.0 0.0)) (ros::spin-once) (block :cond @@ -130,19 +147,30 @@ (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) @@ -159,17 +187,27 @@ (send *ri* :stop-grasp) ;;(unix::sleep 2) ;; ?? prevent from walking???? )) - (send *spot* :arm :angle-vector *default-av*) - (send *ri* :angle-vector (send *spot* :angle-vector) 1000) - (send *ri* :wait-interpolation) + (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) - (send *spot* :arm :wrist-p :joint-angle 30) - (send *ri* :angle-vector (send *spot* :angle-vector) 500) + (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) - (init-walk-arm-setting) + (if *head-lead* (init-walk-arm-setting)) )) (*person* (send *ri* :speak "found person") @@ -184,17 +222,19 @@ (send *ri* :angle-vector (send *spot* :angle-vector) 500) (setq *person* nil) (send *ri* :wait-interpolation) - (init-walk-arm-setting)) + (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 + (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))) + (init-walk-arm-setting) + )) )) ;; cond :walk) ;; @@ -250,6 +290,8 @@ (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) From 6806fc5ae53d8b778beadff192722e9d270839ae Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 4 Apr 2023 18:25:38 +0900 Subject: [PATCH 230/261] [jsk_spot_startup] unstow arm in every cases --- .../jsk_spot_startup/apps/head_lead_demo/head-lead-teleop.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 6506a73a38..8548d7f372 100755 --- 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 @@ -101,11 +101,11 @@ (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)) - (send *ri* :stow-arm) :finished) ;; *cmd-vel* is (cons (ros::time-now) msg) From f1f1167eda82ed8d300fe94f0d8d82fc1c71d495 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 10:07:35 +0900 Subject: [PATCH 231/261] temp --- .../apps/head_lead_demo/head_lead_demo.xml | 16 ++++++++++------ .../launch/include/driver.launch | 3 ++- .../launch/include/interaction.launch | 4 ++-- 3 files changed, 14 insertions(+), 9 deletions(-) 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 index ee7b17ee13..bee6b6b5e4 100644 --- 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 @@ -2,15 +2,19 @@ - - + - - - - + + + + + + + + - + + diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index 1ffcb352ef..624fc0e433 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -23,8 +23,8 @@ - - + + From 12871ba3165784ea3d35889596fa9cd81584518d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 18:57:26 +0900 Subject: [PATCH 232/261] picks google cloud arg name --- .../jsk_spot_startup/launch/include/interaction.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch index 624fc0e433..d4f6ae4788 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/interaction.launch @@ -17,7 +17,7 @@ - + From 2670e32562bef0616d4879ecdfb77cc325ef8516 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 16 Nov 2023 10:01:33 +0900 Subject: [PATCH 233/261] [jsk_spot_robot] spoteus/test/test-spot.l enable test-spot-init --- jsk_spot_robot/spoteus/test/test-spot.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/test/test-spot.l b/jsk_spot_robot/spoteus/test/test-spot.l index c39aec74d0..673cf2d477 100755 --- a/jsk_spot_robot/spoteus/test/test-spot.l +++ b/jsk_spot_robot/spoteus/test/test-spot.l @@ -1,6 +1,7 @@ #!/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) @@ -30,8 +31,8 @@ (look-at robot))) ;; spot-interface is not work on simulator -;; (deftest test-spot-init -;; (spot-init)) +(deftest test-spot-init + (spot-init)) (run-all-tests) (exit) From 271991d56c49c7cf4b3b4352144608697c255773 Mon Sep 17 00:00:00 2001 From: kyoneda Date: Wed, 15 Nov 2023 18:03:18 +0900 Subject: [PATCH 234/261] [spoteus] remove error in spot-init with kinematics simulator --- jsk_spot_robot/spoteus/spot-interface.l | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/spoteus/spot-interface.l b/jsk_spot_robot/spoteus/spot-interface.l index 8ae77e57c4..c384aa122e 100644 --- a/jsk_spot_robot/spoteus/spot-interface.l +++ b/jsk_spot_robot/spoteus/spot-interface.l @@ -11,6 +11,9 @@ (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)) @@ -19,6 +22,9 @@ (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)) @@ -27,6 +33,9 @@ (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)) @@ -35,6 +44,9 @@ (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)) @@ -51,7 +63,7 @@ (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 (ros::wait-for-service "/spot/claim" 5) + (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 @@ -320,11 +332,17 @@ (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" (call-trigger-service "/spot/power_on")) + (: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" - (call-trigger-service "/spot/power_off")) + (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")) From df240b078d5563e1704ece5ccf9e628792c9f775 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 16 Nov 2023 10:57:01 +0900 Subject: [PATCH 235/261] remove outdated comments --- jsk_spot_robot/spoteus/test/test-spot.l | 1 - 1 file changed, 1 deletion(-) diff --git a/jsk_spot_robot/spoteus/test/test-spot.l b/jsk_spot_robot/spoteus/test/test-spot.l index 673cf2d477..1d378779a4 100755 --- a/jsk_spot_robot/spoteus/test/test-spot.l +++ b/jsk_spot_robot/spoteus/test/test-spot.l @@ -30,7 +30,6 @@ (send robot :arm :inverse-kinematics (send (make-coords :pos #f(600 0 600)) :transform robot)) (look-at robot))) -;; spot-interface is not work on simulator (deftest test-spot-init (spot-init)) From 84fbe6a51349c95d2423f57db7d92cdeed113444 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 16 Oct 2023 19:54:12 +0900 Subject: [PATCH 236/261] Add R1 teleop --- .../jsk_spot_startup/launch/include/driver.launch | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 950c4e4c3d..6da02babdd 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -67,6 +67,18 @@ + + + + + + + + + + + + From 9b68d99d4617b7a8ee059bd2fa1a8e9cf649f895 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Nov 2023 14:51:00 +0900 Subject: [PATCH 237/261] [jsk_spot_startup] add switchbot --- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 4 ++++ 1 file changed, 4 insertions(+) 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 index 91643dc584..16234a445b 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -27,4 +27,8 @@ + + + + From b3682c1adc7bd35812b9f93aa46f561fba141663 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 30 Nov 2023 17:41:58 +0900 Subject: [PATCH 238/261] [spoteus] remove find_package(collada_urdf) from CMakeLists.txt --- jsk_spot_robot/spoteus/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt index 2d49a15d63..1289e57fbd 100644 --- a/jsk_spot_robot/spoteus/CMakeLists.txt +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -5,7 +5,6 @@ project(spoteus) ## 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 ) @@ -28,7 +27,6 @@ if(EXISTS ${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}") From 9882356ac04534ca63538cec041f19a6fd4e9c69 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 30 Oct 2024 13:28:38 +0900 Subject: [PATCH 239/261] add launch/sample_walk_to_point_in_image.launch node_scripts/sample_walk_to_point_in_image.py --- .../sample_walk_to_point_in_image.launch | 16 ++++++++ .../sample_walk_to_point_in_image.py | 40 +++++++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch create mode 100755 jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py diff --git a/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch b/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch new file mode 100644 index 0000000000..a22020a9c7 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py new file mode 100755 index 0000000000..4ef0566282 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python + +import rospy +import actionlib +from geometry_msgs.msg import Point, PointStamped +from spot_msgs.msg import WalkToObjectInImageAction, WalkToObjectInImageGoal + +def cb(msg): + rospy.loginfo('screenpoint callback x={}, y={}, distance={}'.format(msg.point.x, msg.point.y, distance)) + timeout = 15 + goal = WalkToObjectInImageGoal( + image_source = image_source, + center = msg.point, + distance = distance, + max_duration = rospy.Duration(timeout) + ) + client.send_goal(goal) + client.wait_for_result(timeout=rospy.Duration(timeout+5)) + if client.get_result().success: + rospy.loginfo("succeeded") + else: + rospy.logerr("failed to walkt ot object") + +if __name__ == '__main__': + try: + rospy.init_node('sample_walk_to_object_in_image') + image_source = rospy.get_param('~image_source', 'hand_color') + distance = rospy.get_param('~distance', 0.5) + # subscribe point in screen + rospy.loginfo('start subscribe screenpoint at {}'.format(image_source)) + rospy.Subscriber('screenpoint', PointStamped, cb) + # action client for walk to point API + rospy.loginfo('conncting to walk_to_point_in_image action') + client = actionlib.SimpleActionClient('/spot/walk_to_object_in_image', WalkToObjectInImageAction) + client.wait_for_server() + # + rospy.loginfo('start main loop') + rospy.spin() + except rospy.ROSInterruptException: + ropsy.logerr("program interrupted before completion") From ed8a7bc6489622afa24f4a5e2ea6bdbe99a53feb Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 15 Nov 2024 11:02:54 +0900 Subject: [PATCH 240/261] CMakeLists.txt add install command to work on > catkin config --install > catkin build -vi --summarize we checked that necessary files are installed ``` (export ROS_PACKAGE_PATH=$(pwd)/src; for dir in install/share/*; do pkg=$(basename $dir); echo -e "\n\n$pkg"; echo diff -r install/share/$pkg $(rospack find $pkg) --exclude CMakeLists.txt --exclude .gitignore --exclude setup.py --exclude README.md --exclude CHANGELOG.rst --exclude cmake ; ./check.py $(basename $dir); done > diff.dir) ``` where `check.py` is ``` import sys import os import copy import filecmp import rospkg def compare_directories(install_dir, src_dir, ignore=None): # Initialize dictionaries to store results results = { 'unique_to_dir1': [], 'unique_to_dir2': [], 'differences': [], 'common_files': [] } # Recursively compare directory structures and files comparison = filecmp.dircmp(install_dir, src_dir, ignore=ignore) # Files only in dir1 results['unique_to_dir1'] = comparison.left_only # Files only in dir2 results['unique_to_dir2'] = comparison.right_only # Files present in both directories but different results['differences'] = comparison.diff_files # Files that are identical in both directories results['common_files'] = comparison.same_files return results def list_all_subdirectories(root_dir): subdirectories = [] for dirpath, dirnames, _ in os.walk(root_dir): for dirname in dirnames: subdirectories.append(os.path.join(dirpath, dirname)) return subdirectories if __name__ == '__main__': rospack = rospkg.RosPack() pkg_name = sys.argv[1] pkg_path = rospack.get_path(pkg_name) print("## Check [{}]".format(pkg_name)) for dir in ['.'] + [os.path.relpath(dir, pkg_path) for dir in list_all_subdirectories(pkg_path)]: install_dir = os.path.join('install/share/', pkg_name, dir) src_dir = os.path.join(pkg_path, dir) if os.path.exists(install_dir): differences = compare_directories(install_dir, src_dir, ignore=['env-hooks', 'cmake', '.gitignore', 'CMakeLists.txt', 'CHANGELOG.rst', 'README.md', 'setup.py']) else: print('> Only in {}'.format(src_dir)) continue # Display the results for file in differences['unique_to_dir1']: print("> Files only in directory 1: {}".format([os.path.join(dir, file)])) for file in copy.copy(differences['unique_to_dir2']): if os.path.exists(os.path.join('install/lib/', pkg_name, file)): differences['unique_to_dir2'].remove(file) for file in differences['unique_to_dir2']: print("> Files only in directory 2: {}".format([os.path.join(dir, file)])) for file in differences['differences']: print("> Different files: {}".format([os.path.join(dir, file)])) #print("Common files (identical):", differences['common_files']) ``` ``` --- jsk_aero_robot/aeroeus/CMakeLists.txt | 3 ++- .../jsk_aero_startup/CMakeLists.txt | 2 +- jsk_baxter_robot/baxtereus/CMakeLists.txt | 3 ++- .../jsk_baxter_startup/CMakeLists.txt | 5 +++-- .../jsk_baxter_web/CMakeLists.txt | 2 +- jsk_denso_robot/cobottaeus/CMakeLists.txt | 5 +++-- .../jsk_cobotta_startup/CMakeLists.txt | 6 +++--- .../jsk_fetch_gazebo_demo/CMakeLists.txt | 2 +- .../jsk_fetch_startup/CMakeLists.txt | 10 ++++++++- .../jsk_kinova_startup/CMakeLists.txt | 2 +- jsk_kinova_robot/kinovaeus/CMakeLists.txt | 2 +- .../jsk_magni_startup/CMakeLists.txt | 2 +- .../jsk_nao_startup/CMakeLists.txt | 21 ++++++++++++++++++- jsk_naoqi_robot/jsk_nao_startup/setup.py | 9 ++++++++ .../jsk_pepper_startup/CMakeLists.txt | 12 ++++++++++- jsk_naoqi_robot/jsk_pepper_startup/setup.py | 9 ++++++++ jsk_naoqi_robot/naoeus/CMakeLists.txt | 10 +++++++++ jsk_naoqi_robot/peppereus/CMakeLists.txt | 9 +++++++- .../jsk_pr2_calibration/CMakeLists.txt | 4 ++++ jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt | 4 +++- .../pr2_base_trajectory_action/CMakeLists.txt | 7 ++++++- .../jsk_robot_startup/CMakeLists.txt | 21 +++++++++++++++++-- .../jsk_robot_utils/CMakeLists.txt | 10 ++++++++- .../speak_and_wait_recovery/CMakeLists.txt | 3 +++ .../CMakeLists.txt | 3 +++ .../jsk_unitree_startup/CMakeLists.txt | 10 ++++++++- jsk_unitree_robot/unitreeeus/CMakeLists.txt | 6 ++++-- 27 files changed, 155 insertions(+), 27 deletions(-) create mode 100644 jsk_naoqi_robot/jsk_nao_startup/setup.py create mode 100644 jsk_naoqi_robot/jsk_pepper_startup/setup.py diff --git a/jsk_aero_robot/aeroeus/CMakeLists.txt b/jsk_aero_robot/aeroeus/CMakeLists.txt index 38590b4cd4..8f3d7c7127 100644 --- a/jsk_aero_robot/aeroeus/CMakeLists.txt +++ b/jsk_aero_robot/aeroeus/CMakeLists.txt @@ -8,4 +8,5 @@ catkin_package( ## rosrun euscollada collada2eus_urdfmodel $(rospack find aero_description)/robots/aero.urdf $(rospack find aero_description)/robots/aero.yaml aero.l -install(FILES aero-interface.l aero-hand.l aero-wheels.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt b/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt index 94a9bdf257..9e060e3e0e 100644 --- a/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt +++ b/jsk_aero_robot/jsk_aero_startup/CMakeLists.txt @@ -10,7 +10,7 @@ endif() find_package(catkin REQUIRED) catkin_package() -install(DIRECTORY launch raw_maps scripts +install(DIRECTORY launch raw_maps scripts config test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_baxter_robot/baxtereus/CMakeLists.txt b/jsk_baxter_robot/baxtereus/CMakeLists.txt index d75a0e0bc8..5a4e1fe79a 100644 --- a/jsk_baxter_robot/baxtereus/CMakeLists.txt +++ b/jsk_baxter_robot/baxtereus/CMakeLists.txt @@ -65,7 +65,8 @@ install(DIRECTORY euslisp test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES baxter.l baxter-interface.l baxter-util.l baxter.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} baxter.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) diff --git a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt index 51ff998466..f4eb3b6789 100644 --- a/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt +++ b/jsk_baxter_robot/jsk_baxter_startup/CMakeLists.txt @@ -17,9 +17,10 @@ include_directories( catkin_add_env_hooks(99.jsk_baxter_startup SHELLS bash zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) -install(DIRECTORY config jsk_baxter_joy jsk_baxter_lifelog jsk_baxter_moveit jsk_baxter_sensors jsk_baxter_tools +install(DIRECTORY config jsk_baxter_joy jsk_baxter_lifelog jsk_baxter_moveit jsk_baxter_sensors jsk_baxter_tools jsk_baxter_description jsk_baxter_machine jsk_baxter_udev DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES baxter.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +file(GLOB LAUNCH_FILES *.launch) +install(FILES ${LAUNCH_FILES} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt index 9c7072a085..a61eefcd87 100644 --- a/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt +++ b/jsk_baxter_robot/jsk_baxter_web/CMakeLists.txt @@ -29,6 +29,6 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -install(DIRECTORY launch +install(DIRECTORY launch www DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_denso_robot/cobottaeus/CMakeLists.txt b/jsk_denso_robot/cobottaeus/CMakeLists.txt index e59ae6d705..ac455f9fb2 100644 --- a/jsk_denso_robot/cobottaeus/CMakeLists.txt +++ b/jsk_denso_robot/cobottaeus/CMakeLists.txt @@ -43,11 +43,12 @@ add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/cobotta.l add_custom_target(compile_cobotta ALL DEPENDS ${PROJECT_SOURCE_DIR}/cobotta.l) -install(DIRECTORY euslisp test +install(DIRECTORY sample test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES cobotta.l cobotta-interface.l cobotta-util.l cobotta.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} cobotta.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) diff --git a/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt b/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt index 50ceb95ec7..805e919076 100644 --- a/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt +++ b/jsk_denso_robot/jsk_cobotta_startup/CMakeLists.txt @@ -33,12 +33,12 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY config launch scripts data +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES jsk_cobotta.machine - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +# install(FILES jsk_cobotta.machine +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) ############# ## Testing ## diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt index f8ab04eead..319eb6314f 100644 --- a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt @@ -4,7 +4,7 @@ project(jsk_fetch_gazebo_demo) find_package(catkin) catkin_package() -install(DIRECTORY euslisp launch config +install(DIRECTORY euslisp launch config test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) diff --git a/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt index be9259dac6..2e813fb0f8 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt +++ b/jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt @@ -23,7 +23,15 @@ catkin_add_env_hooks(99.jsk_fetch_startup SHELLS bash zsh sh ############# ## Install ## ############# -install(DIRECTORY config launch scripts data +file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts/) + endif() +endforeach() +install(DIRECTORY config launch data apps euslisp robots supervisor_scripts tmuxinator_yaml udev_rules upstart_scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt index c0e3d84fbd..c50bda2127 100644 --- a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt +++ b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt @@ -14,7 +14,7 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY config launch +install(DIRECTORY config launch scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt index b6f51a120b..2ef0ba696f 100644 --- a/jsk_kinova_robot/kinovaeus/CMakeLists.txt +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -67,7 +67,7 @@ install(DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES kinova.l kinova-interface.l kinova-util.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES kinova.l kinova-interface.l kinova-util.l gen3_robotiq_2f_85.yaml gen3_robotiq_2f_140.yaml gen3_lite_gen3_lite_2f.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) diff --git a/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt b/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt index 33876c3948..cbf50ad329 100644 --- a/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt +++ b/jsk_magni_robot/jsk_magni_startup/CMakeLists.txt @@ -14,7 +14,7 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY launch +install(DIRECTORY launch data DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt b/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt index 4a945f7df0..a708f3dbb8 100644 --- a/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt +++ b/jsk_naoqi_robot/jsk_nao_startup/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -130,6 +130,22 @@ include_directories( # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) +file(GLOB_RECURSE SCRIPT_PROGRAMS nodes/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes) + endif() +endforeach() +file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts) + endif() +endforeach() ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ @@ -137,6 +153,9 @@ include_directories( # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES diff --git a/jsk_naoqi_robot/jsk_nao_startup/setup.py b/jsk_naoqi_robot/jsk_nao_startup/setup.py new file mode 100644 index 0000000000..5a423aaf02 --- /dev/null +++ b/jsk_naoqi_robot/jsk_nao_startup/setup.py @@ -0,0 +1,9 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup() + +setup(**setup_args) diff --git a/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt b/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt index 4b9e09ae9f..e25a9339f1 100644 --- a/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt +++ b/jsk_naoqi_robot/jsk_pepper_startup/CMakeLists.txt @@ -3,11 +3,21 @@ project(jsk_pepper_startup) find_package(catkin REQUIRED COMPONENTS) +catkin_python_setup() + catkin_package() -install(DIRECTORY launch nodes sample +install(DIRECTORY launch nodes sample apps config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) +file(GLOB_RECURSE SCRIPT_PROGRAMS script/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/script) + endif() +endforeach() if(CATKIN_ENABLE_TESTING AND NOT $ENV{ROS_DISTRO} STREQUAL "hydro") find_package(catkin REQUIRED COMPONENTS rostest roslaunch) diff --git a/jsk_naoqi_robot/jsk_pepper_startup/setup.py b/jsk_naoqi_robot/jsk_pepper_startup/setup.py new file mode 100644 index 0000000000..5a423aaf02 --- /dev/null +++ b/jsk_naoqi_robot/jsk_pepper_startup/setup.py @@ -0,0 +1,9 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup() + +setup(**setup_args) diff --git a/jsk_naoqi_robot/naoeus/CMakeLists.txt b/jsk_naoqi_robot/naoeus/CMakeLists.txt index 9457324d22..5a1e382a0c 100644 --- a/jsk_naoqi_robot/naoeus/CMakeLists.txt +++ b/jsk_naoqi_robot/naoeus/CMakeLists.txt @@ -19,3 +19,13 @@ compile_naoqi_model(nao naoV50_generated_urdf) if(nao_meshes_FOUND) add_rostest(test/naoeus.test) endif() + +### +### install +### +install(DIRECTORY patch test launch sample webots euslisp + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} nao.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/jsk_naoqi_robot/peppereus/CMakeLists.txt b/jsk_naoqi_robot/peppereus/CMakeLists.txt index bf179714e7..1889e522e5 100644 --- a/jsk_naoqi_robot/peppereus/CMakeLists.txt +++ b/jsk_naoqi_robot/peppereus/CMakeLists.txt @@ -20,4 +20,11 @@ if(pepper_meshes_FOUND AND CATKIN_ENABLE_TESTING) add_rostest(test/peppereus.test) endif() - +### +### install +### +install(DIRECTORY doc test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} pepper.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt b/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt index 73fdf60e27..bc76135f60 100644 --- a/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt +++ b/jsk_pr2_robot/jsk_pr2_calibration/CMakeLists.txt @@ -4,3 +4,7 @@ project(jsk_pr2_calibration) find_package(catkin REQUIRED COMPONENTS) catkin_package() + +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) diff --git a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt index b1981968a9..77c3d71a12 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt +++ b/jsk_pr2_robot/jsk_pr2_startup/CMakeLists.txt @@ -9,7 +9,9 @@ catkin_add_env_hooks(99.jsk_pr2_startup SHELLS bash zsh install(DIRECTORY config jsk_pr2_image_transport jsk_pr2_joy jsk_pr2_lifelog jsk_pr2_move_base jsk_pr2_moveit + jsk_pr2_perception jsk_pr2_rocon jsk_pr2_teleop jsk_pr2_sensors jsk_pr2_warning src sample + apps DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) file(GLOB _LAUNCH_FILES ${PROJECT_SOURCE_DIR}/ *.launch) # add / to avoid to get current directory @@ -18,5 +20,5 @@ foreach(_LAUNCH_FILE ${_LAUNCH_FILES}) DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) endforeach() -install(FILES install_pr1040_description.sh jsk_pr2.machine plugin.xml startup.app +install(FILES iai_kitchen.rosinstall jsk_pr2.rosinstall install_pr1040_description.sh jsk_pr2.machine plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt index 048aa8ed00..43769001c5 100644 --- a/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt +++ b/jsk_pr2_robot/pr2_base_trajectory_action/CMakeLists.txt @@ -28,7 +28,12 @@ add_executable(pr2_base_trajectory_action src/pr2_base_trajectory_action_controller_node.cpp) target_link_libraries(pr2_base_trajectory_action ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(DIRECTORY config include launch +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY config launch test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(TARGETS pr2_base_trajectory_action diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index bf9d8a55cb..a38a661a86 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -138,18 +138,35 @@ if($ENV{ROS_DISTRO} STRGREATER "indigo") # quadruped_joystick_teleop.cpp uses c+ ) endif() -install(DIRECTORY lifelog util launch images config cfg +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h") + +install(DIRECTORY util launch images config cfg euslisp apps DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) + +install(FILES lifelog_nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + file(GLOB_RECURSE SCRIPT_PROGRAMS lifelog/*) foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) else() - install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/lifelog) + endif() +endforeach() +file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts) endif() endforeach() + if(mongodb_store_FOUND) install(TARGETS jsk_robot_lifelog ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/jsk_robot_common/jsk_robot_utils/CMakeLists.txt b/jsk_robot_common/jsk_robot_utils/CMakeLists.txt index 5c80baa63b..acb7e736bf 100644 --- a/jsk_robot_common/jsk_robot_utils/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_utils/CMakeLists.txt @@ -10,7 +10,15 @@ if(CATKIN_ENABLE_TESTING) add_rostest(test/joint_state_compressor.test) endif() -install(DIRECTORY euslisp +file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts) + endif() +endforeach() +install(DIRECTORY euslisp test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS diff --git a/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt b/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt index 1e9db5e7f2..a29626d496 100644 --- a/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt +++ b/jsk_robot_common/speak_and_wait_recovery/CMakeLists.txt @@ -53,6 +53,9 @@ install(DIRECTORY include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h" ) +install(DIRECTORY launch test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + install(FILES speak_and_wait_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt b/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt index c684447081..7e3eefc877 100644 --- a/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt +++ b/jsk_robot_common/update_move_base_parameter_recovery/CMakeLists.txt @@ -61,6 +61,9 @@ install(DIRECTORY include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h" ) +install(DIRECTORY config launch tests worlds + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + install(FILES update_move_base_parameter_recovery_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt b/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt index e7595dd01f..6b1c3d909a 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt +++ b/jsk_unitree_robot/jsk_unitree_startup/CMakeLists.txt @@ -14,7 +14,15 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY config launch +file(GLOB_RECURSE SCRIPT_PROGRAMS scripts/*) +foreach(SCRIPT_PROGRAM ${SCRIPT_PROGRAMS}) + if("${SCRIPT_PROGRAM}" MATCHES ".*\\.py$") + catkin_install_python(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + else() + install(PROGRAMS ${SCRIPT_PROGRAM} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts) + endif() +endforeach() +install(DIRECTORY config launch apps autostart ino DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_unitree_robot/unitreeeus/CMakeLists.txt b/jsk_unitree_robot/unitreeeus/CMakeLists.txt index ad64f874f6..f541e2a509 100644 --- a/jsk_unitree_robot/unitreeeus/CMakeLists.txt +++ b/jsk_unitree_robot/unitreeeus/CMakeLists.txt @@ -44,12 +44,14 @@ foreach(ROBOT_TYPE go1) add_custom_target(compile_${ROBOT_TYPE} ALL DEPENDS ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}-simple.l) endforeach() - +### +### install +### install(DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) -install(FILES go1.l unitree-interface.l go1-utils.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES go1.l unitree-interface.l go1-utils.l go1.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) From df654efa8d90944e3ae0f8cb9d78e5cb21937563 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 15 Nov 2024 12:02:52 +0900 Subject: [PATCH 241/261] [coreio/base] update docker container for two-stage system, one for spot user to run system service and another one is for development users --- jsk_spot_robot/coreio/base/Dockerfile | 70 ++++++++++---- jsk_spot_robot/coreio/base/Makefile | 129 +++++++++++++++++++++----- 2 files changed, 158 insertions(+), 41 deletions(-) diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile index 0464b2e58d..bba6958625 100644 --- a/jsk_spot_robot/coreio/base/Dockerfile +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -11,23 +11,33 @@ RUN apt-get update && apt install -y -q python3-catkin-pkg-modules python3-rospk # network tools RUN apt-get update && apt install -y -q net-tools iputils-ping dnsutils traceroute curl +# development tools +RUN apt-get update && apt install -y -q python-catkin-tools ros-melodic-catkin-virtualenv + +# editors +RUN apt-get update && apt install -y -q less emacs vim + +# bluetooth +RUN apt-get update && apt install -y -q bluez bluez-tools + # 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 sed -i 's@%sudo\s*ALL=(ALL:ALL)\s*ALL@%sudo ALL=(ALL) NOPASSWD: ALL@' /etc/sudoers +RUN cat /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 +RUN rosdep update -y --include-eol-distros # 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/ +# setup catkin develeopment environment with mounted workspace +RUN mkdir -p /tmp/ws +COPY package.tar /tmp/ +RUN tar -C /tmp/ws/ -xvf /tmp/package.tar # install pip3 dependencies RUN pip3 install pip==21.3.1 @@ -36,8 +46,8 @@ RUN pip3 install wrapt==1.12.1 six==1.15.0 PyJWT==2.0.0 numpy==1.19.4 grpcio==1. # 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" +# # rosdep install +# RUN sudo apt-get update && 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 @@ -45,9 +55,12 @@ RUN rosdep install -y -r --from-paths /tmp/ws/src --ignore-src --rosdistro melod 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" +COPY package.tar /tmp/ +RUN tar -C /tmp/ws/ -xvf /tmp/package.tar +RUN sudo apt-get update && rosdep install -y -r --from-paths /tmp/ --ignore-src --rosdistro melodic || echo "OK" +# rosdep install for python3 +RUN sudo apt-get update && ROS_PYTHON_VERSION=3 rosdep install -y -r --skip-keys="python3-catkin-pkg python3-catkin-tools python3-rosinstall python3-rosdep" --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" @@ -57,19 +70,16 @@ RUN rosdep install -y -r --from-paths /tmp/ --ignore-src --rosdistro melodic || ## 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 "[ -e /home/spot/ws/install/setup.bash ] && source /home/spot/ws/install/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 +RUN chmod 0777 /home/spot/ros_entrypoint.sh ### FIXME ros_entrypoint did not work with roscd RUN echo "source /opt/ros/melodic/share/rosbash/rosbash" >> ~/.bashrc @@ -78,3 +88,31 @@ ENV TZ=Asia/Tokyo ENTRYPOINT ["/home/spot/ros_entrypoint.sh"] CMD ["bash"] + +FROM dev_build AS user_build + +ARG USER +ENV USER ${USER:-spot} +ARG UID +ENV UID ${UID:-1000} +ARG GID +ENV GID ${GID:-1000} +RUN sudo addgroup --gid ${GID} ${USER} +RUN sudo adduser --disabled-password --gecos '' --uid ${UID} --gid ${GID} ${USER} +RUN sudo adduser ${USER} sudo +USER ${USER} + +RUN echo "#!/bin/bash" > /home/${USER}/ros_entrypoint.sh && \ + echo "set -e" >> /home/${USER}/ros_entrypoint.sh && \ + echo "source /opt/ros/melodic/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \ + echo "source /home/spot/ws/install/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \ + echo "[ -e /home/${USER}/ws/devel/setup.bash ] && source /home/${USER}/ws/devel/setup.bash --" >> /home/${USER}/ros_entrypoint.sh && \ + echo "exec \"\$@\"" >> /home/${USER}/ros_entrypoint.sh + +RUN chmod 0777 /home/${USER}/ros_entrypoint.sh + +RUN echo "source /opt/ros/melodic/share/rosbash/rosbash" >> ~/.bashrc + +## FIXME @@HOME@@ is replaced by create_src_tree_tar in Makefile +ENTRYPOINT ["@@HOME@@/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index ed00da7df5..51f23034c0 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -1,47 +1,126 @@ +HOME=$(shell realpath ~/) + all: $(HOME)/bash.sh - @echo "make build: build docker for spot user" + @echo "make build" $(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 + echo "#!/bin/bash\n\ncd $(CURDIR)\nmake shell\n" > $(HOME)/bash.sh chmod u+x $(HOME)/bash.sh WS_ROOT=$(abspath $(CURDIR)/../../../../../) +HOSTNAME=$(shell hostname) +IPADDRESS=$(shell dig +short $(HOSTNAME).jsk.imi.i.u-tokyo.ac.jp) +IPADDRESS=$(shell hostname -I | sed 's/ /\n/g' | awk '/133.11/') + +ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting +DOCKER_USER=spot +DOCKER_TARGET_NAME=spot_dev_env +$(info "This is dev setting, create $(DOCKER_TARGET_NAME)") +ifneq ($(WS_ROOT),$(HOME)/spot_dev_env) +$(info "We assume WS_ROOT to $(HOME)/spot_dev_env but current WS_ROOT is '$(WS_ROOT)'. Please setup as follows") +$(info "$ mkdir -p ~/spot_dev_env/src") +$(info "$ cd ~/spot_dev_env/src") +$(info "$ git clone $(shell git ls-remote --get-url) -b $(shell git branch | grep ^* | sed s/\*\ // )") +$(error "") +endif +else +DOCKER_USER=$(USER) +DOCKER_TARGET_NAME=$(USER)_dev_env +$(info "This is user setting, create $(DOCKER_TARGET_NAME)") +endif + +define run_uid_not_equal +ifeq ($(shell id -u $$USER),${1}) +$$(info "${2}") +$$(error "Target '$@' need to run except uid ${1} ($(shell id -un ${1})), your current uid is $(shell id -u $$USER) ($(shell echo $$USER))") +endif +endef +define run_uid_equal +ifneq ($(shell id -u $$USER),${1}) +$$(info "${2}") +$$(error "Target '$@' need to run with uid ${1} ($(shell id -un ${1})), your current uid is $(shell id -u $$USER) ($(shell echo $$USER))") +endif +endef + 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}; +docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo --device=/dev/input -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws -ti $(DOCKER_TARGET_NAME) ${1}; +endef + +define create_src_tree_tar +$(info "Create source tree tar file") +$(eval TMP_FILE:=$(shell mktemp)) +$(eval PKG_FILE:=$(WS_ROOT)/package.tar) +cp Dockerfile $(WS_ROOT); sed -i 's+@@HOME@@+$(HOME)+' $(WS_ROOT)/Dockerfile; (cd $(WS_ROOT); find src -iname 'package.xml' -o -path '*jsk_spot_robot/requirements.txt' | tar cf $(TMP_FILE) -T -); chmod 644 $(TMP_FILE) +if test "$$(md5sum $(TMP_FILE) | awk '{print $$1}')" = "$$(md5sum $(PKG_FILE) | awk '{print $$1}')" ; then echo "- We have latest $(PKG_FILE)"; else echo "- source tree is different from $(PKG_FILE), create new source tree tar"; cp $(TMP_FILE) $(PKG_FILE); tar -tf $(PKG_FILE); fi +rm $(TMP_FILE) endef -define docker_run -${1} +define delete_src_tree_tar +rm $(WS_ROOT)/Dockerfile; endef -pre_build: +# setup workspace ( clone source code from jsk_spot_draiver.rosinstall, rosdep install, wstool ) +base_build: + $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000)) # make .ros file mkdir -p ~/.ros + # create tar file to pass package.xml under jsk_spot_robot, so that pip3 install requirements.txt + $(call create_src_tree_tar) # 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 + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build -t spot_dev_env:base_build --target base_build --progress=plain --network=host -t spot_dev_env -f Dockerfile . + # run rosdep update + @$(call run, bash -c '[ -e /etc/ros/rosdep/sources.list.d/ ] || rosdep update -y --include-eol-distros') + # create workspace from jsk_spot_driver.rosinstall + @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) + @if test $$(find $(WS_ROOT)/src/.rosinstall -mmin +1 -print); then echo ".rosinstall has not been chaned, skip wstool update" ; else $(call run, wstool update -t src) fi + # delete package.tar + $(call delete_src_tree_tar) -catkin_build: +# run rosdep install +pre_build: base_build + $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000)) + # create tarfile to pass package.xm under $($WS_ROOT), so that rosdep install finds necessary dependency list + $(call create_src_tree_tar) + # rosdep install + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build -t spot_dev_env:pre_build --target pre_build --progress=plain --network=host -t spot_dev_env -f Dockerfile . + # delete package.tar + $(call delete_src_tree_tar) + +# install dev tools, ros_entrypoint.sh +dev_build: pre_build + $(eval $(call run_uid_equal,1000,This target requires dev setting with uid 1000)) + # dev_build does not use package.xml + $(call create_src_tree_tar) + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build -t spot_dev_env:dev_build --target dev_build --progress=plain --network=host -t spot_dev_env -f Dockerfile . + # delete package.tar + $(call delete_src_tree_tar) + +# run user build +user_build: + $(eval $(call run_uid_not_equal,1000,This target requires user setting, except uid 1000)) + $(call create_src_tree_tar) + cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build -t spot_dev_env:user_build --target user_build --progress=plain --network=host -t $(shell echo $$USER)_dev_env --build-arg USER=$$USER --build-arg UID=$(shell id -u $$USER) --build-arg GID=$(shell id -g $$USER) -f Dockerfile . + +# run catkin build +catkin_config: $(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) + $(call run, catkin config $(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) +catkin_build: + $(call run, catkin build) +# run all build +build: +ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting + make dev_build + make catkin_config CATKIN_CONFIG='--merge-install --blacklist test_tf2' + make catkin_build +else + make user_build + make catkin_config +endif shell: $(call run, bash) From a306bcb71f8c86947c1265478ca37c93ea447122 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 15 Nov 2024 19:38:19 +0900 Subject: [PATCH 242/261] [jsk_spot_driver.rosinstall]: comment outed aques_talk due to missing arm64 library --- jsk_spot_robot/jsk_spot_driver.rosinstall | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 10f5a54ea8..021b6953c2 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -55,10 +55,11 @@ 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 +### aques_talk does not have arm64 libraries +# # 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 From 2b64d4eba1189f45579403cb871d8db6a0afe5f0 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 15 Nov 2024 19:39:20 +0900 Subject: [PATCH 243/261] [jsk_spot_robot/SetupInternalPCAndSpotUser.md] update doc --- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 58 ++++++++++++++++++-- 1 file changed, 54 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index ab7f5a886a..2c763935dc 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -17,14 +17,64 @@ core-io$ passwd ... # change password core-io$ sudo usermod -a -G docker spot # add spot to docker group ``` +# Build Docker containers for Development environment + +Software development on core-is uses Docker containers and we provided two containers. + +1. spot_dev_env +2. `_dev_env`. + +The `spot_dev_env` runs as `spot` user, which will provide basic functions like `roscore` and `jsk_spot_startup` and should be started at boot time. + +Users are expected to run `_dev_env` and create an overay workspace (https://wiki.ros.org/catkin/Tutorials/workspace_overlaying) on top of the workspace of `spot` user. + +## Build `spot_dev_env` (for admin) + +1. Login to spot as `spot` user or any ararch64 machine as user with uid `1000`. Using an aarch64 machine is recommended because `core-io` has less CPU power. +2. Create a ROS workspace in `~/spot_dev_env`. Then change to the `jsk_spot_robot/coreio/base` directory. +3. Run `make build`. This will build the docker image and compile all packages needed for `jsk_spot_startup`. + + +If you have built `spot_dev_env` on a machine other than `core-io, + +1. Run `docker save spot_dev_env:latest -o spot_dev_env.tar` on your aarch64 machine. +2. Copy it to core-io, e.g. `scp spot_dev_env.tar spot@core-io:/tmp'. +3. Run `docker load -i /tmp/spot_dev_env.tar' on `core-io` machine. +4. As this docker image requires `~/spot_dev_env`. Please set up the ROS workspace under `~/spot_dev_env` as the same as your aarch64 machine. +5. Copy `package.tar` in aarch64 machine to `core-io` under workspace. Log in aarch64 machine and go to `~/spot_dev_env`. Then `scp package.tar spot@core-io:~/spot_dev_env/`. +6. Run `make build` in the `jsk_spot_robot/coreio/base` directory. + +## Build `_dev_env` + +1. Create your ROS workspace on `core-io` as your own userid. +2. go to `jsk_spot_robot/coreio/base` and run `make build`. + +# Run Docker container as your development environment + +## Run the startup program (for admin) + +1.Login to `core-io` as `spot` user, run `~/bash.sh` to start `spot_dev_env` container2. run `roslaunch jsk_spot_startup jsk_spot_bringup.launch credential_config:=$(rospack find jsk_spot_startup)/auth/spot_credential.yaml use_gps:=false`. + +Note: +- This process should be run as upstart or supervisor. +- Please do not start multiple `spot_dev_env` containers, it will restart multiple system services including bluetooth, so it will disconnect your joystick. + +## Run your custom development environment + +1. Login to `core-io` as your local user and run `~/bash.sh` to start your build environment. +2. For the first time, you don't have an overlay package, so if you run `rospack list | grep $HOME`, you will get nothing. If you have a package to modify, run `catkin ` and `source devel/setup.bash`, you will have your package on your overlay workspace. + + +# Tips + ### Setup development environment ``` -core-io$ mkdir ~/spot_driver_ws/src -p -core-io$ cd ~/spot_driver_ws/src +core-io$ mkdir ~/spot_dev_env/src -p +core-io$ cd ~/spot_dev_env/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 +core-io$ cd ~/spot_dev_env/src/jsk_robot/jsk_spot_robot/coreio/base +core-io$ make build bash ``` This procedure create `~/bash.sh`. Please use this script when you start development. From e2aa906118f387dc9ef05d7f61cc1713b73abca4 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 16 Nov 2024 16:23:05 +0900 Subject: [PATCH 244/261] [jsk_spot_robot] fix CMakeLists.txt to work on install layout --- jsk_spot_robot/jsk_spot_startup/CMakeLists.txt | 2 +- jsk_spot_robot/spoteus/CMakeLists.txt | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index 7552ce0555..d905b21ec8 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -13,7 +13,7 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY auth config launch scripts services udev_rules template +install(DIRECTORY auth config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) diff --git a/jsk_spot_robot/spoteus/CMakeLists.txt b/jsk_spot_robot/spoteus/CMakeLists.txt index 1289e57fbd..23b23f0bcb 100644 --- a/jsk_spot_robot/spoteus/CMakeLists.txt +++ b/jsk_spot_robot/spoteus/CMakeLists.txt @@ -44,11 +44,12 @@ add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/spot.l add_custom_target(compile_spot ALL DEPENDS ${PROJECT_SOURCE_DIR}/spot.l) -install(DIRECTORY euslisp test +install(DIRECTORY demo 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}) +file(GLOB LISP_FILES *.l) +install(FILES ${LISP_FILES} spot.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) From dddca10384e46b6f49d0aea25246980254acc9f7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 16 Nov 2024 16:27:12 +0900 Subject: [PATCH 245/261] [jsk_spot_robot/coreio/base/Dockerfile] add example to install debian package for urer docker container --- jsk_spot_robot/coreio/base/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile index bba6958625..be53b0674f 100644 --- a/jsk_spot_robot/coreio/base/Dockerfile +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -113,6 +113,9 @@ RUN chmod 0777 /home/${USER}/ros_entrypoint.sh RUN echo "source /opt/ros/melodic/share/rosbash/rosbash" >> ~/.bashrc +## add your favorite package +RUN sudo apt install -y clisp + ## FIXME @@HOME@@ is replaced by create_src_tree_tar in Makefile ENTRYPOINT ["@@HOME@@/ros_entrypoint.sh"] CMD ["bash"] From 60b080005c0a82b1bf7dee9a39fb34bd146da66f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 19 Nov 2024 15:17:55 +0900 Subject: [PATCH 246/261] [jsk_spot_robot/coreio/base/Makefile] add catkin build --summarize --- jsk_spot_robot/coreio/base/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index 51f23034c0..492dfa30a1 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -109,7 +109,7 @@ catkin_config: $(call run, catkin config $(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) catkin_build: - $(call run, catkin build) + $(call run, catkin build --summarize) # run all build build: From 7908647e142399a0e001668dcab6e6e9f1ca1bc0 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 20 Nov 2024 10:34:56 +0900 Subject: [PATCH 247/261] jsk_spot_robot/coreio/bas/Dockerfile: install pip under system space using 'sudo -H pip3' --- jsk_spot_robot/coreio/base/Dockerfile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile index be53b0674f..e343ca403f 100644 --- a/jsk_spot_robot/coreio/base/Dockerfile +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -40,11 +40,11 @@ COPY package.tar /tmp/ RUN tar -C /tmp/ws/ -xvf /tmp/package.tar # 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 +RUN sudo -H pip3 install pip==21.3.1 +RUN sudo -H 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 +RUN sudo -H pip3 install -r /tmp/ws/src/jsk_robot/jsk_spot_robot/requirements.txt # # rosdep install # RUN sudo apt-get update && rosdep install -y -r --from-paths /tmp/ws/src --ignore-src --rosdistro melodic || echo "OK" From 6183e18bd111ee8ffae461a39d535513aa73b8c2 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Nov 2024 11:52:43 +0900 Subject: [PATCH 248/261] jsk_spot_startup/launch/jsk_spot_bringup.launch: add launch_switchbot_ros --- .../jsk_spot_startup/launch/jsk_spot_bringup.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 index 16234a445b..0b484ff7d9 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -3,6 +3,7 @@ + @@ -27,7 +28,8 @@ - + From 0bbb6d7f69051f5e4c565e89dbfb7580427dff43 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Nov 2024 11:53:07 +0900 Subject: [PATCH 249/261] jsk_spot_startup/CMakeLists.txt: add apps to install(DIRECTORY --- jsk_spot_robot/jsk_spot_startup/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt index d905b21ec8..6a4064e0b3 100644 --- a/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_startup/CMakeLists.txt @@ -13,7 +13,7 @@ catkin_package() ############# ## Install ## ############# -install(DIRECTORY auth config launch +install(DIRECTORY apps auth config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) From 56ba9ff975cc1774c2fdd86acebbbb1dc942bd02 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Nov 2024 11:56:33 +0900 Subject: [PATCH 250/261] jsk_spot_robot/coreio/base/Makefile: run 'catkin config' with '--install' --- jsk_spot_robot/coreio/base/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index 492dfa30a1..833766f437 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -115,7 +115,7 @@ catkin_build: build: ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting make dev_build - make catkin_config CATKIN_CONFIG='--merge-install --blacklist test_tf2' + make catkin_config CATKIN_CONFIG='--install --blacklist test_tf2' make catkin_build else make user_build From 1d2382740e3028a86701995b8104c3fd8e4f7d5e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Nov 2024 11:57:30 +0900 Subject: [PATCH 251/261] **FIXME** jsk_spot_robot/coreio/base/Makefile: use --device=/dev/input/js0:/dev/dualsense to run docker --- jsk_spot_robot/coreio/base/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index 833766f437..10be55468a 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -45,7 +45,7 @@ endif endef define run -docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo --device=/dev/input -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws -ti $(DOCKER_TARGET_NAME) ${1}; +docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo --device=/dev/input --device=/dev/input/js0:/dev/dualsense -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws -ti $(DOCKER_TARGET_NAME) ${1}; endef define create_src_tree_tar From 032daa766c894486094635950f0bdc0bc977dca8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 5 Dec 2024 16:40:19 +0900 Subject: [PATCH 252/261] jsk_spot_robot/jsk_spot_driver.rosinstall: update spot_ros to support 'Add world object publisher' https://github.com/k-okada/spot_ros-arm/pull/8 --- jsk_spot_robot/jsk_spot_driver.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_driver.rosinstall b/jsk_spot_robot/jsk_spot_driver.rosinstall index 021b6953c2..b9f4632c81 100644 --- a/jsk_spot_robot/jsk_spot_driver.rosinstall +++ b/jsk_spot_robot/jsk_spot_driver.rosinstall @@ -14,7 +14,7 @@ - git: local-name: spot-ros uri: https://github.com/k-okada/spot_ros-arm.git - version: 0a8c4b29768ae946d7c84aec9a2ae2d185ed97a9 + version: d4bdb07347a8890b96fa45f538000a93645ce21e ## to support custom limb name, we need https://github.com/jsk-ros-pkg/jsk_model_tools/pull/249 - git: local-name: jsk_model_tools From e93eaf774e23c8b023e7d1425f7f7755ca7a6377 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 5 Dec 2024 16:40:53 +0900 Subject: [PATCH 253/261] jsk_spot_robot/coreio/base/Dockerfile: install tmux/expect --- jsk_spot_robot/coreio/base/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_spot_robot/coreio/base/Dockerfile b/jsk_spot_robot/coreio/base/Dockerfile index e343ca403f..1d41e58542 100644 --- a/jsk_spot_robot/coreio/base/Dockerfile +++ b/jsk_spot_robot/coreio/base/Dockerfile @@ -71,6 +71,9 @@ RUN sudo apt-get update && ROS_PYTHON_VERSION=3 rosdep install -y -r --skip-keys ##### FROM pre_build AS dev_build +# extra tools +RUN sudo apt-get update && sudo apt install -y -q tmux expect + 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 && \ From 56da8ee3c58b1bc1c2d0449ba1ddae9f7f93866e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 5 Dec 2024 16:42:12 +0900 Subject: [PATCH 254/261] jsk_spot_robot/coreio/base/Makefile: make shell to support ARGS and NAME --- jsk_spot_robot/coreio/base/Makefile | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index 10be55468a..d6d447d648 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -4,7 +4,10 @@ all: $(HOME)/bash.sh @echo "make build" $(HOME)/bash.sh: - echo "#!/bin/bash\n\ncd $(CURDIR)\nmake shell\n" > $(HOME)/bash.sh + echo "#!/bin/bash\n\n" > $(HOME)/bash.sh +ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting +endif + echo "cd $(CURDIR)\nmake shell ARGS=\$$@\n" >> $(HOME)/bash.sh chmod u+x $(HOME)/bash.sh @@ -45,7 +48,7 @@ endif endef define run -docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo --device=/dev/input --device=/dev/input/js0:/dev/dualsense -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws -ti $(DOCKER_TARGET_NAME) ${1}; +docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo -v /dev:/dev -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws $(shell if test -n "$(NAME)"; then echo '--name $$NAME'; fi) -ti $(DOCKER_TARGET_NAME) ${1}; endef define create_src_tree_tar @@ -122,7 +125,8 @@ else make catkin_config endif shell: - $(call run, bash) + echo "$(ARGS)" > ~/.ros/bash-init.sh + $(call run, bash --init-file /home/$(DOCKER_USER)/.ros/bash-init.sh) emacs: $(call run, emacs -nw) From 15c04d6866e5f9732b1d9f1d7f70a94888b8da1b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 6 Dec 2024 03:46:40 +0000 Subject: [PATCH 255/261] jsk_spot_robot/coreio/base/{Makefile,Dockerfile} update for user_build --- jsk_spot_robot/coreio/base/Makefile | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/coreio/base/Makefile b/jsk_spot_robot/coreio/base/Makefile index d6d447d648..85e0e26acb 100644 --- a/jsk_spot_robot/coreio/base/Makefile +++ b/jsk_spot_robot/coreio/base/Makefile @@ -5,9 +5,20 @@ all: $(HOME)/bash.sh $(HOME)/bash.sh: echo "#!/bin/bash\n\n" > $(HOME)/bash.sh + echo "cd $(CURDIR)\n" >> $(HOME)/bash.sh ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting + $(eval DOCKER_CONTAINER_NAME := spot_dev_env) +else + $(eval DOCKER_CONTAINER_NAME := $(USER)_dev_env) endif - echo "cd $(CURDIR)\nmake shell ARGS=\$$@\n" >> $(HOME)/bash.sh + echo "NAME=$(shell basename $(DOCKER_CONTAINER_NAME) _dev_env)_\$${NAME:-dev}_env" >> $(HOME)/bash.sh + echo "if docker ps --format "{{.Names}}" | grep \$$NAME; then" >> $(HOME)/bash.sh + echo " echo \";; attach \$$NAME, use Ctrl-P Ctrl-Q to exit.\"" >> $(HOME)/bash.sh + echo " docker attach \$$NAME" >> $(HOME)/bash.sh + echo "else" >> $(HOME)/bash.sh + echo " echo \";; run \$$NAME\"" >> $(HOME)/bash.sh + echo " make shell NAME=\$$NAME ARGS=\"\$$@\"" >> $(HOME)/bash.sh + echo "fi\n" >> $(HOME)/bash.sh chmod u+x $(HOME)/bash.sh @@ -48,6 +59,8 @@ endif endef define run +if [ ! -e ~/.ros ]; then mkdir ~/.ros; fi; \ +if [ ! -e ~/.config ]; then mkdir ~/.config; fi; \ docker run --rm --privileged --hostname $(HOSTNAME)-core-io --add-host $(HOSTNAME)-core-io:$(IPADDRESS) --add-host $(HOSTNAME)-core-io.jsk.imi.i.u-tokyo.ac.jp:$(IPADDRESS) --network=host -u=$(shell id -u $$USER):$(shell id -g $$USER) --group-add sudo -v /dev:/dev -v $(shell realpath /home/$(shell id -un 1000))/spot_dev_env:/home/spot/ws -v $(WS_ROOT):/home/$(DOCKER_USER)/ws -v $(HOME)/.ros:/home/$(DOCKER_USER)/.ros -v $(HOME)/.config:/home/$(DOCKER_USER)/.config -w /home/$(DOCKER_USER)/ws $(shell if test -n "$(NAME)"; then echo '--name $$NAME'; fi) -ti $(DOCKER_TARGET_NAME) ${1}; endef @@ -55,7 +68,8 @@ define create_src_tree_tar $(info "Create source tree tar file") $(eval TMP_FILE:=$(shell mktemp)) $(eval PKG_FILE:=$(WS_ROOT)/package.tar) -cp Dockerfile $(WS_ROOT); sed -i 's+@@HOME@@+$(HOME)+' $(WS_ROOT)/Dockerfile; (cd $(WS_ROOT); find src -iname 'package.xml' -o -path '*jsk_spot_robot/requirements.txt' | tar cf $(TMP_FILE) -T -); chmod 644 $(TMP_FILE) +cp Dockerfile $(WS_ROOT); sed -i 's+@@HOME@@+/home/$(shell echo $$USER)+' $(WS_ROOT)/Dockerfile; (cd $(WS_ROOT); find src -iname 'package.xml' -o -path '*jsk_spot_robot/requirements.txt' | tar cf $(TMP_FILE) -T -); chmod 644 $(TMP_FILE) +if [ $(shell id -u $$USER) -ne 1000 ]; then sed -i -n -e '/^FROM dev_build AS user_build$$/,$$p' $(WS_ROOT)/Dockerfile; sed -i 's/^FROM dev_build/FROM spot_dev_env:dev_build/' $(WS_ROOT)/Dockerfile; fi; if test "$$(md5sum $(TMP_FILE) | awk '{print $$1}')" = "$$(md5sum $(PKG_FILE) | awk '{print $$1}')" ; then echo "- We have latest $(PKG_FILE)"; else echo "- source tree is different from $(PKG_FILE), create new source tree tar"; cp $(TMP_FILE) $(PKG_FILE); tar -tf $(PKG_FILE); fi rm $(TMP_FILE) endef @@ -104,7 +118,8 @@ dev_build: pre_build user_build: $(eval $(call run_uid_not_equal,1000,This target requires user setting, except uid 1000)) $(call create_src_tree_tar) - cd $(WS_ROOT); DOCKER_BUILDKIT=1 docker build -t spot_dev_env:user_build --target user_build --progress=plain --network=host -t $(shell echo $$USER)_dev_env --build-arg USER=$$USER --build-arg UID=$(shell id -u $$USER) --build-arg GID=$(shell id -g $$USER) -f Dockerfile . + # multi stage build from different users's image did not work... + cd $(WS_ROOT); DOCKER_BUILDKIT=0 docker build -t $(DOCKER_TARGET_NAME):user_build --progress=plain --network=host -t $(shell echo $$USER)_dev_env --build-arg USER=$$USER --build-arg UID=$(shell id -u $$USER) --build-arg GID=$(shell id -g $$USER) -f Dockerfile . # run catkin build catkin_config: @@ -123,9 +138,10 @@ ifeq ($(shell id -u $$USER),1000) ## This is dev(uid=1000) setting else make user_build make catkin_config + if [ ! -e $(WS_ROOT)/ws/devel ]; then $(call run, catkin build spoteus) $(call run, catkin clean spoteus) fi endif shell: - echo "$(ARGS)" > ~/.ros/bash-init.sh + echo ". ~/.bashrc\n$(ARGS)" > ~/.ros/bash-init.sh $(call run, bash --init-file /home/$(DOCKER_USER)/.ros/bash-init.sh) emacs: From 781dac4fe9daba0d52b7b3d2edf362d7ab8ce7ba Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 6 Dec 2024 04:22:07 +0000 Subject: [PATCH 256/261] jsk_spot_robot: update documents --- jsk_spot_robot/README.md | 37 ++++++++- jsk_spot_robot/SetupInternalPCAndSpotUser.md | 81 ++++++++++++++++++-- 2 files changed, 112 insertions(+), 6 deletions(-) diff --git a/jsk_spot_robot/README.md b/jsk_spot_robot/README.md index 9f642f4988..7e0168e2c5 100644 --- a/jsk_spot_robot/README.md +++ b/jsk_spot_robot/README.md @@ -85,7 +85,42 @@ for example, when you want to move spot 1m forward, type. (send *ri* :sit) ``` -### How to set up a catkin workspace in on-bodyPC +### How to set up a catkin workspace in on-bodyPC (for CORE I/O) + +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 +sudo adduser k-okada docker +``` + +Create a workspace. Make sure that you need to login to spotcore with your account + +```bash +ssh spotcore -l k-okada +mkdir ~/ws/src -p +cd ~/ws/src +git clone https://github.com/k-okada/jsk_robot.git -b spot_arm +cd ~/ws/src/pot_dev_env/src/jsk_robot/jsk_spot_robot/coreio/base +make build all +``` + +To develop your source code, run docker container from `~/bash.sh`. + +```bash +~/bash.sh +``` + +If you need to install deb file, update `add your favorite package` section of `jsk_spot_robot/coreio/base/Dockerfile` and run `make build` under `~/ws/src/pot_dev_env/src/jsk_robot/jsk_spot_robot/coreio/base` directory. Note that you have to run `make build` outside of docker container. + +``` +## add your favorite package +RUN sudo apt install -y clisp +``` + +### How to set up a catkin workspace in on-bodyPC (for SPOT CORE) First create user account into internal PC ``` diff --git a/jsk_spot_robot/SetupInternalPCAndSpotUser.md b/jsk_spot_robot/SetupInternalPCAndSpotUser.md index 2c763935dc..33cdf4b15a 100644 --- a/jsk_spot_robot/SetupInternalPCAndSpotUser.md +++ b/jsk_spot_robot/SetupInternalPCAndSpotUser.md @@ -44,7 +44,7 @@ If you have built `spot_dev_env` on a machine other than `core-io, 5. Copy `package.tar` in aarch64 machine to `core-io` under workspace. Log in aarch64 machine and go to `~/spot_dev_env`. Then `scp package.tar spot@core-io:~/spot_dev_env/`. 6. Run `make build` in the `jsk_spot_robot/coreio/base` directory. -## Build `_dev_env` +## Build `_dev_env` (for users) 1. Create your ROS workspace on `core-io` as your own userid. 2. go to `jsk_spot_robot/coreio/base` and run `make build`. @@ -53,16 +53,87 @@ If you have built `spot_dev_env` on a machine other than `core-io, ## Run the startup program (for admin) -1.Login to `core-io` as `spot` user, run `~/bash.sh` to start `spot_dev_env` container2. run `roslaunch jsk_spot_startup jsk_spot_bringup.launch credential_config:=$(rospack find jsk_spot_startup)/auth/spot_credential.yaml use_gps:=false`. +Login to `core-io` as `spot` user, run `~/start.sh` to start `spot_dev_env` container with tmux to start Bluetooth configuration and jsk_spot_bringup. + +- `start.sh` installs `udev` setting and start `start-tmux.sh` through `~/bash.sh`. + +``` +#!/bin/bash + +if [ ! -e /etc/udev/rules.d/88-dualsense.rules ]; then echo 'KERNEL=="js*", ATTRS{name}=="Wireless Controller", SYMLINK+="dualsense"' | sudo tee /etc/udev/rules.d/88-dualsense.rules; fi + +~/bash.sh ./start-tmux.sh +``` + +- `start-tmux.sh` start `connect-bt.sh`, `launch-jsk-spot.sh` and `bash` within `tmux`. + +``` +#!/bin/bash + +tmux set-option -g history-limit 50000 \; new -d -s spot bash \; send "top" SPACE "-c" ENTER\; new-window -d -n bluetooth bash \; next-window \; send "./connect-bt.sh" ENTER \; new-window -d -n roslaunch bash \; next-window \; send "./launch-jsk-spot.sh" ENTER \; next-window \; new-window -d bash \; attach \; + +``` + +- `launch-jsk-spot.sh` starts `jsk_spot_bringup.launch`, for example +``` +#!/bin/bash + +roslaunch jsk_spot_startup jsk_spot_bringup.launch credential_config:=$(rospack find jsk_spot_startup)/auth/spot_credential.yaml use_voice_text:=true use_gps:=false +``` + +- `connect-bt.sh` connects to Dualsense joystick. +``` +#!/bin/bash -x + +while [ 1 ]; do + if [ -e /dev/input/js0 ]; then sleep 10; continue; fi + sudo hcitool dev + sudo hcitool -i hci0 scan + +/usr/bin/expect -c ' + +set MAC "D0:BC:C1:CB:48:37" +set timeout 30 + +spawn sudo bluetoothctl + +send "scan on\n" + +expect " Device $MAC " { + send "trust $MAC\n" +} + +expect " Device $MAC " { + send "pair $MAC\n" +} + +expect { + "AlreadyExists" { + send "connect $MAC\n" + } + "Paired: yes" { + send "connect $MAC\n" + } +} + +expect "Connection successful" { + send "quit\n" +} +' + sleep 3; +done +``` Note: - This process should be run as upstart or supervisor. -- Please do not start multiple `spot_dev_env` containers, it will restart multiple system services including bluetooth, so it will disconnect your joystick. +- When `~/bash.sh` is invoked a second time, this existing `spot_dev_env` containers is attached. Therefore, when you exit from this environment, the original `start.sh` container will also be destroyed. To exit from attached environment, use the `[Ctrl-p] [Ctrl-q]`. -## Run your custom development environment +## Run your custom development environment (for users) 1. Login to `core-io` as your local user and run `~/bash.sh` to start your build environment. -2. For the first time, you don't have an overlay package, so if you run `rospack list | grep $HOME`, you will get nothing. If you have a package to modify, run `catkin ` and `source devel/setup.bash`, you will have your package on your overlay workspace. +2. `~/bash.sh` attach existing container, so use `[Ctrl-p] [Ctrl-q]` to exit from the shell. Otherwise, for example `exit`, it will destroy all container. +3. If you want to create new contaioner, run `NAME=dummy_name ~/bash.sh` +4. In the first time, you don't have an overlay package, so if you run `rospack list | grep $HOME`, you will get nothing. If you have a package to modify, run `catkin ` and `source devel/setup.bash`, you will have your package on your overlay workspace. # Tips From 4cbe88a883734d16a9582414bde1a53f16648f98 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 18 Dec 2024 11:51:51 +0000 Subject: [PATCH 257/261] jsk_robot_startup/src/quadruped_joystick_teleop.cpp: say something when service call failed --- .../src/quadruped_joystick_teleop.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 d0077eec62..8b171c68e0 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 @@ -93,6 +93,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_estop_hard_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_estop_hard_.getService() << " failed. (" << srv.response.message << ")"); + this->say("estop hard failed" + srv.response.message); } pressed_[button_estop_hard_] = true; } @@ -116,6 +117,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_estop_gentle_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_estop_gentle_.getService() << " failed. (" << srv.response.message << ")"); + this->say("estop gentle failed" + srv.response.message); } pressed_[button_estop_gentle_] = true; } @@ -139,6 +141,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_power_off_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_power_off_.getService() << " failed. (" << srv.response.message << ")"); + this->say("power off failed" + srv.response.message); } pressed_[button_power_off_] = true; } @@ -162,6 +165,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_power_on_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_power_on_.getService() << " failed. (" << srv.response.message << ")"); + this->say("power on failed" + srv.response.message); } pressed_[button_power_on_] = true; } @@ -185,6 +189,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_self_right_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_self_right_.getService() << " failed. (" << srv.response.message << ")"); + this->say("self right failed" + srv.response.message); } pressed_[button_self_right_] = true; } @@ -208,6 +213,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_sit_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_sit_.getService() << " failed. (" << srv.response.message << ")"); + this->say("sit failed" + srv.response.message); } pressed_[button_sit_] = true; } @@ -231,6 +237,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_stand_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_stand_.getService() << " failed. (" << srv.response.message << ")"); + this->say("stand failed" + srv.response.message); } pressed_[button_stand_] = true; } @@ -254,6 +261,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_stop_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_stop_.getService() << " failed. (" << srv.response.message << ")"); + this->say("stop failed" + srv.response.message); } pressed_[button_stop_] = true; } @@ -277,6 +285,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_release_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_release_.getService() << " failed. (" << srv.response.message << ")"); + this->say("release failed" + srv.response.message); } pressed_[button_release_] = true; } @@ -300,6 +309,7 @@ class TeleopManager ROS_INFO_STREAM("Service " << client_claim_.getService() << " succeeded."); } else { ROS_ERROR_STREAM("Service " << client_claim_.getService() << " failed. (" << srv.response.message << ")"); + this->say("claim failed" + srv.response.message); } pressed_[button_claim_] = true; } @@ -329,6 +339,7 @@ class TeleopManager req_next_stair_mode_.data = not req_next_stair_mode_.data; } else { ROS_ERROR_STREAM("Service " << client_stair_mode_.getService() << " failed. (" << srv.response.message << ")"); + this->say("stair mode failed" + srv.response.message); } pressed_[button_stair_mode_] = true; } @@ -351,6 +362,7 @@ class TeleopManager ROS_INFO_STREAM("Service '" << client_dock_.getService() << "' succeeded."); } else { ROS_ERROR_STREAM("Service '" << client_dock_.getService() << "' failed. (" << srv.response.message << ")"); + this->say("dock failed" + srv.response.message); } pressed_axes_[axe_dock_] = true; } @@ -361,6 +373,7 @@ class TeleopManager ROS_INFO_STREAM("Service '" << client_undock_.getService() << "' succeeded."); } else { ROS_ERROR_STREAM("Service '" << client_undock_.getService() << "' failed. (" << srv.response.message << ")"); + this->say("undock failed" + srv.response.message); } pressed_axes_[axe_dock_] = true; } @@ -385,6 +398,7 @@ class TeleopManager ROS_INFO_STREAM("Service '" << client_tuck_.getService() << "' succeeded."); } else { ROS_ERROR_STREAM("Service '" << client_tuck_.getService() << "' failed. (" << srv.response.message << ")"); + this->say("tuck failed" + srv.response.message); } pressed_axes_[axe_tuck_] = true; } @@ -395,6 +409,7 @@ class TeleopManager ROS_INFO_STREAM("Service '" << client_untuck_.getService() << "' succeeded."); } else { ROS_ERROR_STREAM("Service '" << client_untuck_.getService() << "' failed. (" << srv.response.message << ")"); + this->say("untuck failed" + srv.response.message); } pressed_axes_[axe_tuck_] = true; } From f1b51c23ca6c8cca0197e7e91a586b496c280d7b Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Fri, 10 Jan 2025 01:30:06 +0000 Subject: [PATCH 258/261] [strelka] enable launch audio related nodes --- .../launch/include/driver.launch | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 6da02babdd..22e78d329e 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -85,6 +85,15 @@ + + + + + + + + + - + - - - - - - + + + + + From 2e5ecbd1d7c8c04b3b0670aee7165bf446d49c43 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Thu, 16 Jan 2025 09:36:30 +0900 Subject: [PATCH 259/261] [strelka] add option to launch sound_play_jp --- .../launch/include/driver.launch | 19 +++++++++++-------- .../launch/jsk_spot_bringup.launch | 1 + 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 22e78d329e..14f94a414a 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -2,6 +2,7 @@ + @@ -121,14 +122,16 @@ - - - - - - - - + + + + + + + + + + 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 index 0b484ff7d9..787fab8e28 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -4,6 +4,7 @@ + From c44890abc71d28a753bdf7ac3fe1ed7196fa8c63 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Mon, 27 Jan 2025 13:19:08 +0900 Subject: [PATCH 260/261] [jsk_spot_startup] add roslist depend --- jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index fefef06d3d..b1899e7bf8 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -47,6 +47,7 @@ xacro xterm + roslint rostest From e274c392db93acfdf18ed19c6533ba716ed5d45a Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Fri, 7 Feb 2025 20:48:51 +0900 Subject: [PATCH 261/261] [spot] publish compressed hand_depth_in_hand_color_frame topic --- jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch index 14f94a414a..df152b1d40 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/include/driver.launch @@ -101,6 +101,8 @@ args="raw in:=/spot/camera/hand_color/image compressed out:=/spot/camera/hand_color/image" /> +