Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18247] pkg/trunk/stacks
|
From: <tf...@us...> - 2009-07-03 01:04:32
|
Revision: 18247
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18247&view=rev
Author: tfoote
Date: 2009-07-03 01:04:31 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
real fix for #950 including removing work around hacks from rviz, tested with move_base_stage and rviz together
Modified Paths:
--------------
pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
pkg/trunk/stacks/geometry/tf/include/tf/tf.h
pkg/trunk/stacks/geometry/tf/src/tf.cpp
pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp
pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp
Modified: pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-07-03 01:04:31 UTC (rev 18247)
@@ -222,7 +222,7 @@
std::stringstream ss;
for (std::vector<std::string>::const_iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
{
- ss << *it << " ";
+ ss << tf::remap(tf_.getTFPrefix(), *it) << " ";
}
target_frames_string_ = ss.str();
}
Modified: pkg/trunk/stacks/geometry/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-07-03 01:04:31 UTC (rev 18247)
@@ -234,6 +234,11 @@
*/
boost::signals::connection addTransformChangedListener(boost::function<void(void)> callback);
+ /**
+ * \brief Get the tf_prefix this is running with
+ */
+ std::string getTFPrefix() const { return tf_prefix_;};
+
protected:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/stacks/geometry/tf/src/tf.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -45,15 +45,13 @@
if (frame_id.size() > 0)
if (frame_id[0] == '/')
{
- std::string stripped_frame_id = frame_id.substr(1,frame_id.length());
- return stripped_frame_id;
+ return frame_id;
}
if (prefix.size() > 0)
{
if (prefix[0] == '/')
{
- std::string stripped_prefix = prefix.substr(1,prefix.length());
- std::string composite = stripped_prefix;
+ std::string composite = prefix;
composite.append("/");
composite.append(frame_id);
return composite;
@@ -61,7 +59,8 @@
else
{
std::string composite;
- composite = prefix;
+ composite = "/";
+ composite.append(prefix);
composite.append("/");
composite.append(frame_id);
return composite;
@@ -70,7 +69,10 @@
}
else
{
- return frame_id;
+ std::string composite;
+ composite = "/";
+ composite.append(frame_id);
+ return composite;
}
};
@@ -115,46 +117,52 @@
bool Transformer::setTransform(const Stamped<btTransform>& transform, const std::string& authority)
{
+
+ Stamped<btTransform> mapped_transform = transform;
+ mapped_transform.frame_id_ = remap(tf_prefix_, transform.frame_id_);
+ mapped_transform.parent_id_ = remap(tf_prefix_, transform.parent_id_);
+
+
bool error_exists = false;
- if (transform.frame_id_ == transform.parent_id_)
+ if (mapped_transform.frame_id_ == mapped_transform.parent_id_)
{
- ROS_ERROR("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with parent_id and frame_id \"%s\" because they are the same", authority.c_str(), transform.frame_id_.c_str());
+ ROS_ERROR("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with parent_id and frame_id \"%s\" because they are the same", authority.c_str(), mapped_transform.frame_id_.c_str());
error_exists = true;
}
- if (transform.frame_id_ == "")
+ if (mapped_transform.frame_id_ == "/")//empty frame id will be mapped to "/"
{
ROS_ERROR("TF_NO_FRAME_ID: Ignoring transform from authority \"%s\" because frame_id not set ", authority.c_str());
error_exists = true;
}
- if (transform.parent_id_ == "")
+ if (mapped_transform.parent_id_ == "/")//empty parent id will be mapped to "/"
{
- ROS_ERROR("TF_NO_PARENT_ID: Ignoring transform with frame_id \"%s\" from authority \"%s\" because parent_id not set", transform.frame_id_.c_str(), authority.c_str());
+ ROS_ERROR("TF_NO_PARENT_ID: Ignoring transform with frame_id \"%s\" from authority \"%s\" because parent_id not set", mapped_transform.frame_id_.c_str(), authority.c_str());
error_exists = true;
}
- if (std::isnan(transform.getOrigin().x()) || std::isnan(transform.getOrigin().y()) || std::isnan(transform.getOrigin().z())||
- std::isnan(transform.getRotation().x()) || std::isnan(transform.getRotation().y()) || std::isnan(transform.getRotation().z()) || std::isnan(transform.getRotation().w()))
+ if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
+ std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
{
ROS_ERROR("TF_NAN_INPUT: Ignoring transform for frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
- transform.frame_id_.c_str(), authority.c_str(),
- transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z(),
- transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()
+ mapped_transform.frame_id_.c_str(), authority.c_str(),
+ mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
+ mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
);
error_exists = true;
}
if (error_exists)
return false;
- unsigned int frame_number = lookupOrInsertFrameNumber(transform.frame_id_);
- if (getFrame(frame_number)->insertData(TransformStorage(transform, lookupOrInsertFrameNumber(transform.parent_id_))))
+ unsigned int frame_number = lookupOrInsertFrameNumber(mapped_transform.frame_id_);
+ if (getFrame(frame_number)->insertData(TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.parent_id_))))
{
frame_authority_[frame_number] = authority;
}
else
{
- ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", transform.frame_id_.c_str(), transform.stamp_.toSec(), authority.c_str());
+ ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", mapped_transform.frame_id_.c_str(), mapped_transform.stamp_.toSec(), authority.c_str());
return false;
}
@@ -340,11 +348,11 @@
bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
{
-
+ std::string mapped_frame_id = tf::remap(tf_prefix_, frame_id);
tf::TimeCache* cache;
try
{
- cache = getFrame(lookupFrameNumber(frame_id));
+ cache = getFrame(lookupFrameNumber(mapped_frame_id));
}
catch (tf::LookupException &ex)
{
@@ -366,12 +374,15 @@
int Transformer::getLatestCommonTime(const std::string& source, const std::string& dest, ros::Time & time, std::string * error_string) const
{
+ std::string mapped_source = tf::remap(tf_prefix_, source);
+ std::string mapped_dest = tf::remap(tf_prefix_, dest);
+
time = ros::Time(UINT_MAX, 999999999);///\todo replace with ros::TIME_MAX when it is merged from stable
int retval;
TransformLists lists;
try
{
- retval = lookupLists(lookupFrameNumber(dest), ros::Time(), lookupFrameNumber(source), lists, error_string);
+ retval = lookupLists(lookupFrameNumber(mapped_dest), ros::Time(), lookupFrameNumber(mapped_source), lists, error_string);
}
catch (tf::LookupException &ex)
{
@@ -381,7 +392,7 @@
}
if (retval == NO_ERROR)
{
- //Set time to latest timestamp of frameid in case of target and source frame id are the same
+ //Set time to latest timestamp of frameid in case of target and mapped_source frame id are the same
if (lists.inverseTransforms.size() == 0 && lists.forwardTransforms.size() == 0)
{
time = ros::Time::now();
@@ -923,7 +934,7 @@
// for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
for (unsigned int counter = 1; counter < frames_.size(); counter ++)
{
- vec.push_back(std::string("/") + frameIDs_reverse[counter]);
+ vec.push_back(frameIDs_reverse[counter]);
}
return;
}
Modified: pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -270,7 +270,7 @@
Counter<robot_msgs::PointStamped> c("test_message", 20);
- ros::Duration().fromSec(0.2).sleep();
+ ros::Duration().fromSec(0.5).sleep();
ros::Time stamp = ros::Time::now();
@@ -456,7 +456,7 @@
xt.sec += 10;
boost::timed_mutex::scoped_timed_lock lock(n2.mutex_, xt);
-
+ printf("HHHHHHHHHHHHHHHEEEEEEEEEEEEEEEEEEEERRRRRRRRRRRRRRRRREEEEEEEEEEEEEEEEEEE %s\n", notifier2->getTargetFramesString().c_str());
EXPECT_EQ(true, lock.owns_lock());
}
Modified: pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -819,7 +819,7 @@
for (uint64_t i = 0; i < children.size(); i++)
{
EXPECT_TRUE(mTR.getParent(children[i], ros::Time().fromNSec(10), output));
- EXPECT_STREQ(parents[i].c_str(), output.c_str());
+ EXPECT_STREQ(tf::remap("",parents[i]).c_str(), output.c_str());
}
EXPECT_FALSE(mTR.getParent("j", ros::Time().fromNSec(10), output));
@@ -1270,38 +1270,39 @@
{
Transformer mTR;
- EXPECT_FALSE(mTR.frameExists("b"));;
- EXPECT_FALSE(mTR.frameExists("parent"));
- EXPECT_FALSE(mTR.frameExists("other"));
- EXPECT_FALSE(mTR.frameExists("frame"));
- mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "b", "parent"));
+ EXPECT_FALSE(mTR.frameExists("/b"));;
+ EXPECT_FALSE(mTR.frameExists("/parent"));
+ EXPECT_FALSE(mTR.frameExists("/other"));
+ EXPECT_FALSE(mTR.frameExists("/frame"));
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/b", "/parent"));
- EXPECT_TRUE(mTR.frameExists("b"));
- EXPECT_TRUE(mTR.frameExists("parent"));
- EXPECT_FALSE(mTR.frameExists("other"));
- EXPECT_FALSE(mTR.frameExists("frame"));
+ EXPECT_TRUE(mTR.frameExists("/b"));
+ EXPECT_TRUE(mTR.frameExists("/parent"));
+ EXPECT_FALSE(mTR.frameExists("/other"));
+ EXPECT_FALSE(mTR.frameExists("/frame"));
- mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "other", "frame"));
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/other", "/frame"));
- EXPECT_TRUE(mTR.frameExists("b"));
- EXPECT_TRUE(mTR.frameExists("parent"));
- EXPECT_TRUE(mTR.frameExists("other"));
- EXPECT_TRUE(mTR.frameExists("frame"));
+ EXPECT_TRUE(mTR.frameExists("/b"));
+ EXPECT_TRUE(mTR.frameExists("/parent"));
+ EXPECT_TRUE(mTR.frameExists("/other"));
+ EXPECT_TRUE(mTR.frameExists("/frame"));
}
+
TEST(tf, remap)
{
//no prefix
- EXPECT_STREQ("id", tf::remap("","id").c_str());
+ EXPECT_STREQ("/id", tf::remap("","id").c_str());
//prefix w/o /
- EXPECT_STREQ("asdf/id", tf::remap("asdf","id").c_str());
+ EXPECT_STREQ("/asdf/id", tf::remap("asdf","id").c_str());
//prefix w /
- EXPECT_STREQ("asdf/id", tf::remap("/asdf","id").c_str());
+ EXPECT_STREQ("/asdf/id", tf::remap("/asdf","id").c_str());
// frame_id w / -> no prefix
- EXPECT_STREQ("id", tf::remap("asdf","/id").c_str());
+ EXPECT_STREQ("/id", tf::remap("asdf","/id").c_str());
// frame_id w / -> no prefix
- EXPECT_STREQ("id", tf::remap("/asdf","/id").c_str());
+ EXPECT_STREQ("/id", tf::remap("/asdf","/id").c_str());
}
@@ -1580,6 +1581,31 @@
}
+
+TEST(tf, getFrameStrings)
+{
+ Transformer mTR;
+
+
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/b", "/parent"));
+ std::vector <std::string> frames_string;
+ mTR.getFrameStrings(frames_string);
+ ASSERT_EQ(frames_string.size(), 2);
+ EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
+ EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
+
+
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/other", "/frame"));
+
+ mTR.getFrameStrings(frames_string);
+ ASSERT_EQ(frames_string.size(), 4);
+ EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
+ EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
+ EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
+ EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
+
+}
+
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -231,10 +231,8 @@
M_NameToLink::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
{
- ///@todo TODO: REMOVE THIS HACK
- const std::string& name = "/" + link_it->first;
- ///end HACK
-
+ const std::string& name = link_it->first;
+
RobotLink* info = link_it->second;
if ( std::find( frames.begin(), frames.end(), name ) == frames.end() )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
