Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17735] pkg/trunk
|
From: <stu...@us...> - 2009-06-26 01:22:26
|
Revision: 17735
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17735&view=rev
Author: stuglaser
Date: 2009-06-26 00:55:23 +0000 (Fri, 26 Jun 2009)
Log Message:
-----------
The JointVelocityController has the new-style init function, and can also communicate with ROS on its own.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-06-26 00:55:08 UTC (rev 17734)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-06-26 00:55:23 UTC (rev 17735)
@@ -48,10 +48,19 @@
<pid p="1.0" i="2.0" d="3.0" iClamp="4.0" /><br>
</joint><br>
</controller><br>
+
+ Configuration:
+ <controller name>:
+ type: "JointVelocityController"
+ joint: "<joint_name>"
+ pid: { p: <p_gain>, i: <i_gain>, d: <d_gain>, i_clamp: <i_clamp> }
+
+
*/
/***************************************************/
#include <ros/node.h>
+#include <ros/node_handle.h>
#include "mechanism_model/controller.h"
#include "control_toolbox/pid.h"
@@ -86,6 +95,7 @@
*/
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
bool init(mechanism::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid);
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
@@ -113,11 +123,20 @@
double dt_;
private:
+ ros::NodeHandle node_;
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
double last_time_; /**< Last time stamp of update. */
+ int loop_count_;
double command_; /**< Last commanded position. */
friend class JointVelocityControllerNode;
+
+ boost::scoped_ptr<
+ realtime_tools::RealtimePublisher<
+ robot_mechanism_controllers::JointControllerState> > controller_state_publisher_ ;
+
+ ros::Subscriber sub_command_;
+ void setCommandCB(const std_msgs::Float64ConstPtr& msg);
};
/***************************************************/
@@ -161,6 +180,7 @@
JointVelocityController *c_; /**< The controller. */
control_toolbox::PidGainsSetter pid_tuner_;
+ int count;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-06-26 00:55:08 UTC (rev 17734)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-06-26 00:55:23 UTC (rev 17735)
@@ -35,17 +35,19 @@
#include <robot_mechanism_controllers/joint_velocity_controller.h>
using namespace std;
-using namespace controller;
+namespace controller {
+
ROS_REGISTER_CONTROLLER(JointVelocityController)
JointVelocityController::JointVelocityController()
-: joint_state_(NULL), robot_(NULL), last_time_(0), command_(0)
+: joint_state_(NULL), robot_(NULL), last_time_(0), loop_count_(0), command_(0)
{
}
JointVelocityController::~JointVelocityController()
{
+ sub_command_.shutdown();
}
bool JointVelocityController::init(mechanism::RobotState *robot, const std::string &joint_name,
@@ -94,6 +96,31 @@
return init(robot, joint_name, pid);
}
+bool JointVelocityController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
+{
+ assert(robot);
+ node_ = n;
+
+ std::string joint_name;
+ if (!node_.getParam("joint", joint_name)) {
+ ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ if (!pid.init(ros::NodeHandle(node_, "pid")))
+ return false;
+
+ controller_state_publisher_.reset(
+ new realtime_tools::RealtimePublisher<robot_mechanism_controllers::JointControllerState>
+ (node_, "state", 1));
+
+ sub_command_ = node_.subscribe<std_msgs::Float64>("set_command", 1, &JointVelocityController::setCommandCB, this);
+
+ return init(robot, joint_name, pid);
+}
+
+
void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
{
pid_controller_.setGains(p,i,d,i_max,i_min);
@@ -125,20 +152,45 @@
void JointVelocityController::update()
{
assert(robot_ != NULL);
- double error(0);
double time = robot_->hw_->current_time_;
- error =joint_state_->velocity_ - command_;
- dt_= time - last_time_;
+ double error = joint_state_->velocity_ - command_;
+ dt_ = time - last_time_;
joint_state_->commanded_effort_ += pid_controller_.updatePid(error, dt_);
+ if(loop_count_ % 10 == 0)
+ {
+ if(controller_state_publisher_ && controller_state_publisher_->trylock())
+ {
+ controller_state_publisher_->msg_.set_point = command_;
+ controller_state_publisher_->msg_.process_value = joint_state_->velocity_;
+ controller_state_publisher_->msg_.error = error;
+ controller_state_publisher_->msg_.time_step = dt_;
+
+ double dummy;
+ getGains(controller_state_publisher_->msg_.p,
+ controller_state_publisher_->msg_.i,
+ controller_state_publisher_->msg_.d,
+ controller_state_publisher_->msg_.i_clamp,
+ dummy);
+ controller_state_publisher_->unlockAndPublish();
+ }
+ }
+ loop_count_++;
+
last_time_ = time;
}
+void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
+{
+ command_ = msg->data;
+}
+
+
//------ Joint Velocity controller node --------
ROS_REGISTER_CONTROLLER(JointVelocityControllerNode)
-JointVelocityControllerNode::JointVelocityControllerNode(): node_(ros::Node::instance())
+JointVelocityControllerNode::JointVelocityControllerNode(): node_(ros::Node::instance()), count(0)
{
c_ = new JointVelocityController();
controller_state_publisher_ = NULL;
@@ -153,7 +205,6 @@
void JointVelocityControllerNode::update()
{
- static int count = 0;
c_->update();
if(count % 10 == 0)
@@ -217,3 +268,5 @@
resp.v = cmd;
return true;
}
+
+} // namespace
Modified: pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h 2009-06-26 00:55:08 UTC (rev 17734)
+++ pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h 2009-06-26 00:55:23 UTC (rev 17735)
@@ -48,6 +48,7 @@
#include <tinyxml/tinyxml.h>
#include <std_srvs/Empty.h>
#include <ros/node.h>
+#include <ros/node_handle.h>
namespace controller
{
@@ -76,16 +77,17 @@
static RosController##c ROS_CONTROLLER_##c;
-
class Controller
{
public:
enum {CONSTRUCTED, INITIALIZED, RUNNING};
int state_;
+ bool autostart_;
Controller()
{
state_ = CONSTRUCTED;
+ autostart_ = true;
}
virtual ~Controller()
{
@@ -97,6 +99,7 @@
virtual void update(void) = 0;
virtual bool stopping() {return true;}
virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config) = 0;
+ virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n) { return false; };
bool isRunning()
{
@@ -137,6 +140,27 @@
return ret;
}
+ bool initRequest(mechanism::RobotState *robot, const ros::NodeHandle &n)
+ {
+ if (state_ != CONSTRUCTED)
+ return false;
+ else
+ {
+ // initialize
+ if (!init(robot, n))
+ return false;
+ state_ = INITIALIZED;
+
+ // autostart if needed
+ ros::NodeHandle nn;
+ nn.param("~autostart", autostart_, true);
+ if (autostart_ && !startRequest())
+ return false;
+
+ return true;
+ }
+ }
+
bool initXmlRequest(mechanism::RobotState *robot, TiXmlElement *config, std::string controller_name)
{
if (state_ != CONSTRUCTED)
@@ -148,6 +172,11 @@
return false;
state_ = INITIALIZED;
+ // autostart if needed
+ ros::Node::instance()->param(controller_name+"/autostart", autostart_, true);
+ if (autostart_ && !startRequest())
+ return false;
+
return true;
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
