[Personalrobots-commit] SF.net SVN: personalrobots:[5413] pkg/trunk

[<stu...@us...>]

Revision: 5413
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5413&view=rev
Author:   stuglaser
Date:     2008-10-15 23:31:25 +0000 (Wed, 15 Oct 2008)

Log Message:
-----------
Caster calibration is now asynchronous (they all calibrate at once).

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
    pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h	2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h	2008-10-15 23:31:25 UTC (rev 5413)
@@ -35,6 +35,8 @@
 #define CASTER_CALIBRATION_CONTROLLER_H
 
 #include "pr2_mechanism_controllers/caster_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
 #include <robot_mechanism_controllers/CalibrateJoint.h>
 
 namespace controller {
@@ -53,7 +55,7 @@
    */
   virtual void update();
 
-  bool calibrated() { return state_ == STOPPED; }
+  bool calibrated() { return state_ == CALIBRATED; }
   void beginCalibration()
   {
     if (state_ == INITIALIZED)
@@ -62,7 +64,7 @@
 
 protected:
 
-  enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+  enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
   int state_;
 
   double search_velocity_;
@@ -90,8 +92,12 @@
                         robot_mechanism_controllers::CalibrateJoint::response &resp);
 
 private:
+  mechanism::RobotState *robot_;
   CasterCalibrationController c_;
   AdvertisedServiceGuard guard_calibrate_;
+
+  double last_publish_time_;
+  misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
 };
 
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp	2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp	2008-10-15 23:31:25 UTC (rev 5413)
@@ -107,6 +107,7 @@
   case INITIALIZED:
     cc_.steer_velocity_ = 0.0;
     cc_.drive_velocity_ = 0.0;
+    state_ = BEGINNING;
     break;
   case BEGINNING:
     original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
@@ -117,10 +118,12 @@
     bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
     if (switch_state_ != original_switch_state_)
     {
+      Actuator a;
+      mechanism::JointState j;
       std::vector<Actuator*> fake_a;
       std::vector<mechanism::JointState*> fake_j;
-      fake_a.push_back(new Actuator);
-      fake_j.push_back(new mechanism::JointState);
+      fake_a.push_back(&a);
+      fake_j.push_back(&j);
 
       // Where was the joint when the optical switch triggered?
       if (original_switch_state_ == true)
@@ -135,32 +138,49 @@
 
       actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
 
-      state_ = STOPPED;
+      state_ = CALIBRATED;
     }
     break;
   }
-  case STOPPED:
+  case CALIBRATED:
     cc_.steer_velocity_ = 0.0;
     break;
   }
 
-  cc_.update();
+  if (state_ != CALIBRATED)
+    cc_.update();
 }
 
 
 ROS_REGISTER_CONTROLLER(CasterCalibrationControllerNode)
 
 CasterCalibrationControllerNode::CasterCalibrationControllerNode()
+: last_publish_time_(0), pub_calibrated_(NULL)
 {
 }
 
 CasterCalibrationControllerNode::~CasterCalibrationControllerNode()
 {
+  if (pub_calibrated_)
+    delete pub_calibrated_;
 }
 
 void CasterCalibrationControllerNode::update()
 {
   c_.update();
+
+  if (c_.calibrated())
+  {
+    if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+    {
+      assert(pub_calibrated_);
+      if (pub_calibrated_->trylock())
+      {
+        last_publish_time_ = robot_->hw_->current_time_;
+        pub_calibrated_->unlockAndPublish();
+      }
+    }
+  }
 }
 
 
