Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[20792] pkg/trunk (Page 44)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 20792
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20792&view=rev
Author:   wattsk
Date:     2009-08-05 17:45:00 +0000 (Wed, 05 Aug 2009)

Log Message:
-----------
Changed RT service call, ported joint_qual controllers to controllers API, updated control_toolbox for new API

Modified Paths:
--------------
    pkg/trunk/controllers/control_toolbox/include/control_toolbox/dither.h
    pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h
    pkg/trunk/controllers/control_toolbox/src/dither.cpp
    pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/srv/HoldSetData.srv
    pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py
    pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_srv_call.h

Added Paths:
-----------
    pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg

Modified: pkg/trunk/controllers/control_toolbox/include/control_toolbox/dither.h
===================================================================

--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/dither.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/dither.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -35,6 +35,8 @@
 
 #include <cstdlib>
 #include <ctime>
+#include <math.h>
+#include <ros/ros.h>
 
 namespace control_toolbox {
 /***************************************************/
@@ -69,7 +71,7 @@
    *
     * \param amplitude The amplitude of the dither, \f$A\f$.
    */
-  void init(double amplitude);
+  bool init(const ros::NodeHandle &n);
 
 private:
   double amplitude_;                        /**< Amplitude of the sweep. */

Modified: pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -81,7 +81,7 @@
    * \param duration  The duration of the sweep, \f$T\f$.
    * \param amplitude The amplitude of the sweep, \f$A\f$.
    */
-  void init(double start_freq, double end_freq, double duration, double amplitude);
+  bool init(double start_freq, double end_freq, double duration, double amplitude);
 
 private:
   double amplitude_;                        /**< Amplitude of the sweep. */

Modified: pkg/trunk/controllers/control_toolbox/src/dither.cpp
===================================================================
--- pkg/trunk/controllers/control_toolbox/src/dither.cpp	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/control_toolbox/src/dither.cpp	2009-08-05 17:45:00 UTC (rev 20792)
@@ -34,7 +34,6 @@
 
 // Original version: Kevin Watts <wa...@wi...>
 
-#include <math.h>
 #include <control_toolbox/dither.h>
 
 
@@ -62,10 +61,15 @@
 {
 }
 
-void Dither::init(double amplitude)
+bool Dither::init(const ros::NodeHandle &n)
 {
-  //keep the amplitude around
-  amplitude_ = amplitude;
+  if (!n.getParam("amplitude", amplitude_) || amplitude_ < 0.0)
+  {
+    ROS_ERROR("Dither amplitude not set properly.");
+    return false;
+  }
+  
+  return true;
 }
 
 double Dither::update()

Modified: pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp
===================================================================
--- pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp	2009-08-05 17:45:00 UTC (rev 20792)
@@ -52,9 +52,13 @@
 {
 }
 
-void SineSweep::init(double start_freq, double end_freq, double duration, double amplitude)
+bool SineSweep::init(double start_freq, double end_freq, double duration, double amplitude)
 {
-  //keep the amplitude around
+  if (start_freq > end_freq)
+    return false;
+  if (duration < 0 || amplitude < 0)
+    return false;
+  
   amplitude_ = amplitude;
   duration_ = duration;
   //calculate the angular fequencies
@@ -67,6 +71,8 @@
   
   //zero out the command
   cmd_ = 0.0;
+
+  return true;
 }
 
 double SineSweep::update( double dt)

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -47,20 +47,25 @@
 */
 /***************************************************/
 
-
-#include <ros/node.h>
+#include <ros/ros.h>
 #include <string>
 #include <math.h>
 #include <joint_qualification_controllers/RobotData.h>
-#include <realtime_tools/realtime_publisher.h>
 #include <realtime_tools/realtime_srv_call.h>
 #include <mechanism_control/controller.h>
+#include <boost/scoped_ptr.hpp>
 
-
-
 namespace controller
 {
 
+
+/***************************************************/
+/*! \class controller::CheckoutControllerNode
+    \brief Checkout Controller
+
+  */
+/***************************************************/
+
 class CheckoutController : public Controller
 {
 
@@ -73,66 +78,56 @@
   /*!
    * \brief Functional way to initialize.
    * \param *robot The robot that is being controlled.
+   * \param &n NodeHandle of mechanism control
    */
-  void init( double timeout, mechanism::RobotState *robot);
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+  bool init( mechanism::RobotState *robot, const ros::NodeHandle &n);
 
-
   /*!
-   * \brief Perform the test analysis
+   * \brief Called when controller is started or restarted
    */
-  void analysis(double time);
-
+  bool starting();
+  
   /*!
    * \brief Checks joint, actuator status for calibrated and enabled.
    */
-  virtual void update();
-  
-  bool done() { return state_ == DONE; }
+  void update();
 
-  int joint_count_;
-  int actuator_count_;
-  
-  joint_qualification_controllers::RobotData::Request robot_data_;
+  /*! 
+   * \brief Sends data, returns true if sent
+   */
+  bool sendData();
 
-private:
+  /*!
+   * \brief Perform the test analysis
+   */
+  void analysis(double time);
 
+
+private:
   mechanism::RobotState *robot_;          /**< Pointer to robot structure. */
   double initial_time_;                   /**< Start time of the test. */
  
   double timeout_;
+
+  joint_qualification_controllers::RobotData::Request robot_data_;
   
   int state_;
 
-};
+  int joint_count_;
+  int actuator_count_;
 
-/***************************************************/
-/*! \class controller::CheckoutControllerNode
-    \brief Checkout Controller
+  bool done() { return state_ == DONE; }
 
-  */
-/***************************************************/
+  bool data_sent_;
 
-class CheckoutControllerNode : public Controller
-{
-public:
+  double last_publish_time_;
 
-  CheckoutControllerNode();
-  ~CheckoutControllerNode();
+  // RT service call
+  boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::RobotData::Request, joint_qualification_controllers::RobotData::Response> > call_service_;
+};
 
-  void update();
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
 
-private:
-  CheckoutController *c_;
-  mechanism::RobotState *robot_;
-  
-  bool data_sent_;
-  
-  double last_publish_time_;
-  realtime_tools::RealtimeSrvCall<joint_qualification_controllers::RobotData::Request, joint_qualification_controllers::RobotData::Response> call_service_;
 
-};
 }
 
 

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -46,10 +46,9 @@
 
 
 #include <vector>
