Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [556] pkg/trunk
|
From: <tf...@us...> - 2008-05-30 00:53:09
|
Revision: 556
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=556&view=rev
Author: tfoote
Date: 2008-05-29 17:51:25 -0700 (Thu, 29 May 2008)
Log Message:
-----------
switching 2d nav stack to use non caching transforms.
Modified Paths:
--------------
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/erratic_player/manifest.xml
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -82,6 +82,8 @@
// roscpp
#include <ros/node.h>
+//rosTF
+#include "rosTF/rosTF.h"
// Messages that I need
#include <std_msgs/MsgRobotBase2DOdom.h>
//#include <std_msgs/MsgRobotBase2DCmdVel.h>
@@ -102,7 +104,10 @@
//MsgRobotBase2DCmdVel cmdvel;
MsgBaseVel cmdvel;
- ErraticNode() : ros::node("erratic_player")
+ rosTF::rosTFServer tf;
+
+ ErraticNode() : ros::node("erratic_player"),
+ tf(*this)
{
// libplayercore boiler plate
player_globals_init();
@@ -283,11 +288,29 @@
en.odom.vel.th = pdata->vel.pa;
en.odom.stall = pdata->stall;
- en.odom.header.frame_id = 2;
+ en.odom.header.frame_id = FRAMEID_ODOM;
+
+ en.odom.header.stamp.sec = (long long unsigned int)floor(hdr->timestamp);
+ en.odom.header.stamp.sec = (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL);
+
// Publish the new data
en.publish("odom", en.odom);
+ tf.sendInverseEuler(FRAMEID_ODOM,
+ FRAMEID_ROBOT,
+ pdata->pos.px,
+ pdata->pos.py,
+ 0.0,
+ pdata->pos.pa,
+ 0.0,
+ 0.0,
+ (long long unsigned int)floor(hdr->timestamp),
+ (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+
+ std::cout <<"Sent 32" <<std::endl;
+
+
//printf("Published new odom: (%.3f,%.3f,%.3f)\n",
//en.odom.pos.x, en.odom.pos.y, en.odom.pos.th);
}
Modified: pkg/trunk/drivers/robot/erratic_player/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/drivers/robot/erratic_player/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -5,4 +5,5 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="std_msgs" />
+ <depend package="rosTF" />
</package>
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -351,23 +351,28 @@
player_position2d_data_t* pdata =
(player_position2d_data_t*)data;
- /*
+
// publish new transform map->odom
printf("lpose: %.3f %.3f %.3f\n",
odomMsg.pos.x,
odomMsg.pos.y,
RTOD(odomMsg.pos.th));
- this->tf->sendEuler(ROSTF_FRAME_ODOM,
- ROSTF_FRAME_MAP,
- pdata->pos.px-this->odomMsg.pos.x,
- pdata->pos.py-this->odomMsg.pos.y,
+ this->tf->sendEuler(FRAMEID_ROBOT,
+ FRAMEID_MAP,
+ pdata->pos.px,
+ pdata->pos.py,
0.0,
- pdata->pos.pa-this->odomMsg.pos.th,
+ pdata->pos.pa,
0.0,
0.0,
- (long long unsigned int)floor(hdr->timestamp),
- (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+ //(long long unsigned int)floor(hdr->timestamp),
+ //(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+
+ std::cout <<"Sent 21" <<std::endl;
+
printf("pose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
pdata->pos.px,
pdata->pos.py,
@@ -375,7 +380,7 @@
(long long unsigned int)floor(hdr->timestamp),
(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) *
1000000000ULL));
- */
+
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
Modified: pkg/trunk/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/nav_view/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -6,4 +6,6 @@
<depend package="std_msgs"/>
<depend package="sdlgl"/>
<depend package="sdl_image"/>
+ <depend package="rosTF"/>
+ <depend package="rostime"/>
</package>
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-30 00:51:25 UTC (rev 556)
@@ -106,6 +106,9 @@
#include "std_msgs/MsgPose2DFloat32.h"
#include "sdlgl/sdlgl.h"
+#include "rosTF/rosTF.h"
+#include "rostime/clock.h"
+
class NavView : public ros::node, public ros::SDLGL
{
public:
@@ -124,13 +127,17 @@
bool setting_theta;
double gx,gy;
+ rosTFClient tf;
+ ros::time::clock myClock;
+
NavView() : ros::node("nav_view",false),
- view_scale(10), view_x(0), view_y(0)
+ view_scale(10), view_x(0), view_y(0),
+ tf(*this,false)
{
advertise<MsgPlanner2DGoal>("goal");
advertise<MsgPose2DFloat32>("initialpose");
//subscribe("state", pstate, &NavView::pstate_cb);
- subscribe("odom", odom, &NavView::odom_cb);
+ // subscribe("odomm", odom, &NavView::odom_cb); //replaced with rosTF
subscribe("particlecloud", cloud, &NavView::odom_cb);
subscribe("gui_path", pathline, &NavView::odom_cb);
subscribe("gui_laser", laserscan, &NavView::odom_cb);
@@ -201,10 +208,24 @@
}
glPushMatrix();
- odom.lock();
- glTranslatef(odom.pos.x, odom.pos.y, 0);
- glRotatef(odom.pos.th * 180 / M_PI, 0, 0, 1);
- odom.unlock();
+ libTF::TFPose2D robotPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = FRAMEID_ROBOT;
+ robotPose.time = 0;//myClock.ulltime();
+
+ libTF::TFPose2D mapPose = tf.transformPose2D(FRAMEID_MAP, robotPose);
+
+ glTranslatef(mapPose.x, mapPose.y, 0);
+ glRotatef(mapPose.yaw * 180 / M_PI, 0, 0, 1);
+
+ /* printf("lpose: %.3f %.3f %.3f\n",
+ mapPose.x,
+ mapPose.y,
+ mapPose.yaw);
+ */
+
glColor3f(0.2, 1.0, 0.4);
glBegin(GL_LINE_LOOP);
const float robot_rad = 0.3;
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -229,7 +229,7 @@
public:
// Transform client
- rosTFClient* tf;
+ rosTFClient tf;
WavefrontNode(char* fname, double res);
~WavefrontNode();
@@ -293,7 +293,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(NULL)
+ tf(*this, false)
{
// TODO: get map via RPC
char* mapdata;
@@ -331,8 +331,6 @@
// Compute cspace over static map
plan_compute_cspace(this->plan);
- //this->tf = new rosTFClient(*this);
-
this->laser_hitpts_size = this->laser_hitpts_len = 0;
this->laser_hitpts = NULL;
@@ -348,16 +346,16 @@
advertise<MsgPolyline2D>("gui_laser");
advertise<MsgBaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
- //subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
- subscribe("localizedpose", odomMsg, &WavefrontNode::odomReceived);
+ subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
+ //subscribe("localizedpose", odomMsg, &WavefrontNode::odomReceived);
subscribe("scan", laserMsg, &WavefrontNode::laserReceived);
}
WavefrontNode::~WavefrontNode()
{
plan_free(this->plan);
- if(this->tf)
- delete this->tf;
+ // if(this->tf)
+ // delete this->tf;
}
void
@@ -385,19 +383,31 @@
WavefrontNode::odomReceived()
{
this->lock.lock();
- /*
- libTF::TFPose2D odom_pose;
+
+ libTF::TFPose2D robotPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = FRAMEID_ROBOT;
+ // robotPose.time = myClock.ulltime();
+ robotPose.time = odomMsg.header.stamp.sec * 1000000000ULL +
+ odomMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
+
+ // std::cout <<"robotTime" << robotPose.time << std::endl;
+ /* libTF::TFPose2D pose;
odom_pose.x = odomMsg.pos.x;
odom_pose.y = odomMsg.pos.y;
odom_pose.yaw = odomMsg.pos.th;
odom_pose.time = odomMsg.header.stamp.sec * 1000000000ULL +
odomMsg.header.stamp.nsec;
odom_pose.frame = odomMsg.header.frame_id;
-
+ */
try
{
libTF::TFPose2D global_pose =
- this->tf->transformPose2D(ROSTF_FRAME_MAP, odom_pose);
+ this->tf.transformPose2D(FRAMEID_MAP, robotPose);
+ cout << tf.getMatrix(FRAMEID_MAP, FRAMEID_ROBOT, robotPose.time);
+
this->pose[0] = global_pose.x;
this->pose[1] = global_pose.y;
this->pose[2] = global_pose.yaw;
@@ -416,12 +426,13 @@
{
puts("no global->local Tx yet");
}
- */
+
+ /*
this->pose[0] = odomMsg.pos.x;
this->pose[1] = odomMsg.pos.y;
this->pose[2] = odomMsg.pos.th;
- /*
+
printf("gpose: %.3f %.3f %.3f\n",
this->pose[0],
this->pose[1],
Modified: pkg/trunk/simulators/rosstage/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosstage/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/simulators/rosstage/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -6,4 +6,5 @@
<depend package="rosthread" />
<depend package="stage" />
<depend package="std_msgs" />
+ <depend package="rosTF" />
</package>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -87,6 +87,8 @@
#include <std_msgs/MsgRobotBase2DOdom.h>
#include <std_msgs/MsgBaseVel.h>
+#include "rosTF/rosTF.h"
+
#define USAGE "rosstage <worldfile>"
// Our node
@@ -109,6 +111,8 @@
// to search for models of interest.
static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+ rosTFServer tf;
+
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
@@ -158,7 +162,8 @@
}
StageNode::StageNode(int argc, char** argv, const char* fname) :
- ros::node("rosstage")
+ ros::node("rosstage"),
+ tf(*this)
{
this->lasermodel = NULL;
this->positionmodel = NULL;
@@ -255,16 +260,30 @@
this->odomMsg.vel.th = v.a;
this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
- this->odomMsg.header.frame_id = 2;
+ this->odomMsg.header.frame_id = FRAMEID_ODOM;
this->odomMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e3);
+ (unsigned long)floor(world->SimTimeNow() / 1e6);
this->odomMsg.header.stamp.nsec =
- (unsigned long)rint(1e6 * (world->SimTimeNow() -
- this->odomMsg.header.stamp.sec * 1e3));
+ (unsigned long)rint(1e3 * (world->SimTimeNow() -
+ this->odomMsg.header.stamp.sec * 1e6));
this->odomMsg.__timestamp_override = true;
publish("odom",this->odomMsg);
+ // printf("%u \n",world->SimTimeNow());
+ printf("time: %u, %u \n",odomMsg.header.stamp.sec, odomMsg.header.stamp.nsec);
+
+ tf.sendInverseEuler(FRAMEID_ODOM,
+ FRAMEID_ROBOT,
+ odomMsg.pos.x,
+ odomMsg.pos.y,
+ 0.0,
+ odomMsg.pos.th,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
this->lock.unlock();
}
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 00:51:25 UTC (rev 556)
@@ -47,7 +47,7 @@
void rosTFClient::receiveEuler()
{
setWithEulers(eulerIn.frame, eulerIn.parent, eulerIn.x, eulerIn.y, eulerIn.z, eulerIn.yaw, eulerIn.pitch, eulerIn.roll, eulerIn.header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec);
- std::cout << "recieved euler frame: " << eulerIn.frame << " with parent:" << eulerIn.parent << std::endl;
+ std::cout << "received euler frame: " << eulerIn.frame << " with parent:" << eulerIn.parent << "time " << eulerIn.header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
};
void rosTFClient::receiveDH()
@@ -86,6 +86,7 @@
eulerOut.roll = roll;
eulerOut.header.stamp.sec = secs;
eulerOut.header.stamp.nsec = nsecs;
+ eulerOut.__timestamp_override = true;
myNode.publish("TransformEuler", eulerOut);
@@ -107,6 +108,7 @@
eulerOut.roll = odomeuler.roll;
eulerOut.header.stamp.sec = secs;
eulerOut.header.stamp.nsec = nsecs;
+ eulerOut.__timestamp_override = true;
myNode.publish("TransformEuler", eulerOut);
@@ -138,6 +140,7 @@
dhOut.angle = angle;
dhOut.header.stamp.sec = secs;
dhOut.header.stamp.nsec = nsecs;
+ dhOut.__timestamp_override = true;
myNode.publish("TransformDH", dhOut);
@@ -156,6 +159,7 @@
quaternionOut.w = w;
quaternionOut.header.stamp.sec = secs;
quaternionOut.header.stamp.nsec = nsecs;
+ quaternionOut.__timestamp_override = true;
myNode.publish("TransformQuaternion", quaternionOut);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
