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

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

Revision: 19244
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19244&view=rev
Author:   stuglaser
Date:     2009-07-21 00:22:16 +0000 (Tue, 21 Jul 2009)

Log Message:
-----------
JointUDCalibrationController with nodehandle-style init

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
    pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h	2009-07-21 00:16:07 UTC (rev 19243)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h	2009-07-21 00:22:16 UTC (rev 19244)
@@ -34,6 +34,7 @@
 
 #pragma once
 
+#include "ros/node_handle.h"
 #include "mechanism_model/robot.h"
 #include "robot_mechanism_controllers/joint_velocity_controller.h"
 #include "realtime_tools/realtime_publisher.h"
@@ -50,6 +51,7 @@
   virtual ~JointUDCalibrationController();
 
   virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+  virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
 
   virtual void update();
 
@@ -62,6 +64,11 @@
 
 protected:
 
+  mechanism::RobotState* robot_;
+  ros::NodeHandle node_;
+  boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
+  double last_publish_time_;
+
   enum { INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED };
   int state_;
   int countdown_;

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp	2009-07-21 00:16:07 UTC (rev 19243)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp	2009-07-21 00:22:16 UTC (rev 19244)
@@ -38,10 +38,11 @@
 using namespace std;
 using namespace controller;
 
-//ROS_REGISTER_CONTROLLER(JointUDCalibrationController)
+ROS_REGISTER_CONTROLLER(JointUDCalibrationController)
 
 JointUDCalibrationController::JointUDCalibrationController()
-  : state_(INITIALIZED), actuator_(NULL), joint_(NULL), transmission_(NULL)
+: robot_(NULL), last_publish_time_(0), state_(INITIALIZED),
+  actuator_(NULL), joint_(NULL), transmission_(NULL)
 {
 }
 
@@ -110,6 +111,75 @@
   return true;
 }
 
+bool JointUDCalibrationController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
+{
+  robot_ = robot;
+  node_ = n;
+
+  // Joint
+
+  std::string joint_name;
+  if (!node_.getParam("joint", joint_name))
+  {
+    ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
+    return false;
+  }
+  if (!(joint_ = robot->getJointState(joint_name)))
+  {
+    ROS_ERROR("Could not find joint %s (namespace: %s)",
+              joint_name.c_str(), node_.getNamespace().c_str());
+    return false;
+  }
+
+  // Actuator
+
+  std::string actuator_name;
+  if (!node_.getParam("actuator", actuator_name))
+  {
+    ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
+    return false;
+  }
+  if (!(actuator_ = robot->model_->getActuator(actuator_name)))
+  {
+    ROS_ERROR("Could not find actuator %s (namespace: %s)",
+              actuator_name.c_str(), node_.getNamespace().c_str());
+    return false;
+  }
+
+  // Transmission
+
+  std::string transmission_name;
+  if (!node_.getParam("transmission", transmission_name))
+  {
+    ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
+    return false;
+  }
+  if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
+  {
+    ROS_ERROR("Could not find transmission %s (namespace: %s)",
+              transmission_name.c_str(), node_.getNamespace().c_str());
+    return false;
+  }
+
+  if (!node_.getParam("velocity", search_velocity_))
+  {
+    ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
+    return false;
+  }
+
+  // Contained velocity controller
+
+  if (!vc_.init(robot, node_))
+    return false;
+
+  // "Calibrated" topic
+  pub_calibrated_.reset(
+    new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
+
+  return true;
+}
+
+
 void JointUDCalibrationController::update()
 {
   assert(joint_);
@@ -171,6 +241,18 @@
     break;
   }
   case CALIBRATED:
+    if (pub_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();
+        }
+      }
+    }
     break;
   }
 

Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml	2009-07-21 00:16:07 UTC (rev 19243)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml	2009-07-21 00:22:16 UTC (rev 19244)
@@ -2,7 +2,7 @@
   type: JointUDCalibrationController
   joint: torso_lift_joint
   actuator: torso_lift_motor
-  transmision: torso_lift_trans
+  transmission: torso_lift_trans
   velocity: -10.0
   pid:
     p: 2000000
@@ -20,7 +20,7 @@
     i: 0.5
     d: 0
     i_clamp: 1.0
-call_shoulder_pan:
+cal_l_shoulder_pan:
   type: JointUDCalibrationController
   joint: l_shoulder_pan_joint
   actuator: l_shoulder_pan_motor
@@ -188,7 +188,7 @@
 
 cal_caster_fl:
   type: CasterCalibrationController
-  <<: caster_calibration
+  <<: *caster_calibration
   actuator: fl_caster_rotation_motor
   joint: fl_caster_rotation_joint
   transmission: fl_caster_rotation_trans
@@ -198,7 +198,7 @@
     wheel_r: fl_caster_r_wheel_joint
 cal_caster_fr:
   type: CasterCalibrationController
-  <<: caster_calibration
+  <<: *caster_calibration
   actuator: fr_caster_rotation_motor
   joint: fr_caster_rotation_joint
   transmission: fr_caster_rotation_trans
@@ -208,7 +208,7 @@
     wheel_r: fr_caster_r_wheel_joint
 cal_caster_bl:
   type: CasterCalibrationController
-  <<: caster_calibration
+  <<: *caster_calibration
   actuator: bl_caster_rotation_motor
   joint: bl_caster_rotation_joint
   transmission: bl_caster_rotation_trans
@@ -218,7 +218,7 @@
     wheel_r: bl_caster_r_wheel_joint
 cal_caster_br:
   type: CasterCalibrationController
-  <<: caster_calibration
+  <<: *caster_calibration
   actuator: br_caster_rotation_motor
   joint: br_caster_rotation_joint
   transmission: br_caster_rotation_trans


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