-#include <ros/node.h>
+#include <ros/ros.h>
 #include <math.h>
 #include <joint_qualification_controllers/HoldSetData.h>
-#include <realtime_tools/realtime_publisher.h>
 #include <realtime_tools/realtime_srv_call.h>
 #include <mechanism_control/controller.h>
 #include <robot_mechanism_controllers/joint_position_controller.h>
@@ -73,54 +72,70 @@
 
   /*!
    * \brief Functional way to initialize.
-   * \param velocity Target velocity for the velocity controller.
-   * \param max_effort Effort to limit the controller at.
    * \param *robot The robot that is being controlled.
+   * \param &n Node handle for parameters and services
    */
-  void init( double test_duration, double time, std::string name, mechanism::RobotState *robot);
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+  bool init( mechanism::RobotState *robot, const ros::NodeHandle &n);
 
 
   /*!
    * \brief Issues commands to the joint. Should be called at regular intervals
    */
+  void update();
 
-  virtual void update();
-  
+  bool starting();
+
+  bool sendData();
+    
   bool done() { return state_ == DONE; }
   
   joint_qualification_controllers::HoldSetData::Request hold_set_data_;
 
 private:
+  control_toolbox::Dither* lift_dither_;
+  control_toolbox::Dither* flex_dither_;
 
-  std::vector<control_toolbox::Dither*> dithers_;
+  controller::JointPositionController* lift_controller_;
+  controller::JointPositionController* flex_controller_;
 