@@ -176,6 +196,8 @@
 
 bool CasterCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
 {
+  assert(robot);
+  robot_ = robot;
   ros::node *node = ros::node::instance();
 
   std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
@@ -190,6 +212,8 @@
   node->advertise_service(topic + "/calibrate", &CasterCalibrationControllerNode::calibrateCommand, this);
   guard_calibrate_.set(topic + "/calibrate");
 
+  pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
   return true;
 }
 

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp	2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp	2008-10-15 23:31:25 UTC (rev 5413)
@@ -130,10 +130,12 @@
     bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
     if (switch_state_ != original_switch_state_)
     {
+      Actuator a;
+      mechanism::JointState j;
       std::vector<Actuator*> fake_a;
       std::vector<mechanism::JointState*> fake_j;
-      fake_a.push_back(new Actuator);
-      fake_j.push_back(new mechanism::JointState);
+      fake_a.push_back(&a);
+      fake_j.push_back(&j);
 
       // Where was the joint when the optical switch triggered?
       if (original_switch_state_ == true)
@@ -171,6 +173,8 @@
 
 JointCalibrationControllerNode::~JointCalibrationControllerNode()
 {
+  if (pub_calibrated_)
+    delete pub_calibrated_;
 }
 
 void JointCalibrationControllerNode::update()

Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py	2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py	2008-10-15 23:31:25 UTC (rev 5413)
@@ -37,6 +37,7 @@
 import rostools
 import copy
 import threading
+from time import sleep
 
 # Loads interface with the robot.
 rostools.update_path('teleop_robot')
@@ -44,11 +45,9 @@
 rostools.update_path('mechanism_control')
 
 import rospy
+from std_msgs.msg import Empty
 from robot_srvs.srv import *
 from robot_mechanism_controllers.srv import *
-from std_srvs.srv import *
-from robot_srvs.srv import *
-from robot_mechanism_controllers.srv import *
 from mechanism_control.srv import *
 
 def slurp(filename):
@@ -62,7 +61,8 @@
 kill_controller = rospy.ServiceProxy('kill_controller', KillController)
 
 
-def calibrate_optically(config):
+# Hits the joint stops
+def calibrate_blindly(config):
     resp = spawn_controller(config)
     if len(resp.ok) != 1 or not resp.ok[0]:
         print "FAIL"
@@ -75,47 +75,43 @@
         kill_controller(name)
     print "Calibrated"
 
-def calibrate_manually(config):
+
+
+def calibrate(config):
+    # Spawns the controllers
     resp = spawn_controller(config)
-    if len(resp.ok) != 1 or not resp.ok[0]:
-        print "FAIL"
-        return
-    name = resp.name[0]
-    begin = rospy.ServiceProxy("/%s/begin_manual_calibration" % name, CalibrateJoint)
-    end = rospy.ServiceProxy("/%s/end_manual_calibration" % name, CalibrateJoint)
-    begin()
-    print "Move the joint to the limits, and then hit enter"
-    raw_input()
-    end()
-    kill_controller(name)
-    print "Calibrated manually"
 
-# Hits the joint stops
-def calibrate_blindly(config):
-    resp = spawn_controller(config)
-    if len(resp.ok) != 1 or not resp.ok[0]:
-        print "FAIL"
-        return
-    name = resp.name[0]
+    # Accumulates the list of spawned controllers
+    launched = []
     try:
-        do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
-        do_calibration()
+        for i in range(len(resp.ok)):
+            if not resp.ok[i]:
+                print "Failed: %s" % resp.name[i]
+            else:
+                launched.append(resp.name[i])
+                print "Launched: %s" % ', '.join(launched)
+
+        # Sets up callbacks for calibration completion
+        waiting_for = launched[:]
+        def calibrated(msg, name):  # Somewhat not thread-safe
+            if name in waiting_for:
+                waiting_for.remove(name)
+        for name in waiting_for:
+            rospy.Subscriber("/%s/calibrated" % name, Empty, calibrated, name)
+
+        # Waits until all the controllers have calibrated
+        while waiting_for:
+            print "Waiting for: %s" % ', '.join(waiting_for)
+            sleep(0.5)
     finally:
-        kill_controller(name)
-    print "Calibrated"
+        [kill_controller(name) for name in launched]
 
 
-class FunThread(threading.Thread):
-    def __init__(self, fun, *args):
-        self.fun = fun
-        self.start()
+rospy.init_node('calibration', anonymous=True)
 
-    def run():
-        self.fun(*args)
 
 
 template = '''
-<controllers>
   <controller type="CasterCalibrationControllerNode" name="cal_caster_SUFFIX" topic="cal_caster_SUFFIX">
     <calibrate joint="caster_SUFFIX_joint"
                actuator="caster_SUFFIX_motor"
@@ -127,10 +123,19 @@
     <caster_pid p="6" i="0" d="0" iClamp="0" />
     <wheel_pid p="4" i="0" d="0" iClamp="0" />
   </controller>
-</controllers>
 '''
 
-calibrate_optically(template.replace('SUFFIX', 'front_left'))
-calibrate_optically(template.replace('SUFFIX', 'front_right'))
-calibrate_optically(template.replace('SUFFIX', 'rear_left'))
-calibrate_optically(template.replace('SUFFIX', 'rear_right'))
+
+calibrate(
+'''
+<controllers>
+'''
++ template.replace('SUFFIX', 'front_left')
++ template.replace('SUFFIX', 'front_right')
++ template.replace('SUFFIX', 'rear_left')
++ template.replace('SUFFIX', 'rear_right')
++ '''
+</controllers>
+''')
+
+print "Calibrated"


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.