Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21820] pkg/trunk
|
From: <stu...@us...> - 2009-08-13 20:58:23
|
Revision: 21820
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21820&view=rev
Author: stuglaser
Date: 2009-08-13 20:58:14 +0000 (Thu, 13 Aug 2009)
Log Message:
-----------
Ported mechanism control to the pluginlib
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-13 20:57:48 UTC (rev 21819)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-13 20:58:14 UTC (rev 21820)
@@ -6,6 +6,7 @@
genmsg()
gensrv()
set(_srcs
+ src/controller_manifest.cpp
src/cartesian_tff_controller.cpp
src/cartesian_trajectory_controller.cpp
src/cartesian_pose_controller.cpp
Added: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-13 20:58:14 UTC (rev 21820)
@@ -0,0 +1,3 @@
+<library path="lib/librobot_mechanism_controllers">
+ <plugin name="JointEffortController" class="JointEffortController" type="Controller" />
+</library>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-13 20:57:48 UTC (rev 21819)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-13 20:58:14 UTC (rev 21820)
@@ -26,11 +26,13 @@
<depend package="eigen" />
<depend package="filters" />
<depend package="diagnostic_msgs" />
+ <depend package="pluginlib" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp"
lflags="-Wl,-rpath,${prefix}/lib/ -L${prefix}/lib -lrobot_mechanism_controllers" />
+ <mechanism_control controller="${prefix}/controller_plugins.xml" />
</export>
<rosdep name="libtool"/>
</package>
Added: pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-13 20:58:14 UTC (rev 21820)
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "pluginlib/plugin_macros.h"
+#include "mechanism_control/controller.h"
+#include "robot_mechanism_controllers/joint_effort_controller.h"
+
+using namespace controller;
+
+BEGIN_PLUGIN_LIST(Controller)
+REGISTER_PLUGIN(JointEffortController)
+END_PLUGIN_LIST
+
Modified: pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-08-13 20:57:48 UTC (rev 21819)
+++ pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-08-13 20:58:14 UTC (rev 21820)
@@ -44,6 +44,7 @@
#include <realtime_tools/realtime_publisher.h>
#include <ros/node.h>
#include <controller_interface/controller_provider.h>
+#include "pluginlib/plugin_loader.h"
#include <mechanism_msgs/ListControllerTypes.h>
#include <mechanism_msgs/ListControllers.h>
#include <mechanism_msgs/SpawnController.h>
@@ -86,6 +87,7 @@
void getControllerSchedule(std::vector<size_t> &schedule);
ros::NodeHandle node_;
+ pluginlib::PluginLoader<controller::Controller> controller_loader_;
// for controller switching
std::vector<controller::Controller*> start_request_, stop_request_;
@@ -123,7 +125,7 @@
mechanism_msgs::SpawnController::Response &resp);
bool killControllerSrv(mechanism_msgs::KillController::Request &req,
mechanism_msgs::KillController::Response &resp);
- boost::mutex services_lock_;
+ boost::mutex services_lock_;
ros::ServiceServer srv_list_controllers_, srv_list_controller_types_, srv_spawn_controller_;
ros::ServiceServer srv_kill_controller_, srv_switch_controller_;
};
Modified: pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml 2009-08-13 20:57:48 UTC (rev 21819)
+++ pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml 2009-08-13 20:58:14 UTC (rev 21820)
@@ -30,6 +30,7 @@
<depend package="rosconsole" />
<depend package="diagnostic_msgs" />
<depend package="diagnostic_updater" />
+<depend package="pluginlib" />
<export>
<cpp cflags='-I${prefix}/include'
lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-13 20:57:48 UTC (rev 21819)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-13 20:58:14 UTC (rev 21820)
@@ -50,13 +50,14 @@
}
MechanismControl::MechanismControl(HardwareInterface *hw) :
- state_(NULL), hw_(hw),
+ state_(NULL), hw_(hw),
+ controller_loader_("mechanism_control", "controller"),
start_request_(0),
stop_request_(0),
please_switch_(false),
switch_success_(false),
- current_controllers_list_(0),
- used_by_realtime_(-1),
+ current_controllers_list_(0),
+ used_by_realtime_(-1),
pub_diagnostics_(node_, "/diagnostics", 1),
pub_joints_(node_, "joint_states", 1),
pub_mech_state_(node_, "mechanism_state", 1),
@@ -77,7 +78,7 @@
{
model_.initXml(config);
state_ = new RobotState(&model_, hw_);
-
+
// pre-allocate for realtime publishing
pub_mech_state_.msg_.set_actuator_states_size(hw_->actuators_.size());
int joints_size = 0;
@@ -89,21 +90,21 @@
}
pub_joints_.msg_.set_joints_size(joints_size);
pub_mech_state_.msg_.set_joint_states_size(joints_size);
-
+
// Advertise services
srv_list_controllers_ = node_.advertiseService("list_controllers", &MechanismControl::listControllersSrv, this);
srv_list_controller_types_ = node_.advertiseService("list_controller_types", &MechanismControl::listControllerTypesSrv, this);
srv_spawn_controller_ = node_.advertiseService("spawn_controller", &MechanismControl::spawnControllerSrv, this);
srv_kill_controller_ = node_.advertiseService("kill_controller", &MechanismControl::killControllerSrv, this);
srv_switch_controller_ = node_.advertiseService("switch_controller", &MechanismControl::switchControllerSrv, this);
-
+
// get the publish rate for mechanism state and diagnostics
double publish_rate_state, publish_rate_diagnostics;
node_.param("~publish_rate_mechanism_state", publish_rate_state, 100.0);
node_.param("~publish_rate_diagnostics", publish_rate_diagnostics, 1.0);
publish_period_state_ = 1.0/fmax(0.000001, publish_rate_state);
publish_period_diagnostics_ = 1.0/fmax(0.000001, publish_rate_diagnostics);
-
+
return true;
}
@@ -139,7 +140,7 @@
double end = ros::Time::now().toSec();
post_update_stats_.acc(end - end_update);
- // publish diagnostics and state
+ // publish diagnostics and state
publishDiagnostics();
publishState();
@@ -150,7 +151,7 @@
switch_success_ = true;
int last_started = -1;
for (unsigned int i=0; i<start_request_.size(); i++){
- if (!start_request_[i]->startRequest() &&
+ if (!start_request_[i]->startRequest() &&
switch_strictness_ == mechanism_msgs::SwitchController::Request::STRICT){
switch_success_ = false;
break;
@@ -242,8 +243,7 @@
if (c_node.getParam("type", type))
{
ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
- try {c = controller::ControllerFactory::Instance().CreateObject(type);}
- catch(Loki::DefaultFactoryError<std::string, controller::Controller>::Exception){}
+ c = controller_loader_.createPluginInstance(type, true);
}
// checks if controller was constructed
@@ -442,13 +442,13 @@
std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
int active = 0;
TimeStatistics blank_statistics;
-
+
std::vector<diagnostic_msgs::DiagnosticStatus> statuses;
diagnostic_updater::DiagnosticStatusWrapper status;
-
+
status.name = "Mechanism Control";
status.summary(0, "OK");
-
+
for (size_t i = 0; i < controllers.size(); ++i)
{
++active;
@@ -468,10 +468,10 @@
*std::max_element(controllers[i].stats->max1.begin(), controllers[i].stats->max1.end())*1e6,
controllers[i].stats->max*1e6,
state.c_str());
-
+
controllers[i].stats->acc = blank_statistics; // clear
}
-
+
#define REPORT_STATS(stats_, label) \
{ \
double m = extract_result<tag::max>(stats_.acc); \
@@ -485,13 +485,13 @@
stats_.max*1e6); \
stats_.acc = blank_statistics; \
}
-
+
REPORT_STATS(pre_update_stats_, "Before Update");
REPORT_STATS(update_stats_, "Update");
REPORT_STATS(post_update_stats_, "After Update");
-
+
status.addf("Active controllers", "%d", active);
-
+
statuses.push_back(status);
pub_diagnostics_.msg_.set_status_vec(statuses);
pub_diagnostics_.unlockAndPublish();
@@ -585,13 +585,14 @@
}
}
-
bool MechanismControl::listControllerTypesSrv(
mechanism_msgs::ListControllerTypes::Request &req,
mechanism_msgs::ListControllerTypes::Response &resp)
{
(void) req;
- std::vector<std::string> types = controller::ControllerFactory::Instance().RegisteredIds();
+ //std::vector<std::string> types = controller::ControllerHandleFactory::Instance().RegisteredIds();
+ std::vector<std::string> types = controller_loader_.getDeclaredPlugins();
+
resp.set_types_vec(types);
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
