Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13027] pkg/trunk
|
From: <ge...@us...> - 2009-03-27 01:49:13
|
Revision: 13027
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13027&view=rev
Author: gerkey
Date: 2009-03-27 01:49:03 +0000 (Fri, 27 Mar 2009)
Log Message:
-----------
Moved rosstage into stage and renamed it stageros. Updated all the launch
files I could identify.
Modified Paths:
--------------
pkg/trunk/3rdparty/stage/manifest.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/demos/tabletop_manipulation/scripts/movebase.py
pkg/trunk/highlevel/test_executive_trex_pr2/cfg/launch_stage.xml
pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/test_highlevel_controllers/manifest.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/build_for_stage.sh
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_amcl_5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/testcase.xml.in
Added Paths:
-----------
pkg/trunk/3rdparty/stage/CMakeLists.txt
pkg/trunk/3rdparty/stage/Makefile.stage
pkg/trunk/3rdparty/stage/src/
pkg/trunk/3rdparty/stage/src/stageros.cpp
pkg/trunk/3rdparty/stage/test/
pkg/trunk/3rdparty/stage/world/
Removed Paths:
-------------
pkg/trunk/3rdparty/stage/Makefile
pkg/trunk/simulators/rosstage/
Copied: pkg/trunk/3rdparty/stage/CMakeLists.txt (from rev 13026, pkg/trunk/simulators/rosstage/CMakeLists.txt)
===================================================================
--- pkg/trunk/3rdparty/stage/CMakeLists.txt (rev 0)
+++ pkg/trunk/3rdparty/stage/CMakeLists.txt 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+include(FindPkgConfig)
+rospack(stage)
+
+
+add_custom_target(build_stage ALL
+ COMMAND cmake -E chdir ${PROJECT_SOURCE_DIR} make -f Makefile.stage)
+set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES stage-svn)
+
+rospack_add_boost_directories()
+
+pkg_check_modules(STAGE REQUIRED stage)
+include_directories(${STAGE_INCLUDE_DIRS})
+link_directories(${STAGE_LIBRARY_DIRS})
+rospack_add_executable(bin/stageros src/stageros.cpp)
+rospack_link_boost(bin/stageros thread)
+add_dependencies(bin/stageros build_stage)
+
+set(ENV{PKG_CONFIG_PATH} "${PROJECT_SOURCE_DIR}/stage/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}")
+rospack_add_compile_flags(bin/stageros ${STAGE_CFLAGS_OTHERS})
+target_link_libraries(bin/stageros ${STAGE_LIBRARIES})
+rospack_add_link_flags(bin/stageros ${STAGE_LDFLAGS_OTHERS})
+
+
+rospack_add_rostest(test/hztest.xml)
Property changes on: pkg/trunk/3rdparty/stage/CMakeLists.txt
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/3rdparty/stage/Makefile
===================================================================
--- pkg/trunk/3rdparty/stage/Makefile 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/3rdparty/stage/Makefile 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,19 +0,0 @@
-all: installed
-
-SVN_DIR = stage-svn
-SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros
-SVN_REVISION = -r 7126
-include $(shell rospack find mk)/svn_checkout.mk
-
-installed: $(SVN_DIR) patched
- cd $(SVN_DIR) && autoreconf -i -s
- cd $(SVN_DIR) && ./configure --prefix=$(PWD)/stage
- cd $(SVN_DIR) && make install
- touch installed
-
-clean:
- -cd $(SVN_DIR) && make clean
- rm -rf stage installed
-
-wipe: clean
- rm -rf $(SVN_DIR)
Copied: pkg/trunk/3rdparty/stage/Makefile.stage (from rev 13026, pkg/trunk/3rdparty/stage/Makefile)
===================================================================
--- pkg/trunk/3rdparty/stage/Makefile.stage (rev 0)
+++ pkg/trunk/3rdparty/stage/Makefile.stage 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,19 @@
+all: installed
+
+SVN_DIR = stage-svn
+SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros
+SVN_REVISION = -r 7126
+include $(shell rospack find mk)/svn_checkout.mk
+
+installed: $(SVN_DIR) patched Makefile.stage
+ cd $(SVN_DIR) && autoreconf -i -s
+ cd $(SVN_DIR) && ./configure --prefix=`pwd`/../stage
+ cd $(SVN_DIR) && make install
+ touch installed
+
+clean:
+ -cd $(SVN_DIR) && make clean
+ rm -rf stage installed patched
+
+wipe: clean
+ rm -rf $(SVN_DIR)
Property changes on: pkg/trunk/3rdparty/stage/Makefile.stage
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/3rdparty/stage/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/stage/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/3rdparty/stage/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,22 +1,27 @@
<package>
<description brief="Stage">
-This package contains Stage, from the Player Project
+This package contains the Stage robot simulator, from the Player Project
(http://playerstage.sf.net). This package does not modify Stage in any
-way; it simply provides a convenient way to download and compile the
-headers and libraries in a way that can be managed by the ROS dependency
-system.
+way. This package also provides stageros, a ROS node that uses Stage.
</description>
<author>Richard Vaughan, with contributions from many others. See web page for a full credits list.</author>
<license>GPL</license>
-<review status="3rdparty" notes=""/>
+<review status="unreviewed" notes=""/>
<url>http://playerstage.sf.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/stage/lib `PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs stage`" cflags="`PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags stage`"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/stage-cvs"/>
</export>
<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros"/>
+
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
+ <depend package="laser_scan" />
+ <depend package="tf" />
+
<sysdepend os="ubuntu" version="8.04-hardy" package="autoconf"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="automake"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libtool"/>
Copied: pkg/trunk/3rdparty/stage/src/stageros.cpp (from rev 13026, pkg/trunk/simulators/rosstage/rosstage.cc)
===================================================================
--- pkg/trunk/3rdparty/stage/src/stageros.cpp (rev 0)
+++ pkg/trunk/3rdparty/stage/src/stageros.cpp 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,418 @@
+/*
+ * stageros
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * 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
+ */
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b stageros wraps the Stage 2-D multi-robot simulator, via @b libstage.
+
+For detailed documentation,
+consult the <a href="http://playerstage.sourceforge.net/doc/stage-cvs">Stage manual</a>.
+
+This node finds the first Stage model of type @b laser, and the first model
+of type @b position, and maps these models to the ROS topics given below.
+If a laser and a position model are not found, stageros exits.
+
+@todo Define a more general method for mapping Stage models onto ROS topics
+/ services. Something like the Player/Stage model, in which a Player .cfg
+file is used to map named Stage models onto Player devices, is probably the
+way to go. The same technique can be used for rosgazebo.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ stageros <world> [standard ROS args]
+@endverbatim
+
+@param world The Stage .world file to load.
+
+@par Example
+
+@verbatim
+$ stageros willow-erratic.world
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+If there is only one position model defined in the world file, all of these
+topics appear at the top namespace. However, if >1 position models exist,
+these topics are pushed down into their own namespaces, by prefixing the
+topics with "robot_<i>/" , e.g., "robot_0/cmd_vel", etc.
+
+Subscribes to (name/type):
+- @b "cmd_vel"/PoseDot : velocity commands to differentially drive the
+ position model of the robot
+
+Publishes to (name / type):
+- @b "odom"/RobotBase2DOdom : odometry data from the position model.
+- @b "base_scan"/LaserScan : scans from the laser model
+- @b "base_pose_ground_truth"/PoseWithRatesStamped : ground truth pos
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+
+ **/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// libstage
+#include <stage.hh>
+
+// roscpp
+#include <ros/node.h>
+#include "boost/thread/mutex.hpp"
+#include <laser_scan/LaserScan.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <robot_msgs/PoseWithRatesStamped.h>
+#include <robot_msgs/PoseDot.h>
+#include <roslib/Time.h>
+
+#include "tf/transform_broadcaster.h"
+
+#define USAGE "stageros <worldfile>"
+#define ODOM "odom"
+#define BASE_SCAN "base_scan"
+#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
+#define CMD_VEL "cmd_vel"
+
+// Our node
+class StageNode : public ros::Node
+{
+ private:
+ // Messages that we'll send or receive
+ robot_msgs::PoseDot *velMsgs;
+ laser_scan::LaserScan *laserMsgs;
+ deprecated_msgs::RobotBase2DOdom *odomMsgs;
+ robot_msgs::PoseWithRatesStamped *groundTruthMsgs;
+ roslib::Time timeMsg;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ boost::mutex msg_lock;
+
+ // The models that we're interested in
+ std::vector<Stg::StgModelLaser *> lasermodels;
+ std::vector<Stg::StgModelPosition *> positionmodels;
+
+ // 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);
+
+ // Appends the given robot ID to the given message name. If omitRobotID
+ // is true, an unaltered copy of the name is returned.
+ const char *mapName(const char *name, size_t robotID);
+
+ tf::TransformBroadcaster tf;
+
+ // Last time that we received a velocity command
+ ros::Time base_last_cmd;
+ ros::Duration base_watchdog_timeout;
+
+ // Current simulation time
+ ros::Time sim_time;
+
+ public:
+ // Constructor; stage itself needs argc/argv. fname is the .world file
+ // that stage should load.
+ StageNode(int argc, char** argv, bool gui, 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();
+
+ // Do one update of the simulator. May pause if the next update time
+ // has not yet arrived.
+ void Update();
+
+ // Message callback for a MsgBaseVel message, which set velocities.
+ void cmdvelReceived();
+
+ // The main simulator object
+ Stg::StgWorld* world;
+};
+
+// since stageros is single-threaded, this is OK. revisit if that changes!
+const char *
+StageNode::mapName(const char *name, size_t robotID)
+{
+ if (positionmodels.size() > 1)
+ {
+ static char buf[100];
+ snprintf(buf, sizeof(buf), "/robot_%d/%s", robotID, name);
+ return buf;
+ }
+ else
+ return name;
+}
+
+void
+StageNode::ghfunc(gpointer key,
+ Stg::StgModel* mod,
+ StageNode* node)
+{
+ if (dynamic_cast<Stg::StgModelLaser *>(mod))
+ node->lasermodels.push_back(dynamic_cast<Stg::StgModelLaser *>(mod));
+ if (dynamic_cast<Stg::StgModelPosition *>(mod))
+ node->positionmodels.push_back(dynamic_cast<Stg::StgModelPosition *>(mod));
+}
+
+void
+StageNode::cmdvelReceived()
+{
+ boost::mutex::scoped_lock lock(msg_lock);
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ {
+ this->positionmodels[r]->SetSpeed(this->velMsgs[r].vel.vx,
+ this->velMsgs[r].vel.vy,
+ this->velMsgs[r].ang_vel.vz);
+ }
+ this->base_last_cmd = this->sim_time;
+}
+
+StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) :
+ ros::Node("stageros"),
+ tf(*this)
+{
+ this->sim_time.fromSec(0.0);
+ this->base_last_cmd.fromSec(0.0);
+ double t;
+ param("~base_watchdog_timeout", t, 0.2);
+ this->base_watchdog_timeout.fromSec(t);
+
+ // initialize libstage
+ Stg::Init( &argc, &argv );
+
+ if(gui)
+ this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+ else
+ this->world = new Stg::StgWorld();
+
+ this->world->Load(fname);
+
+ this->world->ForEachModel((GHFunc)ghfunc, this);
+ if (lasermodels.size() != positionmodels.size())
+ {
+ ROS_FATAL("number of position models and laser models must be equal in "
+ "the world file.");
+ ROS_BREAK();
+ }
+ size_t numRobots = positionmodels.size();
+ ROS_INFO("found %d position model(s) in the file", numRobots);
+
+ this->velMsgs = new robot_msgs::PoseDot[numRobots];
+ this->laserMsgs = new laser_scan::LaserScan[numRobots];
+ this->odomMsgs = new deprecated_msgs::RobotBase2DOdom[numRobots];
+ this->groundTruthMsgs = new robot_msgs::PoseWithRatesStamped[numRobots];
+}
+
+
+// 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()
+{
+ setParam("/use_sim_time", true);
+
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ {
+ if(this->lasermodels[r])
+ this->lasermodels[r]->Subscribe();
+ else
+ {
+ puts("no laser");
+ return(-1);
+ }
+ if(this->positionmodels[r])
+ this->positionmodels[r]->Subscribe();
+ else
+ {
+ puts("no position");
+ return(-1);
+ }
+ advertise<laser_scan::LaserScan>(mapName(BASE_SCAN,r), 10);
+ advertise<deprecated_msgs::RobotBase2DOdom>(mapName(ODOM,r), 10);
+ advertise<robot_msgs::PoseWithRatesStamped>(
+ mapName(BASE_POSE_GROUND_TRUTH,r), 10);
+ subscribe(mapName(CMD_VEL,r), velMsgs[r], &StageNode::cmdvelReceived, 10);
+ }
+ advertise<roslib::Time>("time",10);
+ return(0);
+}
+
+StageNode::~StageNode()
+{
+ delete[] velMsgs;
+ delete[] laserMsgs;
+ delete[] odomMsgs;
+ delete[] groundTruthMsgs;
+}
+
+void
+StageNode::Update()
+{
+ // Wait until it's time to update
+ this->world->PauseUntilNextUpdateTime();
+ boost::mutex::scoped_lock lock(msg_lock);
+
+ // Let the simulator update (it will sleep if there's time)
+ this->world->Update();
+
+ this->sim_time.fromSec(world->SimTimeNow() / 1e6);
+
+ // TODO make this only affect one robot if necessary
+ if((this->base_watchdog_timeout.toSec() > 0.0) &&
+ ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
+ {
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
+ }
+
+ // Get latest laser data
+ for (size_t r = 0; r < this->lasermodels.size(); r++)
+ {
+ Stg::stg_laser_sample_t* samples = this->lasermodels[r]->GetSamples();
+ if(samples)
+ {
+ // Translate into ROS message format and publish
+ Stg::stg_laser_cfg_t cfg = this->lasermodels[r]->GetConfig();
+ this->laserMsgs[r].angle_min = -cfg.fov/2.0;
+ this->laserMsgs[r].angle_max = +cfg.fov/2.0;
+ this->laserMsgs[r].angle_increment = cfg.fov/(double)(cfg.sample_count-1);
+ this->laserMsgs[r].range_min = 0.0;
+ this->laserMsgs[r].range_max = cfg.range_bounds.max;
+ this->laserMsgs[r].ranges.resize(cfg.sample_count);
+ this->laserMsgs[r].intensities.resize(cfg.sample_count);
+ for(unsigned int i=0;i<cfg.sample_count;i++)
+ {
+ this->laserMsgs[r].ranges[i] = samples[i].range;
+ this->laserMsgs[r].intensities[i] = (uint8_t)samples[i].reflectance;
+ }
+
+ this->laserMsgs[r].header.frame_id = mapName("base_laser", r);
+ this->laserMsgs[r].header.stamp = sim_time;
+ publish(mapName(BASE_SCAN,r), this->laserMsgs[r]);
+ }
+
+ // Also publish the base->base_laser Tx. This could eventually move
+ // into being retrieved from the param server as a static Tx.
+ Stg::stg_pose_t lp = this->lasermodels[r]->GetPose();
+ tf.sendTransform(tf::Stamped<tf::Transform>
+ (tf::Transform(tf::Quaternion(lp.a, 0, 0),
+ tf::Point(lp.x, lp.y, 0.15)),
+ sim_time, mapName("base_laser", r), mapName("base_link", r)));
+ // Send the identity transform between base_footprint and base_link
+ tf::Transform txIdentity(tf::Quaternion(0, 0, 0), tf::Point(0, 0, 0));
+ tf.sendTransform(tf::Stamped<tf::Transform>
+ (txIdentity,
+ sim_time, mapName("base_link", r), mapName("base_footprint", r)));
+ // Get latest odometry data
+ // Translate into ROS message format and publish
+ this->odomMsgs[r].pos.x = this->positionmodels[r]->est_pose.x;
+ this->odomMsgs[r].pos.y = this->positionmodels[r]->est_pose.y;
+ this->odomMsgs[r].pos.th = this->positionmodels[r]->est_pose.a;
+ Stg::stg_velocity_t v = this->positionmodels[r]->GetVelocity();
+ this->odomMsgs[r].vel.x = v.x;
+ this->odomMsgs[r].vel.y = v.y;
+ this->odomMsgs[r].vel.th = v.a;
+ this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
+ this->odomMsgs[r].header.frame_id = mapName("odom", r);
+ this->odomMsgs[r].header.stamp = sim_time;
+
+ publish(mapName(ODOM,r),this->odomMsgs[r]);
+
+ tf::Stamped<tf::Transform> tx(
+ tf::Transform(
+ tf::Quaternion(odomMsgs[r].pos.th, 0, 0),
+ tf::Point(odomMsgs[r].pos.x, odomMsgs[r].pos.y, 0.0)),
+ sim_time, mapName("base_footprint", r), mapName("odom", r));
+ this->tf.sendTransform(tx);
+
+ // Also publish the ground truth pose
+ Stg::stg_pose_t gpose = this->positionmodels[r]->GetGlobalPose();
+ // Note that we correct for Stage's screwed-up coord system.
+ tf::Transform gt(tf::Quaternion(gpose.a-M_PI/2.0, 0, 0),
+ tf::Point(gpose.y, -gpose.x, 0.0));
+
+ this->groundTruthMsgs[r].pos.position.x = gt.getOrigin().x();
+ this->groundTruthMsgs[r].pos.position.y = gt.getOrigin().y();
+ this->groundTruthMsgs[r].pos.position.z = gt.getOrigin().z();
+ this->groundTruthMsgs[r].pos.orientation.x = gt.getRotation().x();
+ this->groundTruthMsgs[r].pos.orientation.y = gt.getRotation().y();
+ this->groundTruthMsgs[r].pos.orientation.z = gt.getRotation().z();
+ this->groundTruthMsgs[r].pos.orientation.w = gt.getRotation().w();
+
+ this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
+ this->groundTruthMsgs[r].header.stamp = sim_time;
+
+ publish(mapName(BASE_POSE_GROUND_TRUTH,r), this->groundTruthMsgs[r]);
+
+ }
+
+ this->timeMsg.rostime = sim_time;
+ publish("time", this->timeMsg);
+}
+
+int
+main(int argc, char** argv)
+{
+ if( argc < 2 )
+ {
+ puts(USAGE);
+ exit(-1);
+ }
+
+ ros::init(argc,argv);
+
+ bool gui = true;
+ for(int i=0;i<(argc-1);i++)
+ {
+ if(!strcmp(argv[i], "-g"))
+ gui = false;
+ }
+
+ StageNode sn(argc-1,argv,gui,argv[argc-1]);
+
+ if(sn.SubscribeModels() != 0)
+ exit(-1);
+
+ while(sn.ok() && !sn.world->TestQuit())
+ {
+ sn.Update();
+ }
+
+ exit(0);
+}
+
Property changes on: pkg/trunk/3rdparty/stage/src/stageros.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/3rdparty/stage/test
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/3rdparty/stage/world
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" >
<remap from="scan" to="base_scan" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" >
<remap from="scan" to="base_scan" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
<param name="robot_0/wavefront_player_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
<node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" output="screen"/>
<param name="robot_radius" value="0.325"/>
<!-- node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="6"/>
<param name="pf_min_samples" value="100"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="6"/>
<param name="pf_min_samples" value="100"/>
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -8,7 +8,7 @@
<depend package="nav_view_sdl"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="rosstage"/>
+ <depend package="stage"/>
<depend package="map_server"/>
<depend package="wavefront_player"/>
<depend package="highlevel_controllers"/>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -2,5 +2,6 @@
<include file="$(find tabletop_manipulation)/launch/laser_sim.launch"/>
<include file="$(find tabletop_manipulation)/launch/arm.launch"/>
<include file="$(find tabletop_manipulation)/launch/base.launch"/>
+
<node pkg="tabletop_manipulation" type="tuckarm.py" args="left"/>
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,22 +1,20 @@
<launch>
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/pr2_defs/robots for full pr2 -->
- <group name="wg">
- <!-- send pr2.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <!-- send pr2.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find tabletop_manipulation)/gazebo/simple.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find tabletop_manipulation)/gazebo/simple.world" respawn="false">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0.5 0 0 0 0 0" respawn="false" output="screen" />
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0.5 0 0 0 0 0" respawn="false" output="screen" />
- <!-- load controllers -->
- <!--include file="$(find pr2_gazebo)/pr2_default_controllers.launch" /-->
- </group>
+ <!-- load controllers -->
+ <!--include file="$(find pr2_gazebo)/pr2_default_controllers.launch" /-->
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,71 +1,69 @@
<launch>
- <group name="wg">
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
- <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
- <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen" machine="three">
- <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
+ <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen" machine="three">
+ <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="30.0"/>
- <param name="move_base/map_update_frequency" value="2.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="0.5"/>
- <param name="costmap_2d/base_laser_max_range" value="20.0"/>
- <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
- <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
- <param name="costmap_2d/z_threshold_max" value="2.0"/>
- <param name="costmap_2d/z_threshold_min" value="0.13"/>
- <param name="costmap_2d/no_information_value" value="255"/>
- <param name="costmap_2d/freespace_projection_height" value="2.0"/>
- <param name="costmap_2d/inflation_radius" value="0.65"/>
- <param name="costmap_2d/circumscribed_radius" value="0.46"/>
- <param name="costmap_2d/inscribed_radius" value="0.325"/>
- <param name="costmap_2d/raytrace_window" value="10.0"/>
- <param name="costmap_2d/weight" value="0.1"/>
+ <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
+ <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
+ <param name="move_base/plannerType" value="ARAPlanner"/>
+ <param name="move_base/environmentType" value="2D"/>
+ <param name="move_base/controller_frequency" value="30.0"/>
+ <param name="move_base/map_update_frequency" value="2.0"/>
+ <param name="move_base/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
+ <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="costmap_2d/z_threshold_min" value="0.13"/>
+ <param name="costmap_2d/no_information_value" value="255"/>
+ <param name="costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="costmap_2d/inflation_radius" value="0.65"/>
+ <param name="costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="costmap_2d/raytrace_window" value="10.0"/>
+ <param name="costmap_2d/weight" value="0.1"/>
- <param name="trajectory_rollout/map_size" value="4.0" />
- <param name="trajectory_rollout/acc_limit_x" value="2.5" />
- <param name="trajectory_rollout/acc_limit_y" value="2.5" />
- <param name="trajectory_rollout/acc_limit_th" value="3.2" />
- <param name="trajectory_rollout/sim_time" value="1.0" />
- <param name="trajectory_rollout/sim_granularity" value="0.05" />
- <param name="trajectory_rollout/vx_samples" value="15" />
- <param name="trajectory_rollout/vtheta_samples" value="15" />
- <param name="trajectory_rollout/path_distance_bias" value="0.6" />
- <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
- <param name="trajectory_rollout/occdist_scale" value="0.2" />
- <param name="trajectory_rollout/heading_lookahead" value="0.325" />
- <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
- <param name="trajectory_rollout/holonomic_robot" value="true" />
- <param name="trajectory_rollout/max_vel_x" value="0.5" />
- <param name="trajectory_rollout/min_vel_x" value="0.1" />
- <param name="trajectory_rollout/max_vel_th" value="1.0" />
- <param name="trajectory_rollout/min_vel_th" value="-1.0" />
- <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
- <param name="trajectory_rollout/freespace_model" value="true" />
- <param name="trajectory_rollout/dwa" value="false" />
- <param name="trajectory_rollout/simple_attractor" value="false" />
+ <param name="trajectory_rollout/map_size" value="4.0" />
+ <param name="trajectory_rollout/acc_limit_x" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_y" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_th" value="3.2" />
+ <param name="trajectory_rollout/sim_time" value="1.0" />
+ <param name="trajectory_rollout/sim_granularity" value="0.05" />
+ <param name="trajectory_rollout/vx_samples" value="15" />
+ <param name="trajectory_rollout/vtheta_samples" value="15" />
+ <param name="trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
+ <param name="trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="trajectory_rollout/heading_lookahead" value="0.325" />
+ <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
+ <param name="trajectory_rollout/holonomic_robot" value="true" />
+ <param name="trajectory_rollout/max_vel_x" value="0.5" />
+ <param name="trajectory_rollout/min_vel_x" value="0.1" />
+ <param name="trajectory_rollout/max_vel_th" value="1.0" />
+ <param name="trajectory_rollout/min_vel_th" value="-1.0" />
+ <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
+ <param name="trajectory_rollout/freespace_model" value="true" />
+ <param name="trajectory_rollout/dwa" value="false" />
+ <param name="trajectory_rollout/simple_attractor" value="false" />
- <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
- <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
+ <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
+ <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
- <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
- way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
- <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
+ way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
+ <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
- <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
- are not getting any data -->
- <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
- <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
- <param name="costmap_2d/stereo_update_rate" value="0.0"/>
- </node>
+ <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
+ are not getting any data -->
+ <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
+ <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
+ <param name="costmap_2d/stereo_update_rate" value="0.0"/>
+ </node>
- </group>
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,73 +1,71 @@
<launch>
- <group name="wg">
- <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
- <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen">
- <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
+ <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen">
+ <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="30.0"/>
- <param name="move_base/map_update_frequency" value="2.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="0.5"/>
- <param name="move_base/trans_stopped_velocity" value="0.01"/>
- <param name="move_base/rot_stopped_velocity" value="0.01"/>
+ <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
+ <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
+ <param name="move_base/plannerType" value="ARAPlanner"/>
+ <param name="move_base/environmentType" value="2D"/>
+ <param name="move_base/controller_frequency" value="30.0"/>
+ <param name="move_base/map_update_frequency" value="2.0"/>
+ <param name="move_base/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="move_base/trans_stopped_velocity" value="0.01"/>
+ <param name="move_base/rot_stopped_velocity" value="0.01"/>
- <param name="costmap_2d/base_laser_max_range" value="20.0"/>
- <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
- <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
- <param name="costmap_2d/z_threshold_max" value="2.0"/>
- <param name="costmap_2d/z_threshold_min" value="0.13"/>
- <param name="costmap_2d/no_information_value" value="255"/>
- <param name="costmap_2d/freespace_projection_height" value="2.0"/>
- <param name="costmap_2d/inflation_radius" value="0.65"/>
- <param name="costmap_2d/circumscribed_radius" value="0.46"/>
- <param name="costmap_2d/inscribed_radius" value="0.325"/>
- <param name="costmap_2d/raytrace_window" value="10.0"/>
- <param name="costmap_2d/weight" value="0.1"/>
+ <param name="costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
+ <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="costmap_2d/z_threshold_min" value="0.13"/>
+ <param name="costmap_2d/no_information_value" value="255"/>
+ <param name="costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="costmap_2d/inflation_radius" value="0.65"/>
+ <param name="costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="costmap_2d/raytrace_window" value="10.0"/>
+ <param name="costmap_2d/weight" value="0.1"/>
- <param name="trajectory_rollout/map_size" value="4.0" />
- <param name="trajectory_rollout/acc_limit_x" value="2.5" />
- <param name="trajectory_rollout/acc_limit_y" value="2.5" />
- <param name="trajectory_rollout/acc_limit_th" value="3.2" />
- <param name="trajectory_rollout/sim_time" value="1.0" />
- <param name="trajectory_rollout/sim_granularity" value="0.05" />
- <param name="trajectory_rollout/vx_samples" value="15" />
- <param name="trajectory_rollout/vtheta_samples" value="15" />
- <param name="trajectory_rollout/path_distance_bias" value="0.6" />
- <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
- <param name="trajectory_rollout/occdist_scale" value="0.2" />
- <param name="trajectory_rollout/heading_lookahead" value="0.325" />
- <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
- <param name="trajectory_rollout/holonomic_robot" value="true" />
- <param name="trajectory_rollout/max_vel_x" value="0.5" />
- <param name="trajectory_rollout/min_vel_x" value="0.1" />
- <param name="trajectory_rollout/max_vel_th" value="1.0" />
- <param name="trajectory_rollout/min_vel_th" value="-1.0" />
- <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
- <param name="trajectory_rollout/freespace_model" value="true" />
- <param name="trajectory_rollout/dwa" value="false" />
- <param name="trajectory_rollout/simple_attractor" value="false" />
+ <param name="trajectory_rollout/map_size" value="4.0" />
+ <param name="trajectory_rollout/acc_limit_x" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_y" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_th" value="3.2" />
+ <param name="trajectory_rollout/sim_time" value="1.0" />
+ <param name="trajectory_rollout/sim_granularity" value="0.05" />
+ <param name="trajectory_rollout/vx_samples" value="15" />
+ <param name="trajectory_rollout/vtheta_samples" value="15" />
+ <param name="trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
+ <param name="trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="trajectory_rollout/heading_lookahead" value="0.325" />
+ <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
+ <param name="trajectory_rollout/holonomic_robot" value="true" />
+ <param name="trajectory_rollout/max_vel_x" value="0.5" />
+ <param name="trajectory_rollout/min_vel_x" value="0.1" />
+ <param name="trajectory_rollout/max_vel_th" value="1.0" />
+ <param name="trajectory_rollout/min_vel_th" value="-1.0" />
+ <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
+ <param name="trajectory_rollout/freespace_model" value="true" />
+ <param name="trajectory_rollout/dwa" value="false" />
+ <param name="trajectory_rollout/simple_attractor" value="false" />
- <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
- <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
+ <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
+ <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
- <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
- way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
- <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
+ way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
+ <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
- <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
- are not getting any data -->
- <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
- <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
- <param name="costmap_2d/stereo_update_rate" value="0.0"/>
- </node>
+ <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
+ are not getting any data -->
+ <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
+ <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
+ <param name="costmap_2d/stereo_update_rate" value="0.0"/>
+ </node>
- </group>
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/sim.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/sim.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -2,9 +2,10 @@
<param name="/use_sim_time" type="bool" value="1"/>
<param name="/global_frame_id" value="odom_offset"/>
<node pkg="tf" type="transform_sender" args="-20 -20 0 0 0 0 odom_offset odom 10"/>
+
<include file="$(find tabletop_manipulation)/launch/gazebo.launch"/>
<include file="$(find tabletop_manipulation)/launch/controllers_sim.launch"/>
<include file="$(find tabletop_manipulation)/launch/nav_sim.launch"/>
<include file="$(find tabletop_manipulation)/launch/perception_sim.launch"/>
- <include file="$(find tabletop_manipulation)/launch/planning.launch"/>
+ <!--include file="$(find tabletop_manipulation)/launch/planning.launch"/-->
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,17 +1,15 @@
<launch>
- <group name="wg">
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
- <param name="axis_vx" value="3" type="int"/>
- <param name="axis_vy" value="2" type="int"/>
- <param name="axis_vw" value="0" type="int"/>
- <param name="pan" value="4" type="int"/>
- <param name="tilt" value="5" type="int"/>
- <param name="max_vw" value="1.0" />
- <param name="max_vx" value="1.0" />
- <param name="max_vy" value="1.0" />
- <param name="deadman_button" value="4" type="int"/>
- <node pkg="teleop_base" type="teleop_base" args="--deadman_no_publish" respawn="true" machine="four" />
+ <param name="axis_vx" value="3" type="int"/>
+ <param name="axis_vy" value="2" type="int"/>
+ <param name="axis_vw" value="0" type="int"/>
+ <param name="pan" value="4" type="int"/>
+ <param name="tilt" value="5" type="int"/>
+ <param name="max_vw" value="1.0" />
+ <param name="max_vx" value="1.0" />
+ <param name="max_vy" value="1.0" />
+ <param name="deadman_button" value="4" type="int"/>
+ <node pkg="teleop_base" type="teleop_base" args="--deadman_no_publish" respawn="true" machine="four" />
- </group>
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-03-27 01:49:03 UTC (rev 13027)
@@ -37,7 +37,7 @@
roslib.load_manifest('tabletop_manipulation')
import rospy
from pr2_msgs.msg import MoveArmGoal, MoveArmState
-from robot_msgs.msg import JointState, PoseConstraint
+from robot_msgs.msg import JointState, PoseConstraint, ControllerStatus
import sys
@@ -50,7 +50,6 @@
def movearmCallback(self, msg):
#print '[MoveArm] Got status: %d'%(msg.status)
- self.goal_id = msg.goal_id
self.status = msg.status
def moveArmState(self, joints):
@@ -69,11 +68,11 @@
print '[MoveArm] Waiting for goal to be taken up...'
rospy.sleep(2.0)
- while self.status == None or self.status == MoveArmState.ACTIVE:
+ while self.status == None or self.status.value == ControllerStatus.ACTIVE:
print '[MoveArm] Waiting for goal achievement...'
rospy.sleep(1.0)
- return self.status == MoveArmState.INACTIVE
+ return self.status.value == ControllerStatus.SUCCESS
def moveArmEEPose(self, frame, position, orientation):
msg = MoveArmGoal()
@@ -125,11 +124,11 @@
print '[MoveArm] Waiting for goal to be taken up...'
rospy.sleep(2.0)
- while self.status == None or self.status == MoveArmState.ACTIVE:
+ while self.status == None or self.status.value == ControllerStatus.ACTIVE:
print '[MoveArm] Waiting for goal achievement...'
rospy.sleep(1.0)
- return self.status == MoveArmState.INACTIVE
+ return self.status.value == ControllerStatus.SUCCESS
USAGE = 'movearm.py {left|right} <shoulder_lift> <shoulder_pan>'
if __name__ == '__main__':
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movebase.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movebase.py 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movebase.py 2009-03-27 01:49:03 UTC (rev 13027)
@@ -36,17 +36,15 @@
import roslib
roslib.load_manifest('tabletop_manipulation')
import rospy
-from robot_msgs.msg import Planner2DGoal, Planner2DState
+from robot_msgs.msg import Planner2DGoal, Planner2DState, ControllerStatus
class MoveBase:
def __init__(self):
self.pub_goal = rospy.Publisher("goal", Planner2DGoal)
rospy.Subscriber("state", Planner2DState, self.stateCallback)
- self.goal_id = -1
self.status = None
def stateCallback(self, msg):
- self.goal_id = msg.goal_id
self.status = msg.status
def moveBase(self, frame, x, y, a):
@@ -64,11 +62,11 @@
print '[MoveBase] Waiting for goal to be taken up...'
rospy.sleep(2.0)
- while self.status == None or self.status == Planner2DState.ACTIVE:
+ while self.status == None or self.status.value == ControllerStatus.ACTIVE:
print '[MoveBase] Waiting for goal achievement...'
rospy.sleep(1.0)
- return self.status == Planner2DState.INACTIVE
+ return self.status.value == ControllerStatus.SUCCESS
if __name__ == '__main__':
import sys
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/cfg/launch_stage.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/cfg/launch_stage.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/cfg/launch_stage.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -2,7 +2,7 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false">
+ <node pkg="stage" type="stageros" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false">
<!--node pkg="rosstage" type="rosstage" name="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" -->
<param name="base_watchdog_timeout" value="0.2"/>
</node>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -15,7 +15,7 @@
<depend package="fake_localization"/>
<depend package="map_server"/>
<depend package="nav_view"/>
- <depend package="rosstage"/>
+ <depend package="stage"/>
<depend package="pr2_gazebo"/>
<depend package="robot_self_filter"/>
<depend package="point_cloud_assembler"/>
Modified: pkg/trunk/highlevel/test_highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -14,6 +14,6 @@
<depend package="fake_localization"/>
<depend package="map_server"/>
<depend package="nav_view"/>
- <depend package="rosstage"/>
+ <depend package="stage"/>
<depend package="pr2_gazebo"/>
</package>
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/build_for_stage.sh
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/build_for_stage.sh 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/build_for_stage.sh 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,6 +1,6 @@
#!/bin/bash
-PKGS="rosstage map_server fake_localization bullet nav_view"
+PKGS="stage map_server fake_localization bullet nav_view"
echo ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" | tee -a build_for_stage.log
date | tee -a build_for_stage.log
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -62,7 +62,7 @@
<param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
<!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
- way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
+ way too slow for the real system, but necessary for stageros, which does not reliably meet a 5Hz update rate. -->
<param name="costmap_2d/base_laser_update_rate" value="2.0"/>
<!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" name="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" >
+ <node pkg="stage" type="stageros" name="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" >
<param name="base_watchdog_timeout" value="0.2"/>
</node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" name="rosstage" args="-g $(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" >
+ <node pkg="stage" type="stageros" name="stageros" args="-g $(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" >
<param name="base_watchdog_timeout" value="0.5"/>
</node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" />
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
+ <node pkg="stage" type="stageros" name="stageros" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.wo...
[truncated message content] |
