Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3240] pkg/trunk
|
From: <tf...@us...> - 2008-08-19 06:19:12
|
Revision: 3240
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3240&view=rev
Author: tfoote
Date: 2008-08-19 06:19:18 +0000 (Tue, 19 Aug 2008)
Log Message:
-----------
Converting libTF to use strings natively. There is no more need for namelookup.
r6920@zug: tfoote | 2008-08-18 12:59:48 -0700
creating branch for libTF change to names
r6941@zug: tfoote | 2008-08-18 15:54:15 -0700
libTF compiling with new API
r6948@zug: tfoote | 2008-08-18 16:50:14 -0700
testServer and testClient now talk
r6979@zug: tfoote | 2008-08-18 17:38:04 -0700
2dnav-stage running with new API
r6982@zug: tfoote | 2008-08-18 17:43:40 -0700
removing unnecessary namelookup server
r6985@zug: tfoote | 2008-08-18 18:07:39 -0700
2dnav-stage and 2dnav-gazebo running sucessfully
r6986@zug: tfoote | 2008-08-18 18:15:19 -0700
removing lookup from exec_trex
r6987@zug: tfoote | 2008-08-18 18:18:34 -0700
added needed std_srvs
r6999@zug: tfoote | 2008-08-18 20:04:11 -0700
removing name lookup
r7000@zug: tfoote | 2008-08-18 20:06:04 -0700
no longer depending on namelookup
r7001@zug: tfoote | 2008-08-18 20:09:03 -0700
no more namelookup
r7002@zug: tfoote | 2008-08-18 20:11:47 -0700
no more tf client in Ros_Camera
r7015@zug: tfoote | 2008-08-18 23:06:00 -0700
exec_trex compiling until it hits java
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-test.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/drivers/cam/dc1394_cam/manifest.xml
pkg/trunk/drivers/cam/dc1394_cam/src/dc1394_node/dc1394_node.cpp
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc
pkg/trunk/exec_trex/ROSNode.cc
pkg/trunk/exec_trex/manifest.xml
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/libTF/include/libTF/libTF.h
pkg/trunk/util/libTF/src/libTF.cpp
pkg/trunk/util/libTF/src/test/main.cpp
pkg/trunk/util/libTF/src/test/test_interp.cc
pkg/trunk/util/libTF/src/test/testtf.cc
pkg/trunk/util/rosTF/CMakeLists.txt
pkg/trunk/util/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/rosTF/manifest.xml
pkg/trunk/util/rosTF/msg/TransformDH.msg
pkg/trunk/util/rosTF/msg/TransformEuler.msg
pkg/trunk/util/rosTF/msg/TransformMatrix.msg
pkg/trunk/util/rosTF/msg/TransformQuaternion.msg
pkg/trunk/util/rosTF/src/rosTF.cpp
pkg/trunk/util/rosTF/src/testClient.cc
pkg/trunk/util/rosTF/src/testServer.cc
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
<launch>
<group name="wg">
- <node pkg="namelookup" type="namelookup_server" respawn="false"/>
<include file="$(find wg_robot_description)/send.xml"/>
<node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-test.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-test.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-test.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
<launch>
<group name="wg">
- <node pkg="namelookup" type="namelookup_server" respawn="false"/>
<include file="$(find wg_robot_description)/send.xml"/>
<node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_test.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
<launch>
<group name="wg">
- <node pkg="namelookup" type="namelookup_server" respawn="false"/>
<include file="$(find wg_robot_description)/send.xml"/>
<node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo_wg.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
<launch>
<group name="wg">
- <node pkg="namelookup" type="namelookup_server" respawn="false"/>
<include file="$(find wg_robot_description)/send.xml"/>
<node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
Modified: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,7 +1,6 @@
<launch>
<group name="wg">
- <node pkg="namelookup" type="namelookup_server" respawn="true"/>
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.png 0.1" respawn="false" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
Modified: pkg/trunk/drivers/cam/dc1394_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam/manifest.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/cam/dc1394_cam/manifest.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -6,7 +6,6 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="libdc1394v2"/>
- <depend package="namelookup"/>
<depend package="sdl"/>
<depend package="opencv"/>
<export>
Modified: pkg/trunk/drivers/cam/dc1394_cam/src/dc1394_node/dc1394_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam/src/dc1394_node/dc1394_node.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/cam/dc1394_cam/src/dc1394_node/dc1394_node.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -41,7 +41,6 @@
#include "std_msgs/Empty.h"
#include "std_msgs/String.h"
#include "dc1394_cam/dc1394_cam.h"
-#include <namelookup/nameLookupClient.hh>
#include <newmat10/newmat.h>
#include <newmat10/newmatio.h>
@@ -149,14 +148,14 @@
}
- Dc1394Node() : ros::node("dc1394_node"), nlc(*this)
+ Dc1394Node() : ros::node("dc1394_node")
{
dc1394_cam::init();
int numCams;
param("numCams", numCams, 1);
- videre_frame_id_ = nlc.lookup("FRAMEID_STEREO_BLOCK");
+ videre_frame_id_ = "FRAMEID_STEREO_BLOCK";
for (int i = 0; i < numCams; i++)
{
@@ -697,8 +696,7 @@
private:
- nameLookupClient nlc;
- int videre_frame_id_;
+ std::string videre_frame_id_;
};
int main(int argc, char **argv)
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -108,7 +108,6 @@
#include "std_msgs/LaserScan.h"
#include "robot_srvs/SelfTest.h"
-#include "namelookup/nameLookupClient.hh"
#include "urg_laser.h"
@@ -125,7 +124,6 @@
int count;
ros::Time next_time;
- nameLookupClient lookup_client;
public:
URG::laser urg;
@@ -146,7 +144,7 @@
string id;
- HokuyoNode() : ros::node("urglaser"), running(false), count(0), lookup_client(*this)
+ HokuyoNode() : ros::node("urglaser"), running(false), count(0)
{
advertise<std_msgs::LaserScan>("scan");
advertise_service("~self_test", &HokuyoNode::SelfTest);
@@ -264,7 +262,7 @@
scan_msg.set_ranges_size(scan.num_readings);
scan_msg.set_intensities_size(scan.num_readings);
scan_msg.header.stamp = ros::Time(scan.system_time_stamp);
- scan_msg.header.frame_id = lookup_client.lookup(frameid);
+ scan_msg.header.frame_id = frameid;
for(int i = 0; i < scan.num_readings; ++i)
{
Modified: pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -6,7 +6,6 @@
<depend package="std_msgs" />
<depend package="robot_srvs" />
<depend package="urg_driver" />
- <depend package="namelookup" />
<depend package="rosthread" />
<url>http://pr.willowgarage.com/wiki/Hokuyo_TOP-URG</url>
</package>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -5,9 +5,7 @@
</description>
<author>Morgan Quigley</author>
<license>BSD</license>
- <depend package="namelookup"/>
<depend package="sicktoolbox"/>
<depend package="roscpp"/>
<depend package="std_msgs"/>
- <depend package="rosTF"/>
</package>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -33,8 +33,6 @@
#include <sicklms-1.0/SickLMS.hh>
#include "ros/node.h"
#include "std_msgs/LaserScan.h"
-#include "rosTF/rosTF.h"
-#include "namelookup/nameLookupClient.hh"
using namespace SickToolbox;
using namespace std;
@@ -44,7 +42,7 @@
got_ctrlc = true;
}
-class SickNode : public ros::node, public nameLookupClient
+class SickNode : public ros::node
{
public:
std_msgs::LaserScan scan_msg;
@@ -52,9 +50,9 @@
string port;
int baud;
double last_print_time;
- SickNode() : ros::node("sicklms"), scan_count(0), last_print_time(0), nameLookupClient(*(ros::node*)this)
+ SickNode() : ros::node("sicklms"), scan_count(0), last_print_time(0)
{
- scan_msg.header.frame_id = this->lookup("FRAMEID_LASER");
+ scan_msg.header.frame_id = "FRAMEID_LASER";
advertise<std_msgs::LaserScan>("scan");
param("sicklms/port", port, string("/dev/ttyUSB1"));
param("sicklms/baud", baud, 500000);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -42,7 +42,6 @@
#include "MonoCameraSensor.hh"
using namespace gazebo;
-using namespace libTF;
GZ_REGISTER_DYNAMIC_CONTROLLER("ros_camera", Ros_Camera);
@@ -71,7 +70,6 @@
rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
printf("-------------------- starting node in camera \n");
}
- tfc = new rosTFClient(*rosnode); //, true, 1 * 1000000000ULL, 0ULL);
}
////////////////////////////////////////////////////////////////////////////////
@@ -173,7 +171,7 @@
this->lock.lock();
// copy data into image
- this->imageMsg.header.frame_id = tfc->nameClient.lookup(this->frameName);
+ this->imageMsg.header.frame_id = this->frameName;
this->imageMsg.header.stamp.sec = (unsigned long)floor(this->cameraIface->data->head.time);
this->imageMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraIface->data->head.time - this->imageMsg.header.stamp.sec) );
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -174,7 +174,7 @@
/* */
/***************************************************************/
this->lock.lock();
- this->laserMsg.header.frame_id = tfc->nameClient.lookup(this->frameName);
+ this->laserMsg.header.frame_id = this->frameName;
this->laserMsg.header.stamp.sec = (unsigned long)floor(this->laserIface->data->head.time);
this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserIface->data->head.time - this->laserMsg.header.stamp.sec) );
Modified: pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc
===================================================================
--- pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -56,9 +56,9 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = rni->tf.lookup("FRAMEID_ARM_R_SHOULDER");
+ aPose.frame = "FRAMEID_ARM_R_SHOULDER";
- libTF::TFPose inShoulderFrame = rni->tf.transformPose(rni->tf.lookup("FRAMEID_ODOM"), aPose);
+ libTF::TFPose inShoulderFrame = rni->tf.transformPose("FRAMEID_ODOM", aPose);
//std::cout << "In shoulder frame in base x " << inShoulderFrame.x << std::endl;
//std::cout << "In shoulder frame in base y " << inShoulderFrame.y << std::endl;
@@ -79,9 +79,9 @@
bPose.pitch = 0;
bPose.yaw = 0;
bPose.time = 0;
- bPose.frame = rni->tf.lookup("FRAMEID_ARM_R_HAND");
+ bPose.frame = "FRAMEID_ARM_R_HAND";
- libTF::TFPose inHandFrame = rni->tf.transformPose(rni->tf.lookup("FRAMEID_ODOM"), bPose);
+ libTF::TFPose inHandFrame = rni->tf.transformPose("FRAMEID_ODOM", bPose);
//std::cout << "In hand frame in base x " << inHandFrame.x << std::endl;
//std::cout << "In hand frame in base y " << inHandFrame.y << std::endl;
Modified: pkg/trunk/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/exec_trex/ROSNode.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/exec_trex/ROSNode.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -75,8 +75,8 @@
//copied from wavefront_planner.cc
//TODO change this to broadcast.
- this->tf.setWithEulers(tf.nameClient.lookup("FRAMEID_LASER"),
- tf.nameClient.lookup("FRAMEID_ROBOT"),
+ this->tf.setWithEulers("FRAMEID_LASER",
+ "FRAMEID_ROBOT",
0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
this->laser_hitpts_size = this->laser_hitpts_len = 0;
@@ -464,7 +464,7 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = tf.nameClient.lookup("FRAMEID_ROBOT");
+ robotPose.frame = "FRAMEID_ROBOT";
robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else,
try {
Modified: pkg/trunk/exec_trex/manifest.xml
===================================================================
--- pkg/trunk/exec_trex/manifest.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/exec_trex/manifest.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -4,6 +4,7 @@
<license>BSD</license>
<depend package="player" />
<depend package="std_msgs" />
+ <depend package="std_srvs" />
<depend package="pr2_msgs" />
<depend package="trex" />
<depend package="rosTF" />
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -393,7 +393,7 @@
localizedOdomMsg.header.stamp.from_double(hdr->timestamp);
try
{
- localizedOdomMsg.header.frame_id = tf->lookup("FRAMEID_MAP");
+ localizedOdomMsg.header.frame_id = "FRAMEID_MAP";
}
catch(...)
{
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -138,7 +138,7 @@
m_currentOdom.pos.x += m_iniPos.x;
m_currentOdom.pos.y += m_iniPos.y;
m_currentOdom.pos.th = math_utils::normalize_angle(m_currentOdom.pos.th + m_iniPos.th);
- m_currentOdom.header.frame_id = m_tfServer->nameClient.lookup("FRAMEID_MAP");
+ m_currentOdom.header.frame_id = "FRAMEID_MAP";
m_tfServer->sendEuler("FRAMEID_ROBOT",
"FRAMEID_MAP",
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -321,7 +321,7 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = tf.lookup("FRAMEID_ROBOT");
+ robotPose.frame = "FRAMEID_ROBOT";
robotPose.time = 0;
libTF::TFPose2D mapPose = tf.transformPose2D("FRAMEID_MAP", robotPose);
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -371,8 +371,8 @@
// Static robot->laser transform
double laser_x_offset;
param("laser_x_offset", laser_x_offset, 0.05);
- this->tf.setWithEulers(tf.nameClient.lookup("FRAMEID_LASER"),
- tf.nameClient.lookup("FRAMEID_ROBOT"),
+ this->tf.setWithEulers("FRAMEID_LASER",
+ "FRAMEID_ROBOT",
laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
advertise<std_msgs::Planner2DState>("state");
@@ -573,7 +573,7 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = tf.nameClient.lookup("FRAMEID_ROBOT");
+ robotPose.frame = "FRAMEID_ROBOT";
robotPose.time = 0; // request most recent pose
//robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -678,7 +678,7 @@
// this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
- this->odomMsg.header.frame_id = tf.nameClient.lookup("FRAMEID_ODOM");
+ this->odomMsg.header.frame_id = "FRAMEID_ODOM";
this->odomMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->odomMsg.header.stamp.sec) );
Modified: pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -27,7 +27,7 @@
robotPose.roll = 0;
robotPose.time = 0;
try {
- robotPose.frame = tf.lookup("FRAMEID_ROBOT");
+ robotPose.frame = "FRAMEID_ROBOT";
} catch(libTF::TransformReference::LookupException& ex) {
std::cerr << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
std::cout << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -241,7 +241,7 @@
}
// TODO: get the frame ID from somewhere
- this->laserMsg.header.frame_id = tf.lookup("FRAMEID_LASER");
+ this->laserMsg.header.frame_id = "FRAMEID_LASER";
this->laserMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
//this->laserMsg.header.stamp.sec =
//(unsigned long)floor(world->SimTimeNow() / 1e6);
@@ -262,7 +262,7 @@
this->odomMsg.vel.th = v.a;
this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
- this->odomMsg.header.frame_id = tf.lookup("FRAMEID_ODOM");
+ this->odomMsg.header.frame_id = "FRAMEID_ODOM";
this->odomMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
//this->odomMsg.header.stamp.sec =
Modified: pkg/trunk/util/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-19 06:19:18 UTC (rev 3240)
@@ -58,7 +58,7 @@
{
double x,y,z;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** ** Point2D ****
@@ -72,7 +72,7 @@
{
double x,y;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
@@ -83,7 +83,7 @@
{
double x,y,z;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** TFVector2D
@@ -94,7 +94,7 @@
{
double x,y;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** TFEulerYPR
@@ -105,7 +105,7 @@
{
double yaw, pitch, roll;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** TFYaw
@@ -115,7 +115,7 @@
{
double yaw;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** TFPose
@@ -125,7 +125,7 @@
{
double x,y,z,yaw,pitch,roll;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
/** TFPose2D
@@ -135,7 +135,7 @@
{
double x,y,yaw;
unsigned long long time;
- unsigned int frame;
+ std::string frame;
};
@@ -151,7 +151,7 @@
*
* Internal Representation
* libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame.
- * Frames are designated using an unsigned int
+ * Frames are designated using an std::string
* 0 is a frame without a parent (the top of a tree)
* The positions of frames over time must be pushed in.
*
@@ -190,7 +190,7 @@
*
* Possible exceptions are: TransformReference::InvaildFrame
*/
- void addFrame(unsigned int frameid, unsigned int parentid);
+ void addFrame(std::string frameid, std::string parentid);
/** \brief Set a new frame or update an old one.
* \param frameid The destination frame
@@ -208,22 +208,22 @@
*
* Possible exceptions are: TransformReference::LookupException
*/
- void setWithEulers(unsigned int frameid, unsigned int parentid, double x, double y, double z, double yaw, double pitch, double roll, ULLtime time);
+ void setWithEulers(std::string frameid, std::string parentid, double x, double y, double z, double yaw, double pitch, double roll, ULLtime time);
/** \brief Set a transform using DH Parameters
* Conventions from http://en.wikipedia.org/wiki/Robotics_conventions
* Possible exceptions are: TransformReference::LookupException
*/
- void setWithDH(unsigned int frameid, unsigned int parentid, double length, double alpha, double offset, double theta, ULLtime time);
+ void setWithDH(std::string frameid, std::string parentid, double length, double alpha, double offset, double theta, ULLtime time);
/** \brief Set the transform using a matrix
* Possible exceptions are: TransformReference::LookupException
*/
- void setWithMatrix(unsigned int frameid, unsigned int parentid, const NEWMAT::Matrix & matrix_in, ULLtime time);
+ void setWithMatrix(std::string frameid, std::string parentid, const NEWMAT::Matrix & matrix_in, ULLtime time);
/** \brief Set the transform using quaternions natively
* Possible exceptions are: TransformReference::LookupException
*/
- void setWithQuaternion(unsigned int frameid, unsigned int parentid, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime time);
+ void setWithQuaternion(std::string frameid, std::string parentid, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime time);
/*********** Accessors *************/
@@ -236,31 +236,31 @@
* Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
- NEWMAT::Matrix getMatrix(unsigned int target_frame, unsigned int source_frame, ULLtime time);
+ NEWMAT::Matrix getMatrix(std::string target_frame, std::string source_frame, ULLtime time);
/** \brief Transform a point to a different frame */
- TFPoint transformPoint(unsigned int target_frame, const TFPoint & point_in);
+ TFPoint transformPoint(std::string target_frame, const TFPoint & point_in);
/** \brief Transform a 2D point to a different frame */
- TFPoint2D transformPoint2D(unsigned int target_frame, const TFPoint2D & point_in);
+ TFPoint2D transformPoint2D(std::string target_frame, const TFPoint2D & point_in);
/** \brief Transform a vector to a different frame */
- TFVector transformVector(unsigned int target_frame, const TFVector & vector_in);
+ TFVector transformVector(std::string target_frame, const TFVector & vector_in);
/** \brief Transform a 2D vector to a different frame */
- TFVector2D transformVector2D(unsigned int target_frame, const TFVector2D & vector_in);
+ TFVector2D transformVector2D(std::string target_frame, const TFVector2D & vector_in);
/** \brief Transform Euler angles between frames */
- TFEulerYPR transformEulerYPR(unsigned int target_frame, const TFEulerYPR & euler_in);
+ TFEulerYPR transformEulerYPR(std::string target_frame, const TFEulerYPR & euler_in);
/** \brief Transform Yaw between frames. Useful for 2D navigation */
- TFYaw transformYaw(unsigned int target_frame, const TFYaw & euler_in);
+ TFYaw transformYaw(std::string target_frame, const TFYaw & euler_in);
/** \brief Transform a 6DOF pose. (x, y, z, yaw, pitch, roll). */
- TFPose transformPose(unsigned int target_frame, const TFPose & pose_in);
+ TFPose transformPose(std::string target_frame, const TFPose & pose_in);
/** \brief Transform a planar pose, x,y,yaw */
- TFPose2D transformPose2D(unsigned int target_frame, const TFPose2D & pose_in);
+ TFPose2D transformPose2D(std::string target_frame, const TFPose2D & pose_in);
/** \brief Debugging function that will print the spanning chain of transforms.
* Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
* TransformReference::MaxDepthException
*/
- std::string viewChain(unsigned int target_frame, unsigned int source_frame);
+ std::string viewChain(std::string target_frame, std::string source_frame);
/** \brief A way to see what frames have been cached
* Useful for debugging
@@ -369,6 +369,11 @@
/// How long to cache transform history
ULLtime cache_time;
+ /// Map for storage of name
+ std::map<std::string, unsigned int> nameMap;
+ std::map<unsigned int, std::string> reverseMap;
+ unsigned int last_number; ///\todo init to zero
+
/// whether or not to interpolate or extrapolate
bool interpolating;
@@ -395,7 +400,12 @@
* Possible Exception: TransformReference::LookupException
*/
inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) { std::stringstream ss; ss << "Frame " << frame_number << " does not exist."; throw LookupException(ss.str());} else return frames[frame_number];};
+ inline RefFrame* getFrame(std::string frame_number_string) { unsigned int frame_number = nameToNumber(frame_number_string); if (frames[frame_number] == NULL) { std::stringstream ss; ss << "Frame " << frame_number << " does not exist."; throw LookupException(ss.str());} else return frames[frame_number];};
+
+ unsigned int nameToNumber(std::string frameid);
+ std::string numberToName(unsigned int frameid);
+
/** Find the list of connected frames necessary to connect two different frames */
TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
Modified: pkg/trunk/util/libTF/src/libTF.cpp
===================================================================
--- pkg/trunk/util/libTF/src/libTF.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/libTF/src/libTF.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -50,9 +50,12 @@
unsigned long long max_extrapolation_distance):
cache_time(cache_time),
interpolating (interpolating),
- max_extrapolation_distance(max_extrapolation_distance)
+ max_extrapolation_distance(max_extrapolation_distance),
+ last_number(1)
{
-
+ nameMap["NO_PARENT"] = 0; //Initialize NO_PARENT
+ reverseMap[0] = "NO_PARENT"; //Initialize NO_PARENT
+
frames = new RefFrame*[MAX_NUM_FRAMES];
assert(frames);
@@ -76,7 +79,12 @@
delete[] frames;
};
-void TransformReference::addFrame(unsigned int frameID, unsigned int parentID) {
+void TransformReference::addFrame(std::string frame_id, std::string parent_id) {
+ unsigned int frameID, parentID;
+ frameID = nameToNumber(frame_id);
+ parentID = nameToNumber(parent_id);
+
+
if (frameID >= MAX_NUM_FRAMES || parentID >= MAX_NUM_FRAMES || frameID == NO_PARENT)
{
std::stringstream ss;
@@ -94,14 +102,14 @@
}
-void TransformReference::setWithEulers(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f, ULLtime time)
+void TransformReference::setWithEulers(std::string frameID, std::string parentID, double a,double b,double c,double d,double e,double f, ULLtime time)
{
addFrame(frameID, parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
}
-void TransformReference::setWithDH(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d, ULLtime time)
+void TransformReference::setWithDH(std::string frameID, std::string parentID, double a,double b,double c,double d, ULLtime time)
{
addFrame(frameID, parentID);
@@ -109,7 +117,7 @@
}
-void TransformReference::setWithMatrix(unsigned int frameID, unsigned int parentID, const NEWMAT::Matrix & matrix_in, ULLtime time)
+void TransformReference::setWithMatrix(std::string frameID, std::string parentID, const NEWMAT::Matrix & matrix_in, ULLtime time)
{
addFrame(frameID, parentID);
@@ -117,7 +125,7 @@
}
-void TransformReference::setWithQuaternion(unsigned int frameID, unsigned int parentID, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime time)
+void TransformReference::setWithQuaternion(std::string frameID, std::string parentID, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime time)
{
addFrame(frameID, parentID);
@@ -127,8 +135,12 @@
-NEWMAT::Matrix TransformReference::getMatrix(unsigned int target_frame, unsigned int source_frame, ULLtime time)
+NEWMAT::Matrix TransformReference::getMatrix(std::string target_frame_string, std::string source_frame_string, ULLtime time)
{
+ unsigned int target_frame, source_frame;
+ target_frame = nameToNumber(target_frame_string);
+ source_frame = nameToNumber(source_frame_string);
+
NEWMAT::Matrix myMat(4,4);
TransformLists lists = lookUpList(target_frame, source_frame);
myMat = computeTransformFromList(lists,time);
@@ -138,7 +150,7 @@
-TFPoint TransformReference::transformPoint(unsigned int target_frame, const TFPoint & point_in)
+TFPoint TransformReference::transformPoint(std::string target_frame, const TFPoint & point_in)
{
//Create a vector
NEWMAT::Matrix pointMat(4,1);
@@ -156,7 +168,7 @@
retPoint.time = point_in.time;
return retPoint;
}
-TFPoint2D TransformReference::transformPoint2D(unsigned int target_frame, const TFPoint2D & point_in)
+TFPoint2D TransformReference::transformPoint2D(std::string target_frame, const TFPoint2D & point_in)
{
//Create a vector
NEWMAT::Matrix pointMat(4,1);
@@ -174,7 +186,7 @@
return retPoint;
}
-TFVector TransformReference::transformVector(unsigned int target_frame, const TFVector & vector_in)
+TFVector TransformReference::transformVector(std::string target_frame, const TFVector & vector_in)
{
//Create a vector
NEWMAT::Matrix vectorMat(4,1);
@@ -192,7 +204,7 @@
return retVector;
}
-TFVector2D TransformReference::transformVector2D(unsigned int target_frame, const TFVector2D & vector_in)
+TFVector2D TransformReference::transformVector2D(std::string target_frame, const TFVector2D & vector_in)
{
//Create a vector
NEWMAT::Matrix vectorMat(4,1);
@@ -211,7 +223,7 @@
return retVector;
}
-TFEulerYPR TransformReference::transformEulerYPR(unsigned int target_frame, const TFEulerYPR & euler_in)
+TFEulerYPR TransformReference::transformEulerYPR(std::string target_frame, const TFEulerYPR & euler_in)
{
NEWMAT::Matrix local = Pose3D::matrixFromEuler(0,0,0,euler_in.yaw, euler_in.pitch, euler_in.roll);
@@ -228,7 +240,7 @@
return retEuler;
}
-TFYaw TransformReference::transformYaw(unsigned int target_frame, const TFYaw & euler_in)
+TFYaw TransformReference::transformYaw(std::string target_frame, const TFYaw & euler_in)
{
TFVector2D vector_in;
vector_in.x = cos(euler_in.yaw);
@@ -244,7 +256,7 @@
return retYaw;
}
-TFPose TransformReference::transformPose(unsigned int target_frame, const TFPose & pose_in)
+TFPose TransformReference::transformPose(std::string target_frame, const TFPose & pose_in)
{
TFPoint point_in;
point_in.x = pose_in.x;
@@ -277,7 +289,7 @@
return pose_out;
}
-TFPose2D TransformReference::transformPose2D(unsigned int target_frame, const TFPose2D & pose_in)
+TFPose2D TransformReference::transformPose2D(std::string target_frame, const TFPose2D & pose_in)
{
TFPoint2D point_in;
point_in.x = pose_in.x;
@@ -383,6 +395,43 @@
}
+unsigned int TransformReference::nameToNumber(std::string frameid)
+{
+ std::map<std::string, unsigned int>::iterator it = nameMap.find(frameid);
+ if ( it == nameMap.end())
+ {
+ unsigned int value = last_number++;
+ nameMap[frameid] = value; //Record lookup
+ reverseMap[value] = frameid; //Record reverse lookup
+
+ return value;
+ }
+ else
+ {
+ return (*it).second;
+ }
+
+}
+
+
+std::string TransformReference::numberToName(unsigned int frameid)
+{
+ std::map<unsigned int, std::string>::iterator it = reverseMap.find(frameid);
+ if ( it == reverseMap.end())
+ {
+ std::stringstream ss;
+ ss << "Number " << frameid << " does not exist!!";
+ throw LookupException(ss.str());
+ return "";//never get here
+ }
+ else
+ {
+ return (*it).second;
+ }
+
+}
+
+
NEWMAT::Matrix TransformReference::computeTransformFromList(const TransformLists & lists, ULLtime time)
{
NEWMAT::Matrix retMat(4,4);
@@ -421,8 +470,11 @@
}
-std::string TransformReference::viewChain(unsigned int target_frame, unsigned int source_frame)
+std::string TransformReference::viewChain(std::string target_frame_string, std::string source_frame_string)
{
+ unsigned int target_frame, source_frame;
+ target_frame = nameToNumber(target_frame_string);
+ source_frame = nameToNumber(source_frame_string);
stringstream mstream;
TransformLists lists = lookUpList(target_frame, source_frame);
@@ -449,7 +501,7 @@
{
if (frames[frameid] != NULL)
{
- mstream << "Frame "<< frameid << " exists with parent " << frames[frameid]->getParent() << "." <<std::endl;
+ mstream << "Frame "<< numberToName(frameid) << " exists with parent " << numberToName(frames[frameid]->getParent()) << "." <<std::endl;
}
}
return mstream.str();
@@ -467,10 +519,10 @@
bool TransformReference::RefFrame::setParent(unsigned int parentID)
{
if (parent != parentID)
- {
- parent = parentID;
- clearList();
- return false;
- }
+ {
+ parent = parentID;
+ clearList();
+ return false;
+ }
return true;
};
Modified: pkg/trunk/util/libTF/src/test/main.cpp
===================================================================
--- pkg/trunk/util/libTF/src/test/main.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/libTF/src/test/main.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -57,27 +57,27 @@
//Fill in some transforms
// mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
- mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime+500);
- mTR.setWithEulers(2,3,1+100,1,1,dyaw,dp,dr,atime+1000);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime+1100);
- mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
+ mTR.setWithDH("10","2",1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers("2","3",1-1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-100);
+ mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-50);
+ mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime+500);
+ mTR.setWithEulers("2","3",1+100,1,1,dyaw,dp,dr,atime+1000);
+ mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime+1100);
+ mTR.setWithEulers("3","5",dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("5","1",dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("6","5",dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("6","5",dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("7","6",1,1,1,dyaw,dp,dr,atime);
+ mTR.setWithDH("8","7",1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers("8","7",1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
//Demonstrate InvalidFrame LookupException
try
{
- std::cout<< mTR.viewChain(10,9);
+ std::cout<< mTR.viewChain("10","9");
}
catch (TransformReference::LookupException &ex)
{
@@ -88,13 +88,13 @@
// See the list of transforms to get between the frames
std::cout<<"Viewing (10,8):"<<std::endl;
- std::cout << mTR.viewChain(10,8);
+ std::cout << mTR.viewChain("10","8");
//See the resultant transform
std::cout <<"Calling getMatrix(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.getMatrix(1,1);
- NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
+ NEWMAT::Matrix mat = mTR.getMatrix("10","8",atime);
std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
@@ -102,33 +102,33 @@
mPoint.x = 1;
mPoint.y = 1;
mPoint.z = 1;
- mPoint.frame = 10;
+ mPoint.frame = "10";
mPoint.time = atime;
TFPoint nPoint = mPoint;
std::cout <<"Point 1,1,1 goes like this:" <<std::endl;
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(2, mPoint);
+ mPoint = mTR.transformPoint("2", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(3, mPoint);
+ mPoint = mTR.transformPoint("3", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(5, mPoint);
+ mPoint = mTR.transformPoint("5", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(6, mPoint);
+ mPoint = mTR.transformPoint("6", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(7, mPoint);
+ mPoint = mTR.transformPoint("7", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(8, mPoint);
+ mPoint = mTR.transformPoint("8", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
- mPoint = mTR.transformPoint(10, mPoint);
+ mPoint = mTR.transformPoint("10", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
//Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("6","7",dx,dy,dz,dyaw,dp,dr,atime);
try {
- std::cout<<mTR.viewChain(10,8);
+ std::cout<<mTR.viewChain("10","8");
}
catch (TransformReference::MaxDepthException &ex)
{
@@ -136,10 +136,10 @@
}
//Break the graph, making it disconnected, and demonstrate catching ConnectivityException
- mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers("6","0",dx,dy,dz,dyaw,dp,dr,atime);
try {
- std::cout<<mTR.viewChain(10,8);
+ std::cout<<mTR.viewChain("10","8");
}
catch (TransformReference::ConnectivityException &ex)
{
@@ -147,7 +147,7 @@
}
//Testing clearing the history with parent change
- mTR.setWithEulers(7,5,1,1,1,dyaw,dp,dr,atime);
+ mTR.setWithEulers("7","5",1,1,1,dyaw,dp,dr,atime);
//todo display this somehow
@@ -162,17 +162,17 @@
pitch = 0.0;
roll = 0.0;
- mTR.setWithEulers(2,1,x,y,z,yaw,pitch,roll,atime);
+ mTR.setWithEulers("2","1",x,y,z,yaw,pitch,roll,atime);
libTF::TFPose2D in, out;
in.x = 0.0;
in.y = 0.0;
in.yaw = 0.0;
- in.frame = 2;
+ in.frame = "2";
in.time = atime;
- out = mTR.transformPose2D(1,in);
+ out = mTR.transformPose2D("1",in);
printf("%.3f %.3f %.3f\n",
out.x, out.y, out.yaw*180.0/M_PI);
@@ -180,7 +180,7 @@
try
{
- out = mTR.transformPose2D(0,in);
+ out = mTR.transformPose2D("0",in);
std::cout << "failed to throw" << std::endl;
}
catch (TransformReference::LookupException &ex)
@@ -193,10 +193,10 @@
ypr_in.pitch = 0;
ypr_in.roll = 0;
ypr_in.time = atime;
- ypr_in.frame = 1;
+ ypr_in.frame = "1";
std::cout <<"YPR in:"<< ypr_in.yaw<<","<<ypr_in.pitch <<"," <<ypr_in.roll<<std::endl;
- libTF::TFEulerYPR ypr_out = mTR.transformEulerYPR(2, ypr_in);
+ libTF::TFEulerYPR ypr_out = mTR.transformEulerYPR("2", ypr_in);
std::cout <<"YPR out:"<< ypr_out.yaw<<","<<ypr_out.pitch <<"," <<ypr_out.roll<<std::endl;
}
Modified: pkg/trunk/util/libTF/src/test/test_interp.cc
===================================================================
--- pkg/trunk/util/libTF/src/test/test_interp.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/libTF/src/test/test_interp.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -15,8 +15,8 @@
for(int i=0;i<10;i++)
{
- mTR.setWithEulers(2,
- 1,
+ mTR.setWithEulers("2",
+ "1",
i*0.2,
i*-0.1,
0.0,
@@ -25,8 +25,8 @@
0.0,
atime-i*1000000000ULL);
}
- mTR.setWithEulers(2,
- 1,
+ mTR.setWithEulers("2",
+ "1",
0.0,
0.0,
0.0,
@@ -35,8 +35,8 @@
0.0,
atime);
- mTR.setWithEulers(2,
- 1,
+ mTR.setWithEulers("2",
+ "1",
1.0,
1.0,
0.0,
@@ -50,10 +50,10 @@
inpose.x = 0.0;
inpose.y = 0.0;
inpose.yaw = 0.0;
- inpose.frame = 2;
+ inpose.frame = "2";
inpose.time = atime;
- libTF::TFPose2D outpose = mTR.transformPose2D(1, inpose);
+ libTF::TFPose2D outpose = mTR.transformPose2D("1", inpose);
printf("in: %.3f %.3f %.3f\n",
inpose.x, inpose.y, inpose.yaw);
Modified: pkg/trunk/util/libTF/src/test/testtf.cc
===================================================================
--- pkg/trunk/util/libTF/src/test/testtf.cc 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/libTF/src/test/testtf.cc 2008-08-19 06:19:18 UTC (rev 3240)
@@ -64,7 +64,7 @@
diffpose2.frame = 1;
diffpose2.time = atime;
- mTR.setWithEulers(2,
+ mTR.setWithEulers("2",
mappose.frame,
mappose.x,
mappose.y,
@@ -74,16 +74,16 @@
0.0,
atime);
- mTR.setWithMatrix(3,
- 2,
+ mTR.setWithMatrix("3",
+ "2",
Pose3D::matrixFromEuler(odompose.x, odompose.y, 0.0, odompose.yaw, 0.0, 0.0).i(),
atime);
// See the list of transforms to get between the frames
std::cout<<"Viewing (1,3):"<<std::endl;
- std::cout << mTR.viewChain(1,3);
+ std::cout << mTR.viewChain("1","3");
std::cout<<"Viewing (2,3):"<<std::endl;
- std::cout << mTR.viewChain(2,3);
+ std::cout << mTR.viewChain("2","3");
libTF::TFPose2D result = mTR.transformPose2D(mappose.frame,odompose);
@@ -113,9 +113,9 @@
printf("Point : %.3f %.3f %.3f\n",
point.x, point.y,point.z);
- std::cout<<"1,2:"<<std::endl<<mTR.getMatrix(1,2,atime);
- std::cout<<"2,3"<<std::endl<<mTR.getMatrix(2,3,atime);
- std::cout<<"1,3:"<<std::endl<<mTR.getMatrix(1,3,atime);
+ std::cout<<"1,2:"<<std::endl<<mTR.getMatrix("1","2",atime);
+ std::cout<<"2,3"<<std::endl<<mTR.getMatrix("2","3",atime);
+ std::cout<<"1,3:"<<std::endl<<mTR.getMatrix("1","3",atime);
NEWMAT::Matrix matPoint(4,1);
matPoint(1,1) = point_in.x;
@@ -144,7 +144,7 @@
ypr_in.roll = 0;
ypr_in.time = atime;
- libTF::TFEulerYPR ypr = mTR.transformEulerYPR(1, ypr_in);
+ libTF::TFEulerYPR ypr = mTR.transformEulerYPR("1", ypr_in);
printf("Ypr : %.3f %.3f %.3f\n",
ypr.yaw, ypr.pitch,ypr.roll);
@@ -156,7 +156,7 @@
vec2_in.frame = 2;
vec2_in.time = atime;
- TFVector2D vec2 = mTR.transformVector2D(1,vec2_in);
+ TFVector2D vec2 = mTR.transformVector2D("1",vec2_in);
printf("Vector : %.3f %.3f\n",
vec2.x, vec2.y);
@@ -166,21 +166,21 @@
xaxis.x = 1;
xaxis.y = 0;
xaxis.z = 0;
- xaxis.frame = 9;
+ xaxis.frame = "9";
xaxis.time = atime;
TFVector yaxis;
yaxis.x = 0;
yaxis.y = 1;
yaxis.z = 0;
- yaxis.frame = 9;
+ yaxis.frame = "9";
yaxis.time = atime;
TFVector zaxis;
zaxis.x = 0;
zaxis.y = 0;
zaxis.z = 1;
- zaxis.frame = 9;
+ zaxis.frame = "9";
zaxis.time = atime;
@@ -191,8 +191,8 @@
for (int ind2 = 0; ind2 < 2; ind2++)
{
- mTR.setWithEulers(9,
- 1,
+ mTR.setWithEulers("9",
+ "1",
diffpose.x,
diffpose.y,
diffpose.z,
@@ -206,17 +206,17 @@
90.0*ind, 90.0*ind1, 90.0*ind2);
- TFVector vec_out = mTR.transformVector(1,xaxis);
+ TFVector vec_out = mTR.transformVector("1",xaxis);
printf("X Axis rotated = ");
printf("Vector : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
- vec_out = mTR.transformVector(1,yaxis);
+ vec_out = mTR.transformVector("1",yaxis);
printf("Y Axis rotated = ");
printf("Vector : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
- vec_out = mTR.transformVector(1,zaxis);
+ vec_out = mTR.transformVector("1",zaxis);
printf("Z Axis rotated = ");
printf("Vector : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
@@ -230,21 +230,21 @@
xunit.x = 1;
xunit.y = 0;
xunit.z = 0;
- xunit.frame = 9;
+ xunit.frame = "9";
xunit.time = atime;
TFPoint yunit;
yunit.x = 0;
yunit.y = 1;
yunit.z = 0;
- yunit.frame = 9;
+ yunit.frame = "9";
yunit.time = atime;
TFPoint zunit;
zunit.x = 0;
zunit.y = 0;
zunit.z = 1;
- zunit.frame = 9;
+ zunit.frame = "9";
zunit.time = atime;
@@ -259,8 +259,8 @@
for (int ind2 = 0; ind2 < 2; ind2++)
{
- mTR.setWithEulers(9,
- 1,
+ mTR.setWithEulers("9",
+ "1",
diffpose.x,
diffpose.y,
diffpose.z,
@@ -274,17 +274,17 @@
90.0*ind, 90.0*ind1, 90.0*ind2);
- TFPoint vec_out = mTR.transformPoint(1,xunit);
+ TFPoint vec_out = mTR.transformPoint("1",xunit);
printf("X Unit rotated = ");
printf("Point : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
- vec_out = mTR.transformPoint(1,yunit);
+ vec_out = mTR.transformPoint("1",yunit);
printf("Y Unit rotated = ");
printf("Point : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
- vec_out = mTR.transformPoint(1,zunit);
+ vec_out = mTR.transformPoint("1",zunit);
printf("Z Unit rotated = ");
printf("Point : %.3f %.3f %.3f\n",
vec_out.x, vec_out.y, vec_out.z);
Modified: pkg/trunk/util/rosTF/CMakeLists.txt
===================================================================
--- pkg/trunk/util/rosTF/CMakeLists.txt 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/CMakeLists.txt 2008-08-19 06:19:18 UTC (rev 3240)
@@ -12,5 +12,5 @@
rospack_add_executable(frameServer src/frameServer.cc)
target_link_libraries(frameServer rosTF)
-rospack_add_executable(viewTF src/viewTF.cpp)
-target_link_libraries(viewTF rosTF)
+#rospack_add_executable(viewTF src/viewTF.cpp)
+#target_link_libraries(viewTF rosTF)
Modified: pkg/trunk/util/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/rosTF/include/rosTF/rosTF.h 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/include/rosTF/rosTF.h 2008-08-19 06:19:18 UTC (rev 3240)
@@ -45,8 +45,6 @@
#include "rosTF/TransformArray.h"
#include "libTF/libTF.h"
#include "std_msgs/PointCloudFloat32.h"
-#include "namelookup/nameLookupClient.hh"
-#include "namelookup/NameToNumber.h"
#include "laser_scan_utils/laser_scan.h"
@@ -85,7 +83,6 @@
using TransformReference::transformYaw;
using TransformReference::transformPose;
using TransformReference::transformPose2D;
- using TransformReference::viewChain;
/** \brief Get the transform between two frames by frame name
* \param target_frame The frame to which data should be transformed
@@ -98,38 +95,6 @@
NEWMAT::Matrix getMatrix(std::string target_frame, std::string source_frame, ros::Time time);
-
- /** \brief Transform a point to a different frame */
- libTF::TFPoint transformPoint(std::string target_frame, const libTF::TFPoint & point_in);
- /** \brief Transform a 2D point to a different frame */
- libTF::TFPoint2D transformPoint2D(std::string target_frame, const libTF::TFPoint2D & point_in);
- /** \brief Transform a vector to a different frame */
- libTF::TFVector transformVector(std::string target_frame, const libTF::TFVector & vector_in);
- /** \brief Transform a 2D vector to a different frame */
- libTF::TFVector2D transformVector2D(std::string target_frame, const libTF::TFVector2D & vector_in);
- /** \brief Transform Euler angles between frames */
- libTF::TFEulerYPR transformEulerYPR(std::string target_frame, const libTF::TFEulerYPR & euler_in);
- /** \brief Transform Yaw between frames. Useful for 2D navigation */
- libTF::TFYaw transformYaw(std::string target_frame, const libTF::TFYaw & euler_in);
- /** \brief Transform a 6DOF pose. (x, y, z, yaw, pitch, roll). */
- libTF::TFPose transformPose(std::string target_frame, const libTF::TFPose & pose_in);
- /** \brief Transform a planar pose, x,y,yaw */
- libTF::TFPose2D transformPose2D(std::string target_frame, const libTF::TFPose2D & pose_in);
-
- /** \brief Debugging function that will print the spanning chain of transforms.
- * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
- * TransformReference::MaxDepthException
- */
- std::string viewChain(std::string target_frame, std::string source_frame);
-
- nameLookupClient nameClient;
- unsigned int lookup(const std::string& name){std::cerr<<"Interface Depricated: use rosTFClient.nameClient.lookup(name) instead."; return nameClient.lookup(name);};
-
- /** \brief Return all frames and their lookups
- This will return a string which is a list of all frames and their parents.
- Viewing both the string and number representation of their frameid. */
- std::string viewNamedFrames();
-
private:
// A reference to the active ros::node to allow setting callbacks
ros::node & myNode;
@@ -188,16 +153,10 @@
/** \brief Send a Transform with 4x4 Matrix */
void sendMatrix(unsigned int frame, unsigned int parent, NEWMAT::Matrix matrix, ros::Time rostime);
- /** \brief A public member to provide name lookup service. */
- nameLookupClient nameClient;
- /** \brief THIS IS TEMPORARY use nameClient.lookup A pass through to prevent breaking the previous API. */
- unsigned int lookup(const std::string& name){std::cerr<<"Interface Depricated: use rosTFServer.nameClient.lookup(name) instead."; return nameClient.lookup(name);};
-
private:
//ros::node reference to allow setting callbacks
ros::node & myNode;
- bool checkInvalidFrame(unsigned int);
};
#endif //ROSTF_HH
Modified: pkg/trunk/util/rosTF/manifest.xml
===================================================================
--- pkg/trunk/util/rosTF/manifest.xml 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/manifest.xml 2008-08-19 06:19:18 UTC (rev 3240)
@@ -14,6 +14,5 @@
<depend package="roscpp"/>
<depend package="laser_scan_utils"/>
<depend package="std_msgs"/>
-<depend package="namelookup"/>
<depend package="newmat10"/>
</package>
Modified: pkg/trunk/util/rosTF/msg/TransformDH.msg
===================================================================
--- pkg/trunk/util/rosTF/msg/TransformDH.msg 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/msg/TransformDH.msg 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
Header header
-uint32 frame
-uint32 parent
+string parent
float64 length
float64 twist
float64 offset
Modified: pkg/trunk/util/rosTF/msg/TransformEuler.msg
===================================================================
--- pkg/trunk/util/rosTF/msg/TransformEuler.msg 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/msg/TransformEuler.msg 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
Header header
-uint32 frame
-uint32 parent
+string parent
float64 x
float64 y
float64 z
Modified: pkg/trunk/util/rosTF/msg/TransformMatrix.msg
===================================================================
--- pkg/trunk/util/rosTF/msg/TransformMatrix.msg 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/msg/TransformMatrix.msg 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,4 +1,3 @@
Header header
-uint32 frame
-uint32 parent
+string parent
float64[] matrix #always 16 elements a 4x4 matrix unwrapped as row major
\ No newline at end of file
Modified: pkg/trunk/util/rosTF/msg/TransformQuaternion.msg
===================================================================
--- pkg/trunk/util/rosTF/msg/TransformQuaternion.msg 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/msg/TransformQuaternion.msg 2008-08-19 06:19:18 UTC (rev 3240)
@@ -1,6 +1,5 @@
Header header
-uint32 frame
-uint32 parent
+string parent
float64 xt
float64 yt
float64 zt
Modified: pkg/trunk/util/rosTF/src/rosTF.cpp
===================================================================
--- pkg/trunk/util/rosTF/src/rosTF.cpp 2008-08-19 06:02:45 UTC (rev 3239)
+++ pkg/trunk/util/rosTF/src/rosTF.cpp 2008-08-19 06:19:18 UTC (rev 3240)
@@ -39,7 +39,6 @@
TransformReference(interpolating,
max_cache_time,
max_extrapolation_distance),
- nameClient(rosnode),
myNode(rosnode)
{
// printf("Constructed rosTF\n");
@@ -49,11 +48,6 @@
void rosTFClient::transformPointCloud(std::string target_frame, std_msgs::PointCloudFloat32 & cloudOut, const std_msgs::PointCloudFloat32 & cloudIn)
{
- transformPointCloud(nameClient.lookup(target_frame), cloudOut, cloudIn);
-}
-
-void rosTFClient::transformPointCloud(unsigned int target_frame, std_msgs::PointCloudFloat32 & cloudOut, const std_msgs::PointCloudFloat32 & cloudIn)
-{
NEWMAT::Matrix transform = TransformReference::getMatrix(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp.sec * 1000000000ULL + cloudIn.header.stamp.nsec);
unsigned int length = cloudIn.get_pts_size();
@@ -96,13 +90,6 @@
std_msgs::PointCloudFloat32 rosTFClient::transformPointCloud(std::string target_frame, const std_msgs::PointCloudFloat32 & cloudIn)
{
- return transformPointCloud(nameClient.lookup(target_frame), cloudIn);
-};
-
-
-//PointCloudFloat32 rosTFClient::transformPointCloud(unsigned int target_frame, const std_msgs::PointCloudFloat32 & cloudIn) //todo add back const when get_pts_size() is const ticket:232
-std_msgs::PointCloudFloat32 rosTFClient::transformPointCloud(unsigned int target_frame, const std_msgs::PointCloudFloat32 & cloudIn)
-{
std_msgs::PointCloudFloat32 cloudOut;
...
[truncated message content] |
