Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2009-08-13 05:42:32
|
Revision: 21780
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21780&view=rev
Author: tfoote
Date: 2009-08-13 05:42:20 +0000 (Thu, 13 Aug 2009)
Log Message:
-----------
switching TransformStamped logic to follow that of all other frame_ids where the frame_id is the operating frame and there is now a child_frame_id which defines the target frame. And the parent frame is gone. This is only changing the message. The API change will come later.
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/test/test_move_base_local.py
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/demos/plug_in/fake_camera_frame.py
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py
pkg/trunk/sandbox/tf_node/src/tf_node.cpp
pkg/trunk/sandbox/webteleop/src/server.py
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/TransformStamped.msg
pkg/trunk/stacks/geometry/tf/include/tf/transform_broadcaster.h
pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h
pkg/trunk/stacks/geometry/tf/src/pytf.cpp
pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp
pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp
pkg/trunk/stacks/geometry/tf/test/testPython.py
pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/scripts/fake_gripper_frame.py
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/test/test_move_base_local.py
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/test/test_move_base_local.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/test/test_move_base_local.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -55,7 +55,7 @@
def tf_cb(self, msg):
if self.active:
for t in msg.transforms:
- if t.parent_id == 'odom_combined':
+ if t.header.frame_id == 'odom_combined':
tx = t.transform
# Convert Transform.msg to bullet
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -157,26 +157,26 @@
msg.transforms[i].header.stamp = mech_state->header.stamp ;
// Laser Chain
- msg.transforms[0].parent_id = "torso_lift_link" ;
- msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[0].header.frame_id = "torso_lift_link" ;
+ msg.transforms[0].child_frame_id = "laser_tilt_mount_link_cal" ;
tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
- msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
- msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
+ msg.transforms[1].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[1].child_frame_id = "laser_tilt_link_cal" ;
tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
- msg.transforms[2].parent_id = "torso_lift_link" ;
- msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[2].header.frame_id = "torso_lift_link" ;
+ msg.transforms[2].child_frame_id = "head_pan_link_cal" ;
tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
- msg.transforms[3].parent_id = "head_pan_link_cal" ;
- msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[3].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[3].child_frame_id = "head_tilt_link_cal" ;
//tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- msg.transforms[4].parent_id = "head_tilt_link_cal" ;
- msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
+ msg.transforms[4].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[4].child_frame_id = "stereo_optical_frame_cal" ;
tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -161,26 +161,26 @@
msg.transforms[i].header.stamp = mech_state->header.stamp ;
// Laser Chain
- msg.transforms[0].parent_id = "torso_lift_link" ;
- msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[0].header.frame_id = "torso_lift_link" ;
+ msg.transforms[0].child_frame_id = "laser_tilt_mount_link_cal" ;
tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
- msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
- msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
+ msg.transforms[1].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[1].child_frame_id = "laser_tilt_link_cal" ;
tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
- msg.transforms[2].parent_id = "torso_lift_link" ;
- msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[2].header.frame_id = "torso_lift_link" ;
+ msg.transforms[2].child_frame_id = "head_pan_link_cal" ;
tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
- msg.transforms[3].parent_id = "head_pan_link_cal" ;
- msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[3].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[3].child_frame_id = "head_tilt_link_cal" ;
//tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- msg.transforms[4].parent_id = "head_tilt_link_cal" ;
- msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
+ msg.transforms[4].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[4].child_frame_id = "stereo_optical_frame_cal" ;
tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -160,26 +160,26 @@
msg.transforms[i].header.stamp = mech_state->header.stamp ;
// Laser Chain
- msg.transforms[0].parent_id = "torso_lift_link" ;
- msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[0].header.frame_id = "torso_lift_link" ;
+ msg.transforms[0].child_frame_id = "laser_tilt_mount_link_cal" ;
tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
- msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
- msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
+ msg.transforms[1].header.frame_id = "laser_tilt_mount_link_cal" ;
+ msg.transforms[1].child_frame_id = "laser_tilt_link_cal" ;
tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
- msg.transforms[2].parent_id = "torso_lift_link" ;
- msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[2].header.frame_id = "torso_lift_link" ;
+ msg.transforms[2].child_frame_id = "head_pan_link_cal" ;
tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
- msg.transforms[3].parent_id = "head_pan_link_cal" ;
- msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[3].header.frame_id = "head_pan_link_cal" ;
+ msg.transforms[3].child_frame_id = "head_tilt_link_cal" ;
//tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- msg.transforms[4].parent_id = "head_tilt_link_cal" ;
- msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
+ msg.transforms[4].header.frame_id = "head_tilt_link_cal" ;
+ msg.transforms[4].child_frame_id = "stereo_optical_frame_cal" ;
tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -358,8 +358,8 @@
geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
out.header.stamp.fromSec(current_time_);
- out.header.frame_id = odom_frame_;
- out.parent_id = base_footprint_frame_;
+ out.header.frame_id = base_footprint_frame_;
+ out.child_frame_id = odom_frame_;
out.transform.translation.x = -x * cos(yaw) - y * sin(yaw);
out.transform.translation.y = +x * sin(yaw) - y * cos(yaw);
out.transform.translation.z = 0;
@@ -372,8 +372,8 @@
geometry_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
out2.header.stamp.fromSec(current_time_);
- out2.header.frame_id = base_link_frame_;
- out2.parent_id = base_footprint_frame_;
+ out2.header.frame_id = base_footprint_frame_;
+ out2.child_frame_id = base_link_frame_;
out2.transform.translation.x = 0;
out2.transform.translation.y = 0;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -37,6 +37,7 @@
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/jacobian.hpp>
+#include "tf/tfMessage.h"
#include "tf_conversions/tf_kdl.h"
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
@@ -587,8 +588,8 @@
if (pub_tf_->trylock())
{
//pub_tf_->msg_.transforms[0].header.stamp.fromSec();
- pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
- pub_tf_->msg_.transforms[0].parent_id = c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName();
+ pub_tf_->msg_.transforms[0].header.frame_id = c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName();
+ pub_tf_->msg_.transforms[0].child_frame_id = name_ + "/tool_frame";
tf::Transform t;
tf::TransformKDLToTF(c_.tool_frame_offset_, t);
tf::transformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
Modified: pkg/trunk/demos/plug_in/fake_camera_frame.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -26,6 +26,8 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
+
+#\todo replace this with usage of transform_sender in tf packge
import roslib
roslib.load_manifest('plug_in')
import rospy
@@ -62,9 +64,9 @@
self.msg = msg
t = TransformStamped()
-t.header.frame_id = 'high_def_frame'
+t.header.frame_id = 'base_link'
t.header.seq = 0
-t.parent_id = 'base_link'
+t.child_frame_id = 'high_def_frame'
t.transform.translation = xyz(0, 0, 1.25)
t.transform.rotation = rpy(0, 1.3, 0)
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -32,7 +32,6 @@
import rospy
from std_msgs.msg import *
from geometry_msgs.msg import Quaternion, PoseStamped
-from tf.msg import tfMessage
from math import *
from time import sleep
import tf.transformations
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -66,7 +66,7 @@
# Hack to get around the fact that we don't have pytf
def tfCallback(self,msg):
- if (msg.transforms[0].header.frame_id == self.global_frame and
+ if (msg.transforms[0].child_frame_id == self.global_frame and
self.odom_pose != None):
self.robot_position = Point((self.odom_pose.pose.pose.position.x -
msg.transforms[0].transform.translation.x),
Modified: pkg/trunk/sandbox/tf_node/src/tf_node.cpp
===================================================================
--- pkg/trunk/sandbox/tf_node/src/tf_node.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/sandbox/tf_node/src/tf_node.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -36,9 +36,6 @@
#include "tf/FrameGraph.h"
-#include "tf/tfMessage.h"
-
-
#include "geometry_msgs/PointStamped.h"
#include "sensor_msgs/PointCloud.h"
#include "geometry_msgs/PoseStamped.h"
@@ -163,10 +160,8 @@
******************************************************************************/
void publishTransformCallback(const boost::shared_ptr<const geometry_msgs::TransformStamped> &message) {
- tf::Transform tft;
- tf::transformMsgToTF(message->transform, tft);
- ROS_INFO((std::string("Sending transform from parent ") + message->parent_id + " to " + message->header.frame_id).c_str());
- tb->sendTransform(tft, message->header.stamp, message->header.frame_id, message->parent_id);
+ ROS_INFO((std::string("Sending transform from parent ") + message->header.frame_id + " to " + message->child_frame_id).c_str());
+ tb->sendTransform(*message);
}
class TimedTransform {
Modified: pkg/trunk/sandbox/webteleop/src/server.py
===================================================================
--- pkg/trunk/sandbox/webteleop/src/server.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/sandbox/webteleop/src/server.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -26,7 +26,6 @@
import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
-from tf.msg import tfMessage
from nav_msgs import *
from nav_msgs.msg import *
from std_msgs.msg import *
Modified: pkg/trunk/stacks/common_msgs/geometry_msgs/msg/TransformStamped.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/geometry_msgs/msg/TransformStamped.msg 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/common_msgs/geometry_msgs/msg/TransformStamped.msg 2009-08-13 05:42:20 UTC (rev 21780)
@@ -1,6 +1,6 @@
-# This expresses a transform between coordinate frames labeled by parent_id
-# and the frame_id field of the header.
+# This expresses a transform from coordinate frame header.frame_id
+# to the coordinate frame child_frame_id
Header header
-string parent_id # the frame id of the parent frame
+string child_frame_id # the frame id of the child frame
Transform transform
Modified: pkg/trunk/stacks/geometry/tf/include/tf/transform_broadcaster.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/transform_broadcaster.h 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/include/tf/transform_broadcaster.h 2009-08-13 05:42:20 UTC (rev 21780)
@@ -61,6 +61,10 @@
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const Stamped<Transform> & transform);
+ /** \brief Send a TransformStamped
+ * The stamped data structure includes frame_id, and time, and parent_id already. */
+ void sendTransform(const geometry_msgs::TransformStamped & transform);
+
/** \brief Send a Transform, stamped with time, frame_id and parent_id */
void sendTransform(const Transform & transform, const ros::Time& time, const std::string& frame_id, const std::string& parent_id);
Modified: pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h 2009-08-13 05:42:20 UTC (rev 21780)
@@ -175,10 +175,10 @@
/** \brief convert TransformStamped msg to Stamped<Transform> */
static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, Stamped<Transform>& bt)
-{transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.parent_id_ = msg.parent_id;};
+{transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.parent_id_ = msg.header.frame_id; bt.frame_id_ = msg.child_frame_id;};
/** \brief convert Stamped<Transform> to TransformStamped msg*/
static inline void transformStampedTFToMsg(const Stamped<Transform>& bt, geometry_msgs::TransformStamped & msg)
-{transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.parent_id = bt.parent_id_;};
+{transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.parent_id_; msg.child_frame_id = bt.frame_id_;};
/** \brief convert Pose msg to Pose */
static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)
Modified: pkg/trunk/stacks/geometry/tf/src/pytf.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/pytf.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/src/pytf.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -233,8 +233,8 @@
return NULL;
tf::Stamped< btTransform > transform;
PyObject *header = PyObject_BorrowAttrString(py_transform, "header");
- transform.frame_id_ = PyString_AsString(PyObject_BorrowAttrString(header, "frame_id"));
- transform.parent_id_ = PyString_AsString(PyObject_BorrowAttrString(py_transform, "parent_id"));
+ transform.frame_id_ = PyString_AsString(PyObject_BorrowAttrString(py_transform, "child_frame_id"));
+ transform.parent_id_ = PyString_AsString(PyObject_BorrowAttrString(header, "frame_id"));
if (rostime_converter(PyObject_BorrowAttrString(header, "stamp"), &transform.stamp_) != 1)
return NULL;
Modified: pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -42,15 +42,20 @@
node_.param(std::string("~tf_prefix"),tf_prefix_, std::string(""));
};
+void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
+{
+ tfMessage message;
+ message.transforms.push_back(msgtf);
+ publisher_.publish(message);
+}
+
void TransformBroadcaster::sendTransform(const Stamped<Transform> & transform)
{
- tfMessage message;
geometry_msgs::TransformStamped msgtf;
transformStampedTFToMsg(transform, msgtf);
msgtf.header.frame_id = tf::remap(tf_prefix_, msgtf.header.frame_id);
- msgtf.parent_id = tf::remap(tf_prefix_, msgtf.parent_id);
- message.transforms.push_back(msgtf);
- publisher_.publish(message);
+ msgtf.child_frame_id = tf::remap(tf_prefix_, msgtf.child_frame_id);
+ sendTransform(msgtf);
}
@@ -59,9 +64,8 @@
tfMessage message;
geometry_msgs::TransformStamped msgtf;
msgtf.header.stamp = time;
- msgtf.header.frame_id = frame_id;
- msgtf.header.frame_id = tf::remap(tf_prefix_, msgtf.header.frame_id);
- msgtf.parent_id = remap(tf_prefix_, parent_id);
+ msgtf.header.frame_id = tf::remap(tf_prefix_, parent_id);
+ msgtf.child_frame_id = tf::remap(tf_prefix_, frame_id);
transformTFToMsg(transform, msgtf.transform);
message.transforms.push_back(msgtf);
publisher_.publish(message);
Modified: pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -318,7 +318,7 @@
{
///\todo Use error reporting
std::string temp = ex.what();
- ROS_ERROR("Failure to set recieved transform %s to %s with error: %s\n", msg_in.transforms[i].header.frame_id.c_str(), msg_in.transforms[i].parent_id.c_str(), temp.c_str());
+ ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
}
}
};
Modified: pkg/trunk/stacks/geometry/tf/test/testPython.py
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/testPython.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/geometry/tf/test/testPython.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -51,8 +51,8 @@
def common(self, t):
m = geometry_msgs.msg.TransformStamped()
- m.header.frame_id = "THISFRAME"
- m.parent_id = "PARENT"
+ m.header.frame_id = "PARENT"
+ m.child_frame_id = "THISFRAME"
m.transform.translation.y = 5.0
m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
t.setTransform(m)
Modified: pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp 2009-08-13 05:42:20 UTC (rev 21780)
@@ -93,8 +93,8 @@
tf::Transform tf_frame;
tf::TransformKDLToTF(frame, tf_frame);
trans.header.stamp = time;
- trans.header.frame_id = tf::remap(tf_prefix_, f->first);
- trans.parent_id = tf::remap(tf_prefix_, root->first);
+ trans.header.frame_id = tf::remap(tf_prefix_, root->first);
+ trans.child_frame_id = tf::remap(tf_prefix_, f->first);
tf::transformTFToMsg(tf_frame, trans.transform);
tf_msg_.transforms[i++] = trans;
}
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/scripts/fake_gripper_frame.py
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/scripts/fake_gripper_frame.py 2009-08-13 05:38:40 UTC (rev 21779)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/scripts/fake_gripper_frame.py 2009-08-13 05:42:20 UTC (rev 21780)
@@ -54,13 +54,13 @@
return q
t = TransformStamped()
-t.header.frame_id = 'r_gripper_tool_frame'
+t.child_frame_id = 'r_gripper_tool_frame'
t.header.seq = 0
#t.parent_id = 'base_link'
#t.transform.translation = xyz(0.506305, -0.194843, 0.284381)
#t.transform.rotation = rpy(0, 1.13, 0)
#t.transform.rotation = Quaternion(0.050555, 0.245594, -0.051239, 0.966697)
-t.parent_id = 'high_def_frame'
+t.header.frame_id = 'high_def_frame'
t.transform.translation = xyz(1.013218, -0.019657, -0.044208)
t.transform.rotation = Quaternion(-0.092009, -0.482387, 0.139255, 0.859910)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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.
|
|
From: <stu...@us...> - 2009-08-13 21:17:27
|
Revision: 21833
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21833&view=rev
Author: stuglaser
Date: 2009-08-13 21:17:19 +0000 (Thu, 13 Aug 2009)
Log Message:
-----------
Dealing with the movement of controller.h
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-13 21:17:19 UTC (rev 21833)
@@ -6,6 +6,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
+ <depend package="controller_interface" />
<depend package="mechanism_model" />
<depend package="wg_robot_description_parser" />
<depend package="misc_utils" />
@@ -34,7 +35,7 @@
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp"
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2_mechanism_controllers" />
- <mechanism_control controller="${prefix}/controller_plugins.xml" />
+ <controller_interface controller="${prefix}/controller_plugins.xml" />
</export>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-13 21:17:19 UTC (rev 21833)
@@ -28,7 +28,7 @@
*/
#include "pluginlib/plugin_macros.h"
-#include "mechanism_control/controller.h"
+#include "controller_interface/controller.h"
#include "pr2_mechanism_controllers/caster_calibration_controller.h"
#include "pr2_mechanism_controllers/caster_controller.h"
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-13 21:17:19 UTC (rev 21833)
@@ -4,6 +4,7 @@
<author> Melonee Wise, Kevin Watts</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
+ <depend package="controller_interface" />
<depend package="mechanism_model" />
<depend package="wg_robot_description_parser" />
<depend package="realtime_tools" />
@@ -17,7 +18,7 @@
<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 -ljoint_qualification_controllers'/>
- <mechanism_control controller="${prefix}/controller_plugins.xml" />
+ <controller_interface controller="${prefix}/controller_plugins.xml" />
</export>
</package>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-13 21:17:19 UTC (rev 21833)
@@ -28,7 +28,7 @@
*/
#include "pluginlib/plugin_macros.h"
-#include "mechanism_control/controller.h"
+#include "controller_interface/controller.h"
#include "joint_qualification_controllers/checkout_controller.h"
#include "joint_qualification_controllers/hold_set_controller.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-13 21:17:19 UTC (rev 21833)
@@ -6,6 +6,7 @@
<review status="unreviewed" notes=""/>
<depend package="kdl_parser"/>
<depend package="rospy"/>
+ <depend package="controller_interface" />
<depend package="mechanism_model" />
<depend package="mechanism_control" />
<depend package="control_toolbox" />
@@ -32,7 +33,7 @@
<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" />
+ <controller_interface controller="${prefix}/controller_plugins.xml" />
</export>
<rosdep name="libtool"/>
</package>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-13 21:17:19 UTC (rev 21833)
@@ -28,7 +28,7 @@
*/
#include "pluginlib/plugin_macros.h"
-#include "mechanism_control/controller.h"
+#include "controller_interface/controller.h"
#include "robot_mechanism_controllers/cartesian_hybrid_controller.h"
#include "robot_mechanism_controllers/cartesian_pose_controller.h"
Modified: pkg/trunk/sandbox/experimental_controllers/manifest.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-08-13 21:17:19 UTC (rev 21833)
@@ -9,6 +9,7 @@
<depend package="kdl_parser"/>
<depend package="rospy"/>
+ <depend package="controller_interface" />
<depend package="mechanism_model" />
<depend package="mechanism_control" />
<depend package="control_toolbox" />
@@ -32,7 +33,7 @@
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp"
lflags="-Wl,-rpath,${prefix}/lib/ -L${prefix}/lib -lexperimental_controllers" />
- <mechanism_control controller="${prefix}/controller_plugins.xml" />
+ <controller_interface controller="${prefix}/controller_plugins.xml" />
</export>
<sysdepend os="ubuntu" version="8.04-hardy" package="libltdl3-dev"/>
<rosdep name="libtool"/>
Modified: pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp 2009-08-13 21:17:19 UTC (rev 21833)
@@ -28,7 +28,7 @@
*/
#include "pluginlib/plugin_macros.h"
-#include "mechanism_control/controller.h"
+#include "controller_interface/controller.h"
#include "experimental_controllers/joint_trajectory_controller2.h"
#include "experimental_controllers/pid_position_velocity_controller.h"
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-13 21:10:03 UTC (rev 21832)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-08-13 21:17:19 UTC (rev 21833)
@@ -51,7 +51,7 @@
MechanismControl::MechanismControl(HardwareInterface *hw) :
state_(NULL), hw_(hw),
- controller_loader_("mechanism_control", "controller"),
+ controller_loader_("controller_interface", "controller"),
start_request_(0),
stop_request_(0),
please_switch_(false),
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-14 05:28:01
|
Revision: 21861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21861&view=rev
Author: isucan
Date: 2009-08-14 05:27:52 +0000 (Fri, 14 Aug 2009)
Log Message:
-----------
monitoring robot velocity to decide when to stop motion (if for instance, the robot bumps into something)
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 05:27:52 UTC (rev 21861)
@@ -52,6 +52,7 @@
#include <planning_environment/util/construct_object.h>
#include <geometric_shapes/bodies.h>
+#include <valarray>
#include <algorithm>
#include <cstdlib>
@@ -545,6 +546,9 @@
ros::Duration eps(0.01);
ros::Duration epsLong(0.1);
+ std::valarray<double> velocityHistory(3);
+ unsigned int velocityHistoryIndex = 0;
+
while (true)
{
// if we have to stop, do so
@@ -624,7 +628,6 @@
if (feedback->mode == move_arm::MoveArmFeedback::MOVING)
{
bool safe = planningMonitor_->isEnvironmentSafe();
- bool valid = true;
// we don't want to check the part of the path that was already executed
currentPos = planningMonitor_->closestStateOnPath(currentPath, currentPos, currentPath.states.size() - 1, planningMonitor_->getRobotState());
@@ -637,14 +640,24 @@
CollisionCost ccost;
planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, &ccost, true, _1), 0);
planningMonitor_->setAllowedContacts(allowed_contacts);
- valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST +
- planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
+ bool valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST +
+ planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
planningMonitor_->clearAllowedContacts();
planningMonitor_->setOnCollisionContactCallback(NULL);
if (!valid)
ROS_INFO("Maximum path contact penetration depth is %f at link %s, sum of all contact depths is %f", ccost.cost, ccost.link.c_str(), ccost.sum);
+ if (velocityHistoryIndex >= velocityHistory.size())
+ {
+ double sum = velocityHistory.sum();
+ if (sum < 1e-3)
+ {
+ ROS_INFO("The total velocity of the robot over the last %d samples is %f. Self-preempting...", (int)velocityHistory.size(), sum);
+ state = PREEMPTED;
+ }
+ }
+
if (state == PREEMPTED || !safe || !valid)
{
if (state == PREEMPTED)
@@ -703,6 +716,7 @@
break;
}
ROS_INFO("Sent trajectory %d to controller", trajectoryId);
+ velocityHistoryIndex = 0;
}
else
{
@@ -711,6 +725,11 @@
break;
}
}
+ else
+ {
+ velocityHistory[velocityHistoryIndex % velocityHistory.size()] = planningMonitor_->getTotalVelocity();
+ velocityHistoryIndex++;
+ }
// monitor controller execution by calling trajectory query
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-14 05:27:52 UTC (rev 21861)
@@ -46,6 +46,7 @@
#include <boost/bind.hpp>
#include <vector>
#include <string>
+#include <map>
namespace planning_environment
{
@@ -125,6 +126,12 @@
return robotState_;
}
+ /** \brief Return the maintained robot velocity (square root of sum of squares of velocity at each joint)*/
+ double getTotalVelocity(void) const
+ {
+ return robotVelocity_;
+ }
+
/** \brief Return the transform listener */
tf::TransformListener *getTransformListener(void) const
{
@@ -204,6 +211,7 @@
*attachedBodyNotifier_;
planning_models::StateParams *robotState_;
+ double robotVelocity_;
tf::Pose pose_;
std::string robot_frame_;
std::string frame_id_;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-14 05:27:52 UTC (rev 21861)
@@ -49,7 +49,8 @@
onAfterAttachBody_ = NULL;
attachedBodyNotifier_ = NULL;
havePose_ = haveJointState_ = false;
-
+ robotVelocity_ = 0.0;
+
if (rm_->loadedModels())
{
kmodel_ = rm_->getKinematicModel().get();
@@ -118,7 +119,8 @@
bool change = !haveJointState_;
static bool first_time = true;
-
+
+ double totalv = 0.0;
unsigned int n = jointState->get_joints_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
@@ -128,6 +130,8 @@
if (joint->usedParams == 1)
{
double pos = jointState->joints[i].position;
+ double vel = jointState->joints[i].velocity;
+ totalv += vel * vel;
planning_models::KinematicModel::RevoluteJoint* rjoint = dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(joint);
if (rjoint)
if (rjoint->continuous)
@@ -147,7 +151,8 @@
ROS_ERROR("Unknown joint: %s", jointState->joints[i].name.c_str());
}
}
-
+ robotVelocity_ = sqrt(totalv);
+
first_time = false;
lastJointStateUpdate_ = jointState->header.stamp;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-14 22:24:08
|
Revision: 21900
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21900&view=rev
Author: isucan
Date: 2009-08-14 22:23:53 +0000 (Fri, 14 Aug 2009)
Log Message:
-----------
splitting genetic algo search from planning
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_chomp_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_fake_planning.launch
pkg/trunk/sandbox/ompl_search/
pkg/trunk/sandbox/ompl_search/CMakeLists.txt
pkg/trunk/sandbox/ompl_search/Makefile
pkg/trunk/sandbox/ompl_search/mainpage.dox
pkg/trunk/sandbox/ompl_search/manifest.xml
pkg/trunk/sandbox/ompl_search/src/
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.h
pkg/trunk/sandbox/ompl_search/src/search_valid_state.cpp
pkg/trunk/sandbox/ompl_search/state_search.launch
Removed Paths:
-------------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,13 +1,12 @@
<launch>
<node pkg="move_arm" type="move_arm_action" output="screen">
- <remap from="robot_description" to="robot_description" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
- <remap from="get_valid_state" to="find_valid_state" />
+ <remap from="get_valid_state" to="ompl_search/find_valid_state" />
<remap from="controller_start" to="/r_arm_joint_waypoint_controller/TrajectoryStart" />
<remap from="controller_query" to="/r_arm_joint_waypoint_controller/TrajectoryQuery" />
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1041,7 +1041,7 @@
private:
ros::NodeHandle nh_;
- MoveArmSetup &setup_;
+ MoveArmSetup &setup_;
actionlib::SingleGoalActionServer<MoveArmAction> as_;
planning_environment::PlanningMonitor *planningMonitor_;
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-08-14 22:23:53 UTC (rev 21900)
@@ -18,7 +18,6 @@
src/helpers/ompl_planner/kinematicpSBLSetup.cpp
src/helpers/ompl_planner/dynamicRRTSetup.cpp
src/helpers/ompl_planner/dynamicKPIECESetup.cpp
- src/helpers/ompl_planner/IKSetup.cpp
src/helpers/Model.cpp
)
rospack_link_boost(ompl_planning_helpers thread)
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -40,7 +40,6 @@
#include "ompl_ros/ModelKinematic.h"
#include "ompl_ros/ModelDynamic.h"
#include "ompl_planning/planners/PlannerSetup.h"
-#include "ompl_planning/planners/IKSetup.h"
#include <boost/shared_ptr.hpp>
#include <string>
@@ -56,7 +55,6 @@
{
planningMonitor = pMonitor;
groupName = gName;
- ik = NULL;
createMotionPlanningInstances(planningMonitor->getCollisionModels()->getGroupPlannersConfig(groupName));
}
@@ -65,15 +63,12 @@
for (std::map<std::string, PlannerSetup*>::iterator i = planners.begin(); i != planners.end() ; ++i)
if (i->second)
delete i->second;
- if (ik)
- delete ik;
}
planning_environment::PlanningMonitor *planningMonitor;
std::string groupName;
std::map<std::string, PlannerSetup*> planners;
- IKSetup *ik;
protected:
Deleted: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,62 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, 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 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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef OMPL_PLANNING_PLANNERS_IK_SETUP_
-#define OMPL_PLANNING_PLANNERS_IK_SETUP_
-
-#include "ompl_ros/ModelKinematic.h"
-#include <ompl/extension/kinematic/extension/ik/GAIK.h>
-#include <boost/shared_ptr.hpp>
-
-namespace ompl_planning
-{
- class IKSetup
- {
- public:
- IKSetup(void);
- ~IKSetup(void);
-
- void setup(planning_environment::PlanningMonitor *planningMonitor, const std::string &groupName,
- boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-
- ompl::kinematic::GAIK *gaik; // ik with genetic algorithms
- ompl_ros::ModelBase *ompl_model;
-
- };
-
-}
-
-#endif
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -90,13 +90,7 @@
else
if (type == "GAIK")
{
- if (ik)
- {
- ROS_WARN("Re-definition of '%s'", type.c_str());
- delete ik;
- }
- ik = new IKSetup();
- ik->setup(planningMonitor, groupName, cfgs[i]);
+ // skip this, but don't output error
}
else
ROS_WARN("Unknown planner type: %s", type.c_str());
Deleted: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,92 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, 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 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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#include "ompl_planning/planners/IKSetup.h"
-#include <ros/console.h>
-
-ompl_planning::IKSetup::IKSetup(void)
-{
- ompl_model = NULL;
- gaik = NULL;
-}
-
-ompl_planning::IKSetup::~IKSetup(void)
-{
- if (gaik)
- delete gaik;
- if (ompl_model)
- delete ompl_model;
-}
-
-void ompl_planning::IKSetup::setup(planning_environment::PlanningMonitor *planningMonitor, const std::string &groupName,
- boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- ROS_DEBUG("Adding IK instance for '%s'", groupName.c_str());
- ompl_model = new ompl_ros::ModelKinematic(planningMonitor, groupName);
- ompl_model->configure();
-
- gaik = new ompl::kinematic::GAIK(dynamic_cast<ompl::kinematic::SpaceInformationKinematic*>(ompl_model->si));
-
- if (options->hasParam("max_improve_steps"))
- {
- gaik->setMaxImproveSteps(options->getParamInt("max_improve_steps", gaik->getMaxImproveSteps()));
- ROS_DEBUG("Max improve steps is set to %u", gaik->getMaxImproveSteps());
- }
-
- if (options->hasParam("range"))
- {
- gaik->setRange(options->getParamDouble("range", gaik->getRange()));
- ROS_DEBUG("Range is set to %g", gaik->getRange());
- }
-
- if (options->hasParam("pool_size"))
- {
- gaik->setPoolSize(options->getParamInt("pool_size", gaik->getPoolSize()));
- ROS_DEBUG("Pool size is set to %u", gaik->getPoolSize());
- }
-
- if (options->hasParam("pool_expansion_size"))
- {
- gaik->setPoolExpansionSize(options->getParamInt("pool_expansion_size", gaik->getPoolExpansionSize()));
- ROS_DEBUG("Pool expansion size is set to %u", gaik->getPoolExpansionSize());
- }
-
- if (options->hasParam("max_improve_steps"))
- {
- gaik->setMaxImproveSteps(options->getParamInt("max_improve_steps", gaik->getMaxImproveSteps()));
- ROS_DEBUG("Max improve steps is set to %u", gaik->getMaxImproveSteps());
- }
-}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -58,7 +58,6 @@
nodeHandle_.param<double>("~state_delay", stateDelay_, 0.01);
planKinematicPathService_ = nodeHandle_.advertiseService("~plan_kinematic_path", &OMPLPlanning::planToGoal, this);
- findValidStateService_ = nodeHandle_.advertiseService("find_valid_state", &OMPLPlanning::findValidState, this);
}
/** Free the memory */
@@ -178,33 +177,12 @@
return st;
}
- bool findValidState(motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res)
- {
- ROS_INFO("Received request for searching a valid state");
- bool st = false;
-
- planning_models::StateParams *startState = fillStartState(req.start_state);
- if (startState)
- {
- std::stringstream ss;
- startState->print(ss);
- ROS_DEBUG("Complete starting state:\n%s", ss.str().c_str());
- st = requestHandler_.findState(models_, startState, req, res);
- delete startState;
- }
- else
- ROS_ERROR("Starting robot state is unknown. Cannot start search.");
-
- return st;
- }
-
// ROS interface
ros::NodeHandle nodeHandle_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
tf::TransformListener tf_;
ros::ServiceServer planKinematicPathService_;
- ros::ServiceServer findValidStateService_;
double stateDelay_;
// planning data
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -44,68 +44,6 @@
onFinishPlan_ = onFinishPlan;
}
-bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req)
-{
- ModelMap::const_iterator pos = models.find(req.params.model_id);
-
- if (pos == models.end())
- {
- ROS_ERROR("Cannot search for '%s'. Model does not exist", req.params.model_id.c_str());
- return false;
- }
-
- /* find the model */
- Model *m = pos->second;
-
- IKSetup *iksetup = m->ik;
- if (iksetup == NULL)
- {
- ROS_ERROR("No IK setup available for model '%s'", pos->first.c_str());
- return false;
- }
-
- /* check if the desired distance metric is defined */
- if (iksetup->ompl_model->sde.find(req.params.distance_metric) == iksetup->ompl_model->sde.end())
- {
- ROS_ERROR("Distance evaluator not found: '%s'", req.params.distance_metric.c_str());
- return false;
- }
-
- // check headers
- for (unsigned int i = 0 ; i < req.constraints.pose_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.constraints.pose_constraint[i].pose.header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for pose constraint message %u", req.constraints.pose_constraint[i].pose.header.frame_id.c_str(), i);
- return false;
- }
-
- for (unsigned int i = 0 ; i < req.constraints.joint_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.constraints.joint_constraint[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for joint constraint message %u", req.constraints.joint_constraint[i].header.frame_id.c_str(), i);
- return false;
- }
-
- if (!req.states.empty())
- {
- // make sure all joints are in the group
- for (unsigned int i = 0 ; i < req.names.size() ; ++i)
- {
- int idx = m->planningMonitor->getKinematicModel()->getJointIndexInGroup(req.names[i], m->groupName);
- if (idx < 0)
- return false;
- }
-
- if (iksetup->ompl_model->si->getStateDimension() != m->planningMonitor->getKinematicModel()->getJointsDimension(req.names))
- {
- ROS_ERROR("The state dimension for model '%s' does not match the dimension of the joints defining the hint states", req.params.model_id.c_str());
- return false;
- }
- }
-
- return true;
-}
-
bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -190,30 +128,6 @@
return true;
}
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup)
-{
- /* clear memory */
- iksetup->ompl_model->si->clearGoal();
-
- // clear clones of environments
- iksetup->ompl_model->clearEnvironmentDescriptions();
-
- setWorkspaceBounds(req.params, iksetup->ompl_model);
- iksetup->ompl_model->si->setStateDistanceEvaluator(iksetup->ompl_model->sde[req.params.distance_metric]);
-
- /* set the pose of the whole robot */
- ompl_ros::EnvironmentDescription *ed = iksetup->ompl_model->getEnvironmentDescription();
- ed->kmodel->computeTransforms(startState->getParams());
- ed->collisionSpace->updateRobotModel();
-
- /* add goal state */
- iksetup->ompl_model->planningMonitor->transformConstraintsToFrame(req.constraints, iksetup->ompl_model->planningMonitor->getFrameId());
- iksetup->ompl_model->si->setGoal(computeGoalFromConstraints(iksetup->ompl_model, req.constraints));
-
- /* print some information */
- printSettings(iksetup->ompl_model->si);
-}
-
void ompl_planning::RequestHandler::setWorkspaceBounds(motion_planning_msgs::KinematicSpaceParameters ¶ms, ompl_ros::ModelBase *ompl_model)
{
/* set the workspace volume for planning */
@@ -341,87 +255,6 @@
return result;
}
-bool ompl_planning::RequestHandler::findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req,
- motion_planning_msgs::ConvertToJointConstraint::Response &res)
-{
- if (!isRequestValid(models, req))
- return false;
-
- // find the data we need
- Model *m = models[req.params.model_id];
- IKSetup *iksetup = m->ik;
-
- // copy the hint states to OMPL datastructures
- const unsigned int dim = iksetup->ompl_model->si->getStateDimension();
- ompl::base::State *goal = new ompl::base::State(dim);
- std::vector<ompl::base::State*> hints;
- planning_models::StateParams hs(*start);
- unsigned int sdim = m->planningMonitor->getKinematicModel()->getJointsDimension(req.names);
- for (unsigned int i = 0 ; i < req.states.size() ; ++i)
- {
- if (req.states[i].vals.size() != sdim)
- {
- ROS_ERROR("Incorrect number of parameters for hint state at index %u. Expected %u, got %d.", i, sdim, (int)req.states[i].vals.size());
- continue;
- }
- ompl::base::State *st = new ompl::base::State(dim);
- hs.setParamsJoints(req.states[i].vals, req.names);
- hs.copyParamsGroup(st->values, iksetup->ompl_model->groupID);
- std::stringstream ss;
- ss << "Hint state: ";
- iksetup->ompl_model->si->printState(st, ss);
- ROS_DEBUG("%s", ss.str().c_str());
- hints.push_back(st);
- }
-
- m->planningMonitor->getEnvironmentModel()->lock();
- m->planningMonitor->getKinematicModel()->lock();
-
- configure(start, req, iksetup);
- ros::WallTime startTime = ros::WallTime::now();
- bool found = iksetup->gaik->solve(req.allowed_time, goal, hints);
- double stime = (ros::WallTime::now() - startTime).toSec();
- m->planningMonitor->getEnvironmentModel()->unlock();
- m->planningMonitor->getKinematicModel()->unlock();
-
- ROS_DEBUG("Spent %f seconds searching for state", stime);
-
- if (found)
- {
- int u = 0;
- std::vector<std::string> jnames;
- std::stringstream ss;
- m->planningMonitor->getKinematicModel()->getJointsInGroup(jnames, iksetup->ompl_model->groupID);
- res.joint_constraint.resize(jnames.size());
- for (unsigned int i = 0; i < jnames.size() ; ++i)
- {
- res.joint_constraint[i].header.frame_id = m->planningMonitor->getFrameId();
- res.joint_constraint[i].header.stamp = m->planningMonitor->lastMapUpdate();
- res.joint_constraint[i].joint_name = jnames[i];
- planning_models::KinematicModel::Joint *joint = m->planningMonitor->getKinematicModel()->getJoint(jnames[i]);
- res.joint_constraint[i].value.resize(joint->usedParams);
- res.joint_constraint[i].tolerance_above.resize(joint->usedParams, 0.0);
- res.joint_constraint[i].tolerance_below.resize(joint->usedParams, 0.0);
- for (unsigned int j = 0 ; j < joint->usedParams ; ++j)
- {
- res.joint_constraint[i].value[j] = goal->values[j + u];
- ss << goal->values[j + u] << " ";
- }
- u += joint->usedParams;
- }
- ROS_DEBUG("Solution was found: %s", ss.str().c_str());
- }
- else
- ROS_DEBUG("No solution was found");
-
- iksetup->ompl_model->si->clearGoal();
- delete goal;
- for (unsigned int i = 0 ; i < hints.size() ; ++i)
- delete hints[i];
-
- return true;
-}
-
bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -41,7 +41,6 @@
#include <ros/ros.h>
#include <motion_planning_msgs/GetMotionPlan.h>
-#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <boost/bind.hpp>
/** \brief Main namespace */
@@ -67,16 +66,10 @@
/** \brief Check if the request is valid */
bool isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req);
- /** \brief Check if the request is valid */
- bool isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req);
-
/** \brief Check and compute a motion plan. Return true if the plan was succesfully computed */
bool computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);
- /** \brief Find a state in the specified goal region. Return true if state was found */
- bool findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res);
-
/** \brief Enable callback for when a motion plan computation is completed */
void setOnFinishPlan(const boost::function<void(PlannerSetup*)> &onFinishPlan);
@@ -92,9 +85,6 @@
/** \brief Set up all the data needed by motion planning based on a request */
void configure(const planning_models::StateParams *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup);
- /** \brief Set up all the data needed by inverse kinematics based on a request */
- void configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup);
-
/** \brief Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol);
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/perception/components:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -30,7 +30,7 @@
<param name="object_padd" type="double" value="0.05" />
</node>
- <include file="$(find 3dnav_pr2)/launch/perception/components/tilt_laser_self_filter.launch" />
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/tilt_laser_self_filter.launch" />
<!-- assemble pointcloud into a full world view -->
<node machine="four" pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
@@ -50,6 +50,6 @@
</node>
<!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/perception/components/collision_map_self_occ.launch" />
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/collision_map_self_occ.launch" />
</launch>
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch (from rev 21868, pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,47 @@
+
+<launch>
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <rosparam ns="/robot_description_planning" command="load" file="$(find pr2_defs)/planning/planning.yaml" />
+ <node machine="two" pkg="chomp_motion_planner" name="chomp_planner_longrange" type="chomp_planner_node" args="collision_map:=collision_map_occ" respawn="true" output="screen" clear_params="true">
+ <param name="collision_clearance" value="0.07"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_radius" value="0.03"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_radius" value="0.03"/>
+ <param name="collision_links/r_gripper_palm_link/link_radius" value="0.05"/>
+ <param name="collision_links/r_forearm_link/link_radius" value="0.065"/>
+ <param name="collision_links/r_upper_arm_link/link_radius" value="0.08"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_palm_link/link_clearance" value="0.1"/>
+ <param name="collision_links/r_forearm_link/link_clearance" value="0.085"/>
+ <param name="collision_links/r_upper_arm_link/link_clearance" value="0.07"/>
+
+ <param name="animate_path" value="false"/>
+ <param name="reference_frame" value="base_link"/>
+
+ <param name="collision_space/size_x" value="2.0" />
+ <param name="collision_space/size_y" value="3.0" />
+ <param name="collision_space/size_z" value="4.0" />
+ <param name="collision_space/origin_x" value="0.1" />
+ <param name="collision_space/origin_y" value="-1.5" />
+ <param name="collision_space/origin_z" value="-2.0" />
+ <param name="collision_space/resolution" value="0.02" />
+
+ <param name="trajectory_duration" value="4.5"/>
+ <param name="trajectory_discretization" value="0.03"/>
+ <param name="learning_rate" value="3.0" />
+ <param name="max_iterations" value="500" />
+ <param name="max_iterations_after_collision_free" value="100" />
+ <param name="...
[truncated message content] |
|
From: <is...@us...> - 2009-08-14 22:43:34
|
Revision: 21901
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21901&view=rev
Author: isucan
Date: 2009-08-14 22:43:25 +0000 (Fri, 14 Aug 2009)
Log Message:
-----------
hopefully a fix for waitForMap bug
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp 2009-08-14 22:23:53 UTC (rev 21900)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp 2009-08-14 22:43:25 UTC (rev 21901)
@@ -65,8 +65,10 @@
else
ROS_INFO("Configuring action for '%s' (IK is %senabled)", group_.c_str(), perform_ik_ ? "" : "not ");
- planningMonitor_->waitForState();
- planningMonitor_->waitForMap();
+ if (planningMonitor_->getExpectedJointStateUpdateInterval() > 1e-3)
+ planningMonitor_->waitForState();
+ if (planningMonitor_->getExpectedMapUpdateInterval() > 1e-3)
+ planningMonitor_->waitForMap();
if (!getControlJointNames(groupJointNames_))
return false;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-14 22:23:53 UTC (rev 21900)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-14 22:43:25 UTC (rev 21901)
@@ -158,6 +158,24 @@
onCollisionContact_ = callback;
maxCollisionContacts_ = maxContacts;
}
+
+ /** \brief Return the maximum amount of time that is allowed to pass between updates to the map. */
+ double getExpectedMapUpdateInterval(void) const
+ {
+ return intervalCollisionMap_;
+ }
+
+ /** \brief Return the maximum amount of time that is allowed to pass between updates to the state. */
+ double getExpectedJointStateUpdateInterval(void) const
+ {
+ return intervalState_;
+ }
+
+ /** \brief Return the maximum amount of time that is allowed to pass between updates to the pose. */
+ double getExpectedPoseUpdateInterval(void) const
+ {
+ return intervalPose_;
+ }
protected:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-15 05:27:12
|
Revision: 21933
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21933&view=rev
Author: isucan
Date: 2009-08-15 05:27:03 +0000 (Sat, 15 Aug 2009)
Log Message:
-----------
bugfix in self collision checking with attached objects
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -41,6 +41,8 @@
#include <move_arm/MoveArmAction.h>
#include <move_arm/ActuateGripperAction.h>
+#include <mapping_msgs/AttachedObject.h>
+
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_msgs/JointTraj.h>
@@ -301,6 +303,9 @@
actionlib::SimpleActionClient<move_arm::ActuateGripperAction> gripper(nh, "actuate_gripper_" + group);
ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 1);
+ ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_object", 1);
+
+
std::map<std::string, move_arm::MoveArmGoal> goals;
std::vector<std::string> names(7);
@@ -363,6 +368,50 @@
printPose(p);
}
else
+ if (cmd == "att0")
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header.frame_id = "r_wrist_roll_link";
+ ao.header.stamp = ros::Time::now();
+ ao.link_name = "r_wrist_roll_link";
+ pubAttach.publish(ao);
+ }
+ else
+ if (cmd == "att1")
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header.frame_id = "r_wrist_roll_link";
+ ao.header.stamp = ros::Time::now();
+ ao.link_name = "r_wrist_roll_link";
+
+ mapping_msgs::Object object;
+ object.type = mapping_msgs::Object::CYLINDER;
+ object.dimensions.push_back(0.02); // 4 cm diam
+ object.dimensions.push_back(1.3); // 48 inch
+
+ // identity transform should place the object in the center
+ geometry_msgs::Pose pose;
+ pose.position.x = 0.16;
+ pose.position.y = 0;
+ pose.position.z = 0;
+
+ pose.orientation.x = 0;
+ pose.orientation.y = 0;
+ pose.orientation.z = 0;
+ pose.orientation.w = 1;
+
+ ao.objects.push_back(object);
+ ao.poses.push_back(pose);
+
+ ao.touch_links.push_back("r_gripper_l_finger_link");
+ ao.touch_links.push_back("r_gripper_r_finger_link");
+ ao.touch_links.push_back("r_gripper_l_finger_tip_link");
+ ao.touch_links.push_back("r_gripper_r_finger_tip_link");
+ ao.touch_links.push_back("r_gripper_palm_link");
+
+ pubAttach.publish(ao);
+ }
+ else
if (cmd == "time")
{
std::cout << " Allowed execution time is " << allowed_time << " seconds" << std::endl;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -546,7 +546,7 @@
ros::Duration eps(0.01);
ros::Duration epsLong(0.1);
- std::valarray<double> velocityHistory(3);
+ std::valarray<double> velocityHistory(20);
unsigned int velocityHistoryIndex = 0;
while (true)
@@ -654,8 +654,9 @@
ROS_INFO("Maximum path contact penetration depth is %f at link %s, sum of all contact depths is %f", ccost.cost, ccost.link.c_str(), ccost.sum);
if (velocityHistoryIndex >= velocityHistory.size())
- {
+ {
double sum = velocityHistory.sum();
+ ROS_DEBUG("vel = %f", sum);
if (sum < 1e-3)
{
ROS_INFO("The total velocity of the robot over the last %d samples is %f. Self-preempting...", (int)velocityHistory.size(), sum);
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -270,6 +270,7 @@
link->attachedBodies.push_back(new planning_models::KinematicModel::AttachedBody(link));
tf::poseMsgToTF(poseP.pose, link->attachedBodies[j]->attachTrans);
link->attachedBodies[j]->shape = shape;
+ link->attachedBodies[j]->touch_links = attachedObject->touch_links;
}
result = true;
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-08-15 05:27:03 UTC (rev 21933)
@@ -253,17 +253,20 @@
}
/** \brief The constant transform applied to the link (needs to be specified by user) */
- btTransform attachTrans;
+ btTransform attachTrans;
/** \brief The geometry of the attached body */
- shapes::Shape *shape;
+ shapes::Shape *shape;
/** \brief The global transform for this link (computed by forward kinematics) */
- btTransform globalTrans;
+ btTransform globalTrans;
/** \brief The link that owns this attached body */
- Link *owner;
+ Link *owner;
+ /** \brief The set of links this body is allowed to touch */
+ std::vector<std::string> touch_links;
+
protected:
/** \brief recompute globalTrans */
void computeTransform(btTransform &parentTrans);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -859,6 +859,7 @@
ab->attachTrans = src->attachedBodies[i]->attachTrans;
ab->shape = shapes::cloneShape(src->attachedBodies[i]->shape);
ab->globalTrans = src->attachedBodies[i]->globalTrans;
+ ab->touch_links = src->attachedBodies[i]->touch_links;
dest->attachedBodies.push_back(ab);
}
for (unsigned int i = 0 ; i < src->after.size() ; ++i)
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-08-15 05:27:03 UTC (rev 21933)
@@ -55,6 +55,9 @@
EnvironmentModelODE(void);
virtual ~EnvironmentModelODE(void);
+ /** \brief Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(const std::vector<std::string> &links);
+
/** \brief Get the list of contacts (collisions) */
virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
@@ -212,6 +215,7 @@
struct kGeom
{
std::vector<dGeomID> geom;
+ std::vector< std::vector<bool> > allowedTouch;
bool enabled;
planning_models::KinematicModel::Link *link;
unsigned int index;
@@ -329,7 +333,9 @@
dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
void updateGeom(dGeomID geom, const btTransform &pose) const;
void removeCollidingObjects(const dGeomID geom);
-
+
+ void updateAllowedTouch(void);
+
/** \brief Check if thread-specific routines have been called */
void checkThreadInit(void) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -134,6 +134,7 @@
}
m_modelGeom.linkGeom.push_back(kg);
}
+ updateAllowedTouch();
}
dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape)
@@ -275,6 +276,7 @@
kg->geom.push_back(ga);
}
}
+ updateAllowedTouch();
}
void collision_space::EnvironmentModelODE::updateRobotModel(void)
@@ -491,12 +493,40 @@
EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
if (kg1 && kg2)
{
- if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
- return;
+ // if we have two body parts (it is the first in the list of geoms for the link), check if they are allowed to collide
+ if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false && kg1->geom[0] == o1 && kg2->geom[0] == o2)
+ return;
else
{
- cdata->link1 = kg1->link;
- cdata->link2 = kg2->link;
+ // if we are looking at a link and an attached object
+ if ((kg1->geom[0] == o1 && kg2->geom[0] != o2) || (kg1->geom[0] != o1 && kg2->geom[0] == o2))
+ {
+ int p1 = -1, p2 = -1;
+ for (unsigned int i = 0 ; i < kg1->geom.size() ; ++i)
+ if (kg1->geom[i] == o1)
+ {
+ p1 = i;
+ break;
+ }
+ for (unsigned int i = 0 ; i < kg2->geom.size() ; ++i)
+ if (kg2->geom[i] == o2)
+ {
+ p2 = i;
+ break;
+ }
+ assert(p1 >= 0 && p2 >= 0);
+ if (p1 == 0)
+ if (kg2->allowedTouch[p2][kg1->index])
+ return;
+ if (p2 == 0)
+ if (kg1->allowedTouch[p1][kg2->index])
+ return;
+ }
+ else
+ {
+ cdata->link1 = kg1->link;
+ cdata->link2 = kg2->link;
+ }
}
}
}
@@ -787,6 +817,49 @@
m_objects->clearObjects(ns);
}
+void collision_space::EnvironmentModelODE::updateAllowedTouch(void)
+{
+ const unsigned int n = m_modelGeom.linkGeom.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ kGeom *kg = m_modelGeom.linkGeom[i];
+ kg->allowedTouch.resize(kg->geom.size());
+
+ if (kg->geom.empty())
+ continue;
+
+ kg->allowedTouch[0].resize(n);
+
+ // compute the allowed touch for robot links
+ for (unsigned int j = 0 ; j < n ; ++j)
+ // if self collision checking with link j is disabled, we are allowed to touch link i with geom 0
+ // otherwise, we are not
+ kg->allowedTouch[0][j] = !m_selfCollisionTest[kg->index][j];
+
+ const unsigned int nab = kg->link->attachedBodies.size();
+ for (unsigned int k = 0 ; k < nab ; ++k)
+ {
+ kg->allowedTouch[k + 1].clear();
+ kg->allowedTouch[k + 1].resize(n, false);
+ for (unsigned int j = 0 ; j < kg->link->attachedBodies[k]->touch_links.size() ; ++j)
+ {
+ const std::string &tlink = kg->link->attachedBodies[k]->touch_links[j];
+ std::map<std::string, unsigned int>::const_iterator it = m_collisionLinkIndex.find(tlink);
+ if (it != m_collisionLinkIndex.end())
+ kg->allowedTouch[k + 1][it->second] = true;
+ else
+ m_msg.error("Unknown link '%s'", tlink.c_str());
+ }
+ }
+ }
+}
+
+void collision_space::EnvironmentModelODE::addSelfCollisionGroup(const std::vector<std::string> &links)
+{
+ EnvironmentModel::addSelfCollisionGroup(links);
+ updateAllowedTouch();
+}
+
void collision_space::EnvironmentModelODE::removeCollidingObjects(const shapes::StaticShape *shape)
{
checkThreadInit();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-17 06:42:55
|
Revision: 21990
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21990&view=rev
Author: isucan
Date: 2009-08-17 06:42:49 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
new feature for robot self filter: removing points at less than a minimum distance to sensor; using this feature when combining pointclouds for the 2 laser sensors (for producing the collision map)
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/self_filter.launch
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
pkg/trunk/util/robot_self_filter/src/test_filter.cpp
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch
Modified: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -266,7 +266,7 @@
currentMap_ = obstacles;
// find out which of these points are now occluded
- sf_->assumeFrame(header_, bi_.sensor_frame);
+ sf_->assumeFrame(header_, bi_.sensor_frame, 0.05);
// OpenMP need an int as the lookup variable, but for set,
// this is not possible, so we copy to a vector
Added: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/base_laser_self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -0,0 +1,43 @@
+<launch>
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="base_scan_cloud_without_known_objects" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="base_scan_cloud_filtered" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_laser" />
+
+ <!-- Minimum distance to sensor for point not to be considered inside -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.02" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link" />
+
+ </node>
+</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -11,6 +11,9 @@
filtered; This parameter is optional. If it is not specified,
shadow points will be considered outside -->
<param name="sensor_frame" type="string" value="laser_tilt_link" />
+
+ <!-- Minimum distance to sensor for point not to be considered inside -->
+ <param name="min_sensor_dist" type="double" value="0.05" />
<!-- The padding to be added for the body parts the robot can see -->
<param name="self_see_padd" type="double" value="0.07" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -9,16 +9,15 @@
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
<node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
- <!-- convert laser scan to pointcloud -->
- <node machine="three" pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <!-- convert tilt laser scan to pointcloud -->
+ <node machine="three" pkg="laser_scan" type="scan_to_cloud" output="screen" name="scan_to_cloud_tilt_laser">
<param name="scan_topic" type="string" value="tilt_scan" />
<param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
<param name="cloud_topic" type="string" value="tilt_scan_cloud" />
</node>
<!-- remove points corresponding to known objects -->
- <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="true" output="screen">
<remap from="robot_description" to="robot_description" />
<!-- define a frame that stays fixed for the known objects -->
@@ -51,10 +50,52 @@
<node machine="four" pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
<remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
<remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_cloud_filtered" />
+ <remap from="full_cloud" to="full_tilt_cloud_filtered" />
</node>
+ <!-- convert base laser scan to pointcloud -->
+ <node machine="three" pkg="laser_scan" type="scan_to_cloud" respawn="true" name="scan_to_cloud_base_laser">
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="scan_topic" value="base_scan" />
+ <param name="cloud_topic" value="base_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="true" output="screen">
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_link" />
+
+ <remap from="cloud_in" to="base_scan_cloud" />
+ <remap from="cloud_out" to="base_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/base_laser_self_filter.launch" />
+
+ <node machine="four" pkg="point_cloud_assembler" type="merge_clouds" output="screen">
+
+ <!-- first input cloud -->
+ <remap from="cloud_in1" to="full_tilt_cloud_filtered"/>
+
+ <!-- second input cloud -->
+ <remap from="cloud_in2" to="base_scan_cloud_filtered"/>
+
+ <!-- output cloud -->
+ <remap from="cloud_out" to="full_cloud_filtered"/>
+
+ <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
+ <param name="output_frame" type="string" value="base_link" />
+ </node>
+
+
<!-- start collision map -->
<include file="$(find 3dnav_pr2)/launch/perception/bits/collision_map_self_occ.launch" />
-
+
</launch>
Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-08-17 06:42:49 UTC (rev 21990)
@@ -108,8 +108,8 @@
the origin of the sensor. A callback can be registered for
the first intersection point on each body.
*/
- void maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
+ void maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
/** \brief Compute the intersection mask for a given pointcloud. If a mask
element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW,
@@ -117,16 +117,20 @@
been seen. If the mask element is INSIDE, the point is inside
the robot. The origin of the sensor is specified as well.
*/
- void maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
+ void maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
/** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function.
* The frame in which the sensor is located is optional */
- void assumeFrame(const roslib::Header& header, const std::string &sensor_frame = std::string());
+ void assumeFrame(const roslib::Header& header);
/** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function.
+ * The frame in which the sensor is located is optional */
+ void assumeFrame(const roslib::Header& header, const std::string &sensor_frame, const double min_sensor_dist);
+
+ /** \brief Assume subsequent calls to getMaskX() will be in the frame passed to this function.
* Also specify which possition to assume for the sensor (frame is not needed) */
- void assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos);
+ void assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos, const double min_sensor_dist);
/** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No
setup is performed, assumeFrame() should be called before use */
@@ -171,6 +175,7 @@
ros::NodeHandle nh_;
btVector3 sensor_pos_;
+ double min_sensor_dist_;
std::vector<SeeLink> bodies_;
std::vector<double> bspheresRadius2_;
Modified: pkg/trunk/util/robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/robot_self_filter/self_filter.launch 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/self_filter.launch 2009-08-17 06:42:49 UTC (rev 21990)
@@ -14,7 +14,10 @@
filtered; This parameter is optional. If it is not specified,
shadow points will be considered outside -->
<param name="sensor_frame" type="string" value="laser_tilt_link" />
-
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
<!-- The padding to be added for the body parts the robot can see -->
<param name="self_see_padd" type="double" value="0.02" />
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -46,6 +46,7 @@
SelfFilter(void)
{
nh_.param<std::string>("~sensor_frame", sensor_frame_, std::string());
+ nh_.param<double>("~min_sensor_dist", min_sensor_dist_, 0.01);
std::vector<std::string> links;
std::string link_names;
@@ -77,7 +78,7 @@
if (!annotate_.empty())
ROS_INFO("Self filter is adding annotation channel '%s'", annotate_.c_str());
if (!sensor_frame_.empty())
- ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'", sensor_frame_.c_str());
+ ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'. Minimum distance to sensor is %f.", sensor_frame_.c_str(), min_sensor_dist_);
}
~SelfFilter(void)
@@ -101,7 +102,7 @@
if (sensor_frame_.empty())
sf_->maskContainment(*cloud, mask);
else
- sf_->maskIntersection(*cloud, sensor_frame_, mask);
+ sf_->maskIntersection(*cloud, sensor_frame_, min_sensor_dist_, mask);
double sec = (ros::WallTime::now() - tm).toSec();
@@ -183,6 +184,8 @@
std::string sensor_frame_;
ros::NodeHandle nh_;
std::string annotate_;
+ double min_sensor_dist_;
+
};
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -115,15 +115,15 @@
}
}
-void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &callback)
+void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
- assumeFrame(data_in.header, sensor_frame);
+ assumeFrame(data_in.header, sensor_frame, min_sensor_dist);
if (sensor_frame.empty())
maskAuxContainment(data_in, mask);
else
@@ -131,15 +131,15 @@
}
}
-void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, std::vector<int> &mask,
- const boost::function<void(const btVector3&)> &callback)
+void robot_self_filter::SelfMask::maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor_pos, const double min_sensor_dist,
+ std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback)
{
mask.resize(data_in.points.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
{
- assumeFrame(data_in.header, sensor);
+ assumeFrame(data_in.header, sensor_pos, min_sensor_dist);
maskAuxIntersection(data_in, mask, callback);
}
}
@@ -154,14 +154,35 @@
}
}
-void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const btVector3 &sensor)
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos, double min_sensor_dist)
{
assumeFrame(header);
- sensor_pos_ = sensor;
+ sensor_pos_ = sensor_pos;
+ min_sensor_dist_ = min_sensor_dist;
}
-void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame)
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame, double min_sensor_dist)
{
+ assumeFrame(header);
+
+ // compute the origin of the sensor in the frame of the cloud
+ try
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(header.frame_id, sensor_frame, header.stamp, transf);
+ sensor_pos_ = transf.getOrigin();
+ }
+ catch(...)
+ {
+ sensor_pos_.setValue(0, 0, 0);
+ ROS_ERROR("Unable to lookup transform from %s to %s", sensor_frame.c_str(), header.frame_id.c_str());
+ }
+
+ min_sensor_dist_ = min_sensor_dist;
+}
+
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header)
+{
const unsigned int bs = bodies_.size();
// place the links in the assumed frame
@@ -185,22 +206,6 @@
}
computeBoundingSpheres();
-
- // compute the origin of the sensor in the frame of the cloud
- if (!sensor_frame.empty())
- {
- try
- {
- tf::Stamped<btTransform> transf;
- tf_.lookupTransform(header.frame_id, sensor_frame, header.stamp, transf);
- sensor_pos_ = transf.getOrigin();
- }
- catch(...)
- {
- sensor_pos_.setValue(0, 0, 0);
- ROS_ERROR("Unable to lookup transform from %s to %s", sensor_frame.c_str(), header.frame_id.c_str());
- }
- }
}
void robot_self_filter::SelfMask::maskAuxContainment(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask)
@@ -257,29 +262,35 @@
{
// we check it the point is a shadow point
btVector3 dir(sensor_pos_ - pt);
- dir.normalize();
- if (callback)
- {
- std::vector<btVector3> intersections;
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
- {
- callback(intersections[0]);
- out = SHADOW;
- }
- }
+ btScalar lng = dir.length();
+ if (lng < min_sensor_dist_)
+ out = INSIDE;
else
- {
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir))
- out = SHADOW;
+ {
+ dir /= lng;
+ if (callback)
+ {
+ std::vector<btVector3> intersections;
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
+ {
+ callback(intersections[0]);
+ out = SHADOW;
+ }
+ }
+ else
+ {
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir))
+ out = SHADOW;
+ }
+
+ // if it is not a shadow point, we check if it is inside the scaled body
+ if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->containsPoint(pt))
+ out = INSIDE;
}
-
- // if it is not a shadow point, we check if it is inside the scaled body
- if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->containsPoint(pt))
- out = INSIDE;
}
mask[i] = out;
@@ -317,28 +328,35 @@
// we check it the point is a shadow point
btVector3 dir(sensor_pos_ - pt);
- dir.normalize();
- if (callback)
- {
- std::vector<btVector3> intersections;
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
- {
- callback(intersections[0]);
- out = SHADOW;
- }
- }
+ btScalar lng = dir.length();
+ if (lng < min_sensor_dist_)
+ out = INSIDE;
else
{
+ dir /= lng;
+
+ if (callback)
+ {
+ std::vector<btVector3> intersections;
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
+ {
+ callback(intersections[0]);
+ out = SHADOW;
+ }
+ }
+ else
+ {
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, dir))
+ out = SHADOW;
+ }
+
+ // if it is not a shadow point, we check if it is inside the scaled body
for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->intersectsRay(pt, dir))
- out = SHADOW;
+ if (bodies_[j].body->containsPoint(pt))
+ out = INSIDE;
}
-
- // if it is not a shadow point, we check if it is inside the scaled body
- for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
- if (bodies_[j].body->containsPoint(pt))
- out = INSIDE;
}
return out;
}
Modified: pkg/trunk/util/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-08-17 06:42:38 UTC (rev 21989)
+++ pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-08-17 06:42:49 UTC (rev 21990)
@@ -118,7 +118,7 @@
ros::WallTime tm = ros::WallTime::now();
std::vector<int> mask;
- sf_->maskIntersection(in, "laser_tilt_mount_link", mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
+ sf_->maskIntersection(in, "laser_tilt_mount_link", 0.01, mask, boost::bind(&TestSelfFilter::gotIntersection, this, _1) );
// sf_->maskContainment(in, mask);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2009-08-17 06:53:09
|
Revision: 21991
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21991&view=rev
Author: morgan_quigley
Date: 2009-08-17 06:53:02 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
tweak opencv manifest to rename its rosdeps to more friendly-looking names, rather than copying the debian scheme. update the existing global rosdep.yaml so that 'old' rosdep works. add a bunch of stack rosdep.yaml files to federate rosdep2 across stacks. this partitioning of the rosdeps is just a starting point; I anticipate we'll have to shuffle these (mostly, moving them upstream over time)
Modified Paths:
--------------
pkg/trunk/stacks/geometry/rosdep.yaml
pkg/trunk/stacks/opencv/opencv_latest/manifest.xml
Added Paths:
-----------
pkg/trunk/openrave_planning/rosdep.yaml
pkg/trunk/stacks/common/rosdep.yaml
pkg/trunk/stacks/joystick_drivers/rosdep.yaml
pkg/trunk/stacks/opencv/rosdep.yaml
pkg/trunk/stacks/simulator_stage/rosdep.yaml
pkg/trunk/stacks/visualization_common/rosdep.yaml
pkg/trunk/util/rosdep.yaml
Added: pkg/trunk/openrave_planning/rosdep.yaml
===================================================================
--- pkg/trunk/openrave_planning/rosdep.yaml (rev 0)
+++ pkg/trunk/openrave_planning/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,19 @@
+coin:
+ ubuntu: libcoin40-dev
+ debian: libcoin40-dev
+ arch: coin
+ macports: coin
+ gentoo: coin
+glew:
+ ubuntu: libglew1.5-dev
+ debian: libglew1.5-dev
+ arch: glew
+ macports: glew
+ gentoo: glew
+jam:
+ ubuntu: jam
+ debian: jam
+ arch: ftjam
+ macports: jam
+ gentoo: ftjam
+
Added: pkg/trunk/stacks/common/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/common/rosdep.yaml (rev 0)
+++ pkg/trunk/stacks/common/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,34 @@
+fltk:
+ ubuntu: libfltk1.1-dev
+ debian: libfltk1.1-dev
+ fedora: fltk-devel
+ arch: fltk
+ macports: fltk
+ gentoo: "\"=x11-libs/fltk-1*\""
+netpbm:
+ ubuntu: libnetpbm10-dev
+ debian: libnetpbm10-dev
+ arch: netpbm
+ macports: netpbm
+ fedora: netpbm
+ gentoo: netpbm
+sdl:
+ ubuntu: libsdl1.2-dev
+ debian: libsdl1.2-dev
+ fedora: SDL-devel
+ arch: sdl
+ macports: libsdl
+sdl-image:
+ ubuntu: libsdl-image1.2-dev
+ debian: libsdl-image1.2-dev
+ fedora: SDL_image-devel
+ arch: sdl_image
+ macports: libsdl_image
+ gentoo: sdl-image
+atlas:
+ ubuntu: libatlas-base-dev libatlas-cpp-0.6-dev
+ debian: libatlas-base-dev libatlas-cpp-0.6-dev
+ arch: |
+ macports: atlas
+ gentoo: atlas
+
Modified: pkg/trunk/stacks/geometry/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/geometry/rosdep.yaml 2009-08-17 06:42:49 UTC (rev 21990)
+++ pkg/trunk/stacks/geometry/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -3,3 +3,9 @@
debian: libglut3-dev
fedora: freeglut-devel
arch: freeglut
+python-sip4:
+ ubuntu: python-sip4-dev sip4
+ debian: python-sip4-dev sip4
+ macports: py25-sip
+ gentoo: dev-python/sip
+ arch: sip
Added: pkg/trunk/stacks/joystick_drivers/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/joystick_drivers/rosdep.yaml (rev 0)
+++ pkg/trunk/stacks/joystick_drivers/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,4 @@
+joystick:
+ ubuntu: joystick
+ debian: joystick
+ arch: kernel-headers
Modified: pkg/trunk/stacks/opencv/opencv_latest/manifest.xml
===================================================================
--- pkg/trunk/stacks/opencv/opencv_latest/manifest.xml 2009-08-17 06:42:49 UTC (rev 21990)
+++ pkg/trunk/stacks/opencv/opencv_latest/manifest.xml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -17,10 +17,8 @@
</export>
<depend package="sensor_msgs" />
<versioncontrol type="svn" url="https://opencvlibrary.svn.sourceforge.net/svnroot/opencvlibrary/tags/latest_tested_snapshot/opencv"/>
-<rosdep name="libavformat"/>
-<rosdep name="libavcodec"/>
-<rosdep name="libjasper"/>
-<rosdep name="libswscale"/>
-<rosdep name="libgraphicsmagick++"/>
+<rosdep name="ffmpeg"/>
+<rosdep name="jasper"/>
+<rosdep name="graphicsmagick"/>
</package>
Added: pkg/trunk/stacks/opencv/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/opencv/rosdep.yaml (rev 0)
+++ pkg/trunk/stacks/opencv/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,18 @@
+ffmpeg:
+ ubuntu: libavcodec-dev libavformat-dev libavutil-dev libswscale-dev
+ debian: libavcodec-dev libavformat-dev libavutil-dev libswscale-dev
+ macports: ffmpeg
+ arch: ffmpeg
+ gentoo: ffmpeg
+graphicsmagick:
+ ubuntu: libgraphicsmagick++1-dev
+ debian: libgraphicsmagick++1-dev
+ macports: GraphicsMagick
+ arch: graphicsmagick
+ gentoo: imagemagick
+jasper:
+ ubuntu: libjasper-dev
+ debian: libjasper-dev
+ macports: jasper
+ arch: jasper
+ gentoo: jasper
Added: pkg/trunk/stacks/simulator_stage/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/simulator_stage/rosdep.yaml (rev 0)
+++ pkg/trunk/stacks/simulator_stage/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,7 @@
+libjpeg:
+ ubuntu: libjpeg62-dev
+ debian: libjpeg62-dev
+ fedora: libjpeg-devel
+ macports: jpeg
+ arch: libjpeg
+
Added: pkg/trunk/stacks/visualization_common/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/visualization_common/rosdep.yaml (rev 0)
+++ pkg/trunk/stacks/visualization_common/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,13 @@
+freeimage:
+ ubuntu: libfreeimage-dev
+ macports: freeimage
+ debian: libfreeimage-dev
+ arch: freeimage
+ gentoo: freeimage
+nvidia-cg:
+ ubuntu: nvidia-cg-toolkit
+ debian: nvidia-cg-toolkit
+ arch: |
+ if ! pacman -Q nvidia-cg-toolkit ; then yaourt -S nvidia-cg-toolkit; fi
+ gentoo: nvidia-cg-toolkit
+ debian: nvidia-cg-toolkit
Added: pkg/trunk/util/rosdep.yaml
===================================================================
--- pkg/trunk/util/rosdep.yaml (rev 0)
+++ pkg/trunk/util/rosdep.yaml 2009-08-17 06:53:02 UTC (rev 21991)
@@ -0,0 +1,7 @@
+qhull:
+ ubuntu: libqhull-dev
+ debian: libqhull-dev
+ arch: qhull
+ macports: qhull
+ gentoo: qhull
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-08-17 16:31:08
|
Revision: 22016
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22016&view=rev
Author: meeussen
Date: 2009-08-17 16:30:50 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
Move convex_decomposition, ply and ivcon out of the mechanism stack into sandbox. Move realtime_tools into the mechanism stack
Added Paths:
-----------
pkg/trunk/sandbox/convex_decomposition/
pkg/trunk/sandbox/ivcon/
pkg/trunk/sandbox/ply/
pkg/trunk/stacks/mechanism/realtime_tools/
Removed Paths:
-------------
pkg/trunk/stacks/mechanism/convex_decomposition/
pkg/trunk/stacks/mechanism/ivcon/
pkg/trunk/stacks/mechanism/ply/
pkg/trunk/util/realtime_tools/
Property changes on: pkg/trunk/sandbox/ply
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/mechanism/ply:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Property changes on: pkg/trunk/stacks/mechanism/realtime_tools
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-17 22:40:13
|
Revision: 22045
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22045&view=rev
Author: stuglaser
Date: 2009-08-17 22:39:59 +0000 (Mon, 17 Aug 2009)
Log Message:
-----------
Moved controllers that won't be released out of robot_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/endeffector_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_blind_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/probe.h
pkg/trunk/sandbox/experimental_controllers/msg/CartesianHybridState.msg
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_limit_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
pkg/trunk/controllers/robot_mechanism_controllers/msg/CartesianHybridState.msg
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/probe.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-17 22:39:59 UTC (rev 22045)
@@ -7,35 +7,19 @@
gensrv()
set(_srcs
src/controller_manifest.cpp
- src/cartesian_tff_controller.cpp
src/cartesian_trajectory_controller.cpp
src/cartesian_pose_controller.cpp
src/cartesian_twist_controller.cpp
- src/cartesian_twist_controller_ik.cpp
src/cartesian_wrench_controller.cpp
- src/cartesian_hybrid_controller.cpp
- src/dynamic_loader_controller.cpp
src/joint_effort_controller.cpp
src/joint_position_controller.cpp
- src/joint_position_smoothing_controller.cpp
src/joint_velocity_controller.cpp
- src/joint_inverse_dynamics_controller.cpp
- src/joint_autotuner.cpp
src/joint_pd_controller.cpp
- src/joint_calibration_controller.cpp
src/joint_ud_calibration_controller.cpp
- src/joint_limit_calibration_controller.cpp
- src/joint_blind_calibration_controller.cpp
- src/joint_chain_constraint_controller.cpp
- src/joint_chain_sine_controller.cpp
src/trigger_controller.cpp
- src/probe.cpp
)
# For some reason, the endeffector_constraint_controller won't build on OSX
-if(NOT APPLE)
- set(_srcs ${_srcs} src/endeffector_constraint_controller.cpp)
-endif(NOT APPLE)
rospack_add_library(robot_mechanism_controllers ${_srcs})
rospack_remove_compile_flags(robot_mechanism_controllers -W)
target_link_libraries(robot_mechanism_controllers ltdl)
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,135 +0,0 @@
-
-/*
- * Copyright (c) 2008, 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.
- */
-
-// Author: Stuart Glaser
-
-#include "ros/node.h"
-#include "boost/scoped_ptr.hpp"
-#include "controller_interface/controller.h"
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include "mechanism_model/chain.h"
-#include "control_toolbox/pid.h"
-#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
-#include "realtime_tools/realtime_publisher.h"
-#include "filters/filter_chain.h"
-#include "control_toolbox/pid_gains_setter.h"
-
-#include "robot_mechanism_controllers/SetPoseStamped.h"
-#include "manipulation_msgs/TaskFrameFormalism.h"
-#include "robot_mechanism_controllers/CartesianHybridState.h"
-
-namespace controller {
-
-class CartesianHybridController : public Controller
-{
-public:
- CartesianHybridController();
- ~CartesianHybridController() {}
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
- virtual void update(void);
- virtual bool starting();
-
- KDL::Twist pose_error_;
- KDL::Twist twist_error_;
-
- KDL::Twist pose_desi_;
- KDL::Twist twist_desi_;
- KDL::Wrench wrench_desi_;
- KDL::Frame pose_meas_;
- KDL::Twist twist_meas_;
- KDL::Twist twist_meas_filtered_;
- std::vector<double> measured_torque_, desired_torque_, max_jnt_eff_;
-
- control_toolbox::Pid pose_pids_[6]; // (x,y,z) position, then (x,y,z) rot
- control_toolbox::Pid twist_pids_[6];
- control_toolbox::PidGainsSetter pose_pid_tuner_;
- control_toolbox::PidGainsSetter pose_rot_pid_tuner_;
- control_toolbox::PidGainsSetter twist_pid_tuner_;
- control_toolbox::PidGainsSetter twist_rot_pid_tuner_;
-
- // x, y, z, rx, ry, rz
- int mode_[6];
- double setpoint_[6];
-
- KDL::Frame task_frame_offset_; // task frame in the root frame
- KDL::Frame tool_frame_offset_; // tool frame in the ee frame
-
- mechanism::Chain chain_;
- KDL::Chain kdl_chain_;
- mechanism::RobotState *robot_;
- double last_time_;
-
- int initial_mode_;
-
- double saturated_velocity_, saturated_rot_velocity_;
-
- bool use_filter_;
- filters::FilterChain<double> twist_filter_;
-
- ros::NodeHandle node_;
-};
-
-class CartesianHybridControllerNode : public Controller
-{
-public:
- CartesianHybridControllerNode();
- ~CartesianHybridControllerNode();
-
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- virtual bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
- virtual void update(void);
- virtual bool starting() { return c_.starting(); }
-
- void command(const tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism>::MessagePtr& tff_msg);
-
- bool setToolFrame(robot_mechanism_controllers::SetPoseStamped::Request &req,
- robot_mechanism_controllers::SetPoseStamped::Response &resp);
-
-private:
- ros::NodeHandle node_;
- ros::ServiceServer serve_set_tool_frame_;
- std::string name_;
- tf::TransformListener TF;
- CartesianHybridController c_;
- //manipulation_msgs::TaskFrameFormalism command_msg_;
- boost::scoped_ptr<tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism> > command_notifier_;
-
- std::string task_frame_name_;
-
- unsigned int loop_count_;
- boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState> > pub_state_;
- boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
-};
-
-}
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,103 +0,0 @@
-/*
- * Copyright (c) 2008, 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.
- */
-
-/*
- * Author: Wim Meeussen
- */
-
-#ifndef CARTESIAN_TFF_CONTROLLER_H
-#define CARTESIAN_TFF_CONTROLLER_H
-
-#include <vector>
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/frames.hpp>
-#include <ros/node.h>
-#include <manipulation_msgs/TaskFrameFormalism.h>
-#include <geometry_msgs/Twist.h>
-#include <controller_interface/controller.h>
-#include <tf/transform_datatypes.h>
-#include <control_toolbox/pid.h>
-#include <boost/scoped_ptr.hpp>
-#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
-#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
-
-namespace controller {
-
-class CartesianTFFController : public Controller
-{
-public:
- CartesianTFFController();
- ~CartesianTFFController();
-
- bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
- bool starting();
- void update();
-
- void command(const manipulation_msgs::TaskFrameFormalismConstPtr& tff_msg);
-
-private:
- ros::NodeHandle node_;
- ros::Subscriber sub_command_;
- double last_time_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> vel_pid_controller_, pos_pid_controller_;
-
- // robot description
- mechanism::RobotState *robot_state_;
- mechanism::Chain chain_;
-
- // kdl stuff for kinematics
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainFkSolverVel> jnt_to_twist_solver_;
- KDL::JntArrayVel jnt_posvel_;
-
- // command for tff
- std::vector<int> mode_;
- std::vector<double> value_, twist_to_wrench_;
-
- // output of the controller
- KDL::Wrench wrench_desi_;
-
- KDL::Twist position_, twist_meas_;
- KDL::Frame pose_meas_, pose_meas_old_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > state_position_publisher_;
- unsigned int loop_count_;
-
- // internal wrench controller
- CartesianWrenchController* wrench_controller_;
-};
-
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,97 +0,0 @@
-/*
- * Copyright (c) 2008, Ruben Smits and 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.
- */
-
-/*
- * Author: Wim Meeussen
- * Ruben Smits
- */
-
-#ifndef CARTESIAN_TWIST_CONTROLLER_IK_H
-#define CARTESIAN_TWIST_CONTROLLER_IK_H
-
-#include <vector>
-#include <boost/scoped_ptr.hpp>
-#include <kdl/chain.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/chainiksolver.hpp>
-#include <ros/node.h>
-#include <geometry_msgs/Twist.h>
-#include <controller_interface/controller.h>
-#include <mechanism_model/chain.h>
-#include <tf/transform_datatypes.h>
-#include <control_toolbox/pid.h>
-
-#include "robot_mechanism_controllers/joint_inverse_dynamics_controller.h"
-
-namespace controller {
-
- class CartesianTwistControllerIk : public Controller
- {
- public:
- CartesianTwistControllerIk();
- ~CartesianTwistControllerIk();
-
- bool init(mechanism::RobotState *robot_state, const ros::NodeHandle& n);
- bool starting();
- void update();
-
- void command(const geometry_msgs::TwistConstPtr& twist_msg);
-
- // input of the controller
- KDL::Twist twist_desi_;
-
- private:
- KDL::Twist twist_meas_,error,twist_out_;
-
- ros::NodeHandle node_;
- ros::Subscriber sub_command_;
-
- double last_time_,ff_trans_,ff_rot_;
-
- // pid controllers
- std::vector<control_toolbox::Pid> fb_pid_controller_;
-
- // robot description
- mechanism::RobotState *robot_state_;
- mechanism::Chain chain_;
-
- // kdl stuff for kinematics
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainFkSolverVel> jnt_to_twist_solver_;
- boost::scoped_ptr<KDL::ChainIkSolverVel> twist_to_jnt_solver_;
- KDL::JntArrayVel jnt_posvel_;
-
- JointInverseDynamicsController* id_controller_;
- };
-
-} // namespace
-
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,113 +0,0 @@
-/*
- * Copyright (c) 2008, 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.
- */
-
-/*
- * Author: Rob Wheeler
- */
-
-#ifndef DYNAMIC_LOADER_CONTROLLER_H
-#define DYNAMIC_LOADER_CONTROLLER_H
-
-#include "controller_interface/controller.h"
-#include <tinyxml/tinyxml.h>
-#include <ltdl.h>
-
-namespace controller {
-
-/***************************************************/
-/*! \class controller::DynamicLoaderController
- \brief Dynamic Loader
-
- This class implements a pseudo-controller that can dynamically
- load a package's shared object and instantiate controllers from
- that shared object.
-
- When the DynamicLoaderController is killed, it shuts down the
- controllers that it started and unloads the shared object.
-
- Example configuration:
- <pre>
- <controller name="dynamic_loader" type="DynamicLoaderController"
- package="my_controllers" lib="libmy_controllers">
-
- <controllers>
-
- <controller type="MyController" name="my_controller1">
- <joint name="joint_to_control" />
- </controller><br>
-
- <controller type="MyController" name="my_controller2">
- <joint name="another_joint_to_control" />
- </controller><br>
-
- </controllers>
-
- </controller>
- </pre>
-
- The above example creates an instance of the DynamicLoaderController
- that loads the shared object libmy_controllers.so from the
- my_controllers package. It then instantiates two controllers,
- my_controller1 and my_controller2, from that shared object.
- When the DynamicLoaderController is killed, it will kill the two
- controllers it started and unload the libmy_controllers.so shared object.
-
-*/
-/***************************************************/
-
-class DynamicLoaderController : public Controller
-{
-public:
- DynamicLoaderController();
- ~DynamicLoaderController();
-
- /*!
- * \brief Specifies the package and shared object to load.
- * \param *robot The robot (not used by this controller).
- * \param *config The XML configuration for this controller
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief This function is called in the control loop. For this
- * pseudo-controller, the update function is a noop.
- */
- void update();
-
-private:
- std::vector<std::string> names_;
- lt_dlhandle handle_;
-
- void loadLibrary(std::string& name);
- static void unloadLibrary(std::vector<std::string> names, lt_dlhandle handle);
-};
-
-}
-
-#endif
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-17 22:36:03 UTC (rev 22044)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-17 22:39:59 UTC (rev 22045)
@@ -1,143 +0,0 @@
-/*
- * Copyright (c) 2008, 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.
- */
-
-/*
- * Author: Melonee Wise
- */
-
-#ifndef ENDEFFECTOR_CONSTRAINT_CONTROLLER_H
-#define ENDEFFECTOR_CONSTRAINT_CONTROLLER_H
-
-#include <vector>
-#include "boost/scoped_ptr.hpp"
-#include "mechanism_model/chain.h"
-#include "kdl/chain.hpp"
-#include "kdl/frames.hpp"
-#include "kdl/chainfksolver.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-#include "ros/node.h"
-#include "geometry_msgs/Wrench.h"
-#include "misc_utils/subscription_guard.h"
-#include "controller_interface/controller.h"
-#include "tf/transform_datatypes.h"
-#include "misc_utils/advertised_service_guard.h"
-#include "joy/Joy.h"
-#include "Eigen/LU"
-#include "Eigen/Core"
-#include <visualization_msgs/Marker.h>
-
-
-namespace controller {
-
-class EndeffectorConstraintController : public Controller
-{
-public:
- EndeffectorConstraintController();
- ~EndeffectorConstraintController();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- void update();
- void computeConstraintJacobian();
- void computeConstraintNullSpace();
- // input of the controller
- KDL::Wrench wrench_desi_;
- Eigen::Matrix<float,6,1> task_wrench_;
-
-private:
-
- mechanism::RobotState *robot_;
-
- // kdl stuff for kinematics
- mechanism::Chain chain_;
- KDL::Chain kdl_chain_;
- boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
- boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
-
- // to get joint positions, velocities, and to set joint torques
- Eigen::Matrix<float,6,5> constraint_jac_;
- Eigen::Matrix<float,6,1> constraint_wrench_;
- Eigen::Matrix<float,5,1> constraint_force_;
- // joint constraint
- Eigen::MatrixXf joint_constraint_force_;
- Eigen::MatrixXf joint_constraint_jac_;
- Eigen::MatrixXf joint_constraint_null_space_;
-
- Eigen::MatrixXf tas...
[truncated message content] |
|
From: <mee...@us...> - 2009-08-18 00:00:56
|
Revision: 22066
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22066&view=rev
Author: meeussen
Date: 2009-08-18 00:00:50 +0000 (Tue, 18 Aug 2009)
Log Message:
-----------
move mechanism_bringup package out of the mechanism stack because it creates a dependency on controllers that are not part of any stack. Ticket #2366
Added Paths:
-----------
pkg/trunk/sandbox/mechanism_bringup/
Removed Paths:
-------------
pkg/trunk/stacks/mechanism/mechanism_bringup/
Property changes on: pkg/trunk/sandbox/mechanism_bringup
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/mechanism/mechanism_bringup:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-18 01:23:47
|
Revision: 22086
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22086&view=rev
Author: stuglaser
Date: 2009-08-18 01:23:37 +0000 (Tue, 18 Aug 2009)
Log Message:
-----------
Forgot to move controllers in the XML manifests
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-18 01:22:14 UTC (rev 22085)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-18 01:23:37 UTC (rev 22086)
@@ -12,28 +12,9 @@
<plugin name="CartesianTrajectoryController" class="CartesianTrajectoryController" type="Controller" />
- <plugin name="DynamicLoaderController" class="DynamicLoaderController" type="Controller" />
-
<plugin name="TriggerController" class="TriggerController" type="Controller" />
<plugin name="TriggerControllerNode" class="TriggerControllerNode" type="Controller" />
- <plugin name="Probe" class="Probe" type="Controller" />
<plugin name="JointPDController" class="JointPDController" type="Controller" />
<plugin name="JointPDControllerNode" class="JointPDControllerNode" type="Controller" />
- <plugin name="CartesianTwistControllerIk" class="CartesianTwistControllerIk" type="Controller" />
- <plugin name="EndeffectorConstraintController" class="EndeffectorConstraintController" type="Controller" />
- <plugin name="EndeffectorConstraintControllerNode" class="EndeffectorConstraintControllerNode" type="Controller" />
- <plugin name="JointChainConstraintControllerNode" class="JointChainConstraintControllerNode" type="Controller" />
- <plugin name="JointInverseDynamicsController" class="JointInverseDynamicsController" type="Controller" />
- <plugin name="JointPositionSmoothController" class="JointPositionSmoothController" type="Controller" />
- <plugin name="JointPositionSmoothControllerNode" class="JointPositionSmoothControllerNode" type="Controller" />
-
- <plugin name="JointAutotuner" class="JointAutotuner" type="Controller" />
- <plugin name="JointAutotunerNode" class="JointAutotunerNode" type="Controller" />
- <plugin name="JointBlindCalibrationController" class="JointBlindCalibrationController" type="Controller" />
- <plugin name="JointBlindCalibrationControllerNode" class="JointBlindCalibrationControllerNode" type="Controller" />
- <plugin name="JointCalibrationControllerNode" class="JointCalibrationControllerNode" type="Controller" />
- <plugin name="JointChainSineController" class="JointChainSineController" type="Controller" />
- <plugin name="JointLimitCalibrationControllerNode" class="JointLimitCalibrationControllerNode" type="Controller" />
-
</library>
Modified: pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml 2009-08-18 01:22:14 UTC (rev 22085)
+++ pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml 2009-08-18 01:23:37 UTC (rev 22086)
@@ -7,5 +7,24 @@
<plugin name="CartesianHybridController" class="CartesianHybridController" type="Controller" />
<plugin name="CartesianHybridControllerNode" class="CartesianHybridControllerNode" type="Controller" />
+ <plugin name="DynamicLoaderController" class="DynamicLoaderController" type="Controller" />
+ <plugin name="Probe" class="Probe" type="Controller" />
+ <plugin name="CartesianTwistControllerIk" class="CartesianTwistControllerIk" type="Controller" />
+ <plugin name="EndeffectorConstraintController" class="EndeffectorConstraintController" type="Controller" />
+ <plugin name="EndeffectorConstraintControllerNode" class="EndeffectorConstraintControllerNode" type="Controller" />
+ <plugin name="JointChainConstraintControllerNode" class="JointChainConstraintControllerNode" type="Controller" />
+ <plugin name="JointInverseDynamicsController" class="JointInverseDynamicsController" type="Controller" />
+ <plugin name="JointPositionSmoothController" class="JointPositionSmoothController" type="Controller" />
+ <plugin name="JointPositionSmoothControllerNode" class="JointPositionSmoothControllerNode" type="Controller" />
+
+ <plugin name="JointAutotuner" class="JointAutotuner" type="Controller" />
+ <plugin name="JointAutotunerNode" class="JointAutotunerNode" type="Controller" />
+ <plugin name="JointBlindCalibrationController" class="JointBlindCalibrationController" type="Controller" />
+ <plugin name="JointBlindCalibrationControllerNode" class="JointBlindCalibrationControllerNode" type="Controller" />
+ <plugin name="JointCalibrationControllerNode" class="JointCalibrationControllerNode" type="Controller" />
+ <plugin name="JointChainSineController" class="JointChainSineController" type="Controller" />
+ <plugin name="JointLimitCalibrationControllerNode" class="JointLimitCalibrationControllerNode" type="Controller" />
+
+
</library>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-19 00:43:08
|
Revision: 22196
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22196&view=rev
Author: stuglaser
Date: 2009-08-19 00:42:57 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Moved controllers that are not set for M3 release out of pr2_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_position_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/caster_calibration_controller_effort.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/caster_controller_effort.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/laser_scanner_qualification.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/base_position_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/base_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/caster_calibration_controller_effort.cpp
pkg/trunk/sandbox/experimental_controllers/src/caster_controller_effort.cpp
pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/laser_scanner_qualification.cpp
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-19 00:42:57 UTC (rev 22196)
@@ -14,15 +14,12 @@
src/controller_manifest.cpp
src/laser_scanner_traj_controller.cpp
src/head_position_controller.cpp
- src/head_servoing_controller.cpp
src/caster_controller.cpp
#src/caster_controller_effort.cpp
src/caster_calibration_controller.cpp
#src/caster_calibration_controller_effort.cpp
src/wrist_calibration_controller.cpp
src/gripper_calibration_controller.cpp
- src/arm_trajectory_controller.cpp
- src/joint_trajectory_controller.cpp
src/base_kinematics.cpp
#src/pr2_gripper_controller.cpp
src/pr2_odometry.cpp
@@ -31,6 +28,3 @@
rospack_link_boost(pr2_mechanism_controllers thread)
rospack_remove_compile_flags(pr2_mechanism_controllers -W)
-
-rospack_add_executable(base_trajectory_controller src/base_trajectory_controller.cpp)
-rospack_declare_test(base_trajectory)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-19 00:42:57 UTC (rev 22196)
@@ -5,12 +5,9 @@
<plugin name="GripperCalibrationController" class="GripperCalibrationController" type="Controller" />
<plugin name="GripperCalibrationControllerNode" class="GripperCalibrationControllerNode" type="Controller" />
<plugin name="HeadPositionController" class="HeadPositionController" type="Controller" />
- <plugin name="HeadServoingController" class="HeadServoingController" type="Controller" />
- <plugin name="JointTrajectoryController" class="JointTrajectoryController" type="Controller" />
<plugin name="LaserScannerTrajController" class="LaserScannerTrajController" type="Controller" />
<plugin name="LaserScannerTrajControllerNode" class="LaserScannerTrajControllerNode" type="Controller" />
<plugin name="Pr2BaseController" class="Pr2BaseController" type="Controller" />
<plugin name="Pr2Odometry" class="Pr2Odometry" type="Controller" />
<plugin name="WristCalibrationController" class="WristCalibrationController" type="Controller" />
- <plugin name="ArmTrajectoryControllerNode" class="ArmTrajectoryControllerNode" type="Controller" />
</library>
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,312 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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 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.
- *********************************************************************/
-
-#pragma once
-
-#include <ros/node.h>
-#include <boost/thread/mutex.hpp>
-
-#include <controller_interface/controller.h>
-#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <robot_mechanism_controllers/joint_effort_controller.h>
-#include <robot_mechanism_controllers/joint_pd_controller.h>
-
-// Services
-#include <pr2_mechanism_controllers/SetJointPosCmd.h>
-#include <pr2_mechanism_controllers/GetJointPosCmd.h>
-
-#include <pr2_mechanism_controllers/SetJointGains.h>
-#include <pr2_mechanism_controllers/GetJointGains.h>
-
-#include <pr2_mechanism_controllers/SetCartesianPosCmd.h>
-#include <pr2_mechanism_controllers/GetCartesianPosCmd.h>
-
-#include <pr2_mechanism_controllers/SetJointTarget.h>
-#include <pr2_mechanism_controllers/JointPosCmd.h>
-
-#include <manipulation_msgs/JointTraj.h>
-#include <manipulation_msgs/JointTrajPoint.h>
-#include <diagnostic_msgs/DiagnosticArray.h>
-
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-
-#include <trajectory/trajectory.h>
-
-#include <pr2_mechanism_controllers/ControllerState.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <std_msgs/String.h>
-
-namespace controller
-{
-
- #define GOAL_REACHED_THRESHOLD 0.01
-
-
- // comment this out if the controller is not supposed to publish its own max execution time
- #define PUBLISH_MAX_TIME
-
-
-// The maximum number of joints expected in an arm.
- static const int MAX_ARM_JOINTS = 7;
-
- class ArmTrajectoryController : public Controller
- {
- public:
-
- /*!
- * \brief Default Constructor of the JointController class.
- *
- */
- ArmTrajectoryController();
-
- /*!
- * \brief Destructor of the JointController class.
- */
- virtual ~ArmTrajectoryController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief set the joint trajectory for the arm
- */
- void setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
-
- void getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const;
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update(void); // Real time safe.
-
- boost::mutex arm_controller_lock_;
-
- controller::JointPDController* getJointControllerByName(std::string name);
-
- private:
-
- std::vector<JointPDController *> joint_pd_controllers_;
-
- trajectory::Trajectory *joint_trajectory_;
-
- trajectory::Trajectory::TPoint trajectory_point_;
-
- std::vector<double> joint_cmd_rt_;
-
- std::vector<double> joint_cmd_dot_rt_;
-
- std::vector<int> joint_type_;
-
- mechanism::Robot* robot_;
-
- void updateJointControllers(void);
-
- bool reachedGoalPosition(std::vector<double> joint_cmd);
-
- int getJointControllerPosByName(std::string name);
-
- // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
- bool refresh_rt_vals_;
-
- double trajectory_start_time_;
-
- double trajectory_end_time_;
-
- double current_time_;
-
- bool trajectory_done_;
-
- int dimension_;
-
- std::vector<double> joint_velocity_limits_;
-
- std::string trajectory_type_;
-
- double velocity_scaling_factor_;
-
- friend class ArmTrajectoryControllerNode;
-
- double trajectory_wait_time_;
-
- std::vector<double> goal_reached_threshold_;
-
- realtime_tools::RealtimePublisher <pr2_mechanism_controllers::ControllerState>* controller_state_publisher_ ; //!< Publishes controller information
-
-
- double max_update_time_;
- };
-
-/** @class ArmTrajectoryControllerNode
- * @brief ROS interface for the arm controller.
- * @author Sachin Chitta <sa...@wi...>
- *
- * This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
- * - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
- * <listen_topic name="the name of my message" />
- * (only one topic can be specified)
- * - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
- * - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
- * @note This controller makes the assumptiom that a point update is real-time safe.
- * This is not the case for example for the LQR controller.
- *
- */
- class ArmTrajectoryControllerNode : public Controller
- {
- public:
- /*!
- * \brief Default Constructor
- *
- */
- ArmTrajectoryControllerNode();
-
- /*!
- * \brief Destructor
- */
- ~ArmTrajectoryControllerNode();
-
- void update();
-
- bool init(mechanism::RobotState *robot, const ros::NodeHandle& n);
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /** @brief service that returns the goal of the controller
- * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
- * @param req
- * @param resp the response, contains a JointPosCmd message with the goal of the controller
- * @return
- */
- bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
- pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
-
- bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
- pr2_mechanism_controllers::TrajectoryStart::Response &resp);
-
- bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
- pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
-
- bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
- pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
-
- void deleteTrajectoryFromQueue(int id);
-
- void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
-
- int createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
-
- void updateTrajectoryQueue(int last_trajectory_finish_status);
-
- void getJointTrajectoryThresholds();
-
- private:
-
- void publishDiagnostics();
-
- realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticArray>* diagnostics_publisher_ ; //!< Publishes controller information
-
- /*!
- * \brief mutex lock for setting and getting ros messages
- */
- boost::mutex ros_lock_;
-
- manipulation_msgs::JointTraj traj_msg_;
-
- pr2_mechanism_controllers::JointPosCmd msg_; //The message used by the ROS callback
- ArmTrajectoryController *c_;
-
- /*!
- * \brief service prefix
- */
- std::string service_prefix_;
-
- /*
- * \brief save topic name for unsubscribe later
- */
- std::string topic_name_;
-
- /*!
- * \brief xml pointer to ros topic name
- */
- TiXmlElement * topic_name_ptr_;
-
- /*
- * \brief pointer to ros node
- */
- ros::Node * const node_;
-
-
- /*
- * \brief receive and set the trajectory in the controller
- */
- void CmdTrajectoryReceived();
-
- int trajectory_id_;
-
- std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_;
-
- std::vector<int> joint_trajectory_id_;
-
- void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg);
-
- int request_trajectory_id_;
-
- int current_trajectory_id_;
-
- std::map<int,int> joint_trajectory_status_;
-
- std::map<int,double>joint_trajectory_time_;
-
- enum JointTrajectoryStatus{
- ACTIVE,
- DONE,
- QUEUED,
- DELETED,
- FAILED,
- CANCELED,
- NUM_STATUS
- };
-
- double trajectory_wait_timeout_;
-
- double last_diagnostics_publish_time_;
-
- double diagnostics_publish_delta_time_;
-
- };
-
-}
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,115 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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 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.
- *********************************************************************/
-
-#ifndef PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
-#define PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
-
-#include "control_toolbox/base_position_pid.h"
-#include "pr2_mechanism_controllers/base_controller.h"
-#include "tf/transform_listener.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "geometry_msgs/Point.h"
-
-#include "misc_utils/advertised_service_guard.h"
-
-namespace pr2_mechanism_controllers
-{
-
-class BasePositionControllerNode : public controller::Controller
-{
-public :
- BasePositionControllerNode() ;
- ~BasePositionControllerNode() ;
-
- /**
- * Initializes the Controller.
- * A list of XML sections are necessary for this to work
- *
- *
- *
- *
- */
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config) ;
-
- /**
- * Realtime safe update method called from the realtime loop. Compares the target position to the current position,
- * generates error terms, and then uses these to generate a Velocity command for BaseController
- */
- void update() ;
-
- /**
- * Sets the target pose of the controller. The cmd is transformed and stored into the odometric frame using the latest transform. The x,y position
- * of the pose define the target location of the robot (in the odometric frame). The theta rotation of the robot is defined by the pure z rotation that is
- * closest to the orientation of the pose in the odometric frame.
- * \param cmd The pose that we want to reach
- */
- void setPoseCommand(geometry_msgs::PoseStamped cmd) ;
-
- /**
- * Sets an x,y,theta position for the base to reach in wheel odometry frame. This command doesn't do any transforms. It simply sets the PID targets
- * for the 3 different axes of the base.
- * \param x The x position that we want to reach [in the odometric frame]
- * \param y The y position that we want to reach [in the odometric frame]
- * \param w The theta angle that we want to reach [in the odometric frame]
- */
- void setPoseOdomFrameCommand(double x, double y, double w) ;
-
-private :
- control_toolbox::BasePositionPid base_position_pid_ ; // Does the math to compute a command velocity
- controller::BaseControllerNode base_controller_node_ ; // Converts a commanded velocity into a wheel velocities and turret angles
- ros::Node *node_ ;
-
- mechanism::RobotState *robot_state_ ;
-
- double last_time_ ; // Store the last time that we called the update
-
- SubscriptionGuard guard_set_pose_command_ ; // Automatically unsubscribes set_pose_cmd
- void setPoseCommandCallback() ; // Ros callback for "set_pose_command" messages
-
- SubscriptionGuard guard_set_pose_odom_frame_command_ ; // Automatically unsubscribes set_pose_odom_frame_cmd
- void setPoseOdomFrameCommandCallback() ; // Ros callback for "set_pose_command_odom_frame" messages
-
- tf::Vector3 xyt_target_ ; // The current x,y,theta target in position space (NOT velocity space)
-
- tf::TransformListener tf_ ; // Transformer used to convert commands from native frame into odometric frame
- std::string odom_frame_name_ ; // Stores the name of the odometric frame. This is the frame that we control in
-
- // Message Holders
- geometry_msgs::PoseStamped pose_cmd_ ;
- geometry_msgs::Point pose_odom_frame_cmd_ ;
-} ;
-
-}
-
-#endif // PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,129 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, 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 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.
-*
-* Author: Sachin Chitta
-*********************************************************************/
-
-#include <geometry_msgs/Twist.h>
-#include <pr2_robot_actions/Pose2D.h>
-#include <manipulation_msgs/JointTrajPoint.h>
-#include <manipulation_msgs/JointTraj.h>
-#include <trajectory/trajectory.h>
-
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-#include <control_toolbox/pid.h>
-
-#include <angles/angles.h>
-
-namespace pr2_mechanism_controllers
-{
- class BaseTrajectoryController
- {
- public:
-
- BaseTrajectoryController(ros::Node& node, tf::TransformListener& tf);
-
- ~BaseTrajectoryController();
-
- trajectory::Trajectory::TPoint getPose2D(const tf::Stamped<tf::Pose> &pose);
-
- void pathCallback();
-
- void updateGlobalPose();
-
- double distance(double x1, double y1, double x2, double y2);
-
- bool goalPositionReached();
-
- bool goalOrientationReached();
-
- bool goalReached();
-
- void updatePath();
-
- void spin();
-
- void updateControl();
-
- geometry_msgs::Twist getCommand();
-
- private:
- tf::Stamped<tf::Pose> global_pose_;
- std::vector<control_toolbox::Pid> pid_;
- std::string global_frame_, robot_base_frame_;
- double controller_frequency_;
- std::string control_topic_name_, path_input_topic_name_;
- std::string trajectory_type_;
- manipulation_msgs::JointTraj path_msg_in_;
- manipulation_msgs::JointTraj path_msg_;
-
- ros::Node& ros_node_;
- tf::TransformListener& tf_;
- int dimension_;
- double current_time_;
- double sample_time_;
- double last_update_time_;
- double trajectory_start_time_;
- trajectory::Trajectory *trajectory_;
- bool stop_motion_;
- int stop_motion_count_;
- bool new_path_available_;
-
- std::vector<double> velocity_limits_;
-
- trajectory::Trajectory::TPoint goal_;
- trajectory::Trajectory::TPoint current_position_;
- double yaw_goal_tolerance_;
- double xy_goal_tolerance_;
-
- boost::mutex ros_lock_;
- double path_updated_time_;
- double max_update_time_;
-
- geometry_msgs::Twist checkCmd(const geometry_msgs::Twist &cmd);
-
- double diagnostics_expected_publish_time_;
-
- ros::Time last_diagnostics_publish_time_;
-
- void publishDiagnostics(bool force);
-
- geometry_msgs::Twist cmd_vel_;
- double error_x_,error_y_,error_th_;
-
- std::string control_state_;
- };
-};
-
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h 2009-08-19 00:41:27 UTC (rev 22195)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h 2009-08-19 00:42:57 UTC (rev 22196)
@@ -1,105 +0,0 @@
-/*
- * Copyright (c) 2008, 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.
- */
-
-/*
- * Author: Wim Meeussen
- */
-
-#ifndef CASTER_CALIBRATION_CONTROLLER_EFFORT_H
-#define CASTER_CALIBRATION_CONTROLLER_EFFORT_H
-
-#include "pr2_mechanism_controllers/caster_controller_effort.h"
-#include "realtime_too...
[truncated message content] |
|
From: <ei...@us...> - 2009-08-19 03:27:59
|
Revision: 22237
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22237&view=rev
Author: eitanme
Date: 2009-08-19 03:27:46 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Merging the new version of pluginlib back into trunk
r31894@att (orig r22146): eitanme | 2009-08-18 10:30:37 -0700
Creating a branch to work on pluginlib and get things changed
r31896@att (orig r22148): eitanme | 2009-08-18 10:32:35 -0700
Starting rework... need to commit so that I can move some files around
r31942@att (orig r22182): eitanme | 2009-08-18 16:36:37 -0700
Commit because Scott is moving into the office and I have to shut down my computer
r31978@att (orig r22216): eitanme | 2009-08-18 19:20:47 -0700
Working on changing things over to work with the new pluginlib
r31980@att (orig r22218): eitanme | 2009-08-18 19:24:54 -0700
Converted pluginlib tutorials to new pluginlib code
r31982@att (orig r22220): eitanme | 2009-08-18 19:28:34 -0700
Moving joint qualification controllers over to the new pluginlib model
r31985@att (orig r22223): eitanme | 2009-08-18 19:40:36 -0700
Moving people_aware_nav to new pluginlib interface
r31986@att (orig r22224): eitanme | 2009-08-18 19:43:09 -0700
Moving diagnostic aggregator to the pluginlib interface
r31987@att (orig r22225): eitanme | 2009-08-18 19:43:51 -0700
Moving generic analyzer to the new pluginlib interface
r31988@att (orig r22226): eitanme | 2009-08-18 19:44:21 -0700
Moving carrot planner to the new pluginlib interface
r31992@att (orig r22230): eitanme | 2009-08-18 19:54:15 -0700
Changing REGISTER_CLASS to PLUGINLIB_REGISTER_CLASS
r31996@att (orig r22234): eitanme | 2009-08-18 20:19:30 -0700
Fixing a plugin .xml file
r31998@att (orig r22236): eitanme | 2009-08-18 20:25:05 -0700
Fixing more incorrect tags
Modified Paths:
--------------
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/nav/people_aware_nav/bgp_plugin.xml
pkg/trunk/nav/people_aware_nav/manifest.xml
pkg/trunk/nav/people_aware_nav/src/navfn_constrained.cpp
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/CMakeLists.txt
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/manifest.xml
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/src/line.cpp
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/src/square.cpp
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/src/triangle.cpp
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_user/example.cpp
pkg/trunk/stacks/hardware_test/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_aggregator.h
pkg/trunk/stacks/hardware_test/diagnostic_aggregator/src/diagnostic_aggregator.cpp
pkg/trunk/stacks/hardware_test/generic_analyzer/generic_analyzer_plugin.xml
pkg/trunk/stacks/hardware_test/generic_analyzer/manifest.xml
pkg/trunk/stacks/hardware_test/generic_analyzer/src/plugin_list.cpp
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/navigation/base_local_planner/blp_plugin.xml
pkg/trunk/stacks/navigation/base_local_planner/manifest.xml
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/carrot_planner/bgp_plugin.xml
pkg/trunk/stacks/navigation/carrot_planner/manifest.xml
pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
pkg/trunk/stacks/navigation/navfn/bgp_plugin.xml
pkg/trunk/stacks/navigation/navfn/manifest.xml
pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp
Added Paths:
-----------
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/line.h
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/square.h
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/triangle.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_desc.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_list_macros.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader_imp.h
pkg/trunk/stacks/pr2/pr2_dashboard2/formbuilder/
pkg/trunk/stacks/pr2/pr2_dashboard2/formbuilder/motor_panel.fbp
pkg/trunk/stacks/pr2/pr2_dashboard2/xrc/
pkg/trunk/stacks/pr2/pr2_dashboard2/xrc/motor_panel.xrc
Removed Paths:
-------------
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/line.h
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/square.h
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/triangle.h
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/polygon_plugins.xml
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/shape_plugins.xml
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/src/polygon_manifest.cpp
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/src/shape_manifest.cpp
pkg/trunk/stacks/common/pluginlib/include/pluginlib/plugin.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/plugin_loader.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/plugin_loader_imp.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/plugin_macros.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v2:20514
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v3:20628
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs_0.1:20375
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/plugin_branch:20980
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v2:20514
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v3:20628
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs_0.1:20375
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/plugin_branch:20980
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/pluginlib_rework:22236
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/controller_plugins.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -1,6 +1,6 @@
<library path="lib/libjoint_qualification_controllers">
- <plugin name="CheckoutController" class="CheckoutController" type="Controller" />
- <plugin name="HoldSetController" class="HoldSetController" type="Controller" />
- <plugin name="HysteresisController" class="HysteresisController" type="Controller" />
- <plugin name="SineSweepController" class="SineSweepController" type="Controller" />
+ <class name="CheckoutController" type="CheckoutController" base_class_type="Controller" />
+ <class name="HoldSetController" type="HoldSetController" base_class_type="Controller" />
+ <class name="HysteresisController" type="HysteresisController" base_class_type="Controller" />
+ <class name="SineSweepController" type="SineSweepController" base_class_type="Controller" />
</library>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -19,7 +19,7 @@
<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 -ljoint_qualification_controllers'/>
- <controller_interface controller="${prefix}/controller_plugins.xml" />
+ <controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>
</package>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/controller_manifest.cpp 2009-08-19 03:27:46 UTC (rev 22237)
@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include "pluginlib/plugin_macros.h"
+#include "pluginlib/class_list_macros.h"
#include "controller_interface/controller.h"
#include "joint_qualification_controllers/checkout_controller.h"
@@ -37,12 +37,7 @@
using namespace controller;
-BEGIN_PLUGIN_LIST(Controller)
-
-REGISTER_PLUGIN(CheckoutController)
-REGISTER_PLUGIN(HoldSetController)
-REGISTER_PLUGIN(HysteresisController)
-REGISTER_PLUGIN(SineSweepController)
-
-END_PLUGIN_LIST
-
+PLUGINLIB_REGISTER_CLASS(CheckoutController, CheckoutController, Controller)
+PLUGINLIB_REGISTER_CLASS(HoldSetController, HoldSetController, Controller)
+PLUGINLIB_REGISTER_CLASS(HysteresisController, HysteresisController, Controller)
+PLUGINLIB_REGISTER_CLASS(SineSweepController, SineSweepController, Controller)
Modified: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -1,20 +1,20 @@
<library path="lib/librobot_mechanism_controllers">
- <plugin name="JointEffortController" class="JointEffortController" type="Controller" />
+ <class name="JointEffortController" type="JointEffortController" base_class_type="Controller" />
- <plugin name="JointVelocityController" class="JointVelocityController" type="Controller" />
- <plugin name="JointPositionController" class="JointPositionController" type="Controller" />
+ <class name="JointVelocityController" type="JointVelocityController" base_class_type="Controller" />
+ <class name="JointPositionController" type="JointPositionController" base_class_type="Controller" />
- <plugin name="CartesianWrenchController" class="CartesianWrenchController" type="Controller" />
- <plugin name="CartesianTwistController" class="CartesianTwistController" type="Controller" />
- <plugin name="CartesianPoseController" class="CartesianPoseController" type="Controller" />
+ <class name="CartesianWrenchController" type="CartesianWrenchController" base_class_type="Controller" />
+ <class name="CartesianTwistController" type="CartesianTwistController" base_class_type="Controller" />
+ <class name="CartesianPoseController" type="CartesianPoseController" base_class_type="Controller" />
- <plugin name="JointUDCalibrationController" class="JointUDCalibrationController" type="Controller" />
+ <class name="JointUDCalibrationController" type="JointUDCalibrationController" base_class_type="Controller" />
- <plugin name="CartesianTrajectoryController" class="CartesianTrajectoryController" type="Controller" />
+ <class name="CartesianTrajectoryController" type="CartesianTrajectoryController" base_class_type="Controller" />
- <plugin name="TriggerController" class="TriggerController" type="Controller" />
- <plugin name="TriggerControllerNode" class="TriggerControllerNode" type="Controller" />
+ <class name="TriggerController" type="TriggerController" type="Controller" />
+ <class name="TriggerControllerNode" type="TriggerControllerNode" base_class_type="Controller" />
- <plugin name="JointPDController" class="JointPDController" type="Controller" />
- <plugin name="JointPDControllerNode" class="JointPDControllerNode" type="Controller" />
+ <class name="JointPDController" type="JointPDController" base_class_type="Controller" />
+ <class name="JointPDControllerNode" type="JointPDControllerNode" base_class_type="Controller" />
</library>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -33,7 +33,7 @@
<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" />
- <controller_interface controller="${prefix}/controller_plugins.xml" />
+ <controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>
<rosdep name="libtool"/>
</package>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-19 03:27:46 UTC (rev 22237)
@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include "pluginlib/plugin_macros.h"
+#include "pluginlib/class_list_macros.h"
#include "controller_interface/controller.h"
#include "robot_mechanism_controllers/cartesian_pose_controller.h"
@@ -43,25 +43,22 @@
using namespace controller;
-BEGIN_PLUGIN_LIST(Controller)
-REGISTER_PLUGIN(JointEffortController)
-REGISTER_PLUGIN(JointVelocityController)
-REGISTER_PLUGIN(JointPositionController)
+PLUGINLIB_REGISTER_CLASS(JointEffortController, JointEffortController, Controller)
+PLUGINLIB_REGISTER_CLASS(JointVelocityController, JointVelocityController, Controller)
+PLUGINLIB_REGISTER_CLASS(JointPositionController, JointPositionController, Controller)
-REGISTER_PLUGIN(CartesianWrenchController)
-REGISTER_PLUGIN(CartesianTwistController)
-REGISTER_PLUGIN(CartesianPoseController)
+PLUGINLIB_REGISTER_CLASS(CartesianWrenchController, CartesianWrenchController, Controller)
+PLUGINLIB_REGISTER_CLASS(CartesianTwistController, CartesianTwistController, Controller)
+PLUGINLIB_REGISTER_CLASS(CartesianPoseController, CartesianPoseController, Controller)
-REGISTER_PLUGIN(JointUDCalibrationController)
+PLUGINLIB_REGISTER_CLASS(JointUDCalibrationController, JointUDCalibrationController, Controller)
-REGISTER_PLUGIN(CartesianTrajectoryController)
+PLUGINLIB_REGISTER_CLASS(CartesianTrajectoryController, CartesianTrajectoryController, Controller)
-REGISTER_PLUGIN(TriggerController);
-REGISTER_PLUGIN(TriggerControllerNode);
+PLUGINLIB_REGISTER_CLASS(TriggerController, TriggerController, Controller)
+PLUGINLIB_REGISTER_CLASS(TriggerControllerNode, TriggerControllerNode, Controller)
-REGISTER_PLUGIN(JointPDController)
-REGISTER_PLUGIN(JointPDControllerNode)
+PLUGINLIB_REGISTER_CLASS(JointPDController, JointPDController, Controller)
+PLUGINLIB_REGISTER_CLASS(JointPDControllerNode, JointPDControllerNode, Controller)
-END_PLUGIN_LIST
-
Modified: pkg/trunk/nav/people_aware_nav/bgp_plugin.xml
===================================================================
--- pkg/trunk/nav/people_aware_nav/bgp_plugin.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/nav/people_aware_nav/bgp_plugin.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -1,7 +1,7 @@
<library path="lib/libnavfn_constrained">
- <plugin name="NavfnROSConstrained" class="people_aware_nav::NavfnROSConstrained" type="nav_core::BaseGlobalPlanner">
+ <class name="NavfnROSConstrained" type="people_aware_nav::NavfnROSConstrained" base_class_type="nav_core::BaseGlobalPlanner">
<description>
A constrained version of the navfn planner
</description>
- </plugin>
+ </class>
</library>
Modified: pkg/trunk/nav/people_aware_nav/manifest.xml
===================================================================
--- pkg/trunk/nav/people_aware_nav/manifest.xml 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/nav/people_aware_nav/manifest.xml 2009-08-19 03:27:46 UTC (rev 22237)
@@ -24,7 +24,7 @@
<depend package="roscpp" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/srv/cpp -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lnavfn_constrained"/>
- <nav_core BaseGlobalPlanner="${prefix}/bgp_plugin.xml" />
+ <nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>
</package>
Modified: pkg/trunk/nav/people_aware_nav/src/navfn_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/navfn_constrained.cpp 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/nav/people_aware_nav/src/navfn_constrained.cpp 2009-08-19 03:27:46 UTC (rev 22237)
@@ -37,11 +37,9 @@
#include <people_aware_nav/navfn_constrained.h>
#include <visualization_msgs/Marker.h>
#include <ros/ros.h>
-#include <pluginlib/plugin_macros.h>
+#include <pluginlib/class_list_macros.h>
-BEGIN_PLUGIN_LIST(nav_core::BaseGlobalPlanner);
-REGISTER_PLUGIN(people_aware_nav::NavfnROSConstrained);
-END_PLUGIN_LIST
+PLUGINLIB_REGISTER_CLASS(NavfnROSConstrained, people_aware_nav::NavfnROSConstrained, nav_core::BaseGlobalPlanner);
namespace people_aware_nav {
Modified: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/CMakeLists.txt 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/CMakeLists.txt 2009-08-19 03:27:46 UTC (rev 22237)
@@ -29,6 +29,4 @@
#rospack_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
-rospack_add_library(square src/square.cpp src/triangle.cpp src/polygon_manifest.cpp)
-
-rospack_add_library(line src/line.cpp src/shape_manifest.cpp)
+rospack_add_library(polyShapes src/square.cpp src/triangle.cpp src/line.cpp src/manifest.cpp)
Deleted: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/line.h
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/line.h 2009-08-19 03:25:05 UTC (rev 22236)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/line.h 2009-08-19 03:27:46 UTC (rev 22237)
@@ -1,46 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, 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 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.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef LINE_H_
-#define LINE_H_
-
-#include "pluginlib_tutorial_interfaces/shape.h"
-
-class line : public shape {
-public:
- virtual double area() const;
-};
-#endif
Copied: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/line.h (from rev 21542, pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/line.h)
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/line.h (rev 0)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/line.h 2009-08-19 03:27:46 UTC (rev 22237)
@@ -0,0 +1,46 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef LINE_H_
+#define LINE_H_
+
+#include "pluginlib_tutorial_interfaces/shape.h"
+
+class line : public shape {
+public:
+ virtual double area() const;
+};
+#endif
Copied: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/square.h (from rev 21542, pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/square.h)
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/square.h (rev 0)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/square.h 2009-08-19 03:27:46 UTC (rev 22237)
@@ -0,0 +1,49 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef SQUARE_H_
+#define SQUARE_H_
+
+#include "pluginlib_tutorial_interfaces/polygon.h"
+
+class square : public polygon {
+public:
+ square();
+ virtual double area() const;
+private:
+ std::string name_;
+};
+#endif
Copied: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/triangle.h (from rev 21542, pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/triangle.h)
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/triangle.h (rev 0)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_provider/include/pluginlib_tutorial_provider/triangle.h 2009-08-19 03:27:46 UTC (rev 22237)
@@ -0,0 +1,47 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products...
[truncated message content] |
|
From: <vij...@us...> - 2009-08-19 03:41:05
|
Revision: 22240
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22240&view=rev
Author: vijaypradeep
Date: 2009-08-19 03:40:56 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Adding spinThread option to simple action client
Modified Paths:
--------------
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
Modified: pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-19 03:40:56 UTC (rev 22240)
@@ -60,10 +60,10 @@
ROS_INFO("Got Feedback!");
}
-void spinThread()
+/*void spinThread()
{
ros::spin();
-}
+}*/
int main(int argc, char** argv)
{
@@ -71,9 +71,9 @@
ros::NodeHandle n;
- boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+ //boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
- MoveBaseClient ac("move_base");
+ MoveBaseClient ac("move_base", true);
sleep(2.0);
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-19 03:40:56 UTC (rev 22240)
@@ -38,6 +38,7 @@
#include <boost/thread/condition.hpp>
#include "ros/ros.h"
+#include "ros/callback_queue_interface.h"
#include "actionlib/client/client_helpers.h"
namespace actionlib
@@ -69,10 +70,12 @@
*
* Constructs an ActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
+ * \param queue CallbackQueue from which this action will process messages.
+ * The default (NULL) is to use the global queue
*/
- ActionClient(const std::string& name) : n_(name)
+ ActionClient(const std::string& name, ros::CallbackQueueInterface* queue = NULL) : n_(name)
{
- initClient();
+ initClient(queue);
}
/**
@@ -82,10 +85,12 @@
* and namespaces them according the a specified NodeHandle
* \param n The node handle on top of which we want to namespace our action
* \param name The action name. Defines the namespace in which the action communicates
+ * \param queue CallbackQueue from which this action will process messages.
+ * The default (NULL) is to use the global queue
*/
- ActionClient(const ros::NodeHandle& n, const std::string& name) : n_(n, name)
+ ActionClient(const ros::NodeHandle& n, const std::string& name, ros::CallbackQueueInterface* queue = NULL) : n_(n, name)
{
- initClient();
+ initClient(queue);
}
/**
@@ -196,7 +201,7 @@
cancel_pub_.publish(cancel_msg);
}
- void initClient()
+ void initClient(ros::CallbackQueueInterface* queue)
{
// Start publishers and subscribers
server_connected_ = false;
@@ -205,11 +210,21 @@
manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
- status_sub_ = n_.subscribe("status", 1, &ActionClientT::statusCb, this);
- feedback_sub_ = n_.subscribe("feedback", 1, &ActionClientT::feedbackCb, this);
- result_sub_ = n_.subscribe("result", 1, &ActionClientT::resultCb, this);
+ status_sub_ = queue_subscribe("status", 1, &ActionClientT::statusCb, this, queue);
+ feedback_sub_ = queue_subscribe("feedback", 1, &ActionClientT::feedbackCb, this, queue);
+ result_sub_ = queue_subscribe("result", 1, &ActionClientT::resultCb, this, queue);
}
+ template<class M, class T>
+ ros::Subscriber queue_subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr<M const>&), T* obj, ros::CallbackQueueInterface* queue)
+ {
+ ros::SubscribeOptions ops;
+ ops.init<M>(topic, queue_size, boost::bind(fp, obj, _1));
+ ops.transport_hints = ros::TransportHints();
+ ops.callback_queue = queue;
+ return n_.subscribe(ops);
+ }
+
void statusCb(const GoalStatusArrayConstPtr& status_array)
{
manager_.updateStatuses(status_array);
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h 2009-08-19 03:40:56 UTC (rev 22240)
@@ -37,8 +37,10 @@
#include <boost/thread/condition.hpp>
#include <boost/thread/mutex.hpp>
+#include <boost/scoped_ptr.hpp>
#include "ros/ros.h"
+#include "ros/callback_queue.h"
#include "actionlib/client/action_client.h"
#include "actionlib/client/simple_goal_state.h"
#include "actionlib/client/terminal_state.h"
@@ -71,10 +73,12 @@
*
* Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
+ * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false,
+ * then the user has to call ros::spin().
*/
- SimpleActionClient(const std::string& name) : ac_(name), cur_simple_state_(SimpleGoalState::PENDING)
+ SimpleActionClient(const std::string& name, bool spin_thread = false) : cur_simple_state_(SimpleGoalState::PENDING)
{
- initSimpleClient();
+ initSimpleClient(nh_, name, spin_thread);
}
/**
@@ -84,18 +88,23 @@
* the ActionInterface, and namespaces them according the a specified NodeHandle
* \param n The node handle on top of which we want to namespace our action
* \param name The action name. Defines the namespace in which the action communicates
+ * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false,
+ * then the user has to call ros::spin().
*/
- SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name), cur_simple_state_(SimpleGoalState::PENDING)
+ SimpleActionClient(const ros::NodeHandle& n, const std::string& name, bool spin_thread = false) : cur_simple_state_(SimpleGoalState::PENDING)
{
- initSimpleClient();
+ initSimpleClient(n, name, spin_thread);
+
}
+ ~SimpleActionClient();
+
/**
* \brief Blocks until the action server connects to this client
* \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
* \return True if the server connected in the allocated time. False on timeout
*/
- bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_.waitForActionServerToStart(timeout); }
+ bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_->waitForActionServerToStart(timeout); }
/**
* \brief Sends a goal to the ActionServer, and also registers callbacks
@@ -170,7 +179,7 @@
private:
typedef ActionClient<ActionSpec> ActionClientT;
ros::NodeHandle nh_;
- ActionClientT ac_;
+ boost::scoped_ptr<ActionClientT> ac_;
GoalHandleT gh_;
SimpleGoalState cur_simple_state_;
@@ -184,23 +193,66 @@
SimpleActiveCallback active_cb_;
SimpleFeedbackCallback feedback_cb_;
+ // Spin Thread Stuff
+ boost::mutex terminate_mutex_;
+ bool need_to_terminate_;
+ boost::thread* spin_thread_;
+ ros::CallbackQueue callback_queue;
+
// ***** Private Funcs *****
- void initSimpleClient();
+ void initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread);
void handleTransition(GoalHandleT gh);
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
void setSimpleState(const SimpleGoalState::StateEnum& next_state);
void setSimpleState(const SimpleGoalState& next_state);
+ void spinThread();
};
template<class ActionSpec>
-void SimpleActionClient<ActionSpec>::initSimpleClient()
+void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread)
{
+ if (spin_thread)
+ {
+ ROS_DEBUG("Spinning up a thread for the SimpleActionClient");
+ need_to_terminate_ = false;
+ spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
+ ac_.reset(new ActionClientT(n, name, &callback_queue));
+ }
+ else
+ ac_.reset(new ActionClientT(n, name));
+}
+template<class ActionSpec>
+SimpleActionClient<ActionSpec>::~SimpleActionClient()
+{
+ if (spin_thread_)
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ need_to_terminate_ = true;
+ }
+ spin_thread_->join();
+ delete spin_thread_;
+ }
}
template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::spinThread()
+{
+ while (nh_.ok())
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ if (need_to_terminate_)
+ break;
+ }
+ callback_queue.callAvailable();
+ }
+}
+
+template<class ActionSpec>
void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
{
setSimpleState( SimpleGoalState(next_state) );
@@ -232,8 +284,8 @@
cur_simple_state_ = SimpleGoalState::PENDING;
// Send the goal to the ActionServer
- gh_ = ac_.sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
- boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
+ gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
+ boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
}
template<class ActionSpec>
@@ -289,13 +341,13 @@
template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelAllGoals()
{
- ac_.cancelAllGoals();
+ ac_->cancelAllGoals();
}
template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time& time)
{
- ac_.cancelAllGoalsBeforeTime(time);
+ ac_->cancelAllGoalsBeforeTime(time);
}
template<class ActionSpec>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-08-19 06:41:52
|
Revision: 22266
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22266&view=rev
Author: mariusmuja
Date: 2009-08-19 06:41:42 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Moved tabletop code from recognitian_lambertian to a new package, tabletop_objects.
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/manifest.xml
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fit.launch
pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fitter.vcg
pkg/trunk/sandbox/point_cloud_tutorials/scripts/test_table_model_fitter.py
pkg/trunk/util/prosilica_capture_rectified/launch/prosilica_capture.launch
pkg/trunk/vision/recognition_lambertian/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/TableTopObject.msg
pkg/trunk/sandbox/tabletop_objects/
pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt
pkg/trunk/sandbox/tabletop_objects/Makefile
pkg/trunk/sandbox/tabletop_objects/data/
pkg/trunk/sandbox/tabletop_objects/data/template.png
pkg/trunk/sandbox/tabletop_objects/data/template2.png
pkg/trunk/sandbox/tabletop_objects/data/template3.png
pkg/trunk/sandbox/tabletop_objects/data/templates.txt
pkg/trunk/sandbox/tabletop_objects/include/
pkg/trunk/sandbox/tabletop_objects/include/ply.h
pkg/trunk/sandbox/tabletop_objects/launch/
pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch
pkg/trunk/sandbox/tabletop_objects/launch/publish_objects.launch
pkg/trunk/sandbox/tabletop_objects/launch/tabletop_contours.launch
pkg/trunk/sandbox/tabletop_objects/mainpage.dox
pkg/trunk/sandbox/tabletop_objects/manifest.xml
pkg/trunk/sandbox/tabletop_objects/scripts/
pkg/trunk/sandbox/tabletop_objects/scripts/publish_stereo_data.py
pkg/trunk/sandbox/tabletop_objects/scripts/test_service
pkg/trunk/sandbox/tabletop_objects/src/
pkg/trunk/sandbox/tabletop_objects/src/convert_model.py
pkg/trunk/sandbox/tabletop_objects/src/model_fitter.cpp
pkg/trunk/sandbox/tabletop_objects/src/ply.c
pkg/trunk/sandbox/tabletop_objects/src/ply_import.py
pkg/trunk/sandbox/tabletop_objects/src/publish_scene.cpp
pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp
pkg/trunk/sandbox/tabletop_objects/src/tabletop_objects/
pkg/trunk/sandbox/tabletop_objects/src/tabletop_objects/__init__.py
pkg/trunk/sandbox/tabletop_objects/src/voxel_grid.py
pkg/trunk/sandbox/tabletop_objects/srv/
pkg/trunk/sandbox/tabletop_objects/srv/FindObjectPoses.srv
pkg/trunk/sandbox/tabletop_objects/srv/ModelFit.srv
Removed Paths:
-------------
pkg/trunk/vision/recognition_lambertian/data/template.png
pkg/trunk/vision/recognition_lambertian/data/template2.png
pkg/trunk/vision/recognition_lambertian/data/template3.png
pkg/trunk/vision/recognition_lambertian/data/templates.txt
pkg/trunk/vision/recognition_lambertian/include/ply.h
pkg/trunk/vision/recognition_lambertian/launch/model_fitter.launch
pkg/trunk/vision/recognition_lambertian/launch/publish_objects.launch
pkg/trunk/vision/recognition_lambertian/launch/recognition.launch
pkg/trunk/vision/recognition_lambertian/launch/recognition_bag.launch
pkg/trunk/vision/recognition_lambertian/launch/recognition_camera.launch
pkg/trunk/vision/recognition_lambertian/launch/recognition_lambertian.launch
pkg/trunk/vision/recognition_lambertian/msg/TableTopObject.msg
pkg/trunk/vision/recognition_lambertian/scripts/publish_stereo_data.py
pkg/trunk/vision/recognition_lambertian/scripts/test_service
pkg/trunk/vision/recognition_lambertian/src/convert_model.py
pkg/trunk/vision/recognition_lambertian/src/model_fitter.cpp
pkg/trunk/vision/recognition_lambertian/src/ply.c
pkg/trunk/vision/recognition_lambertian/src/ply_import.py
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian/__init__.py
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/recognition_lambertian/src/voxel_grid.py
pkg/trunk/vision/recognition_lambertian/srv/FindObjectPoses.srv
pkg/trunk/vision/recognition_lambertian/srv/ModelFit.srv
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/manifest.xml 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/manifest.xml 2009-08-19 06:41:42 UTC (rev 22266)
@@ -11,6 +11,7 @@
<depend package="geometry_msgs" />
<depend package="std_msgs"/>
+ <depend package="mapping_msgs" />
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
Copied: pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/TableTopObject.msg (from rev 22265, pkg/trunk/vision/recognition_lambertian/msg/TableTopObject.msg)
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/TableTopObject.msg (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/TableTopObject.msg 2009-08-19 06:41:42 UTC (rev 22266)
@@ -0,0 +1,3 @@
+geometry_msgs/PoseStamped grasp_pose
+geometry_msgs/PoseStamped object_pose
+mapping_msgs/Object object
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-19 06:41:42 UTC (rev 22266)
@@ -46,7 +46,7 @@
#include <move_arm/ActuateGripperAction.h>
#include <boost/thread/thread.hpp>
-#include <recognition_lambertian/FindObjectPoses.h>
+#include <tabletop_objects/FindObjectPoses.h>
#include <visualization_msgs/Marker.h>
@@ -87,12 +87,12 @@
vmPub.publish(mk);
}
- bool findObject(recognition_lambertian::TableTopObject &obj)
+ bool findObject(tabletop_msgs::TableTopObject &obj)
{
- recognition_lambertian::FindObjectPoses::Request req;
- recognition_lambertian::FindObjectPoses::Response res;
+ tabletop_objects::FindObjectPoses::Request req;
+ tabletop_objects::FindObjectPoses::Response res;
- ros::ServiceClient client = nh.serviceClient<recognition_lambertian::FindObjectPoses>("table_top/find_object_poses");
+ ros::ServiceClient client = nh.serviceClient<tabletop_objects::FindObjectPoses>("table_top/find_object_poses");
if (client.call(req, res))
{
ROS_INFO("Found %d objects", (int)res.objects.size());
@@ -150,7 +150,7 @@
}
}
- bool moveTo(recognition_lambertian::TableTopObject &obj)
+ bool moveTo(tabletop_msgs::TableTopObject &obj)
{
move_arm::MoveArmGoal goal;
@@ -239,7 +239,7 @@
}
}
- bool graspObject(recognition_lambertian::TableTopObject &obj)
+ bool graspObject(tabletop_msgs::TableTopObject &obj)
{
if (gripClose())
{
@@ -303,7 +303,7 @@
CleanTable ct;
- recognition_lambertian::TableTopObject obj;
+ tabletop_msgs::TableTopObject obj;
if (ct.findObject(obj))
{
@@ -311,7 +311,7 @@
if (ct.moveTo(obj))
{
ct.graspObject(obj);
- recognition_lambertian::TableTopObject obj2 = obj;
+ tabletop_msgs::TableTopObject obj2 = obj;
obj2.grasp_pose.pose.position.y -= 0.25;
if (ct.moveTo(obj2))
{
Modified: pkg/trunk/sandbox/3dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-08-19 06:41:42 UTC (rev 22266)
@@ -34,7 +34,7 @@
<!-- object recognition -->
<depend package="table_object_detector"/>
- <depend package="recognition_lambertian" />
+ <depend package="tabletop_objects" />
<!-- planning -->
Modified: pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fit.launch
===================================================================
--- pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fit.launch 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fit.launch 2009-08-19 06:41:42 UTC (rev 22266)
@@ -1,6 +1,6 @@
<launch>
- <include file="$(find recognition_lambertian)/launch/model_fitter.launch" />
+ <include file="$(find tabletop_objects)/launch/model_fitter.launch" />
<node pkg="point_cloud_tutorials" type="test_table_model_fitter.py" respawn="true" output="screen" />
<node pkg="rviz" type="rviz" args="-d $(find point_cloud_tutorials)/launch/table_model_fitter.vcg" />
Modified: pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fitter.vcg
===================================================================
--- pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fitter.vcg 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/sandbox/point_cloud_tutorials/launch/table_model_fitter.vcg 2009-08-19 06:41:42 UTC (rev 22266)
@@ -51,7 +51,7 @@
Point\ Cloud2.Min\ Intensity=39
Point\ Cloud2.Selectable=1
Point\ Cloud2.Style=1
-Point\ Cloud2.Topic=/recognition_lambertian/objects
+Point\ Cloud2.Topic=/tabletop_objects/objects
TF2.All\ Enabled=1
TF2.Enabled=0
TF2.Show\ Arrows=1
Modified: pkg/trunk/sandbox/point_cloud_tutorials/scripts/test_table_model_fitter.py
===================================================================
--- pkg/trunk/sandbox/point_cloud_tutorials/scripts/test_table_model_fitter.py 2009-08-19 06:40:05 UTC (rev 22265)
+++ pkg/trunk/sandbox/point_cloud_tutorials/scripts/test_table_model_fitter.py 2009-08-19 06:41:42 UTC (rev 22266)
@@ -36,7 +36,7 @@
## Simple demo of a rospy service client that calls a service to add
## two integers.
-PKG = 'recognition_lambertian' # this package name
+PKG = 'tabletop_objects' # this package name
import roslib; roslib.load_manifest(PKG)
@@ -47,7 +47,7 @@
import rospy
# imports the AddTwoInts service
-from recognition_lambertian.srv import *
+from tabletop_objects.srv import *
def call_service():
Copied: pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt (from rev 22265, pkg/trunk/vision/recognition_lambertian/CMakeLists.txt)
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt 2009-08-19 06:41:42 UTC (rev 22266)
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.4.6)
+
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
+
+rosbuild_init()
+rosbuild_add_boost_directories()
+
+rosbuild_genmsg()
+rosbuild_gensrv()
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+
+add_custom_command(
+ OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/build/Ikea_meshes.tar.gz
+ COMMAND wget
+ ARGS pr.willowgarage.com/downloads/Ikea_meshes.tar.gz
+ DEPENDS )
+
+#un tar Ikea mesh files
+add_custom_command(
+ OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/meshes
+ COMMAND mkdir -p
+ ARGS ${CMAKE_CURRENT_SOURCE_DIR}/meshes
+ COMMAND tar
+ ARGS -xvzf ${CMAKE_CURRENT_SOURCE_DIR}/build/Ikea_meshes.tar.gz -C ${CMAKE_CURRENT_SOURCE_DIR}/meshes
+ DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/build/Ikea_meshes.tar.gz)
+
+add_custom_target(media_files ALL DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/meshes)
+
+
+rosbuild_add_executable(tabletop_detector src/tabletop_detector.cpp)
+rosbuild_add_executable(model_fitter src/model_fitter.cpp src/ply.c)
+rosbuild_link_boost(model_fitter filesystem)
Added: pkg/trunk/sandbox/tabletop_objects/Makefile
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/Makefile (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/Makefile 2009-08-19 06:41:42 UTC (rev 22266)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/sandbox/tabletop_objects/data/template.png (from rev 22265, pkg/trunk/vision/recognition_lambertian/data/template.png)
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/data/template.png (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/data/template.png 2009-08-19 06:41:42 UTC (rev 22266)
@@ -0,0 +1,8 @@
+\x89PNG
+
+ |
|
From: <mee...@us...> - 2009-08-19 17:06:34
|
Revision: 22281
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22281&view=rev
Author: meeussen
Date: 2009-08-19 17:06:24 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
tested version of robot_pose_ekf after many message changes
Modified Paths:
--------------
pkg/trunk/demos/milestone2/milestone2_actions/milestone2.launch
pkg/trunk/stacks/navigation/robot_pose_ekf/robot_pose_ekf.launch
pkg/trunk/stacks/navigation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/pr2/pr2_alpha/teleop_controllers.launch
Modified: pkg/trunk/demos/milestone2/milestone2_actions/milestone2.launch
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/milestone2.launch 2009-08-19 16:30:43 UTC (rev 22280)
+++ pkg/trunk/demos/milestone2/milestone2_actions/milestone2.launch 2009-08-19 17:06:24 UTC (rev 22281)
@@ -51,18 +51,9 @@
<!-- Robot pose ekf -->
-<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" machine="four">
- <param name="freq" value="30.0"/>
- <param name="sensor_timeout" value="1.0"/>
- <param name="publish_tf" value="true"/>
- <param name="odom_used" value="true"/>
- <param name="imu_used" value="true"/>
- <param name="vo_used" value="false"/>
- <remap from="odom" to="pr2_base_odometry/odom"/>
-</node>
+ <include file="$(find robot_pose_ekf)/robot_pose_ekf.launch" />
-
<!-- Sends robot pose when robot moves for irosweb interface -->
<node pkg="tf" machine="two" type="change_notifier"/>
Modified: pkg/trunk/stacks/navigation/robot_pose_ekf/robot_pose_ekf.launch
===================================================================
--- pkg/trunk/stacks/navigation/robot_pose_ekf/robot_pose_ekf.launch 2009-08-19 16:30:43 UTC (rev 22280)
+++ pkg/trunk/stacks/navigation/robot_pose_ekf/robot_pose_ekf.launch 2009-08-19 17:06:24 UTC (rev 22281)
@@ -5,8 +5,8 @@
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
- <param name="imu_used" value="false"/>
- <param name="vo_used" value="false"/>
+ <param name="imu_used" value="true"/>
+ <param name="vo_used" value="true"/>
<remap from="odom" to="/pr2_base_odometry/odom" />
</node>
Modified: pkg/trunk/stacks/navigation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/stacks/navigation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-19 16:30:43 UTC (rev 22280)
+++ pkg/trunk/stacks/navigation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-19 17:06:24 UTC (rev 22281)
@@ -270,8 +270,8 @@
#ifdef __EKF_DEBUG_FILE__
// write to file
double Rx, Ry, Rz;
- vo_meas_base.getBasis().getEulerZYX(Rz, Ry, Rx);
- vo_file_ << vo_meas_base.getOrigin().x() << " " << vo_meas_base.getOrigin().y() << " " << vo_meas_base.getOrigin().z() << " "
+ vo_meas_.getBasis().getEulerZYX(Rz, Ry, Rx);
+ vo_file_ << vo_meas_.getOrigin().x() << " " << vo_meas_.getOrigin().y() << " " << vo_meas_.getOrigin().z() << " "
<< Rx << " " << Ry << " " << Rz << endl;
#endif
};
Modified: pkg/trunk/stacks/pr2/pr2_alpha/teleop_controllers.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/teleop_controllers.launch 2009-08-19 16:30:43 UTC (rev 22280)
+++ pkg/trunk/stacks/pr2/pr2_alpha/teleop_controllers.launch 2009-08-19 17:06:24 UTC (rev 22281)
@@ -3,8 +3,8 @@
reuse in teleop_joystick, teleop_ps3 -->
<rosparam file="$(find pr2_default_controllers)/pr2_base_controller.yaml" command="load" ns="pr2_base_controller" />
- <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_odometry" />
- <node pkg="mechanism_control" type="spawner.py" args="pr2_base_controller pr2_odometry" output="screen"/>
+ <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_base_odometry" />
+ <node pkg="mechanism_control" type="spawner.py" args="pr2_base_controller pr2_base_odometry" output="screen"/>
<rosparam file="$(find pr2_default_controllers)/pr2_joint_velocity_controllers.yaml" command="load" />
<node pkg="mechanism_control" type="spawner.py" args="torso_lift_velocity_controller" output="screen" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-08-19 23:47:33
|
Revision: 22362
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22362&view=rev
Author: meeussen
Date: 2009-08-19 23:47:27 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
move robot model into new stack, and rename to urdf
Added Paths:
-----------
pkg/trunk/stacks/robot_model/
pkg/trunk/stacks/robot_model/stack.xml
pkg/trunk/stacks/robot_model/urdf/
Removed Paths:
-------------
pkg/trunk/sandbox/robot_model/
Copied: pkg/trunk/stacks/robot_model/stack.xml (from rev 22343, pkg/trunk/stacks/mechanism/stack.xml)
===================================================================
--- pkg/trunk/stacks/robot_model/stack.xml (rev 0)
+++ pkg/trunk/stacks/robot_model/stack.xml 2009-08-19 23:47:27 UTC (rev 22362)
@@ -0,0 +1,13 @@
+<stack name="robot_model" version="0.1">
+ <description brief="robot_model packages from ros-pkg">
+ These are robot_model-related packages.
+ </description>
+ <author>John Hsu jo...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/robot_model</url>
+
+ <depend stack="common"/> <!-- tinyxml -->
+ <depend stack="ros"/> <!-- rosconsole -->
+
+</stack>
Property changes on: pkg/trunk/stacks/robot_model/stack.xml
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/mechanism/stack.xml:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-08-19 23:51:27
|
Revision: 22363
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22363&view=rev
Author: meeussen
Date: 2009-08-19 23:51:20 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Move ply, ivcon and convex_decomposition to the new robot_model stack
Added Paths:
-----------
pkg/trunk/stacks/robot_model/convex_decomposition/
pkg/trunk/stacks/robot_model/ivcon/
pkg/trunk/stacks/robot_model/ply/
Removed Paths:
-------------
pkg/trunk/sandbox/convex_decomposition/
pkg/trunk/sandbox/ivcon/
pkg/trunk/sandbox/ply/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-08-20 00:24:31
|
Revision: 22371
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22371&view=rev
Author: jfaustwg
Date: 2009-08-20 00:24:25 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
move resource_retriever to robot_model stack
Added Paths:
-----------
pkg/trunk/stacks/robot_model/resource_retriever/
Removed Paths:
-------------
pkg/trunk/sandbox/resource_retriever/
Property changes on: pkg/trunk/stacks/robot_model/resource_retriever
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/resource_retriever:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pmi...@us...> - 2009-08-20 01:00:59
|
Revision: 22373
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22373&view=rev
Author: pmihelich
Date: 2009-08-20 01:00:51 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
camera_calibration: Moved from sandbox to calibration stack.
Added Paths:
-----------
pkg/trunk/calibration/camera_calibration/
Removed Paths:
-------------
pkg/trunk/sandbox/camera_calibration/
Property changes on: pkg/trunk/calibration/camera_calibration
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/camera_calibration:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-08-20 01:02:40
|
Revision: 22374
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22374&view=rev
Author: meeussen
Date: 2009-08-20 01:02:29 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
move tf conversions package from sandbox to geometry stack
Modified Paths:
--------------
pkg/trunk/stacks/geometry/tf_conversions/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/geometry/tf_conversions/
Removed Paths:
-------------
pkg/trunk/sandbox/tf_conversions/
Modified: pkg/trunk/stacks/geometry/tf_conversions/manifest.xml
===================================================================
--- pkg/trunk/sandbox/tf_conversions/manifest.xml 2009-08-20 00:25:13 UTC (rev 22372)
+++ pkg/trunk/stacks/geometry/tf_conversions/manifest.xml 2009-08-20 01:02:29 UTC (rev 22374)
@@ -1,13 +1,13 @@
<package>
<description brief='Transform Library Conversions'>
- Conversion functions between tf, KDL, and Eigen
+This package contains a set of conversion functions to translate common tf datatypes into datatypes used by other libraries. Currently this package has support for the Kinematics and Dynamics Library (KDL) and the Eigen matrix library.
</description>
<author>Tully Foote</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/</url>
+ <url>http://www.ros.org/wiki/tf_conversions</url>
<depend package="tf"/>
<depend package="kdl"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-08-20 01:10:45
|
Revision: 22379
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22379&view=rev
Author: hsujohnhsu
Date: 2009-08-20 01:10:39 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
moving gazebo_plugin into pr2_simulator stack.
Modified Paths:
--------------
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/
Removed Paths:
-------------
pkg/trunk/drivers/simulator/gazebo_plugin/
Property changes on: pkg/trunk/stacks/pr2_simulator/gazebo_plugin
___________________________________________________________________
Added: svn:ignore
+ .rosgcov_files
bin
.build_version
urdf2file
urdf2factory
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/drivers/simulator/gazebo_plugin:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-08-19 21:53:36 UTC (rev 22333)
+++ pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml 2009-08-20 01:10:39 UTC (rev 22379)
@@ -31,8 +31,8 @@
<depend package="mechanism_model" />
<depend package="tinyxml" />
<depend package="bullet" />
+ <depend package="urdf" />
<depend package="wg_robot_description_parser" />
- <depend package="axis_cam" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
</export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-08-20 02:39:26
|
Revision: 22394
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22394&view=rev
Author: eitanme
Date: 2009-08-20 02:39:16 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
Moving move_base messages and action description to the move_base_msgs package to remove a lot of dependencies
Modified Paths:
--------------
pkg/trunk/sandbox/move_base_client/manifest.xml
pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/manifest.xml
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/manifest.xml
pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/src/simple_navigation_goals.cpp
Added Paths:
-----------
pkg/trunk/stacks/navigation/move_base_msgs/
pkg/trunk/stacks/navigation/move_base_msgs/CMakeLists.txt
pkg/trunk/stacks/navigation/move_base_msgs/Makefile
pkg/trunk/stacks/navigation/move_base_msgs/action/
pkg/trunk/stacks/navigation/move_base_msgs/mainpage.dox
pkg/trunk/stacks/navigation/move_base_msgs/manifest.xml
pkg/trunk/stacks/navigation/move_base_msgs/msg/
Removed Paths:
-------------
pkg/trunk/stacks/navigation/move_base/action/
pkg/trunk/stacks/navigation/move_base/msg/
Modified: pkg/trunk/sandbox/move_base_client/manifest.xml
===================================================================
--- pkg/trunk/sandbox/move_base_client/manifest.xml 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/sandbox/move_base_client/manifest.xml 2009-08-20 02:39:16 UTC (rev 22394)
@@ -7,7 +7,8 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/move_base_client</url>
- <depend package="move_base" />
+ <depend package="move_base_msgs" />
+ <depend package="nav_robot_actions" />
<depend package="actionlib" />
<depend package="action_translator" />
<depend package="geometry_msgs" />
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-08-20 02:39:16 UTC (rev 22394)
@@ -34,14 +34,14 @@
#include "ros/ros.h"
-#include "move_base/MoveBaseAction.h"
+#include "move_base_msgs/MoveBaseAction.h"
#include "actionlib/client/action_client.h"
#include "boost/thread.hpp"
using namespace actionlib;
using namespace geometry_msgs;
-using namespace move_base;
+using namespace move_base_msgs;
typedef ActionClient<MoveBaseAction> MoveBaseClient;
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-20 02:39:16 UTC (rev 22394)
@@ -37,7 +37,7 @@
#include "geometry_msgs/PoseStamped.h"
#include "nav_robot_actions/MoveBaseState.h"
-#include "move_base/MoveBaseAction.h"
+#include "move_base_msgs/MoveBaseAction.h"
#include "actionlib/client/action_client.h"
#include "action_translator/action_translator.h"
#include "robot_actions/action_runner.h"
@@ -47,17 +47,17 @@
move_base::MoveBaseGoal fromOldGoal(const PoseStamped& old_goal)
{
- move_base::MoveBaseGoal new_goal;
+ move_base_msgs::MoveBaseGoal new_goal;
new_goal.target_pose = old_goal;
return new_goal;
}
-/*PoseStamped fromActionFeedback(const move_base::MoveBaseFeedback& action_feedback)
+/*PoseStamped fromActionFeedback(const move_base_msgs::MoveBaseFeedback& action_feedback)
{
return action_feedback.cur_pose;
}*/
-PoseStamped fromActionResult(const move_base::MoveBaseResult& action_result)
+PoseStamped fromActionResult(const move_base_msgs::MoveBaseResult& action_result)
{
return action_result.final_pose;
}
@@ -68,7 +68,7 @@
ros::NodeHandle n;
- action_translator::ActionTranslator<move_base::MoveBaseAction, PoseStamped, PoseStamped>
+ action_translator::ActionTranslator<move_base_msgs::MoveBaseAction, PoseStamped, PoseStamped>
translator("move_base", &fromOldGoal, NULL, &fromActionResult);
robot_actions::ActionRunner runner(10.0);
Modified: pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-20 02:39:16 UTC (rev 22394)
@@ -34,14 +34,14 @@
#include "ros/ros.h"
-#include "move_base/MoveBaseAction.h"
+#include "move_base_msgs/MoveBaseAction.h"
#include "actionlib/client/simple_action_client.h"
#include "boost/thread.hpp"
using namespace actionlib;
using namespace geometry_msgs;
-using namespace move_base;
+using namespace move_base_msgs;
typedef SimpleActionClient<MoveBaseAction> MoveBaseClient;
Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-20 02:39:16 UTC (rev 22394)
@@ -39,7 +39,7 @@
#include <ros/ros.h>
#include <actionlib/server/single_goal_action_server.h>
-#include <move_base/MoveBaseAction.h>
+#include <move_base_msgs/MoveBaseAction.h>
#include <nav_core/base_local_planner.h>
#include <nav_core/base_global_planner.h>
@@ -56,7 +56,7 @@
namespace move_base {
//typedefs to help us out with the action server so that we don't hace to type so much
- typedef actionlib::SingleGoalActionServer<MoveBaseAction> MoveBaseActionServer;
+ typedef actionlib::SingleGoalActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
enum MoveBaseState {
PLANNING,
@@ -155,7 +155,7 @@
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
- void executeCb(const MoveBaseGoalConstPtr& move_base_goal);
+ void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
Modified: pkg/trunk/stacks/navigation/move_base/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-08-20 02:39:16 UTC (rev 22394)
@@ -12,6 +12,7 @@
<depend package="roscpp"/>
<depend package="rosconsole"/>
<depend package="std_msgs"/>
+ <depend package="move_base_msgs"/>
<depend package="geometry_msgs"/>
<depend package="visualization_msgs"/>
<depend package="robot_actions"/>
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-20 02:39:16 UTC (rev 22394)
@@ -63,7 +63,7 @@
position_pub_ = ros_node_.advertise<geometry_msgs::PoseStamped>("~current_position", 1);
ros::NodeHandle action_node(ros_node_, name);
- action_goal_pub_ = action_node.advertise<MoveBaseActionGoal>("goal", 1);
+ action_goal_pub_ = action_node.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
//we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
//they won't get any useful information back about its status, but this is useful for tools
@@ -130,7 +130,7 @@
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
ROS_DEBUG("In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
- MoveBaseActionGoal action_goal;
+ move_base_msgs::MoveBaseActionGoal action_goal;
action_goal.goal.target_pose = *goal;
action_goal_pub_.publish(action_goal);
@@ -381,7 +381,7 @@
}
- void MoveBase::executeCb(const MoveBaseGoalConstPtr& move_base_goal)
+ void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{
geometry_msgs::PoseStamped goal = move_base_goal->target_pose;
std::vector<geometry_msgs::PoseStamped> global_plan;
@@ -462,7 +462,7 @@
//check that the observation buffers for the costmap are current, we don't want to drive blind
if(!controller_costmap_ros_->isCurrent()){
- ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros_node_.getName().c_str());
+ ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
publishZeroVelocity();
return;
}
Added: pkg/trunk/stacks/navigation/move_base_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/navigation/move_base_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/navigation/move_base_msgs/CMakeLists.txt 2009-08-20 02:39:16 UTC (rev 22394)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/stacks/navigation/move_base_msgs/Makefile
===================================================================
--- pkg/trunk/stacks/navigation/move_base_msgs/Makefile (rev 0)
+++ pkg/trunk/stacks/navigation/move_base_msgs/Makefile 2009-08-20 02:39:16 UTC (rev 22394)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/stacks/navigation/move_base_msgs/mainpage.dox
===================================================================
--- pkg/trunk/stacks/navigation/move_base_msgs/mainpage.dox (rev 0)
+++ pkg/trunk/stacks/navigation/move_base_msgs/mainpage.dox 2009-08-20 02:39:16 UTC (rev 22394)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b move_base_msgs is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/stacks/navigation/move_base_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/move_base_msgs/manifest.xml (rev 0)
+++ pkg/trunk/stacks/navigation/move_base_msgs/manifest.xml 2009-08-20 02:39:16 UTC (rev 22394)
@@ -0,0 +1,22 @@
+<package>
+ <description brief="Holds the action description and relevant messages for the move_base package">
+
+ Holds the action description and relevant messages for the move_base package
+
+ </description>
+ <author>Eitan Marder-Eppstein</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/move_base_msgs</url>
+ <depend package="roslib"/>
+ <depend package="std_msgs"/>
+ <depend package="actionlib"/>
+ <depend package="geometry_msgs"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+
+</package>
+
+
Modified: pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/manifest.xml 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/manifest.xml 2009-08-20 02:39:16 UTC (rev 22394)
@@ -8,7 +8,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/simple_navigation_goals</url>
- <depend package="move_base"/>
+ <depend package="move_base_msgs"/>
<depend package="actionlib"/>
<depend package="roscpp"/>
Modified: pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/src/simple_navigation_goals.cpp
===================================================================
--- pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/src/simple_navigation_goals.cpp 2009-08-20 02:33:37 UTC (rev 22393)
+++ pkg/trunk/stacks/navigation/tutorials/simple_navigation_goals/src/simple_navigation_goals.cpp 2009-08-20 02:39:16 UTC (rev 22394)
@@ -38,12 +38,12 @@
* http://pr.willowgarage.com/wiki/navigation/Tutorials/SendingSimpleGoals
*********************************************************************/
#include <ros/ros.h>
-#include <move_base/MoveBaseAction.h>
+#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <boost/thread.hpp>
-typedef actionlib::SimpleActionClient<move_base::MoveBaseAction> MoveBaseClient;
+typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
void spinThread(){
ros::spin();
@@ -61,7 +61,7 @@
//give some time for connections to register
sleep(2.0);
- move_base::MoveBaseGoal goal;
+ move_base_msgs::MoveBaseGoal goal;
//we'll send a goal to the robot to move 2 meters forward
goal.target_pose.header.frame_id = "base_link";
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X