Squashed commit of the following:

commit 0cc9265 Author: Chris Iverach-Brereton <[email protected]> Date: Fri Aug 13 11:49:21 2021 -0400 Change the default camera to use gpe as the fixed frame; the keeps the robot visible on-screen at all times; using odom the robot can appear off-screen too easily commit 667e26f Author: Chris Iverach-Brereton <[email protected]> Date: Fri Aug 13 11:45:16 2021 -0400 Add a subscriber to the spot panel to monitor the lease status, correctly setting the button states if we call the service e.g. before launching rviz
Rufinius · Aug 13, 2021 · 243dfed · 243dfed
1 parent 8758e6b
commit 243dfed
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 17 deletions.
diff --git a/spot_viz/rviz/robot.rviz b/spot_viz/rviz/robot.rviz
@@ -297,7 +297,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: odom
+    Fixed Frame: gpe
     Frame Rate: 30
   Name: root
   Tools:
@@ -329,9 +329,9 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.1434779316186905
-        Y: 0.6153659224510193
-        Z: 0.8802458643913269
+        X: -0.09166686236858368
+        Y: 0.17000189423561096
+        Z: 0.21145612001419067
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false

diff --git a/spot_viz/src/spot_panel.cpp b/spot_viz/src/spot_panel.cpp
@@ -11,6 +11,8 @@
 #include <QDoubleValidator>
 #include <tf/transform_datatypes.h>
 #include <spot_msgs/SetVelocity.h>
+#include <spot_msgs/LeaseArray.h>
+#include <string.h>
 
 
 namespace spot_viz
@@ -28,6 +30,7 @@ namespace spot_viz
         QVBoxLayout* topLayout = new QVBoxLayout();
         this->setLayout(topLayout);
         topLayout->addWidget(ui);
+        haveLease = false;
 
         sitService_ = nh_.serviceClient<std_srvs::Trigger>("/spot/sit");
         standService_ = nh_.serviceClient<std_srvs::Trigger>("/spot/stand");
@@ -38,6 +41,8 @@ namespace spot_viz
         maxVelocityService_ = nh_.serviceClient<spot_msgs::SetVelocity>("/spot/max_velocity");
         bodyPosePub_ = nh_.advertise<geometry_msgs::Pose>("/spot/body_pose", 1);
 
+        leaseSub_ = nh_.subscribe("/spot/status/leases", 1, &ControlPanel::leaseCallback, this);
+
         claimLeaseButton = this->findChild<QPushButton*>("claimLeaseButton");
         releaseLeaseButton = this->findChild<QPushButton*>("releaseLeaseButton");
         powerOnButton = this->findChild<QPushButton*>("powerOnButton");
@@ -114,15 +119,15 @@ namespace spot_viz
         label->setText(QString((current_text+ limit_range).c_str()));
     }
 
-    void ControlPanel::toggleControlButtons() {
-        claimLeaseButton->setEnabled(!claimLeaseButton->isEnabled());
-        releaseLeaseButton->setEnabled(!releaseLeaseButton->isEnabled());
-        powerOnButton->setEnabled(!powerOnButton->isEnabled());
-        powerOffButton->setEnabled(!powerOffButton->isEnabled());
-        sitButton->setEnabled(!sitButton->isEnabled());
-        standButton->setEnabled(!standButton->isEnabled());
-        setBodyPoseButton->setEnabled(!setBodyPoseButton->isEnabled());
-        setMaxVelButton->setEnabled(!setMaxVelButton->isEnabled());
+    void ControlPanel::setControlButtons() {
+        claimLeaseButton->setEnabled(!haveLease);
+        releaseLeaseButton->setEnabled(haveLease);
+        powerOnButton->setEnabled(haveLease);
+        powerOffButton->setEnabled(haveLease);
+        sitButton->setEnabled(haveLease);
+        standButton->setEnabled(haveLease);
+        setBodyPoseButton->setEnabled(haveLease);
+        setMaxVelButton->setEnabled(haveLease);
     }
 
     bool ControlPanel::callTriggerService(ros::ServiceClient service, std::string serviceName) {
@@ -146,6 +151,29 @@ namespace spot_viz
         }
     }
 
+    void ControlPanel::leaseCallback(const spot_msgs::LeaseArray::ConstPtr &leases) {
+        // check to see if the body is already owned by the ROS node
+        // the resource will be "body" and the lease_owner.client_name will begin with "ros_spot"
+        // if the claim exists, treat this as a successful click of the Claim button
+        // if the claim does not exist, treat this as a click of the Release button
+
+        bool msg_has_lease = false;
+        for (int i=leases->resources.size()-1; i>=0; i--) {
+            const spot_msgs::LeaseResource &resource = leases->resources[i];
+            bool right_resource = resource.resource.compare("body") == 0;
+            bool owned_by_ros = resource.lease_owner.client_name.compare(0, 8, "ros_spot") == 0;
+
+            if (right_resource && owned_by_ros) {
+                msg_has_lease = true;
+            }
+        }
+
+        if (msg_has_lease != haveLease) {
+            haveLease = msg_has_lease;
+            setControlButtons();
+        }
+    }
+
     void ControlPanel::sit() {
         callTriggerService(sitService_, "sit");
     }
@@ -165,14 +193,16 @@ namespace spot_viz
     void ControlPanel::claimLease() {
         if (callTriggerService(claimLeaseService_, "claim lease")) {
             // If we successfully got the lease, enable buttons to command the robot
-            toggleControlButtons();
+            haveLease = true;
+            setControlButtons();
         }
     }
 
     void ControlPanel::releaseLease() {
         if (callTriggerService(releaseLeaseService_, "release lease")) {
             // If we successfully got the lease, enable buttons to command the robot
-            toggleControlButtons();
+            haveLease = false;
+            setControlButtons();
         }
     }
 

diff --git a/spot_viz/src/spot_panel.hpp b/spot_viz/src/spot_panel.hpp
@@ -10,6 +10,7 @@
 #include <QPushButton>
 #include <QLabel>
 #include <QDoubleSpinBox>
+#include <spot_msgs/LeaseArray.h>
 
 namespace spot_viz
 {
@@ -35,10 +36,11 @@ class ControlPanel : public rviz::Panel
 
     private:
 
-    void toggleControlButtons();
+    void setControlButtons();
     void toggleBodyPoseButtons();
     bool callTriggerService(ros::ServiceClient service, std::string serviceName);
     void updateLabelTextWithLimit(QLabel* label, double limit);
+    void leaseCallback(const spot_msgs::LeaseArray::ConstPtr &leases);
 
     ros::NodeHandle nh_;
     ros::ServiceClient sitService_;
@@ -49,6 +51,7 @@ class ControlPanel : public rviz::Panel
     ros::ServiceClient powerOffService_;
     ros::ServiceClient maxVelocityService_;
     ros::Publisher bodyPosePub_;
+    ros::Subscriber leaseSub_;
 
     QPushButton* claimLeaseButton;
     QPushButton* releaseLeaseButton;
@@ -80,4 +83,4 @@ class ControlPanel : public rviz::Panel
 
 } // end namespace spot_viz
 
-#endif // SPOT_CONTROL_PANEL_H
+#endif // SPOT_CONTROL_PANEL_H