-  std::vector<controller::JointPositionController*> joint_position_controllers_;
-  std::vector<std::vector<double> > hold_set_;
+  mechanism::JointState* flex_state_;    
+  mechanism::JointState* lift_state_;    
 
-  std::vector<mechanism::JointState*> joint_states_;      /**< Joint we're controlling. */
   mechanism::RobotState *robot_;            /**< Pointer to robot structure. */
 
-  uint num_joints_;
-
   int starting_count_;
 
   int state_;
-  uint current_position_;
 
+  double lift_cmd_, flex_cmd_;
+
+
   double initial_time_;
 
   double settle_time_;
   double start_time_;
   double dither_time_;
+  double timeout_;
+  double lift_min_, lift_max_, lift_delta_;
+  double flex_min_, flex_max_, flex_delta_;
 
   int dither_count_;
-  double timeout_;
 
+  uint lift_index_;
+  uint flex_index_;
+
+  bool data_sent_;
+
+  boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::HoldSetData::Request, joint_qualification_controllers::HoldSetData::Response> > call_service_;
+
+
 };
 
+}
+
+
 /***************************************************/
-/*! \class controller::HoldSetControllerNode
+/*! \class controller::HoldSetController
     \brief Hold Set Controller
 
     This holds a joint at a set of positions, measuring effort
@@ -184,25 +199,4 @@
 */
 /***************************************************/
 
-class HoldSetControllerNode : public Controller
-{
-public:
 
-  HoldSetControllerNode();
-  ~HoldSetControllerNode();
-  
-  void update();
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-private:
-  HoldSetController *c_;
-  mechanism::RobotState *robot_;
-  
-  bool data_sent_;
-  
-  double last_publish_time_;
-  realtime_tools::RealtimeSrvCall<joint_qualification_controllers::HoldSetData::Request, joint_qualification_controllers::HoldSetData::Response> call_service_;
-};
-}
-
-

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -44,16 +44,14 @@
 */
 /***************************************************/
 
-
-#include <ros/node.h>
+#include <ros/ros.h>
+#include <string>
 #include <math.h>
-#include <sstream>
-#include <diagnostic_msgs/DiagnosticMessage.h>
 #include <joint_qualification_controllers/TestData.h>
-#include <realtime_tools/realtime_publisher.h>
 #include <realtime_tools/realtime_srv_call.h>
 #include <mechanism_control/controller.h>
 #include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <boost/scoped_ptr.hpp>
 
 namespace controller
 {
@@ -69,13 +67,15 @@
 
   /*!
    * \brief Functional way to initialize.
-   * \param velocity Target velocity for the velocity controller.
-   * \param max_effort Effort to limit the controller at.
    * \param *robot The robot that is being controlled.
+   * \param &n NodeHandle of mechanism control
    */
-  void init( double velocity, double max_effort, double max_expected_effort, double min_expected_effort, double min_pos, double max_pos, double time, double timeout, double slope, std::string name, mechanism::RobotState *robot);
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+  bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
 
+  /*!
+   * \brief Called when controller is started
+   */
+  bool starting();
 
   /*!
    * \brief Perform the test analysis
@@ -83,17 +83,20 @@
   void analysis();
 
   /*!
-   * \brief Issues commands to the joint. Should be called at regular intervals
+   * \brief Issues commands to the joint to perform hysteresis test.
    */
+  void update();
 
-  virtual void update();
+  /*! 
+   * \brief Sends data, returns true if sent
+   */
+  bool sendData();
   
   bool done() { return state_ == DONE; }
   
-  diagnostic_msgs::DiagnosticMessage diagnostic_message_;
-  joint_qualification_controllers::TestData::Request test_data_;
 
 private:
+  joint_qualification_controllers::TestData::Request test_data_;
 
   mechanism::JointState *joint_;     /**< Joint we're controlling. */
   mechanism::RobotState *robot_;     /**< Pointer to robot structure. */
@@ -112,37 +115,13 @@
   int state_;
   int starting_count;
 
-};
-
-/***************************************************/
-/*! \class controller::HysteresisControllerNode
-    \brief Hystersis Controller Node
-
-    This tests the hysteresis of a joint using a
-    velocity controller.
-
-*/
-/***************************************************/
-
-class HysteresisControllerNode : public Controller
-{
-public:
-
-  HysteresisControllerNode();
-  ~HysteresisControllerNode();
-
-  void update();
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-private:
-  HysteresisController *c_;
-  mechanism::RobotState *robot_;
-  
   bool data_sent_;
   
   double last_publish_time_;
-  realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> call_service_;
 
+  // RT service call
+  boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> > call_service_;
+
 };
 }
 

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h	2009-08-05 17:45:00 UTC (rev 20792)
@@ -43,13 +43,13 @@
 /***************************************************/
 
 
