Added button to calibrate arm in teleop.

arbie_teleop/include/command_publisher.h

@@ -4,6 +4,7 @@
 #include "ros/ros.h"
 #include "std_msgs/Float64.h"
 #include "std_msgs/Float64MultiArray.h"
+#include "std_srvs/Trigger.h"
 #include "geometry_msgs/Twist.h"
 #include "arbie_msgs/ManipulationCommand.h"
 
@@ -20,6 +21,9 @@ public:
     void publish_all();
 
     bool send_gripper_command(const std::string &command, const std::string &argument);
+
+    bool calibrate();
+
 private:
     // Tracks
     ros::Publisher tracks_command_pub;
@@ -36,6 +40,10 @@ private:
     // Gripper commands (move to named pose)
     ros::ServiceClient gripper_command_service;
     arbie_msgs::ManipulationCommand gripper_command_msg;
+
+    // Calibrate arm
+    ros::ServiceClient calibrate_service;
+    std_srvs::Trigger calibrate_msg;
 };
 
 #endif

arbie_teleop/src/command_publisher.cpp

@@ -38,6 +38,11 @@ CommandPublisher::CommandPublisher(ros::NodeHandle &n)
     gripper_command_service = n.serviceClient<arbie_msgs::ManipulationCommand>(
         "gripper_command"
     );
+
+    // Calibration service
+    calibrate_service = n.serviceClient<std_srvs::Trigger>(
+        "calibrate"
+    );
 }
 
 void CommandPublisher::set_gripper_velocity(double x, double theta, double z, double yaw, double pitch, double roll, double gripper)
@@ -83,3 +88,13 @@ bool CommandPublisher::send_gripper_command(
     ROS_INFO("Gripper command failed");
     return false;
 }
+
+bool CommandPublisher::calibrate()
+{
+    if (calibrate_service.call(calibrate_msg)) {
+        ROS_INFO("Calibrate status: %d", calibrate_msg.response.success);
+        return calibrate_msg.response.success;
+    }
+    ROS_INFO("Calibrate command failed");
+    return false;
+}

arbie_teleop/src/main.cpp

@@ -58,6 +58,10 @@ int main(int argc, char **argv)
             if (joystick_listener.query_button_state(JoyButton::X) == JoyButtonState::PRESSED) {
                 command_publisher.send_gripper_command("named_target", "ready");
             }
+
+            if (joystick_listener.query_button_state(JoyButton::START) == JoyButtonState::PRESSED) {
+                command_publisher.calibrate();
+            }
         }
 
         command_publisher.publish_all();