Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [398] pkg/trunk
|
From: <ge...@us...> - 2008-05-12 18:28:01
|
Revision: 398
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=398&view=rev
Author: gerkey
Date: 2008-05-12 11:26:25 -0700 (Mon, 12 May 2008)
Log Message:
-----------
added license statements
Modified Paths:
--------------
pkg/trunk/3rdparty/player/manifest.xml
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/nav/amcl_player/Makefile
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/3rdparty/player/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/player/manifest.xml 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/3rdparty/player/manifest.xml 2008-05-12 18:26:25 UTC (rev 398)
@@ -12,7 +12,7 @@
<license>LGPL</license>
<url>http://playerstage.sf.net</url>
<export>
- <cpp lflags="-Xlinker -rpath ${prefix}/player/lib -L${prefix}/player/lib -lplayerdrivers -lplayercore -lplayerxdr -lplayerutils -lplayererror" cflags="-I${prefix}/player/include/player-2.2"/>
+ <cpp lflags="-Xlinker -rpath ${prefix}/player/lib -L${prefix}/player/lib -lplayercore -lplayerxdr -lplayerutils -lplayererror" cflags="-I${prefix}/player/include/player-2.2"/>
</export>
</package>
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-12 18:26:25 UTC (rev 398)
@@ -1,4 +1,24 @@
#include <assert.h>
+/*
+ * Software License Agreement (GNU LGPL)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
#include <libstandalone_drivers/urg_laser.h>
Modified: pkg/trunk/nav/amcl_player/Makefile
===================================================================
--- pkg/trunk/nav/amcl_player/Makefile 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/nav/amcl_player/Makefile 2008-05-12 18:26:25 UTC (rev 398)
@@ -3,7 +3,7 @@
all: $(PKG)
CFLAGS = -g -Wall `rospack export/cpp/cflags $(PKG)` `pkg-config --cflags gdk-pixbuf-2.0`
-LFLAGS = `rospack export/cpp/lflags $(PKG)` `pkg-config --libs gdk-pixbuf-2.0`
+LFLAGS = `rospack export/cpp/lflags $(PKG)` -lplayerdrivers `pkg-config --libs gdk-pixbuf-2.0`
amcl_player: amcl_player.cc
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-12 18:26:25 UTC (rev 398)
@@ -1,3 +1,24 @@
+/*
+ * Software License Agreement (GNU LGPL)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
#include <assert.h>
// For core Player stuff (message queues, config file objects, etc.)
@@ -54,6 +75,7 @@
ConfigFile* cf;
// incoming messages
+ MsgRobotBase2DOdom localizedOdomMsg;
MsgRobotBase2DOdom odomMsg;
MsgLaserScan laserMsg;
@@ -126,6 +148,7 @@
// TODO: remove XDR dependency
playerxdr_ftable_init();
+ advertise<MsgRobotBase2DOdom>("localizedpose");
subscribe("odom", odomMsg, &AmclNode::odomReceived);
subscribe("scan", laserMsg, &AmclNode::laserReceived);
@@ -251,6 +274,7 @@
odomMsg.pos.x,
odomMsg.pos.y,
RTOD(odomMsg.pos.th));
+ /*
this->tf->sendEuler(2,1,
this->odomMsg.pos.x-pdata->pos.px,
this->odomMsg.pos.y-pdata->pos.py,
@@ -269,6 +293,15 @@
(long long unsigned int)floor(hdr->timestamp),
(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) *
1000000000ULL));
+ */
+ printf("pose: (%.3f %.3f %.3f)\n",
+ pdata->pos.px,
+ pdata->pos.py,
+ RTOD(pdata->pos.pa));
+ localizedOdomMsg.pos.x = pdata->pos.px;
+ localizedOdomMsg.pos.y = pdata->pos.py;
+ localizedOdomMsg.pos.th = pdata->pos.pa;
+ publish("localizedpose", localizedOdomMsg);
return(0);
}
@@ -408,7 +441,7 @@
if(!this->Driver::InQueue->Empty())
this->Driver::ProcessMessages();
else
- usleep(1000000);
+ usleep(100000);
return(0);
}
Modified: pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-12 18:26:25 UTC (rev 398)
@@ -1,3 +1,35 @@
+//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.
+
#include <termios.h>
#include <signal.h>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-12 18:26:25 UTC (rev 398)
@@ -1,3 +1,24 @@
+/*
+ * Software License Agreement (GNU LGPL)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
@@ -13,14 +34,14 @@
// The messages that we'll use
#include <std_msgs/MsgPlanner2DState.h>
#include <std_msgs/MsgPlanner2DGoal.h>
-#include <std_msgs/MsgRobotBase2DCmdVel.h>
+#include <std_msgs/MsgBaseVel.h>
#include <std_msgs/MsgRobotBase2DOdom.h>
#include <std_msgs/MsgLaserScan.h>
// For transform support
#include <rosTF/rosTF.h>
-#define ANG_NORM(a) atan2(cos((a)),sin((a)))
+#define ANG_NORM(a) atan2(sin((a)),cos((a)))
#define DTOR(a) ((a)*M_PI/180.0)
#define RTOD(a) ((a)*180.0/M_PI)
#define SIGN(x) (((x) < 0.0) ? -1 : 1)
@@ -44,7 +65,6 @@
{
NO_GOAL,
PURSUING_GOAL,
- ROTATING_AT_GOAL,
REACHED_GOAL
} planner_state;
// Are we enabled?
@@ -145,7 +165,7 @@
rotate_dir(0),
printed_warning(false),
stopped(false),
- robot_radius(0.16),
+ robot_radius(0.2),
safety_dist(0.05),
max_radius(0.25),
dist_penalty(1.0),
@@ -158,9 +178,9 @@
lookahead_maxdist(2.0),
lookahead_distweight(10.0),
tvmin(0.1),
- tvmax(0.5),
+ tvmax(0.35),
avmin(DTOR(10.0)),
- avmax(DTOR(90.0)),
+ avmax(DTOR(60.0)),
amin(DTOR(5.0)),
amax(DTOR(20.0)),
tf(NULL)
@@ -186,8 +206,12 @@
{
for(int i=0;i<sx;i++)
{
- this->plan->cells[i+j*sx].occ_state =
- mapdata[MAP_IDX(sx,i,j)];
+ if(mapdata[i+j*sx] < 0.1*255)
+ this->plan->cells[i+j*sx].occ_state = -1;
+ else if(mapdata[i+j*sx] > 0.9*255)
+ this->plan->cells[i+j*sx].occ_state = 1;
+ else
+ this->plan->cells[i+j*sx].occ_state = 0;
}
}
free(mapdata);
@@ -207,9 +231,10 @@
this->tf = new rosTFClient(*this);
advertise<MsgPlanner2DState>("state");
- advertise<MsgRobotBase2DCmdVel>("cmdvel");
+ advertise<MsgBaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
- subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
+ //subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
+ subscribe("localizedpose", odomMsg, &WavefrontNode::odomReceived);
subscribe("scan", laserMsg, &WavefrontNode::laserReceived);
}
@@ -226,6 +251,11 @@
this->lock.lock();
// Got a new goal message; handle it
this->enable = goalMsg.enable;
+ printf("got new goal: %.3f %.3f %.3f\n",
+ goalMsg.goal.x,
+ goalMsg.goal.y,
+ RTOD(goalMsg.goal.th));
+
if(this->enable)
{
this->goal[0] = goalMsg.goal.x;
@@ -240,6 +270,7 @@
WavefrontNode::odomReceived()
{
this->lock.lock();
+ /*
libTF::TFPose2D odom_pose;
odom_pose.x = odomMsg.pos.x;
odom_pose.y = odomMsg.pos.y;
@@ -270,6 +301,14 @@
{
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],
+ RTOD(this->pose[2]));
this->lock.unlock();
}
@@ -283,27 +322,26 @@
void
WavefrontNode::stopRobot()
{
- if(!this->stopped)
- {
+ //if(!this->stopped)
+ //{
// TODO: should we send more than once, or perhaps use RPC for this?
this->sendVelCmd(0.0,0.0,0.0);
this->stopped = true;
- }
+ //}
}
// Declare this globally, so that it never gets desctructed (message
// desctruction causes master disconnect)
-MsgRobotBase2DCmdVel* cmdvel;
+MsgBaseVel* cmdvel;
void
WavefrontNode::sendVelCmd(double vx, double vy, double vth)
{
if(!cmdvel)
- cmdvel = new MsgRobotBase2DCmdVel();
- cmdvel->vel.x = vx;
- cmdvel->vel.y = vy;
- cmdvel->vel.th = vth;
- this->ros::node::publish("cmdvel", *cmdvel);
+ cmdvel = new MsgBaseVel();
+ cmdvel->vx = vx;
+ cmdvel->vw = vth;
+ this->ros::node::publish("cmd_vel", *cmdvel);
}
@@ -320,14 +358,14 @@
this->lock.lock();
switch(this->planner_state)
{
- // Treat these states the same; do nothing
case NO_GOAL:
+ puts("no goal");
+ this->stopRobot();
+ break;
case REACHED_GOAL:
+ puts("still done");
this->stopRobot();
break;
- case ROTATING_AT_GOAL:
-
- break;
case PURSUING_GOAL:
{
// Are we done?
@@ -336,6 +374,7 @@
this->goal[0], this->goal[1], this->goal[2],
this->dist_eps, this->ang_eps))
{
+ puts("done");
this->stopRobot();
this->planner_state = REACHED_GOAL;
break;
@@ -352,11 +391,11 @@
// no global plan
this->stopRobot();
- if(!this->printed_warning)
- {
+ //if(!this->printed_warning)
+ //{
puts("global plan failed");
- this->printed_warning = true;
- }
+ //this->printed_warning = true;
+ //}
break;
}
else
@@ -393,6 +432,7 @@
break;
}
+ printf("computed velocities: %.3f %.3f\n", vx, RTOD(va));
this->sendVelCmd(vx, 0.0, va);
break;
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-12 16:56:50 UTC (rev 397)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-12 18:26:25 UTC (rev 398)
@@ -1,3 +1,24 @@
+/*
+ * Software License Agreement (GNU GPL)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -13,26 +34,46 @@
#define USAGE "rosstage <worldfile>"
+// Our node
class StageNode : public ros::node
{
private:
+ // Messages that we'll send or receive
MsgBaseVel velMsg;
MsgLaserScan laserMsg;
MsgRobotBase2DOdom odomMsg;
+
+ // A mutex to lock access to fields that are used in message callbacks
ros::thread::mutex lock;
+ // The models that we're interested in
+ Stg::StgModelLaser* lasermodel;
+ Stg::StgModelPosition* positionmodel;
+
+ // A helper function that is executed for each stage model. We use it
+ // to search for models of interest.
+ static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+
public:
+ // Constructor; stage itself needs argc/argv. fname is the .world file
+ // that stage should load.
StageNode(int argc, char** argv, const char* fname);
~StageNode();
+
+ // Subscribe to models of interest. Currently, we find and subscribe
+ // to the first 'laser' model and the first 'position' model. Returns
+ // 0 on success (both models subscribed), -1 otherwise.
int SubscribeModels();
- void cmdvelReceived();
+
+ // Do one update of the simulator. May pause if the next update time
+ // has not yet arrived.
void Update();
- static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+ // Message callback for a MsgBaseVel message, which set velocities.
+ void cmdvelReceived();
+ // The main simulator object
Stg::StgWorldGui* world;
- Stg::StgModelLaser* lasermodel;
- Stg::StgModelPosition* positionmodel;
};
void
@@ -80,6 +121,12 @@
this->world->ForEachModel((GHFunc)ghfunc, this);
}
+// Subscribe to models of interest. Currently, we find and subscribe
+// to the first 'laser' model and the first 'position' model. Returns
+// 0 on success (both models subscribed), -1 otherwise.
+//
+// Eventually, we should provide a general way to map stage models onto ROS
+// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
@@ -111,13 +158,18 @@
void
StageNode::Update()
{
+ // Wait until it's time to update
this->world->PauseUntilNextUpdateTime();
this->lock.lock();
+
+ // Let the simulator update
this->world->Update();
+ // Get latest laser data
Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
if(samples)
{
+ // Translate into ROS message format and publish
Stg::stg_laser_cfg_t cfg = this->lasermodel->GetConfig();
this->laserMsg.angle_min = -cfg.fov/2.0;
this->laserMsg.angle_max = +cfg.fov/2.0;
@@ -134,20 +186,20 @@
publish("scan",this->laserMsg);
}
+ // Get latest odometry data
+ // Translate into ROS message format and publish
this->odomMsg.pos.x = this->positionmodel->est_pose.x;
this->odomMsg.pos.y = this->positionmodel->est_pose.y;
this->odomMsg.pos.th = this->positionmodel->est_pose.a;
-
Stg::stg_velocity_t v = this->positionmodel->GetVelocity();
-
this->odomMsg.vel.x = v.x;
this->odomMsg.vel.y = v.y;
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;
publish("odom",this->odomMsg);
- puts("published");
this->lock.unlock();
}
@@ -174,7 +226,7 @@
}
// have to call this explicitly for some reason. probably interference
- // from signal handling in Stage?
+ // from signal handling in Stage / FLTK?
ros::msg_destruct();
exit(0);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