-#include <ros/node.h>
+#include <ros/ros.h>
 #include <math.h>
 #include <joint_qualification_controllers/TestData.h>
-#include <realtime_tools/realtime_publisher.h>
 #include <realtime_tools/realtime_srv_call.h>
 #include <mechanism_control/controller.h>
 #include <control_toolbox/sine_sweep.h>
+#include <boost/scoped_ptr.hpp>
 
 
 namespace controller
@@ -71,20 +71,20 @@
 
   /*!
    * \brief Functional way to initialize.
-   * \param start_freq The start value of the sweep (Hz).
-   * \param end_freq  The end value of the sweep (Hz).
-   * \param amplitude The amplitude of the sweep (N).
-   * \param duration The duration in seconds from start to finish (s).
-   * \param time The current hardware time.
    * \param *robot The robot that is being controlled.
+   * \param &n NodeHandle of mechanism control
    */
-  void init(double start_freq, double end_freq, double duration, double amplitude, double first_mode, double second_mode, double error_tolerance, double time, std::string name,mechanism::RobotState *robot);
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+  bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
 
+  bool starting();
+
   void analysis();
-  virtual void update();
+  void update();
+
+  bool sendData();
   
   bool done() { return done_ == 1; }
+
   joint_qualification_controllers::TestData::Request test_data_;
 
 private:
@@ -95,34 +95,10 @@
   double initial_time_;                     /**< Start time of the sweep. */
   int count_;
   bool done_;
- 
-};
-
-/***************************************************/
-/*! \class controller::SineSweepControllerNode
-    \brief Sine Sweep Controller ROS Node
-
-*/
-/***************************************************/
-
-class SineSweepControllerNode : public Controller
-{
-public:
- 
-  SineSweepControllerNode();
-  ~SineSweepControllerNode();
-
-  void update();
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-private:
-  SineSweepController *c_;
-  mechanism::RobotState *robot_;
   
   bool data_sent_;
   
-  double last_publish_time_;
-  realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> call_service_;
+  boost::scoped_ptr<realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response>  >call_service_;
 
 };
 }

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml	2009-08-05 17:45:00 UTC (rev 20792)
@@ -1,17 +1,16 @@
 <package>
   <description brief='Joint Qualification Controllers'>
   </description>
-  <author> Melonee Wise</author>
+  <author> Melonee Wise, Kevin Watts</author>
   <license>BSD</license>
   <review status="unreviewed" notes=""/>
   <depend package="mechanism_model" />
   <depend package="wg_robot_description_parser" />
   <depend package="realtime_tools" />
   <depend package="robot_msgs" />
-
+  <depend package="control_toolbox" />
+  <depend package="roscpp" />
   <depend package="robot_mechanism_controllers" />
-  <depend package="rospy" />
-  <depend package="robot_msgs" />
   <depend package="robot_srvs" />
   <url>http://pr.willowgarage.com</url>
   <rosdep name="wxwidgets"/>

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldPositionData.msg	2009-08-05 17:45:00 UTC (rev 20792)
@@ -1 +1,3 @@
-JointPositionData[] joint_data
\ No newline at end of file
+float32 flex_position
+JointPositionData lift_hold
+JointPositionData flex_hold

Added: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg	                        (rev 0)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/HoldRunData.msg	2009-08-05 17:45:00 UTC (rev 20792)
@@ -0,0 +1,2 @@
+float32 lift_position
+HoldPositionData[] flex_data # Same lift position, diff flex
\ No newline at end of file

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/msg/JointPositionData.msg	2009-08-05 17:45:00 UTC (rev 20792)
@@ -1,4 +1,3 @@
-float32 desired
 float32[] time
 float32[] position
 float32[] velocity

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp	2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp	2009-08-05 17:45:00 UTC (rev 20792)
@@ -39,8 +39,10 @@
 
 ROS_REGISTER_CONTROLLER(CheckoutController)
 
-CheckoutController::CheckoutController():
-  robot_(NULL)
+CheckoutController::CheckoutController()
+: robot_(NULL), 
+  data_sent_(false),
+  call_service_(NULL)
 {
   robot_data_.test_time = 0;
   robot_data_.num_joints = 0;
@@ -57,7 +59,7 @@
 {
 }
 
-void CheckoutController::init( double timeout, mechanism::RobotState *robot)
+bool CheckoutController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
 {
   assert(robot);
   robot_ = robot;
@@ -121,28 +123,21 @@
     robot_data_.actuator_data[i].enabled = 0;
   }
 
-  timeout_ = timeout;
+  n.param("timeout", timeout_, 30.0);
 
   initial_time_ = robot_->hw_->current_time_;
+
+  call_service_.reset(new realtime_tools::RealtimeSrvCall<joint_qualification_controllers::RobotData::Request, joint_qualification_controllers::RobotData::Response>(n, "/robot_checkout"));
+
+  return true;
 }
 
-bool CheckoutController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+bool CheckoutController::starting()
 {
-  assert(robot);
-  
-  TiXmlElement *cd = config->FirstChildElement("controller_defaults");
-  if (cd)
-  {
-    // Pull timeout attribute if it exists
-    const char *time_char = cd->Attribute("timeout");
-    double timeout = time_char ? atof(cd->Attribute("timeout")) : timeout_;
+  initial_time_ = robot_->hw_->current_time_;
 
-    init(timeout, robot);
-  }
-  else
-  {
-    init(timeout_, robot);
-  }
+  data_sent_ = false;
+
   return true;
 }
 
@@ -168,34 +163,21 @@
   case LISTENING:
     {
     cal = true;
-    for (int i = 0; i < joint_count_; i++)
+    for (int i = 0; i < joint_count_; ++i)
     {
       // Check for caster wheel joints and fingers
       // Wheel joints and fingers don't calibrate, so don't wait for them
       if (cal && (robot_->joint_states_[i].joint_->name_.find("wheel_joint") != string::npos))
-      {     
-        //cal = true;
         continue;
-      }
 
+
       if (cal && (robot_->joint_states_[i].joint_->name_.find("finger") != string::npos))
-      {
-        //cal = true;
         continue;
-      }
+      
 
       // Base joint is a dummy joint used to set up visualization
       if (cal && robot_->joint_states_[i].joint_->name_ == "base_joint")
-      {
-        //cal = true;
         continue;
-      }
-
-      //if (cal && robot_->joint_states_[i].calibrated_)
-      //  cal = true;
-      //else
-      //  cal = false;
-      // break;
       
       if (!robot_->joint_states_[i].calibrated_)
       {     
@@ -207,11 +189,11 @@
     enabled = true;
     for (int i = 0; i < actuator_count_; i++)
     {
-      if (robot_->hw_->actuators_[i]->state_.is_enabled_ && enabled)
-        enabled = true;
-      else
+      if (!robot_->hw_->actuators_[i]->state_.is_enabled_)
+      {
         enabled = false;
         break;
+      }
     }
 
     if (cal && enabled)
@@ -224,11 +206,51 @@
     state_ = DONE;
     break;
   case DONE:
+    if (!data_sent_)
+      data_sent_ = sendData();
+    
+
     break;
   }
 
 }
 
+bool CheckoutController::sendData()
+{
+  if (call_service_->trylock())
+  {
+    joint_qualification_controllers::RobotData::Request *out = &call_service_->srv_req_;
+    out->test_time     = robot_data_.test_time;
+    out->num_joints    = robot_data_.num_joints;
+    out->num_actuators = robot_data_.num_actuators;
+    
+    out->joint_data.resize(robot_data_.num_joints);
+    out->actuator_data.resize(robot_data_.num_actuators);
+    
+    for (int i = 0; i < joint_count_; i++)
+    {
+      out->joint_data[i].index      = robot_data_.joint_data[i].index;
+      out->joint_data[i].name       = robot_data_.joint_data[i].name;
+      out->joint_data[i].is_cal     = robot_data_.joint_data[i].is_cal;
+      out->joint_data[i].has_safety = robot_data_.joint_data[i].has_safety;
+      out->joint_data[i].type       = robot_data_.joint_data[i].type;
+    }
+    
+    for (int i = 0; i < actuator_count_; i++)
+    {
+      out->actuator_data[i].index   = robot_data_.actuator_data[i].index;
+      out->actuator_data[i].name    = robot_data_.actuator_data[i].name;
+      out->actuator_data[i].id      = robot_data_.actuator_data[i].id;
+      out->actuator_data[i].enabled = robot_data_.actuator_data[i].enabled;
+    }
+    
+    call_service_->unlockAndCall();
+    
+    return true;
+  }
+  return false;
+}
+
 void CheckoutController::analysis(double time)
 {
   robot_data_.test_time = time;
@@ -242,68 +264,4 @@
   return; 
 }
 
-ROS_REGISTER_CONTROLLER(CheckoutControllerNode)
-CheckoutControllerNode::CheckoutControllerNode()
-: data_sent_(false), last_publish_time_(0), call_service_("/robot_checkout")
-{
-  c_ = new CheckoutController();
-}
 
-CheckoutControllerNode::~CheckoutControllerNode()
-{
-  delete c_;
-}
-
-void CheckoutControllerNode::update()
-{
-  c_->update();
-
-  if (c_->done())
-  {
-    if(!data_sent_)
-    {
-      if (call_service_.trylock())
-      {
-        joint_qualification_controllers::RobotData::Request *out = &call_service_.srv_req_;
-        out->test_time     = c_->robot_data_.test_time;
-        out->num_joints    = c_->robot_data_.num_joints;
-        out->num_actuators = c_->robot_data_.num_actuators;
-        
-        out->joint_data.resize(c_->robot_data_.num_joints);
-        out->actuator_data.resize(c_->robot_data_.num_actuators);
-
-        for (int i = 0; i < c_->joint_count_; i++)
-        {
-          out->joint_data[i].index      = c_->robot_data_.joint_data[i].index;
-          out->joint_data[i].name       = c_->robot_data_.joint_data[i].name;
-          out->joint_data[i].is_cal     = c_->robot_data_.joint_data[i].is_cal;
-          out->joint_data[i].has_safety = c_->robot_data_.joint_data[i].has_safety;
-          out->joint_data[i].type       = c_->robot_data_.joint_data[i].type;
-        }
-
-        for (int i = 0; i < c_->actuator_count_; i++)
-        {
-          out->actuator_data[i].index    = c_->robot_data_.actuator_data[i].index;
-          out->actuator_data[i].name     = c_->robot_data_.actuator_data[i].name;
-          out->actuator_data[i].id       = c_->robot_data_.actuator_data[i].id;
-          out->actuator_data[i].enabled  = c_->robot_data_.actuator_data[i].enabled;
-        }
-          
-        call_service_.unlockAndCall();
-        data_sent_ = true;
-      }
-    }
-  }
-}
-
-bool CheckoutControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
-  assert(robot);
-  robot_ = robot;
-  
-  if (!c_->initXml(robot, config))
-    return false;
-    
-  return true;
-}
-

Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_...
 
[truncated message content]

Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[20792] pkg/trunk (Page 44)

personalrobots-commit