Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20792] pkg/trunk
|
From: <wa...@us...> - 2009-08-05 17:45:15
|
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_controllers/joint_qualification_controllers/src/hold_set_controller.cpp 2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp 2009-08-05 17:45:00 UTC (rev 20792)
@@ -39,191 +39,214 @@
using namespace std;
using namespace controller;
-
ROS_REGISTER_CONTROLLER(HoldSetController)
-HoldSetController::HoldSetController():
- robot_(NULL)
+HoldSetController::HoldSetController()
+: robot_(NULL),
+ initial_time_(0.0),
+ start_time_(0.0),
+ lift_min_(0.0),
+ lift_max_(0.0),
+ flex_min_(0.0),
+ flex_max_(0.0),
+ flex_delta_(0.0)
{
-
- hold_set_data_.test_name = "hold_set";
-
- state_ = STARTING;
-
- current_position_ = 0;
- starting_count_ = 0;
-
- initial_time_ = 0.0;
- start_time_ = 0.0;
dither_time_ = 0.0;
- dither_count_ = 0.0;
- timeout_ = 120.0;
+ lift_cmd_ = 0.0;
+ lift_delta_ = 0.0;
+ flex_cmd_ = 0.0;
+ timeout_ = 120;
+ lift_index_ = 0;
+ flex_index_ = 0;
+ hold_set_data_.arg_name.resize(10);
+ hold_set_data_.arg_value.resize(10);
+ hold_set_data_.arg_name[0] = "Settle Time";
+ hold_set_data_.arg_name[1] = "Start Time";
+ hold_set_data_.arg_name[2] = "Dither Time";
+ hold_set_data_.arg_name[3] = "Timeout";
+ hold_set_data_.arg_name[4] = "Lift Min";
+ hold_set_data_.arg_name[5] = "Lift Max";
+ hold_set_data_.arg_name[6] = "Lift Delta";
+ hold_set_data_.arg_name[7] = "Flex Min";
+ hold_set_data_.arg_name[8] = "Flex Max";
+ hold_set_data_.arg_name[9] = "Flex Delta";
+
+ ///\todo Need PID's for lift, flex
+
+ state_ = STARTING;
}
HoldSetController::~HoldSetController()
{
}
-bool HoldSetController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+bool HoldSetController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
{
assert(robot);
robot_ = robot;
- initial_time_ = robot_->hw_->current_time_;
-
- // Adding Joint Position Controllers
- TiXmlElement *elt = config->FirstChildElement("controller");
- if (!elt)
+ // Lift joint
+ std::string lift_name;
+ if (!n.getParam("lift_name", lift_name)){
+ ROS_ERROR("CounterbalanceTestController: No lift joint name found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!(lift_state_ = robot->getJointState(lift_name)))
{
- fprintf(stderr, "HoldSetController's config did not give the required joint position controllers.\n");
+ ROS_ERROR("CounterbalanceTestController could not find lift joint named \"%s\"\n", lift_name.c_str());
return false;
}
+ hold_set_data_.lift_name = lift_name;
- ROS_INFO("Setting up joint position controllers!");
- while (elt)
+ lift_controller_ = new JointPositionController();
+ if (!lift_controller_->init(robot, ros::NodeHandle(n, "lift"))) return false;
+
+ // Flex joint
+ std::string flex_name;
+ if (!n.getParam("flex_name", flex_name)){
+ ROS_ERROR("CounterbalanceTestController: No flex joint name found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!(flex_state_ = robot->getJointState(flex_name)))
{
- ROS_INFO("Making JPC");
- JointPositionController * jpc = new JointPositionController();
- //std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
- assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPositionController"));
- //ROS_INFO("Passed assert");
+ ROS_ERROR("CounterbalanceTestController could not find flex joint named \"%s\"\n", flex_name.c_str());
+ return false;
+ }
+ hold_set_data_.flex_name = flex_name;
- ROS_INFO("Pushing back JPC, joint states");
+ flex_controller_ = new JointPositionController();
+ if (!flex_controller_->init(robot, ros::NodeHandle(n, "flex"))) return false;
- // Store controller, joint state, name
- joint_position_controllers_.push_back(jpc);
+ // Initialize dithers
+ lift_dither_ = new control_toolbox::Dither(100.0);
+ if (!lift_dither_->init(ros::NodeHandle(n, "lift")))
+ return false;
- if (!jpc->initXml(robot, elt))
- return false;
+ flex_dither_ = new control_toolbox::Dither(200.0);
+ if (!flex_dither_->init(ros::NodeHandle(n, "flex")))
+ return false;
- hold_set_data_.joint_names.push_back(jpc->getJointName());
- joint_states_.push_back(robot->getJointState(jpc->getJointName()));
-
- TiXmlElement *dith = elt->FirstChildElement("dither");
- double dither_amp = atof(dith->Attribute("dither_amp"));
- control_toolbox::Dither *dither = new control_toolbox::Dither::Dither(100.0);
- dither->init(dither_amp);
- dithers_.push_back(dither);
-
- hold_set_data_.dither_amps.push_back(dither_amp);
-
- ROS_INFO("Next controller!");
- elt = elt->NextSiblingElement("controller");
+ // Lift range
+ if (!n.getParam("lift/min", lift_min_)){
+ ROS_ERROR("CounterbalanceTestController: No min lift position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
}
-
- num_joints_ = joint_position_controllers_.size();
-
- hold_set_data_.joint_names.resize(num_joints_);
- hold_set_data_.dither_amps.resize(num_joints_);
+ if (!n.getParam("lift/max", lift_max_)){
+ ROS_ERROR("CounterbalanceTestController: No max lift position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!n.getParam("lift/delta", lift_delta_)){
+ ROS_ERROR("CounterbalanceTestController: No lift delta found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
- // Setting controller defaults
- TiXmlElement *cd = config->FirstChildElement("controller_defaults");
- if (cd)
- {
- settle_time_ = atof(cd->Attribute("settle_time"));
- dither_time_ = atof(cd->Attribute("dither_time"));
- timeout_ = atof(cd->Attribute("timeout"));
- }
- else
- {
- fprintf(stderr, "HoldSetController was not given required controller defaults\n");
+ // Flex range
+ if (!n.getParam("flex/min", flex_max_)){
+ ROS_ERROR("CounterbalanceTestController: No min flex position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
return false;
}
+ if (!n.getParam("flex/max", flex_min_)){
+ ROS_ERROR("CounterbalanceTestController: No max flex position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!n.getParam("flex/delta", flex_delta_)){
+ ROS_ERROR("CounterbalanceTestController: No flex delta found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
- // Setting holding points
- // Need to pass in a list of points here
- // Store as vector of vectors...
- TiXmlElement *hs = config->FirstChildElement("hold_pt");
- if (!hs)
- {
- fprintf(stderr, "HoldSetController's config did not give the required holding set.\n");
+ // Setting controller defaults
+ if (!n.getParam("settle_time", settle_time_)){
+ ROS_ERROR("CounterbalanceTestController: No settle time found on parameter namespace: %s)",
+ n.getNamespace().c_str());
return false;
}
+ if (!n.getParam("dither_time", dither_time_)){
+ ROS_ERROR("CounterbalanceTestController: No dither time found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!n.getParam("timeout", timeout_)){
+ ROS_ERROR("CounterbalanceTestController: No timeout found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
- ROS_INFO("Storing Hold Points");
+ lift_cmd_ = lift_min_;
+ flex_cmd_ = flex_min_ - flex_delta_;
- while (hs)
- {
- std::vector<double> position;
-
- TiXmlElement *jnt_pt = hs->FirstChildElement("joint");
-
- while (jnt_pt)
- {
-
- double point = atof(jnt_pt->Attribute("position"));
+ return true;
+}
- position.push_back(point);
-
- jnt_pt = jnt_pt->NextSiblingElement("joint");
- }
-
- if (position.size() != num_joints_)
- {
- ROS_ERROR("Incorrect number of points for joint");
- fprintf(stderr, "HoldSetController's points did not have the correct number of joints.\n");
- return false;
- }
-
- hold_set_.push_back(position);
-
- hs = hs->NextSiblingElement("hold_pt");
- }
-
- // Set up correct number of holding points
- hold_set_data_.hold_data.resize(hold_set_.size());
-
+bool HoldSetController::starting()
+{
+ initial_time_ = robot_->hw_->current_time_;
return true;
}
void HoldSetController::update()
{
// wait until the joints are calibrated to start
- for (unsigned int i = 0; i < num_joints_; i++)
- {
- if (!joint_states_[i]->calibrated_)
- {
- ROS_INFO("Not calibrated!");
- return;
- }
- }
-
+ if (!flex_state_->calibrated_ || !lift_state_->calibrated_)
+ return;
+
double time = robot_->hw_->current_time_;
if (time - initial_time_ > timeout_ && state_ != DONE)
{
- ROS_INFO("Timeout!");
+ ROS_ERROR("CounterbalanceTestController timed out during test. Timeout: %f.", timeout_);
state_ = DONE;
}
- for (unsigned int i = 0; i < num_joints_; ++i)
- {
- joint_position_controllers_[i]->update();
- }
-
+ lift_controller_->update();
+ flex_controller_->update();
+
switch (state_)
{
case STARTING:
{
- ROS_INFO("Starting!");
+ ROS_INFO("Starting");
- std::vector<double> current = hold_set_[current_position_];
- assert(current.size() == num_joints_);
-
- hold_set_data_.hold_data[current_position_].joint_data.resize(num_joints_);
-
- for (unsigned int i = 0; i < num_joints_; ++i)
+ // Set the flex and lift commands
+ // If
+ flex_cmd_ += flex_delta_;
+ flex_index_++;
+ // Move to next lift position, reset flex
+ if (flex_cmd_ > flex_max_)
{
- double cmd = current[i];
+ flex_cmd_ = flex_min_ - flex_delta_;
+ lift_cmd_ += lift_delta_;
+ lift_index_++;
- joint_position_controllers_[i]->setCommand(cmd);
- hold_set_data_.hold_data[current_position_].joint_data[i].desired = cmd;
+ // We're done after we finished the lifts
+ if (lift_cmd_ > lift_max_)
+ {
+ state_ = DONE;
+ break;
+ }
+ hold_set_data_.lift_data.resize(lift_index_ + 1);
+ hold_set_data_.lift_data[lift_index_].lift_position = lift_cmd_;
}
+ else
+ {
+ hold_set_data_.lift_data[lift_index_].flex_data.resize(flex_index_ + 1);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_position = flex_cmd_;
+ }
+
+ // Set controllers
+ lift_controller_->setCommand(lift_cmd_);
+ flex_controller_->setCommand(flex_cmd_);
start_time_ = time;
state_ = SETTLING;
- ROS_INFO("Settling");
break;
}
case SETTLING:
@@ -239,113 +262,62 @@
}
case DITHERING:
{
- joint_qualification_controllers::HoldPositionData *data = &hold_set_data_.hold_data[current_position_];
-
+ // Add dither
+ lift_state_->commanded_effort_ += lift_dither_->update();
+ flex_state_->commanded_effort_ += flex_dither_->update();
- for (unsigned int i = 0; i < num_joints_; ++i)
- {
- // Add dither
- joint_states_[i]->commanded_effort_ += dithers_[i]->update();
-
- // Record state
- data->joint_data[i].time.push_back(time - start_time_);
- data->joint_data[i].position.push_back(joint_states_[i]->position_);
- data->joint_data[i].velocity.push_back(joint_states_[i]->velocity_);
- data->joint_data[i].cmd.push_back(joint_states_[i]->commanded_effort_);
- data->joint_data[i].effort.push_back(joint_states_[i]->applied_effort_);
- }
+ // Lift
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.time.push_back(time - start_time_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.position.push_back(lift_state_->position_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.velocity.push_back(lift_state_->velocity_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.cmd.push_back(lift_state_->commanded_effort_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.effort.push_back(lift_state_->applied_effort_);
+ // Flex
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.time.push_back(time - start_time_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.position.push_back(flex_state_->position_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.velocity.push_back(flex_state_->velocity_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.cmd.push_back(flex_state_->commanded_effort_);
+ hold_set_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.effort.push_back(flex_state_->applied_effort_);
+
if (time - start_time_ > dither_time_)
{
- state_ = PAUSING;
+ state_ = STARTING;
}
break;
}
- case PAUSING:
+ case DONE:
{
- if (++current_position_ >= hold_set_.size())
- {
- state_ = DONE;
- ROS_INFO("Done!");
+ if (!data_sent_)
+ data_sent_ = sendData();
+
+ break;
}
- else
- {
- state_ = STARTING;
- ROS_INFO("Next point!");
- }
- break;
- }
- case DONE:
- break;
+
}
-
-
}
-
-
-ROS_REGISTER_CONTROLLER(HoldSetControllerNode)
-HoldSetControllerNode::HoldSetControllerNode()
-: data_sent_(false), call_service_("hold_set_data")
+bool HoldSetController::sendData()
{
- c_ = new HoldSetController();
-}
-
-HoldSetControllerNode::~HoldSetControllerNode()
-{
- delete c_;
-}
-
-void HoldSetControllerNode::update()
-{
- c_->update();
- if (c_->done())
+ if (call_service_->trylock())
{
- if(!data_sent_)
- {
- if (call_service_.trylock())
- {
- ROS_INFO("Calling results service!");
- joint_qualification_controllers::HoldSetData::Request *out = &call_service_.srv_req_;
- out->test_name = c_->hold_set_data_.test_name;
- out->joint_names = c_->hold_set_data_.joint_names;
- out->dither_amps = c_->hold_set_data_.dither_amps;
+ ROS_INFO("Calling results service!");
+
+ // Copy data and send
+ joint_qualification_controllers::HoldSetData::Request *out = &call_service_->srv_req_;
+
+ out->lift_name = hold_set_data_.lift_name;
+ out->flex_name = hold_set_data_.flex_name;
+ out->lift_amplitude = hold_set_data_.lift_amplitude;
+ out->flex_amplitude = hold_set_data_.flex_amplitude;
- out->hold_data.resize(c_->hold_set_data_.hold_data.size());
+ out->arg_name = hold_set_data_.arg_name;
+ out->arg_value = hold_set_data_.arg_value;
- for (uint i = 0; i < c_->hold_set_data_.hold_data.size(); ++i)
- {
- out->hold_data[i].joint_data.resize(c_->hold_set_data_.hold_data[i].joint_data.size());
-
- for (unsigned int j = 0; j < c_->hold_set_data_.hold_data[i].joint_data.size(); ++j)
- {
- out->hold_data[i].joint_data[j].desired = c_->hold_set_data_.hold_data[i].joint_data[j].desired;
- out->hold_data[i].joint_data[j].time = c_->hold_set_data_.hold_data[i].joint_data[j].time;
- out->hold_data[i].joint_data[j].position = c_->hold_set_data_.hold_data[i].joint_data[j].position;
- out->hold_data[i].joint_data[j].velocity = c_->hold_set_data_.hold_data[i].joint_data[j].velocity;
- out->hold_data[i].joint_data[j].cmd = c_->hold_set_data_.hold_data[i].joint_data[j].cmd;
- out->hold_data[i].joint_data[j].effort = c_->hold_set_data_.hold_data[i].joint_data[j].effort;
-
- }
- }
- call_service_.unlockAndCall();
- data_sent_ = true;
- }
- }
-
+ out->lift_data = hold_set_data_.lift_data;
+ call_service_->unlockAndCall();
+ return true;
}
+ return false;
}
-bool HoldSetControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
- robot_ = robot;
-
- ROS_INFO("Initing hold set controller");
- if (!c_->initXml(robot, config))
- return false;
-
- ROS_INFO("Hold set controller initialized");
- return true;
-}
-
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp 2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp 2009-08-05 17:45:00 UTC (rev 20792)
@@ -40,8 +40,11 @@
ROS_REGISTER_CONTROLLER(HysteresisController)
-HysteresisController::HysteresisController():
- joint_(NULL), robot_(NULL)
+HysteresisController::HysteresisController()
+: joint_(NULL),
+ robot_(NULL),
+ data_sent_(false),
+ call_service_(NULL)
{
test_data_.test_name ="hysteresis";
test_data_.joint_name = "default joint";
@@ -52,20 +55,19 @@
test_data_.velocity.resize(MAX_DATA_POINTS);
test_data_.arg_name.resize(11);
+ test_data_.arg_value.resize(11);
test_data_.arg_name[0] = "min_expected_effort";
test_data_.arg_name[1] = "max_expected_effort";
test_data_.arg_name[2] = "min_pos";
test_data_.arg_name[3] = "max_pos";
test_data_.arg_name[4] = "search_vel";
test_data_.arg_name[5] = "timeout";
- test_data_.arg_name[6] = "slope";
+ test_data_.arg_name[6] = "max_effort";
test_data_.arg_name[7] = "p_gain";
test_data_.arg_name[8] = "i_gain";
test_data_.arg_name[9] = "d_gain";
test_data_.arg_name[10] = "iClamp";
- test_data_.arg_value.resize(11);
-
state_ = STOPPED;
starting_count = 0;
velocity_ = 0;
@@ -76,6 +78,7 @@
loop_count_ = 0;
count_ = 0;
+ // Assume 1KHz update rate
timeout_ = MAX_DATA_POINTS / 1000;
}
@@ -83,31 +86,86 @@
{
}
-void HysteresisController::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 HysteresisController::init( mechanism::RobotState *robot, const ros::NodeHandle &n)
{
assert(robot);
robot_ = robot;
- joint_ = robot->getJointState(name);
- velocity_ = velocity;
- max_effort_ = max_effort;
- initial_time_ = time;
+ std::string name;
+ if (!n.getParam("name", name)){
+ ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!(joint_ = robot->getJointState(name)))
+ {
+ ROS_ERROR("HysteresisController could not find joint named \"%s\"\n", name.c_str());
+ return false;
+ }
+
+ if (!n.getParam("velocity", velocity_)){
+ ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ if (!n.getParam("max_effort", max_effort_)){
+ ROS_ERROR("Hysteresis Controller: No max effort found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ double min_expected, max_expected, max_pos, min_pos;
+
+ if (!n.getParam("min_expected", min_expected)){
+ ROS_ERROR("Hysteresis Controller: No min expected effort found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ if (!n.getParam("max_expected", max_expected)){
+ ROS_ERROR("Hysteresis Controller: No max expected effort found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ if (!n.getParam("max_position", max_pos)){
+ ROS_ERROR("Hysteresis Controller: No max position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ if (!n.getParam("min_position", min_pos)){
+ ROS_ERROR("Hysteresis Controller: No min position found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ if (!n.getParam("timeout", timeout_)){
+ ROS_ERROR("Hysteresis Controller: No timeout found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+
+ initial_time_ = robot_->hw_->current_time_;
initial_position_ = joint_->position_;
- timeout_ = timeout;
// Set values in test data output
test_data_.joint_name = name;
- test_data_.arg_value[0] = min_expected_effort;
- test_data_.arg_value[1] = max_expected_effort;
+ test_data_.arg_value[0] = min_expected;
+ test_data_.arg_value[1] = max_expected;
test_data_.arg_value[2] = min_pos;
test_data_.arg_value[3] = max_pos;
- test_data_.arg_value[4] = velocity;
- test_data_.arg_value[5] = timeout;
- test_data_.arg_value[6] = slope;
+ test_data_.arg_value[4] = velocity_;
+ test_data_.arg_value[5] = timeout_;
+ test_data_.arg_value[6] = max_effort_;
+
+ velocity_controller_ = new JointVelocityController();
+ if (!velocity_controller_->init(robot, ros::NodeHandle(n, "vel_control"))) return false;
+
// Get the gains, add them to test data
double p, i, d, iClamp, imin;
-
velocity_controller_->getGains(p, i, d, iClamp, imin);
test_data_.arg_value[7] = p;
@@ -115,64 +173,26 @@
test_data_.arg_value[9] = d;
test_data_.arg_value[10] = iClamp;
-
+ call_service_.reset(new realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response>(n, "/test_data"));
+
+ return true;
}
-bool HysteresisController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+bool HysteresisController::starting()
{
- assert(robot);
+ velocity_controller_->starting();
- TiXmlElement *j = config->FirstChildElement("joint");
- if (!j)
- {
- fprintf(stderr, "HysteresisController was not given a joint\n");
- return false;
- }
+ initial_time_ = robot_->hw_->current_time_;
+ initial_position_ = joint_->position_;
- const char *joint_name = j->Attribute("name");
- joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
- if (!joint_)
- {
- fprintf(stderr, "HysteresisController could not find joint named \"%s\"\n", joint_name);
- return false;
- }
- ///\todo check velocity controller comes up
- velocity_controller_ = new JointVelocityController();
- velocity_controller_->initXml(robot, config);
-
- TiXmlElement *cd = j->FirstChildElement("controller_defaults");
- if (cd)
- {
- double velocity = atof(cd->Attribute("velocity"));
- double max_effort = atof(cd->Attribute("max_effort"));
- double max_expected_effort = atof(cd->Attribute("max_expected_effort"));
- double min_expected_effort = atof(cd->Attribute("min_expected_effort"));
- double min_pos = atof(cd->Attribute("min_pos"));
- double max_pos = atof(cd->Attribute("max_pos"));
-
- // Pull timeout attribute if it exists
- const char *time_char = cd->Attribute("timeout");
- double timeout = time_char ? atof(cd->Attribute("timeout")) : timeout_;
-
- const char *slope_char = cd->Attribute("slope");
- double slope = slope_char ? atof(cd->Attribute("slope")) : 0;
-
-
-
- init(velocity, max_effort, max_expected_effort, min_expected_effort, min_pos, max_pos, robot->hw_->current_time_, timeout, slope, j->Attribute("name"), robot);
- }
- else
- {
- fprintf(stderr, "HysteresisController's config did not specify the default control parameters.\n");
- return false;
- }
+ count_ = 0;
return true;
}
void HysteresisController::update()
{
- // wait until the joint is calibrated if it has limits
- if(!joint_->calibrated_ && joint_->joint_->type_!=mechanism::JOINT_CONTINUOUS)
+ // wait until the joint is calibrated if its not a wheel
+ if(!joint_->calibrated_ && joint_->joint_->name_.find("wheel_joint") != string::npos)
{
return;
}
@@ -245,6 +265,8 @@
break;
case DONE:
velocity_controller_->setCommand(0.0);
+ if (!data_sent_)
+ data_sent_ = sendData();
break;
}
@@ -265,56 +287,27 @@
return;
}
-ROS_REGISTER_CONTROLLER(HysteresisControllerNode)
-HysteresisControllerNode::HysteresisControllerNode()
-: data_sent_(false), last_publish_time_(0), call_service_("/test_data")
+bool HysteresisController::sendData()
{
- c_ = new HysteresisController();
-}
-
-HysteresisControllerNode::~HysteresisControllerNode()
-{
- delete c_;
-}
-
-void HysteresisControllerNode::update()
-{
- c_->update();
- if (c_->done())
+ if (call_service_->trylock())
{
- if(!data_sent_)
- {
- if (call_service_.trylock())
- {
- joint_qualification_controllers::TestData::Request *out = &call_service_.srv_req_;
- out->test_name = c_->test_data_.test_name;
- out->joint_name = c_->test_data_.joint_name;
-
- out->time = c_->test_data_.time;
- out->cmd = c_->test_data_.cmd;
- out->effort = c_->test_data_.effort;
- out->position = c_->test_data_.position;
- out->velocity = c_->test_data_.velocity;
-
- out->arg_name = c_->test_data_.arg_name;
- out->arg_value = c_->test_data_.arg_value;
- call_service_.unlockAndCall();
- data_sent_ = true;
- }
- }
+ joint_qualification_controllers::TestData::Request *out = &call_service_->srv_req_;
+ out->test_name = test_data_.test_name;
+ out->joint_name = test_data_.joint_name;
+
+ out->time = test_data_.time;
+ out->cmd = test_data_.cmd;
+ out->effort = test_data_.effort;
+ out->position = test_data_.position;
+ out->velocity = test_data_.velocity;
+
+ out->arg_name = test_data_.arg_name;
+ out->arg_value = test_data_.arg_value;
+ call_service_->unlockAndCall();
+ return true;
}
-
-
+ return false;
}
-bool HysteresisControllerNode::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/sine_sweep_controller.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp 2009-08-05 17:40:29 UTC (rev 20791)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp 2009-08-05 17:45:00 UTC (rev 20792)
@@ -31,10 +31,10 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+
#include <joint_qualification_controllers/sine_sweep_controller.h>
-#include <math.h>
-#define MAX_DATA_POINTS 80000
+#define MAX_DATA_POINTS 120000
using namespace std;
using namespace control_toolbox;
@@ -44,7 +44,7 @@
ROS_REGISTER_CONTROLLER(SineSweepController)
SineSweepController::SineSweepController():
-joint_state_(NULL), robot_(NULL)
+joint_state_(NULL), robot_(NULL), sweep_(NULL)
{
test_data_.test_name = "sinesweep";
test_data_.joint_name = "default joint";
@@ -53,16 +53,20 @@
test_data_.effort.resize(MAX_DATA_POINTS);
test_data_.position.resize(MAX_DATA_POINTS);
test_data_.velocity.resize(MAX_DATA_POINTS);
- test_data_.arg_name.resize(3);
- test_data_.arg_name[0]="first_mode";
- test_data_.arg_name[1]="second_mode";
- test_data_.arg_name[2]="error_tolerance";
- test_data_.arg_value.resize(3);
- sweep_=NULL;
- duration_ =0.0;
- initial_time_=0;
- count_=0;
- done_=0;
+ test_data_.arg_name.resize(7);
+ test_data_.arg_value.resize(7);
+ test_data_.arg_name[0] = "first_mode";
+ test_data_.arg_name[1] = "second_mode";
+ test_data_.arg_name[2] = "error_tolerance";
+ test_data_.arg_name[3] = "start_freq";
+ test_data_.arg_name[4] = "end_freq";
+ test_data_.arg_name[5] = "amplitude";
+ test_data_.arg_name[6] = "duration";
+
+ duration_ = 0.0;
+ initial_time_ = 0;
+ count_ = 0;
+ done_ = 0;
}
SineSweepController::~SineSweepController()
@@ -70,79 +74,129 @@
delete sweep_;
}
-void SineSweepController::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 SineSweepController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
{
assert(robot);
robot_ = robot;
- joint_state_ = robot->getJointState(name);
- if(name=="r_gripper_joint" || name=="l_gripper_joint")
+
+ std::string name;
+ if (!n.getParam("name", name)){
+ ROS_ERROR("SineSweepController: No joint name found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!(joint_state_ = robot->getJointState(name)))
{
- joint_state_->calibrated_ = true;
+ ROS_ERROR("SineSweepController could not find joint named \"%s\"\n", name.c_str());
+ return false;
+ }
+ test_data_.joint_name = name;
+
+ double first_mode, second_mode, error_tolerance;
+ if (!n.getParam("first_mode", first_mode)){
+ ROS_ERROR("SineSweepController: No first mode found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
}
+ if (!n.getParam("second_mode", second_mode)){
+ ROS_ERROR("SineSweepController: No second mode found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
+ if (!n.getParam("error_tolearance", error_tolerance)){
+ ROS_ERROR("SineSweepController: No error tolerance found on parameter namespace: %s)",
+ n.getNamespace().c_str());
+ return false;
+ }
- test_data_.joint_name = name;
+
+ double start_freq, end_f...
[truncated message content] |
