improve calibration consistency, pass robot description new way (#30)

ycheng517 · Dec 2, 2024 · 155cea5 · 155cea5
1 parent 7f42062
commit 155cea5
Show file tree

Hide file tree

Showing 8 changed files with 45 additions and 24 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -80,7 +80,13 @@
       "variant": "cpp",
       "*.ipp": "cpp",
       "callback": "cpp",
-      "image": "cpp"
+      "image": "cpp",
+      "bufferobject": "cpp",
+      "mixinvector": "cpp",
+      "texture": "cpp",
+      "vertexarraystate": "cpp",
+      "buffered_value": "cpp",
+      "fast_back_stack": "cpp"
     },
     "ros.distro": "iron",
     "C_Cpp.default.includePath": [

diff --git a/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp b/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
@@ -51,8 +51,6 @@ class ARHardwareInterface : public hardware_interface::SystemInterface {
   std::vector<double> joint_effort_commands_;
 
   // Misc
-  bool calibrated_ = false;
-
   void init_variables();
   double degToRad(double deg) { return deg / 180.0 * M_PI; };
   double radToDeg(double rad) { return rad / M_PI * 180.0; };

diff --git a/ar_hardware_interface/launch/ar_hardware.launch.py b/ar_hardware_interface/launch/ar_hardware.launch.py
@@ -55,9 +55,9 @@ def generate_launch_description():
         executable="ros2_control_node",
         parameters=[
             update_rate_config_file,
-            robot_description,
             ParameterFile(joint_controllers_cfg, allow_substs=True),
         ],
+        remappings=[('~/robot_description', 'robot_description')],
         output="screen",
     )
 

diff --git a/ar_hardware_interface/src/ar_hardware_interface.cpp b/ar_hardware_interface/src/ar_hardware_interface.cpp
@@ -28,6 +28,19 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_init(
   if (!success) {
     return hardware_interface::CallbackReturn::ERROR;
   }
+
+  // calibrate joints if needed
+  bool calibrate = info_.hardware_parameters.at("calibrate") == "True";
+  if (calibrate) {
+    // run calibration
+    RCLCPP_INFO(logger_, "Running joint calibration...");
+    if (!driver_.calibrateJoints()) {
+      RCLCPP_INFO(logger_, "calibration failed.");
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+  }
+  RCLCPP_INFO(logger_, "calibration succeeded.");
+
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
@@ -63,17 +76,6 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_activate(
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  // calibrate joints if needed
-  bool calibrate = info_.hardware_parameters.at("calibrate") == "True";
-  if (calibrate && !calibrated_) {
-    // run calibration
-    RCLCPP_INFO(logger_, "Running joint calibration...");
-    if (!driver_.calibrateJoints()) {
-      return hardware_interface::CallbackReturn::ERROR;
-    }
-    calibrated_ = true;
-  }
-
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 

diff --git a/ar_hardware_interface/src/ar_servo_gripper_hw_interface.cpp b/ar_hardware_interface/src/ar_servo_gripper_hw_interface.cpp
@@ -47,9 +47,6 @@ hardware_interface::CallbackReturn ARServoGripperHWInterface::on_deactivate(
 std::vector<hardware_interface::StateInterface>
 ARServoGripperHWInterface::export_state_interfaces() {
   std::vector<hardware_interface::StateInterface> state_interfaces;
-
-  RCLCPP_INFO(logger_, "Debug: Exporting state interfaces for joint %s",
-              info_.joints[0].name.c_str());
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     state_interfaces.emplace_back(info_.joints[i].name, "position", &position_);
     state_interfaces.emplace_back(info_.joints[i].name, "velocity", &velocity_);

diff --git a/ar_hardware_interface/src/teensy_driver.cpp b/ar_hardware_interface/src/teensy_driver.cpp
@@ -155,6 +155,7 @@ bool TeensyDriver::exchange(std::string outMsg) {
   std::string inMsg;
   std::string errTransmit = "";
 
+  // RCLCPP_INFO(logger_, "Sending message: %s", outMsg.c_str());
   if (!transmit(outMsg, errTransmit)) {
     RCLCPP_ERROR(logger_, "Error in transmit: %s", errTransmit.c_str());
     return false;

diff --git a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino
@@ -348,6 +348,18 @@ void MoveTo(String inData, int* motorSteps) {
   MoveTo(cmdEncSteps, motorSteps);
 }
 
+bool AtPosition(const int* targetMotorSteps, const int* currMotorSteps,
+                const int maxDiff) {
+  bool allDone = true;
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    int diffEncSteps = targetMotorSteps[i] - currMotorSteps[i];
+    if (abs(diffEncSteps) > maxDiff) {
+      allDone = false;
+    }
+  }
+  return allDone;
+}
+
 void setAllMaxSpeeds() {
   for (int i = 0; i < NUM_JOINTS; ++i) {
     stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] *
@@ -483,14 +495,20 @@ bool doCalibrationRoutine(String& outputMsg) {
   }
 
   // return to original position
+  unsigned long startTime = millis();
   int curMotorSteps[NUM_JOINTS];
   readMotorSteps(curMotorSteps);
-  MoveTo(REST_MOTOR_STEPS[MODEL], curMotorSteps);
-  for (int i = 0; i < NUM_JOINTS; ++i) {
-    if (estop_pressed) {
+  while (!AtPosition(REST_MOTOR_STEPS[MODEL], curMotorSteps, 5)) {
+    if (millis() - startTime > 10000) {
+      outputMsg = "ER: Failed to return to original position.";
       return false;
     }
-    stepperJoints[i].runToPosition();
+
+    readMotorSteps(curMotorSteps);
+    MoveTo(REST_MOTOR_STEPS[MODEL], curMotorSteps);
+    for (int i = 0; i < NUM_JOINTS; ++i) {
+      safeRun(stepperJoints[i]);
+    }
   }
 
   // calibration done, send calibration values

diff --git a/ar_moveit_config/launch/ar_moveit.launch.py b/ar_moveit_config/launch/ar_moveit.launch.py
@@ -167,9 +167,8 @@ def generate_launch_description():
         "publish_geometry_updates": True,
         "publish_state_updates": True,
         "publish_transforms_updates": True,
-        "publish_robot_description": True,
+        # Added due to https://github.com/moveit/moveit2_tutorials/issues/528
         "publish_robot_description_semantic": True,
-        # Two above added due to https://github.com/moveit/moveit2_tutorials/issues/528
     }
 
     # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers